cm

python/examples/cart_pulling.py

@@ -76,6 +76,32 @@ def isGripperRelativeToBaseOK(args, robot):
         isOK = True
     return isOK, grasp_pose 
 
+def areDualGrippersRelativeToBaseOK(args, goal_transform, robot):
+    isOK = False
+    # we want to be in the back of the base (x-axis) and on handlebar height
+    T_w_base = robot.data.oMi[1]
+    # rotation for the gripper is base with z flipped to point into the ground
+    rotate = pin.SE3(pin.rpy.rpyToMatrix(np.pi, 0.0, 0.0), np.zeros(3))
+    # translation is prefered distance from base
+    translate = pin.SE3(np.eye(3), np.array([args.base_to_handlebar_preferred_distance, 0.0, args.handlebar_height]))
+    #grasp_pose = T_w_base.act(rotate.act(translate))
+    grasp_pose = T_w_base.act(translate.act(rotate))
+
+    grasp_pose_left = grasp_pose.act(goal_transform)
+    grasp_pose_right = grasp_pose.act(goal_transform.inverse())
+
+    T_w_e_left, T_w_e_right = robot.getT_w_e()
+    SEerror_left = T_w_e_left.actInv(grasp_pose_left)
+    SEerror_right = T_w_e_right.actInv(grasp_pose_right)
+    err_vector_left = pin.log6(SEerror_left).vector 
+    err_vector_right = pin.log6(SEerror_right).vector 
+    # TODO: figure this out
+    # it seems you have to use just the arm to get to finish with this precision 
+    #if np.linalg.norm(err_vector) < robot.args.goal_error:
+    if (np.linalg.norm(err_vector_left) < 2*1e-1) and (np.linalg.norm(err_vector_right) < 2*1e-1):
+        isOK = True
+    return isOK, grasp_pose
+
 def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solver_pulling,
                            path_planner : ProcessManager, i : int, past_data):
     """
@@ -99,7 +125,10 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
     priority_register = ['0','1','1']
     # TODO implement this based on laser scanning or whatever
     #priority_register[0] = str(int(areObstaclesTooClose()))
+    if robot.robot_name != "yumi":
         graspOK, grasp_pose = isGripperRelativeToBaseOK(args, robot)
+    else:
+        graspOK, grasp_pose = areDualGrippersRelativeToBaseOK(args, goal_transform, robot)
     # NOTE: this keeps getting reset after initial grasp has been completed.
     # and we want to let mpc cook
     priority_register[1] = str(int(not graspOK)) # set if not ok
@@ -127,10 +156,19 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
         # TODO: make goal an argument, remove Mgoal from robot
         robot.Mgoal = grasp_pose
         if usempc:
-            # TODO: make it not hit the handlebar or other shit in the process
             for i, runningModel in enumerate(solver_grasp.problem.runningModels):
+                if robot.robot_name != "yumi":
                     runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+                else:
+                    # MASSIVE TODO: CREATE A DIFFERENT REFERENCE FOR EACH ARM
+                    runningModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose
+                    runningModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose
+            if robot.robot_name != "yumi":
                 solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = grasp_pose
+            else:
+                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_l'].cost.residual.reference = grasp_pose
+                solver_grasp.problem.terminalModel.differential.costs.costs['gripperPose_r'].cost.residual.reference = grasp_pose
+
             robot.Mgoal = grasp_pose
             #breakFlag, save_past_item, log_item = CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)
             CrocoIKMPCControlLoop(args, robot, solver_grasp, i, past_data)