cm

python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py

@@ -516,7 +516,7 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
                     # MASSIVE TODO: actually put in reference for left arm
                     path_ee[i],
                     state.nv)
-            l_eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+            l_eeTrackingCost = crocoddyl.CostModelResidual(state, l_eePoseResidual)
             runningCostModel.addCost("l_ee_pose" + str(i), l_eeTrackingCost, args.ee_pose_cost)
             r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
                     state,
@@ -524,7 +524,7 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
                     # MASSIVE TODO: actually put in reference for left arm
                     path_ee[i], 
                     state.nv)
-            r_eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+            r_eeTrackingCost = crocoddyl.CostModelResidual(state, r_eePoseResidual)
             runningCostModel.addCost("r_ee_pose" + str(i), r_eeTrackingCost, args.ee_pose_cost)