diff --git a/python/examples/.cart_pulling.py.swp b/python/examples/.cart_pulling.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..e874c004798a168923f4377f425b22573deb4629
Binary files /dev/null and b/python/examples/.cart_pulling.py.swp differ
diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp
index b627567f5f7baa20c97ef87c749e9283073497e2..d737650ae8977398706a53b075ca34bd36a1186a 100644
Binary files a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp and b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp differ
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 5b61558c475d4b3719efbca8210232c95eeae414..3be6cc063006a5e2240fb0fce2d549d9322efdcc 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -12,6 +12,8 @@ if importlib.util.find_spec('mim_solvers'):
import mim_solvers
def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
+ if robot.robot_name == "yumi":
+ goal_l, goal_r = goal
# create torque bounds which correspond to percentage
# of maximum allowed acceleration
# NOTE: idk if this makes any sense, but let's go for it
@@ -46,29 +48,64 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# to do so, you need to implement a residualmodel for that in crocoddyl.
# there's an example of exending crocoddyl in colmpc repo
# (look at that to see how to compile, make python bindings etc)
- framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
- state,
- # TODO: check if this is the frame we actually want (ee tip)
- # the id is an integer and that's what api wants
- robot.model.getFrameId("tool0"),
- goal.copy(),
- state.nv)
- goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
- # cost 4) final ee velocity is 0 (can't directly formulate joint velocity,
- # because that's a part of the state, and we don't know final joint positions)
- frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
- state,
- robot.model.getFrameId("tool0"),
- pin.Motion(np.zeros(6)),
- pin.ReferenceFrame.WORLD)
- frameVelocityCost = crocoddyl.CostModelResidual(state, frameVelocityResidual)
+ if robot.robot_name != "yumi":
+ framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+ state,
+ # TODO: check if this is the frame we actually want (ee tip)
+ # the id is an integer and that's what api wants
+ robot.model.getFrameId("tool0"),
+ goal.copy(),
+ state.nv)
+ goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
+ # cost 4) final ee velocity is 0 (can't directly formulate joint velocity,
+ # because that's a part of the state, and we don't know final joint positions)
+ frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
+ state,
+ robot.model.getFrameId("tool0"),
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD)
+ frameVelocityCost = crocoddyl.CostModelResidual(state, frameVelocityResidual)
+ runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ else:
+ framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
+ state,
+ # TODO: check if this is the frame we actually want (ee tip)
+ # the id is an integer and that's what api wants
+ robot.model.getFrameId("robl_joint_7"),
+ goal_l.copy(),
+ state.nv)
+ framePlacementResidual_r = crocoddyl.ResidualModelFramePlacement(
+ state,
+ # TODO: check if this is the frame we actually want (ee tip)
+ # the id is an integer and that's what api wants
+ robot.model.getFrameId("robr_joint_7"),
+ goal_r.copy(),
+ state.nv)
+ goalTrackingCost_l = crocoddyl.CostModelResidual(state, framePlacementResidual_l)
+ goalTrackingCost_r = crocoddyl.CostModelResidual(state, framePlacementResidual_r)
+ frameVelocityResidual_l = crocoddyl.ResidualModelFrameVelocity(
+ state,
+ robot.model.getFrameId("robl_joint_7"),
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD)
+ frameVelocityResidual_r = crocoddyl.ResidualModelFrameVelocity(
+ state,
+ robot.model.getFrameId("robr_joint_7"),
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD)
+ frameVelocityCost_l = crocoddyl.CostModelResidual(state, frameVelocityResidual_l)
+ frameVelocityCost_r = crocoddyl.CostModelResidual(state, frameVelocityResidual_r)
+ runningCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
+ runningCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
+ terminalCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
+ terminalCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
+
# put these costs into the running costs
- runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
runningCostModel.addCost("xReg", xRegCost, 1e-3)
runningCostModel.addCost("uReg", uRegCost, 1e-3)
# and add the terminal cost, which is the distance to the goal
# NOTE: shouldn't there be a final velocity = 0 here?
- terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
terminalCostModel.addCost("uReg", uRegCost, 1e3)
terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
@@ -104,7 +141,7 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# and this should be 2 * model.nv the way things are defined here.
- if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros":
+ if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros" or robot.robot_name == "yumi":
xlb = xlb[1:]
xub = xub[1:]