cm

python/ur_simple_control/optimal_control/crocoddyl_mpc.py

@@ -329,7 +329,12 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
         #print('adding base', path_base[i])
         #print("this was the prev ref", runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference)
         runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
+        if robot.robot_name != "yumi":
             runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+        # MASSIVE TODO: actually have different references for left and right arm
+        else:
+            runningModel.differential.costs.costs['l_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+            runningModel.differential.costs.costs['r_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
 
     # idk if that's necessary
     solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]

python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py

@@ -491,6 +491,11 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
         runningCostModel.addCost("uReg", uRegCost, 1e-3)
         if args.solver == "boxfddp":
             runningCostModel.addCost("limitCost", limitCost, 1e3)
+
+        ##########################
+        #  single arm reference  #
+        ##########################
+        if robot.robot_name != "yumi":
             eePoseResidual = crocoddyl.ResidualModelFramePlacement(
                     state,
                     robot.model.getFrameId("tool0"),
@@ -499,6 +504,28 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
                     state.nv)
             eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
             runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+        #########################
+        #  dual arm references  #
+        #########################
+        else:
+            l_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+                    state,
+                    robot.model.getFrameId("robl_joint_7"),
+                    # MASSIVE TODO: actually put in reference for left arm
+                    path_ee[i],
+                    state.nv)
+            eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+            runningCostModel.addCost("l_ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+            r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+                    state,
+                    robot.model.getFrameId("robr_joint_7"),
+                    # MASSIVE TODO: actually put in reference for left arm
+                    path_ee[i], 
+                    state.nv)
+            eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+            runningCostModel.addCost("r_ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+
+
         baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
                 state,
                 robot.model.getFrameId("mobile_base"),