comming for saving

python/examples/drawing_from_input_drawing.py

@@ -31,7 +31,7 @@ import matplotlib.pyplot as plt
 import copy
 from ur_simple_control.util.get_model import get_model
 from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.clik.clik_point_to_point import getController
+from ur_simple_control.clik.clik_point_to_point import getClikController
 
 #######################################################################
 #                            arguments                                #
@@ -56,8 +56,10 @@ def get_args():
     parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
             help="whether you're using the gripper", default=True)
 # not applicable here, but leaving it in the case it becomes applicable
-#    parser.add_argument('--goal-error', type=float, \
-#            help="the final position error you are happy with", default=1e-3)
+# it's also in the robot manager even though it shouldn't be
+    parser.add_argument('--goal-error', type=float, \
+            help="the final position error you are happy with. NOTE: not used here", \
+            default=1e-3)
     parser.add_argument('--max-iterations', type=int, \
             help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
     parser.add_argument('--acceleration', type=float, \
@@ -122,28 +124,26 @@ def get_args():
 #                            control loop                             #
 #######################################################################
 
-def controlLoopWriting(robot, controller, i):
+# feedforward velocity, feedback position and force for impedance
+def controller():
+    pass
+
+def controlLoopWriting(robot, dmp, controller, i):
     # dmp
-    dmp.step(dt)
+    dmp.step(robot.dt)
     # temporal coupling
-    tau = dmp.tau + tc.update(dmp, dt) * dt
+    tau = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
     dmp.set_tau(tau)
-    q = rtde_receive.getActualQ()
-    # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
-    # you already found an API in rtde_control for this, just put it in initialization 
-    # under using/not-using gripper parameters
+    q = robot.getQ()
     # TODO: document the mathematics with one-line comments
-    wrench = np.array(rtde_receive.getActualTCPForce())
+    wrench = robot.getWrench()
     # rolling average
     # TODO: try doing a low-pass filter instead
+    # TODO handle past data in some nice way from here or elsewhere?
     wrench_avg[i % 5] = wrench
     wrench = np.average(wrench_avg, axis=0)
     # pinocchio model is 8-dof because it treats the gripper
     # as two prismatic joints
-    q6 = np.array(copy.deepcopy(q))
-    q.append(0.0)
-    q.append(0.0)
-    q = np.array(q)
     pin.forwardKinematics(model, data, q)
     # calculate joint torques based on the force-torque sensor
     # TODO look into UR code/api for estimating the same
@@ -179,7 +179,7 @@ def controlLoopWriting(robot, controller, i):
 
 if __name__ == "__main__":
     args = parser.get_args()
-    controller = getController(args)
+    clik_controller = getController(args)
     robot = RobotManager(args)
     plot_dict = {
         'qs' : np.zeros((args.max_iterations, 6)),
@@ -202,8 +202,17 @@ if __name__ == "__main__":
     if not args.temporal_coupling:
         tc = NoTC()
     else:
-        # TODO learn the math already
         # TODO test whether this works (it should, but test it)
-        v_max = np.ones(6) * 0.5 * speed_slider
-        a_max = np.ones(6) * 1.7
+        # test the interplay between this and the speed slider
+        v_max_ndarray = np.ones(6) * robot.max_qd
+        # test the interplay between this and the speed slider
+        # args.acceleration is the actual maximum you're using
+        a_max_ndarray = np.ones(6) * args.acceleration
         tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps_tc)
+
+    # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
+    # you already found an API in rtde_control for this, just put it in initialization 
+    # under using/not-using gripper parameters
+    # ALSO NOTE: to use this you need to change the version inclusions in
+    # ur_rtde due to a bug there in the current ur_rtde + robot firmware version 
+    # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)

python/ur_simple_control/clik/clik_point_to_point.py

@@ -3,7 +3,7 @@ import numpy as np
 import copy
 import argparse
 from functools import partial
-from ur_simple_control.util.boilerplate_wrapper import ControlLoopManager, RobotManager
+from ur_simple_control.managers import ControlLoopManager, RobotManager
 
 """
 Every imaginable magic number, flag and setting is put in here.
@@ -44,7 +44,7 @@ def get_args():
     parser.add_argument('--tikhonov-damp', type=float, \
             help="damping scalar in tiknohov regularization", default=1e-2)
     # TODO add the rest
-    parser.add_argument('--controller', type=str, \
+    parser.add_argument('--clik-controller', type=str, \
             help="select which click algorithm you want", \
             default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose'])
         # maybe you want to scale the control signal
@@ -76,10 +76,10 @@ if they have extra stuff, just map it in the beginning with partial
 NOTE: this could be changed to something else if it proves inappropriate later
 TODO: write out other algorithms
 """
-def getController(args):
-    if args.controller == "dampedPseudoinverse":
+def getClikController(args):
+    if args.clik_controller == "dampedPseudoinverse":
         return partial(dampedPseudoinverse, args.tikhonov_damp)
-    if args.controller == "jacobianTranspose":
+    if args.clik_controller == "jacobianTranspose":
         return jacobianTranspose
     # TODO implement and add in the rest
     #if controller_name == "invKinmQPSingAvoidE_kI":
@@ -102,7 +102,7 @@ def getController(args):
 
 
 # modularity yo
-def controlLoopClik(robot, controller, i):
+def controlLoopClik(robot, clik_controller, i):
     breakFlag = False
     # know where you are, i.e. do forward kinematics
     q = robot.getQ()
@@ -117,7 +117,7 @@ def controlLoopClik(robot, controller, i):
     J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
     # compute the joint velocities.
     # just plug and play different ones
-    qd = controller(J, err_vector)
+    qd = clik_controller(J, err_vector)
     robot.sendQd(qd)
     # we do the printing here because controlLoopManager should be general.
     # and these prints are click specific (although i'm not 100% happy with the arrangement)
@@ -134,7 +134,7 @@ if __name__ == "__main__":
     args = get_args()
     robot = RobotManager(args)
     Mgoal = robot.defineGoalPoint()
-    controller = getController(args)
-    controlLoop = partial(controlLoopClik, robot, controller)
+    clik_controller = getClikController(args)
+    controlLoop = partial(controlLoopClik, robot, clik_controller)
     loop_manager = ControlLoopManager(robot, controlLoop, args)
     loop_manager.run()

python/ur_simple_control/boilerplate_wrapper.py → python/ur_simple_control/managers.py

@@ -15,6 +15,38 @@ from ur_simple_control.util.get_model import get_model
 from ur_simple_control.util.robotiq_gripper import RobotiqGripper
 import argparse
 
+"""
+general notes
+---------------
+The first design principle of this library is to minimize the time needed
+to go from a control algorithm on paper to the same control algorithm 
+running on the real robot. The second design principle is to have
+the code as simple as possible. In particular, this pertains to avoiding
+overly complex abstractions and having the code as concise as possible.
+The user is expected to read and understand the entire codebase because
+changes will have to accomodate it to their specific project.
+Target users are control engineers.
+The final design choices are made to accommodate these sometimes opposing goals
+to the best of the author's ability.
+
+This file contains a robot manager and a control loop manager.
+The point of these managers is to handle:
+    - boiler plate code around the control loop which is always the same
+    - have all the various parameters neatly organized and in one central location
+    - hide the annoying if-elses of different APIs required 
+      for the real robot and various simulations with single commands
+      that just do exactly what you want them to do
+
+current state
+-------------
+Everything is UR specific. 
+
+long term vision
+-------------------
+Cut out the robot-specific parts out of the manager classes,
+and create child classes for particular robots.
+"""
+
 """
 ControlLoopManager
 -------------------
@@ -154,7 +186,6 @@ class RobotManager:
             self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
             self.rtde_io = RTDEIOInterface("127.0.0.1")
 
-
         # ur specific magic numbers 
         self.n_joints = 6
         # last joint because pinocchio adds base frame as 0th joint.
@@ -167,7 +198,7 @@ class RobotManager:
         # and i'm not clipping it, you're fixing it
         assert args.acceleration <= 1.7 and args.acceleration > 0.0
         # we're not saying it's qdd because it isn't directly (apparently)
-        # TODO: check that
+        # TODO: check that again
         self.acceleration = args.acceleration
         if not args.pinocchio_only:
             self.rtde_io.setSpeedSlider(args.speed_slider)
@@ -175,6 +206,7 @@ class RobotManager:
         # TODO: these are almost certainly higher
         # maybe you want to have them high and cap the rest with speed slider?
         # idk really, but it's better to have this low and burried for safety and robot longevity reasons
+        # TODO: 
         self.max_qdd = 1.7
         # NOTE: i had this thing at 2 in other code
         self.max_qd = 0.5
@@ -212,6 +244,22 @@ class RobotManager:
             return self.q
         return q
 
+    """
+    getWrench
+    -----
+    different things need to be send depending on whether you're running a simulation,
+    you're on a real robot, you're running some new simulator bla bla. this is handled
+    here because this things depend on the arguments which are manager here (hence the 
+    class name RobotManager)
+    """
+    def getWrench(self):
+        if not self.pinocchio_only:
+            wrench = np.array(self.rtde_receive.getActualTCPForce())
+        else:
+            raise NotImplementedError("Don't have time to implement this right now.")
+
+        return wrench
+
     """
     sendQd
     -----
@@ -235,6 +283,7 @@ class RobotManager:
             self.q = pin.integrate(self.model, self.q, qd * self.dt)
 
 
+
     """
     defineGoalPoint
     ------------------