cm

python/examples/cart_pulling.py

@@ -10,7 +10,7 @@ from ur_simple_control.optimal_control.crocoddyl_mpc import *
 from ur_simple_control.basics.basics import followKinematicJointTrajP
 from ur_simple_control.util.logging_utils import LogManager
 from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse, controlLoopClikArmOnly
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse, controlLoopClikArmOnly, controlLoopClikDualArmsOnly
 import pinocchio as pin
 import crocoddyl
 from functools import partial
@@ -88,7 +88,11 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
     """
 
     q = robot.getQ()
+    if robot.robot_name != "yumi"
         T_w_e = robot.getT_w_e()
+    else:
+        T_w_e_l, T_w_e_right = robot.getT_w_e()
+
     # we use binary as string representation (i don't want to deal with python's binary representation).
     # the reason for this is that then we don't have disgusting nested ifs
     # TODO: this has to have named entries for code to be readable
@@ -134,7 +138,12 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
             #controlLoopClik(robot, invKinmQP, i, past_data)
             clikController = partial(dampedPseudoinverse, 1e-3)
             #controlLoopClik(robot, clikController, i, past_data)
+            if robot.robot_name != "yumi":
                 controlLoopClikArmOnly(robot, clikController, i, past_data)
+            else:
+                # TODO: DEFINE SENSIBLE TRANSFOR
+                goal_transform = pin.SE3.Identity()
+                controlLoopClikDualArmsOnly(robot, clikController, goal_transform, i, past_data)
 
     # case 2)
     # MASSIVE TODO: 
@@ -207,10 +216,16 @@ def cartPulling(args, robot : RobotManager, goal, path_planner):
 
     log_item = {}
     q = robot.getQ()
+    if robot.robot_name != "yumi":
         T_w_e = robot.getT_w_e()
+    else:
+        T_w_e_l, T_w_e_right = robot.getT_w_e()
     log_item['qs'] = q.reshape((robot.model.nq,))
     log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
-    save_past_item = {'path2D_untimed' : T_w_e.translation[:2],
+    #T_base = self.robot_manager.data.oMi[1]
+    # NOTE: why the fuck was the past path defined from the end-effector?????
+    #save_past_item = {'path2D_untimed' : T_w_e.translation[:2],
+    save_past_item = {'path2D_untimed' : q[:2],
                       'priority_register' : ['0','1','1']}
     loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_item, log_item)
     loop_manager.run()

python/ur_simple_control/clik/clik.py

@@ -590,6 +590,45 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
     np.savetxt("./joint_trajectory.csv", joint_trajectory, delimiter=',', fmt='%.5f')
     return joint_trajectory
 
+def controlLoopClikDualArmsOnly(robot : RobotManager, clik_controller, goal_transform, i, past_data)
+    """
+    controlLoopClikDualArmsOnly
+    ---------------
+    """
+    breakFlag = False
+    log_item = {}
+    q = robot.getQ()
+    T_w_e_left, T_w_e_right = robot.getT_w_e()
+    # 
+    Mgoal_left = robot.Mgoal.act(goal_transform)
+    Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+
+    SEerror_left = T_w_e_left.actInv(Mgoal_left)
+    SEerror_right = T_w_e_right.actInv(Mgoal_right)
+
+    err_vector_left = pin.log6(SEerror_left).vector 
+    err_vector_right = pin.log6(SEerror_right).vector 
+
+    if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (np.linalg.norm(err_vector_right) < robot.args.goal_error):
+        breakFlag = True
+    J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
+    J_left = J_left[:,3:]
+    J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
+    J_right = J_right[:,3:]
+
+
+    # compute the joint velocities based on controller you passed
+    qd_left = clik_controller(J_left, err_vector_left)
+    qd_right = clik_controller(J_right, err_vector_right)
+    qd = qd_left + qd_right
+    robot.sendQd(qd)
+    
+    log_item['qs'] = q.reshape((robot.model.nq,))
+    log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+    log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+    # we're not saving here, but need to respect the API, 
+    # hence the empty dict
+    return breakFlag, {}, log_item
 
 if __name__ == "__main__": 
     args = get_args()