diff --git a/examples/optimal_control/dual_arm_ik_mpc.py b/examples/optimal_control/dual_arm_ik_mpc.py
index 7b0031825a7d33ce3723ae590450d5d6dee56b1f..82a3cc27a7b348efc16945186a6dda93f156a528 100644
--- a/examples/optimal_control/dual_arm_ik_mpc.py
+++ b/examples/optimal_control/dual_arm_ik_mpc.py
@@ -21,7 +21,7 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = getRobotFromArgs(args)
- T_w_goal = getRandomlyGeneratedGoal(args)
+ T_w_absgoal = getRandomlyGeneratedGoal(args)
T_absgoal_lgoal = pin.SE3.Identity()
T_absgoal_lgoal.translation[1] = 0.1
T_absgoal_rgoal = pin.SE3.Identity()
@@ -29,9 +29,9 @@ if __name__ == "__main__":
if args.visualizer:
# TODO: document this somewhere
- robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
+ robot.visualizer_manager.sendCommand({"Mgoal": T_w_absgoal})
- CrocoDualEEP2PMPC(args, robot, T_w_goal)
+ CrocoDualEEP2PMPC(args, robot, T_w_absgoal, T_absgoal_lgoal, T_absgoal_rgoal)
if args.real:
robot.stopRobot()
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index e2551af9dd86faa346d107d208d10fe1492a943b..26127244403d396c6cbb99b8441428bab036295d 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -1,4 +1,4 @@
-from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.robots.abstract_robotmanager import AbstractRobotManager
import numpy as np
import crocoddyl
@@ -84,8 +84,12 @@ class CrocoOCP(abc.ABC):
# put these costs into the running costs
for i in range(self.args.n_knots):
- self.runningCostModels[i].addCost("xReg", self.xRegCost, self.args.u_reg_cost)
- self.runningCostModels[i].addCost("uReg", self.uRegCost, self.args.x_reg_cost)
+ self.runningCostModels[i].addCost(
+ "xReg", self.xRegCost, self.args.u_reg_cost
+ )
+ self.runningCostModels[i].addCost(
+ "uReg", self.uRegCost, self.args.x_reg_cost
+ )
# and add the terminal cost, which is the distance to the goal
# NOTE: shouldn't there be a final velocity = 0 here?
# --> no if you're not stopping at the last knot!
@@ -123,12 +127,7 @@ class CrocoOCP(abc.ABC):
# TODO: replace this with subclass or whatever call
# to check if the robot has mobilebase interface
- if (
- self.robot.robot_name == "heron"
- or self.robot.robot_name == "heronros"
- or self.robot.robot_name == "mirros"
- or self.robot.robot_name == "yumi"
- ):
+ if self.robot.robot_name == "heron" or self.robot.robot_name == "myumi":
self.xlb = self.xlb[1:]
self.xub = self.xub[1:]
diff --git a/python/smc/control/optimal_control/croco_mpc_path_following.py b/python/smc/control/optimal_control/croco_mpc_path_following.py
index c2aa9cf6b1fddd7c02fd6e3cbf3776c76e5e4406..5e0fb60542795bc809249e236dd44f2438c33e56 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -393,7 +393,6 @@ def BaseAndEEPathFollowingMPC(
# TODO: the robot put in is a whole body robot,
# which you need to implement
-THIS IS THE STILL BROKEN ONE
def BaseAndDualArmEEPathFollowingMPCControlLoop(
args,
robot,
@@ -413,6 +412,7 @@ def BaseAndDualArmEEPathFollowingMPCControlLoop(
or an instance of ProcessManager which spits out path points
by calling ProcessManager.getData()
"""
+ raise NotImplementedError
breakFlag = False
log_item = {}
save_past_dict = {}
@@ -530,7 +530,6 @@ def BaseAndDualArmEEPathFollowingMPCControlLoop(
return breakFlag, save_past_dict, log_item
-NOT TEMPLETIZED YET
def BaseAndDualArmEEPathFollowingMPC(args, robot, path_planner):
"""
CrocoEndEffectorPathFollowingMPC
@@ -540,7 +539,7 @@ def BaseAndDualArmEEPathFollowingMPC(args, robot, path_planner):
a dynamics level, and velocities we command
are actually extracted from the state x(q,dq).
"""
-
+ raise NotImplementedError("this has to be templetized like the previous cases")
x0 = np.concatenate([robot.q, robot.v])
ocp = BaseAndDualArmEEPathFollowingOCP(args, robot, x0)
ocp.solveInitialOCP(x0)
diff --git a/python/smc/control/optimal_control/croco_mpc_point_to_point.py b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
index 8d4b489135c644568645d468fbe27ca512a1ba1b..d79472566a2f9bdeda9e74dc0cac8c9fe218fcc1 100644
--- a/python/smc/control/optimal_control/croco_mpc_point_to_point.py
+++ b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
@@ -121,7 +121,10 @@ def CrocoDualEEP2PMPC(
are actually extracted from the state x(q,dq)
"""
x0 = np.concatenate([robot.q, robot.v])
- ocp = DualArmIKOCP(args, robot, x0, T_w_absgoal)
+ T_w_lgoal = T_absgoal_l.act(T_w_absgoal)
+ T_w_rgoal = T_absgoal_r.act(T_w_absgoal)
+
+ ocp = DualArmIKOCP(args, robot, x0, (T_w_lgoal, T_w_rgoal))
ocp.solveInitialOCP(x0)
controlLoop = partial(
@@ -136,9 +139,10 @@ def CrocoDualEEP2PMPC(
)
log_item = {
"qs": np.zeros(robot.nq),
- "err_norm": np.zeros(1),
"dqs": np.zeros(robot.nv),
"dqs_cmd": np.zeros(robot.nv),
+ "l_err_norm": np.zeros(1),
+ "r_err_norm": np.zeros(1),
}
save_past_dict = {}
loop_manager = ControlLoopManager(
diff --git a/python/smc/control/optimal_control/path_following_croco_ocp.py b/python/smc/control/optimal_control/path_following_croco_ocp.py
index d91b9794979d3a499296b19881dca6469adf210b..698bf71ea0b5b8c25d157c6c1739199bc5bbee1f 100644
--- a/python/smc/control/optimal_control/path_following_croco_ocp.py
+++ b/python/smc/control/optimal_control/path_following_croco_ocp.py
@@ -2,7 +2,10 @@ from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.robots.interfaces.dual_arm_interface import DualArmInterface
-from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface, DualArmWholeBodyInterface
+from smc.robots.interfaces.whole_body_interface import (
+ SingleArmWholeBodyInterface,
+ DualArmWholeBodyInterface,
+)
import numpy as np
import crocoddyl
@@ -178,8 +181,12 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
].cost.residual.reference = pathSE3[-1]
+class DualArmEEPathFollowingOCP(CrocoOCP):
+ def __init__(self, args, robot: DualArmInterface, x0):
+ goal = None
+ raise NotImplementedError
+ super().__init__(args, robot, x0, goal)
-JUST THE DUAL ARM IS MISSING
class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
def __init__(self, args, robot: DualArmWholeBodyInterface, x0):
diff --git a/python/smc/control/optimal_control/point_to_point_croco_ocp.py b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
index d4042997061d0f567150e23d0daa17737740cf5e..1b9625dc447bb6e835c8919bf14357e8e0fb0216 100644
--- a/python/smc/control/optimal_control/point_to_point_croco_ocp.py
+++ b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
@@ -33,15 +33,17 @@ class SingleArmIKOCP(CrocoOCP):
goalTrackingCost = crocoddyl.CostModelResidual(
self.state, framePlacementResidual
)
- frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
- self.state,
- self.robot.ee_frame_id,
- pin.Motion(np.zeros(6)),
- pin.ReferenceFrame.WORLD,
- )
- frameVelocityCost = crocoddyl.CostModelResidual(
- self.state, frameVelocityResidual
- )
+ # TODO: final velocity costs only make sense if you're running a single ocp, but not mpc!!
+ # TODO: have an argument or something to include or not include them!
+ # frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
+ # self.state,
+ # self.robot.ee_frame_id,
+ # pin.Motion(np.zeros(6)),
+ # pin.ReferenceFrame.WORLD,
+ # )
+ # frameVelocityCost = crocoddyl.CostModelResidual(
+ # self.state, frameVelocityResidual
+ # )
for i in range(self.args.n_knots):
self.runningCostModels[i].addCost(
"gripperPose" + str(i), goalTrackingCost, 1e2
@@ -49,7 +51,7 @@ class SingleArmIKOCP(CrocoOCP):
self.terminalCostModel.addCost(
"gripperPose" + str(self.args.n_knots), goalTrackingCost, 1e2
)
- self.terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
+ # self.terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
# there is nothing to update in a point-to-point task
def updateCosts(self, data):
@@ -76,13 +78,13 @@ class DualArmIKOCP(CrocoOCP):
T_w_lgoal, T_w_rgoal = goal
framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
self.state,
- self.robot.model.l_ee_frame_id,
+ self.robot.l_ee_frame_id,
T_w_lgoal.copy(),
self.state.nv,
)
framePlacementResidual_r = crocoddyl.ResidualModelFramePlacement(
self.state,
- self.robot.model.r_ee_frame_id,
+ self.robot.r_ee_frame_id,
T_w_rgoal.copy(),
self.state.nv,
)
@@ -92,24 +94,27 @@ class DualArmIKOCP(CrocoOCP):
goalTrackingCost_r = crocoddyl.CostModelResidual(
self.state, framePlacementResidual_r
)
- frameVelocityResidual_l = crocoddyl.ResidualModelFrameVelocity(
- self.state,
- self.robot.model.l_ee_frame_id,
- pin.Motion(np.zeros(6)),
- pin.ReferenceFrame.WORLD,
- )
- frameVelocityResidual_r = crocoddyl.ResidualModelFrameVelocity(
- self.state,
- self.robot.model.r_ee_frame_id,
- pin.Motion(np.zeros(6)),
- pin.ReferenceFrame.WORLD,
- )
- frameVelocityCost_l = crocoddyl.CostModelResidual(
- self.state, frameVelocityResidual_l
- )
- frameVelocityCost_r = crocoddyl.CostModelResidual(
- self.state, frameVelocityResidual_r
- )
+ # TODO: final velocity costs only make sense if you're running a single ocp, but not mpc!!
+ # TODO: have an argument or something to include or not include them!
+
+ # frameVelocityCost_l = crocoddyl.CostModelResidual(
+ # self.state, frameVelocityResidual_l
+ # frameVelocityResidual_l = crocoddyl.ResidualModelFrameVelocity(
+ # self.state,
+ # self.robot.l_ee_frame_id,
+ # pin.Motion(np.zeros(6)),
+ # pin.ReferenceFrame.WORLD,
+ # )
+ # frameVelocityResidual_r = crocoddyl.ResidualModelFrameVelocity(
+ # self.state,
+ # self.robot.r_ee_frame_id,
+ # pin.Motion(np.zeros(6)),
+ # pin.ReferenceFrame.WORLD,
+ # )
+ # )
+ # frameVelocityCost_r = crocoddyl.CostModelResidual(
+ # self.state, frameVelocityResidual_r
+ # )
for i in range(self.args.n_knots):
self.runningCostModels[i].addCost(
"gripperPose_l" + str(i), goalTrackingCost_l, 1e2
@@ -123,8 +128,8 @@ class DualArmIKOCP(CrocoOCP):
self.terminalCostModel.addCost(
"gripperPose_r" + str(self.args.n_knots), goalTrackingCost_r, 1e2
)
- self.terminalCostModel.addCost("velFinal_l", frameVelocityCost_l, 1e3)
- self.terminalCostModel.addCost("velFinal_r", frameVelocityCost_r, 1e3)
+ # self.terminalCostModel.addCost("velFinal_l", frameVelocityCost_l, 1e3)
+ # self.terminalCostModel.addCost("velFinal_r", frameVelocityCost_r, 1e3)
# there is nothing to update in a point-to-point task
def updateCosts(self, data):