diff --git a/examples/cart_pulling/control_sub_problems/with_planner/base_and_dual_arm_ee_path_following_from_planner.py b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
similarity index 100%
rename from examples/cart_pulling/control_sub_problems/with_planner/base_and_dual_arm_ee_path_following_from_planner.py
rename to examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
diff --git a/examples/cart_pulling/control_sub_problems/fixed_paths/base_and_ee_path_following.py b/examples/cart_pulling/control_sub_problems/fixed_paths/base_and_ee_path_following.py
deleted file mode 100644
index f7c1fd338071b2cb6a7e836b4fcc7576cca945f0..0000000000000000000000000000000000000000
--- a/examples/cart_pulling/control_sub_problems/fixed_paths/base_and_ee_path_following.py
+++ /dev/null
@@ -1,56 +0,0 @@
-from smc import getRobotFromArgs
-from smc import getMinimalArgParser
-from smc.control.optimal_control.util import get_OCP_args
-from smc.control.cartesian_space import getClikArgs
-from smc.control.optimal_control.croco_mpc_path_following import (
- BaseAndEEPathFollowingMPC,
-)
-import numpy as np
-
-
-def path(T_w_e, i):
- # 2) do T_mobile_base_ee_pos to get
- # end-effector reference frame(s)
-
- # generate bullshit just to see it works
- path_ee = []
- path_base = []
- t = i * robot.dt
- for i in range(args.n_knots):
- t += 0.01
- new = T_w_e.copy()
- translation = 2 * np.array([np.cos(t / 20), np.sin(t / 20), 0.3])
- new.translation = translation
- path_ee.append(new)
- translation[2] = 0.0
- path_base.append(translation)
- return path_base, path_ee
-
-
-def get_args():
- parser = getMinimalArgParser()
- parser = get_OCP_args(parser)
- parser = getClikArgs(parser) # literally just for goal error
- args = parser.parse_args()
- return args
-
-
-if __name__ == "__main__":
- args = get_args()
- robot = getRobotFromArgs(args)
- x0 = np.concatenate([robot.q, robot.v])
- robot._step()
-
- BaseAndEEPathFollowingMPC(args, robot, path)
-
- print("final position:", robot.T_w_e)
-
- if args.real:
- robot.stopRobot()
-
- if args.save_log:
- robot._log_manager.saveLog()
- robot._log_manager.plotAllControlLoops()
-
- if args.visualizer:
- robot.killManipulatorVisualizer()
diff --git a/examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py b/examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py
deleted file mode 100644
index 40021e9420f356e4e10b5b85c5da0ebce7bb9361..0000000000000000000000000000000000000000
--- a/examples/cart_pulling/control_sub_problems/fixed_paths/dual_arm_path_following.py
+++ /dev/null
@@ -1,53 +0,0 @@
-from smc import getRobotFromArgs
-from smc import getMinimalArgParser
-from smc.control.optimal_control.util import get_OCP_args
-from smc.control.cartesian_space import getClikArgs
-from smc.control.optimal_control.croco_mpc_path_following import (
- CrocoEEPathFollowingMPC,
-)
-import numpy as np
-
-
-def path(T_w_e, i):
- # 2) do T_mobile_base_ee_pos to get
- # end-effector reference frame(s)
-
- # generate bullshit just to see it works
- path = []
- t = i * robot.dt
- for i in range(args.n_knots):
- t += 0.01
- new = T_w_e.copy()
- translation = 2 * np.array([np.cos(t / 20), np.sin(t / 20), 0.3])
- new.translation = translation
- path.append(new)
- return path
-
-
-def get_args():
- parser = getMinimalArgParser()
- parser = get_OCP_args(parser)
- parser = getClikArgs(parser) # literally just for goal error
- args = parser.parse_args()
- return args
-
-
-if __name__ == "__main__":
- args = get_args()
- robot = getRobotFromArgs(args)
- x0 = np.concatenate([robot.q, robot.v])
- robot._step()
-
- CrocoDualEEPathFollowingMPC(args, robot, x0, path)
-
- print("final position:", robot.T_w_e)
-
- if args.real:
- robot.stopRobot()
-
- if args.save_log:
- robot._log_manager.saveLog()
- robot._log_manager.plotAllControlLoops()
-
- if args.visualizer:
- robot.killManipulatorVisualizer()
diff --git a/examples/cart_pulling/control_sub_problems/fixed_paths/ee_only_path_following.py b/examples/cart_pulling/control_sub_problems/fixed_paths/ee_only_path_following.py
deleted file mode 100644
index 2e11a54104211e46764924edde04603c624414d5..0000000000000000000000000000000000000000
--- a/examples/cart_pulling/control_sub_problems/fixed_paths/ee_only_path_following.py
+++ /dev/null
@@ -1,53 +0,0 @@
-from smc import getRobotFromArgs
-from smc import getMinimalArgParser
-from smc.control.optimal_control.util import get_OCP_args
-from smc.control.cartesian_space import getClikArgs
-from smc.control.optimal_control.croco_mpc_path_following import (
- CrocoEEPathFollowingMPC,
-)
-import numpy as np
-
-
-def path(T_w_e, i):
- # 2) do T_mobile_base_ee_pos to get
- # end-effector reference frame(s)
-
- # generate bullshit just to see it works
- path = []
- t = i * robot.dt
- for i in range(args.n_knots):
- t += 0.01
- new = T_w_e.copy()
- translation = 2 * np.array([np.cos(t / 20), np.sin(t / 20), 0.3])
- new.translation = translation
- path.append(new)
- return path
-
-
-def get_args():
- parser = getMinimalArgParser()
- parser = get_OCP_args(parser)
- parser = getClikArgs(parser) # literally just for goal error
- args = parser.parse_args()
- return args
-
-
-if __name__ == "__main__":
- args = get_args()
- robot = getRobotFromArgs(args)
- x0 = np.concatenate([robot.q, robot.v])
- robot._step()
-
- CrocoEEPathFollowingMPC(args, robot, x0, path)
-
- print("final position:", robot.T_w_e)
-
- if args.real:
- robot.stopRobot()
-
- if args.save_log:
- robot._log_manager.saveLog()
- robot._log_manager.plotAllControlLoops()
-
- if args.visualizer:
- robot.killManipulatorVisualizer()
diff --git a/examples/cart_pulling/control_sub_problems/with_planner/base_and_ee_path_following_from_planner.py b/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling.py
similarity index 89%
rename from examples/cart_pulling/control_sub_problems/with_planner/base_and_ee_path_following_from_planner.py
rename to examples/cart_pulling/control_sub_problems/single_arm_cart_pulling.py
index 2d34a8528513c6f8fafbda71ef093e0b260ef59a..993171a1fae2a5a455f527f347f2d85dee603afc 100644
--- a/examples/cart_pulling/control_sub_problems/with_planner/base_and_ee_path_following_from_planner.py
+++ b/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling.py
@@ -6,15 +6,15 @@ from smc.control.optimal_control.util import get_OCP_args
from smc.control.cartesian_space import getClikArgs
from smc.path_generation.planner import starPlanner, getPlanningArgs
from smc.control.optimal_control.croco_mpc_point_to_point import CrocoEEAndBaseP2PMPC
-from smc.control.optimal_control.croco_mpc_path_following import (
- BaseAndEEPathFollowingMPC,
-)
+from single_arm_cart_pulling_control_loop import SingleArmCartPullingMPC
from smc.multiprocessing import ProcessManager
import time
import numpy as np
from functools import partial
import pinocchio as pin
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
def get_args():
@@ -43,6 +43,8 @@ if __name__ == "__main__":
robot = getRobotFromArgs(args)
# TODO: HOW IS IT POSSIBLE THAT T_W_E IS WRONG WITHOUT STEP CALLED HERE?????????????????
robot._step()
+ assert issubclass(robot.__class__, SingleArmInterface)
+ assert issubclass(robot.__class__, MobileBaseInterface)
T_w_e = robot.T_w_e
robot._q[0] = 9.0
robot._q[1] = 4.0
@@ -88,9 +90,7 @@ if __name__ == "__main__":
# TODO: UNCOMMENT
CrocoEEAndBaseP2PMPC(args, robot, pathSE3[0], p_base)
print("initialized!")
- BaseAndEEPathFollowingMPC(args, robot, path_planner)
-
- print("final position:", robot.T_w_e)
+ SingleArmCartPullingMPC(args, robot, path_planner)
if args.real:
robot.stopRobot()
diff --git a/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling_control_loop.py b/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling_control_loop.py
new file mode 100644
index 0000000000000000000000000000000000000000..8a944a57f2fd5f4daba2d7b1f7c5d4c271d90f55
--- /dev/null
+++ b/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling_control_loop.py
@@ -0,0 +1,135 @@
+from smc.robots.interfaces.whole_body_interface import (
+ SingleArmWholeBodyInterface,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.multiprocessing.process_manager import ProcessManager
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.path_following_croco_ocp import (
+ BaseAndEEPathFollowingOCP,
+)
+from smc.path_generation.path_math.cart_pulling_path_math import (
+ construct_EE_path,
+)
+from smc.path_generation.path_math.path_to_trajectory import (
+ path2D_to_trajectory2D,
+)
+from smc.control.controller_templates.path_following_template import (
+ PathFollowingFromPlannerCtrllLoopTemplate,
+)
+
+import numpy as np
+from functools import partial
+import types
+from argparse import Namespace
+from pinocchio import SE3, log6
+from collections import deque
+
+
+def initializePastData(
+ args: Namespace, T_w_e: SE3, p_base: np.ndarray, max_base_v: float
+) -> np.ndarray:
+ # prepopulate past data to make base and cart be on the same path in the past
+ # (which didn't actually happen because this just started)
+ p_ee = T_w_e.translation[:2]
+ straight_line_path = np.linspace(p_ee, p_base, args.past_window_size)
+ # straight_line_path_timed = path2D_timed(args, straight_line_path, max_base_v)
+ # return straight_line_path_timed # this one is shortened to args.n_knots! and we want the whole buffer
+ return straight_line_path
+
+
+def SingleArmCartPullingMPCControlLoop(
+ ocp: CrocoOCP,
+ path2D_base: np.ndarray,
+ args: Namespace,
+ robot: SingleArmWholeBodyInterface,
+ t: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ p = robot.q[:2]
+
+ # NOTE: this one is obtained as the future path from path planner
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+ trajectory_base = path2D_to_trajectory2D(args, path2D_base, max_base_v)
+ trajectory_base = np.hstack((trajectory_base, np.zeros((len(trajectory_base), 1))))
+
+ trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D"])
+
+ if args.visualizer:
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ robot.visualizer_manager.sendCommand({"path": trajectory_base})
+ robot.visualizer_manager.sendCommand(
+ {"frame_path": trajectorySE3_handlebar}
+ )
+
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0, data=(trajectory_base, trajectorySE3_handlebar))
+ xs = np.array(ocp.solver.xs)
+ v_cmd = xs[1, robot.model.nq :]
+
+ err_vector_ee = log6(robot.T_w_e.actInv(trajectorySE3_handlebar[0]))
+ err_vector_base = np.linalg.norm(p - trajectory_base[0][:2]) # z axis is irrelevant
+ log_item = {}
+ log_item["err_vec_ee"] = err_vector_ee
+ log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
+ log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
+ save_past_item = {"path2D": p}
+ return v_cmd, save_past_item, log_item
+
+
+def SingleArmCartPullingMPC(
+ args: Namespace,
+ robot: SingleArmWholeBodyInterface,
+ path_planner: ProcessManager | types.FunctionType,
+ run=True,
+) -> None | ControlLoopManager:
+ """
+ BaseAndEEPathFollowingMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq).
+ """
+
+ robot._mode = SingleArmWholeBodyInterface.control_mode.whole_body
+ T_w_e = robot.T_w_e
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp = BaseAndEEPathFollowingOCP(args, robot, x0)
+ ocp.solveInitialOCP(x0)
+
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+
+ path2D_handlebar = initializePastData(args, T_w_e, robot.q[:2], float(max_base_v))
+
+ get_position = lambda robot: robot.q[:2]
+ controlLoop = partial(
+ PathFollowingFromPlannerCtrllLoopTemplate,
+ path_planner,
+ get_position,
+ ocp,
+ SingleArmCartPullingMPCControlLoop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee": np.zeros((6,)),
+ "err_norm_ee": np.zeros((1,)),
+ "err_norm_base": np.zeros((1,)),
+ }
+ save_past_dict = {"path2D": T_w_e.translation[:2]}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+
+ # actually put past data into the past window
+ loop_manager.past_data["path2D"].clear()
+ loop_manager.past_data["path2D"].extend(
+ path2D_handlebar[i] for i in range(args.past_window_size)
+ )
+
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/examples/cart_pulling/control_sub_problems/with_planner/ee_only_path_following_from_plannner.py b/examples/cart_pulling/control_sub_problems/with_planner/ee_only_path_following_from_plannner.py
deleted file mode 100644
index 783743ab63ea13a1c2fe0c05610867a5a1ff60fc..0000000000000000000000000000000000000000
--- a/examples/cart_pulling/control_sub_problems/with_planner/ee_only_path_following_from_plannner.py
+++ /dev/null
@@ -1,88 +0,0 @@
-from smc import getRobotFromArgs
-from smc import getMinimalArgParser
-from smc.path_generation.maps.premade_maps import createSampleStaticMap
-from smc.path_generation.path_math.path2d_to_6d import path2D_to_SE3
-from smc.control.optimal_control.util import get_OCP_args
-from smc.control.cartesian_space import getClikArgs
-from smc.path_generation.planner import starPlanner, getPlanningArgs
-from smc.control.optimal_control.croco_mpc_point_to_point import CrocoEEP2PMPC
-from smc.control.optimal_control.croco_mpc_path_following import (
- CrocoEEPathFollowingMPC,
-)
-from smc.multiprocessing import ProcessManager
-
-import time
-import numpy as np
-from functools import partial
-import pinocchio as pin
-
-
-def get_args():
- parser = getMinimalArgParser()
- parser = get_OCP_args(parser)
- parser = getClikArgs(parser) # literally just for goal error
- parser = getPlanningArgs(parser)
- parser.add_argument(
- "--handlebar-height",
- type=float,
- default=0.5,
- help="heigh of handlebar of the cart to be pulled",
- )
- args = parser.parse_args()
- return args
-
-
-if __name__ == "__main__":
- args = get_args()
- robot = getRobotFromArgs(args)
- # TODO: HOW IS IT POSSIBLE THAT T_W_E IS WRONG WITHOUT STEP CALLED HERE?????????????????
- robot._step()
- T_w_e = robot.T_w_e
- robot._q[0] = 9.0
- robot._q[1] = 4.0
- robot._step()
- x0 = np.concatenate([robot.q, robot.v])
- goal = np.array([0.5, 5.5])
-
- planning_function = partial(starPlanner, goal)
- # here we're following T_w_e reference so that's what we send
- path_planner = ProcessManager(
- args, planning_function, T_w_e.translation[:2], 3, None
- )
- _, map_as_list = createSampleStaticMap()
- if args.visualizer:
- robot.sendRectangular2DMapToVisualizer(map_as_list)
- # time.sleep(5)
-
- T_w_e = robot.T_w_e
- data = None
- # get first path
- while data is None:
- path_planner.sendCommand(T_w_e.translation[:2])
- data = path_planner.getData()
- # time.sleep(1 / args.ctrl_freq)
- time.sleep(1)
-
- _, path2D = data
- path2D = np.array(path2D).reshape((-1, 2))
- pathSE3 = path2D_to_SE3(path2D, args.handlebar_height)
- if args.visualizer:
- # TODO: document this somewhere
- robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})
- if np.linalg.norm(pin.log6(T_w_e.actInv(pathSE3[0]))) > 1e-2:
- print("going to initial path position")
- CrocoEEP2PMPC(args, robot, pathSE3[0])
- print("initialized!")
- CrocoEEPathFollowingMPC(args, robot, x0, path_planner)
-
- print("final position:", robot.T_w_e)
-
- if args.real:
- robot.stopRobot()
-
- if args.save_log:
- robot._log_manager.saveLog()
- robot._log_manager.plotAllControlLoops()
-
- if args.visualizer:
- robot.killManipulatorVisualizer()
diff --git a/examples/optimal_control/path_following/base_and_ee_path_following.py b/examples/optimal_control/path_following/base_and_ee_path_following.py
new file mode 100644
index 0000000000000000000000000000000000000000..7c6cc521c42791d166bb841ee3d977cfb2da8493
--- /dev/null
+++ b/examples/optimal_control/path_following/base_and_ee_path_following.py
@@ -0,0 +1,156 @@
+from smc import getRobotFromArgs
+from smc import getMinimalArgParser
+from smc.control.optimal_control.util import get_OCP_args
+from smc.control.cartesian_space import getClikArgs
+from smc.control.optimal_control.croco_mpc_path_following import (
+ BaseAndEEPathFollowingMPC,
+)
+from os.path import exists
+from smc import getRobotFromArgs
+from smc import getMinimalArgParser
+from smc.control.optimal_control.util import get_OCP_args
+from smc.control.cartesian_space import getClikArgs
+from smc.control.optimal_control.croco_mpc_point_to_point import (
+ CrocoEEAndBaseP2PMPC,
+)
+from smc.path_generation.planner import starPlanner, getPlanningArgs
+from smc.path_generation.maps.premade_maps import createSampleStaticMap
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.multiprocessing import ProcessManager
+
+from pinocchio import SE3
+import numpy as np
+from functools import partial
+import argparse
+import matplotlib
+from smc.util.draw_path import drawPath
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ parser = getClikArgs(parser) # literally just for goal error
+ parser = getPlanningArgs(parser)
+ parser.add_argument(
+ "--planner",
+ action=argparse.BooleanOptionalAction,
+ help="if on, you're in a pre-set map and a planner produce a plan to navigate. if off, you draw the path to be followed",
+ default=True,
+ )
+ parser.add_argument(
+ "--draw-new",
+ action=argparse.BooleanOptionalAction,
+ help="are you drawing a new path or reusing the previous one",
+ default=False,
+ )
+ parser.add_argument(
+ "--map-width",
+ type=float,
+ help="width of the map in meters (x-axis) - only used for drawing of the path",
+ default=3.0,
+ )
+ parser.add_argument(
+ "--map-height",
+ type=float,
+ help="height of the map in meters (y-axis) - only used for drawing of the path",
+ default=3.0,
+ )
+ # TODO: rename argument here and in path following mpc
+ parser.add_argument(
+ "--handlebar-height",
+ type=float,
+ default=0.5,
+ help="heigh of handlebar of the cart to be pulled",
+ )
+ args = parser.parse_args()
+ return args
+
+
+# NOTE: lil bit of evil with passing the integer by reference -> make it a list
+# to make this non-evil this should be a method in a class but i ain't gonna bother with that now
+def fixedPathPlanner(
+ path_parameter: list[int],
+ path2D: np.ndarray,
+ rotation: np.ndarray,
+ base_offset: np.ndarray,
+ T_w_e: SE3,
+) -> tuple[list[SE3], list[np.ndarray]]:
+ p_ee = T_w_e.translation
+ distances = np.linalg.norm(p_ee - path2D, axis=1)
+ index = np.argmin(distances)
+ # if index > path_parameter[0]:
+ # NOTE: bullshit heuristic to deal with the path intersecting with itself.
+ # i can't be bothered with anything better
+ if (index - path_parameter[0]) > 0 and (index - path_parameter[0]) < 10:
+ path_parameter[0] += 1
+ path2D = path2D[path_parameter[0] :]
+ # NOTE: this is of course horribly inefficient,
+ # but since it's just for testing who cares
+ path_SE3 = []
+ path_base = []
+ for translation in path2D:
+ path_base.append(translation + base_offset)
+ path_SE3.append(SE3(rotation, translation))
+ return path_base, path_SE3
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = getRobotFromArgs(args)
+ # TODO: make this go away
+ robot._step()
+ assert issubclass(robot.__class__, SingleArmInterface)
+ assert issubclass(robot.__class__, MobileBaseInterface)
+ x0 = np.concatenate([robot.q, robot.v])
+ rotation = SE3.Random().rotation
+ height = 0.5
+ base_offset = np.array([0.0, 0.4, 0.0])
+ goal = np.array([0.5, 5.5])
+ T_w_e = robot.T_w_e
+
+ if args.planner:
+ planning_function = partial(starPlanner, goal)
+ # here we're following T_w_e reference so that's what we send
+ path_planner = ProcessManager(
+ args, planning_function, T_w_e.translation[:2], 3, None
+ )
+ _, map_as_list = createSampleStaticMap()
+ if args.visualizer:
+ robot.sendRectangular2DMapToVisualizer(map_as_list)
+ # time.sleep(5)
+ else:
+ if not args.draw_new:
+ assert exists("./parameters/path_in_pixels.csv")
+ pixel_path_file_path = "./parameters/path_in_pixels.csv"
+ path2D = np.genfromtxt(pixel_path_file_path, delimiter=",")
+ else:
+ matplotlib.use("tkagg")
+ path2D = drawPath(args)
+ matplotlib.use("qtagg")
+ path2D[:, 0] = path2D[:, 0] * args.map_width
+ path2D[:, 1] = path2D[:, 1] * args.map_height
+ path2D = np.hstack((path2D, height * np.ones((len(path2D), 1))))
+ robot.updateViz({"fixed_path": path2D})
+ T_w_goal = SE3(rotation, path2D[0])
+ p_basegoal = T_w_goal.copy().translation.copy() + base_offset
+ p_basegoal[2] = 0.0
+ CrocoEEAndBaseP2PMPC(args, robot, T_w_goal, p_basegoal)
+ # note: making it a list is a lil'bit of evil to make it persistent in memory
+ # jesus i really need to switch to cpp to pass by reference or value at will
+ path_parameter = [0]
+ path_planner = partial(
+ fixedPathPlanner, path_parameter, path2D, rotation, base_offset
+ )
+
+ BaseAndEEPathFollowingMPC(args, robot, path_planner)
+
+ if args.real:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
diff --git a/examples/optimal_control/path_following/croco_dual_ee_reference_path_following_mpc.py b/examples/optimal_control/path_following/croco_dual_ee_reference_path_following_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..6917c4e0b3310c33d646a9522d0c065f2f48b0d1
--- /dev/null
+++ b/examples/optimal_control/path_following/croco_dual_ee_reference_path_following_mpc.py
@@ -0,0 +1,162 @@
+from os.path import exists
+from smc import getRobotFromArgs
+from smc import getMinimalArgParser
+from smc.control.optimal_control.util import get_OCP_args
+from smc.control.cartesian_space import getClikArgs
+from smc.control.cartesian_space.cartesian_space_point_to_point import moveLDualArm
+from smc.control.cartesian_space.pink_p2p import (
+ DualArmIKSelfAvoidanceViaEndEffectorSpheres,
+)
+from smc.control.optimal_control.croco_point_to_point.mpc.dual_arm_reference_mpc import (
+ CrocoDualEEP2PMPC,
+)
+from smc.control.optimal_control.croco_path_following.mpc.dual_arm_reference_mpc import (
+ CrocoDualArmEEPathFollowingMPC,
+)
+from smc.path_generation.planner import starPlanner, getPlanningArgs
+from smc.path_generation.maps.premade_maps import createSampleStaticMap
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from smc.multiprocessing import ProcessManager
+
+from pinocchio import SE3
+import numpy as np
+from functools import partial
+import argparse
+import matplotlib
+from smc.util.draw_path import drawPath
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ parser = getClikArgs(parser) # literally just for goal error
+ parser = getPlanningArgs(parser)
+ parser.add_argument(
+ "--planner",
+ action=argparse.BooleanOptionalAction,
+ help="if on, you're in a pre-set map and a planner produce a plan to navigate. if off, you draw the path to be followed",
+ default=True,
+ )
+ parser.add_argument(
+ "--draw-new",
+ action=argparse.BooleanOptionalAction,
+ help="are you drawing a new path or reusing the previous one",
+ default=False,
+ )
+ parser.add_argument(
+ "--map-width",
+ type=float,
+ help="width of the map in meters (x-axis) - only used for drawing of the path",
+ default=3.0,
+ )
+ parser.add_argument(
+ "--map-height",
+ type=float,
+ help="height of the map in meters (y-axis) - only used for drawing of the path",
+ default=3.0,
+ )
+ # TODO: rename argument here and in path following mpc
+ parser.add_argument(
+ "--handlebar-height",
+ type=float,
+ default=0.5,
+ help="heigh of handlebar of the cart to be pulled",
+ )
+ args = parser.parse_args()
+ return args
+
+
+# NOTE: lil bit of evil with passing the integer by reference -> make it a list
+# to make this non-evil this should be a method in a class but i ain't gonna bother with that now
+def fixedPathPlanner(
+ path_parameter: list[int], path2D: np.ndarray, rotation: np.ndarray, T_w_e: SE3
+) -> list[SE3]:
+ p_ee = T_w_e.translation
+ distances = np.linalg.norm(p_ee - path2D, axis=1)
+ index = np.argmin(distances)
+ # if index > path_parameter[0]:
+ # NOTE: bullshit heuristic to deal with the path intersecting with itself.
+ # i can't be bothered with anything better
+ if (index - path_parameter[0]) > 0 and (index - path_parameter[0]) < 10:
+ path_parameter[0] += 1
+ path2D = path2D[path_parameter[0] :]
+ # NOTE: this is of course horribly inefficient,
+ # but since it's just for testing who cares
+ path_SE3 = []
+ for translation in path2D:
+ path_SE3.append(SE3(rotation, translation))
+ return path_SE3
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = getRobotFromArgs(args)
+ # TODO: make this go away
+ robot._step()
+ assert issubclass(robot.__class__, DualArmInterface)
+ if args.planner:
+ robot._q[0] = 9.0
+ robot._q[1] = 4.0
+ robot._step()
+ x0 = np.concatenate([robot.q, robot.v])
+ # rotation = SE3.Random().rotation
+ rotation = SE3.Identity().rotation
+ height = 0.5
+ goal = np.array([0.5, 5.5])
+ T_w_abs = robot.T_w_abs
+ T_absgoal_l = SE3.Identity()
+ T_absgoal_l.translation[1] = 0.15
+ T_absgoal_r = SE3.Identity()
+ T_absgoal_r.translation[1] = -0.15
+
+ if args.planner:
+ planning_function = partial(starPlanner, goal)
+ # we define the path using the T_w_abs frame,
+ # from which the controller constructs T_w_l and T_w_r references to follow
+ path_planner = ProcessManager(
+ args, planning_function, T_w_abs.translation[:2], 3, None
+ )
+ _, map_as_list = createSampleStaticMap()
+ if args.visualizer:
+ robot.sendRectangular2DMapToVisualizer(map_as_list)
+ # time.sleep(5)
+ else:
+ if not args.draw_new:
+ assert exists("./parameters/path_in_pixels.csv")
+ pixel_path_file_path = "./parameters/path_in_pixels.csv"
+ path2D = np.genfromtxt(pixel_path_file_path, delimiter=",")
+ else:
+ matplotlib.use("tkagg")
+ path2D = drawPath(args)
+ matplotlib.use("qtagg")
+ path2D[:, 0] = path2D[:, 0] * args.map_width
+ path2D[:, 1] = path2D[:, 1] * args.map_height
+ path2D = np.hstack((path2D, height * np.ones((len(path2D), 1))))
+ robot.updateViz({"fixed_path": path2D})
+ T_w_absgoal = SE3(rotation, path2D[0])
+ moveLDualArm(args, robot, T_w_absgoal, T_absgoal_l, T_absgoal_r)
+ # and if that doesn't work either use pink:
+ # DualArmIKSelfAvoidanceViaEndEffectorSpheres(
+ # T_w_absgoal, T_absgoal_l, T_absgoal_r, args, robot
+ # )
+ # NOTE: if this doesn't work just switch to moveL dual arm:
+ # CrocoDualEEP2PMPC(args, robot, T_w_goal, T_absgoal_l, T_absgoal_r)
+
+ # note: making it a list is a lil'bit of evil to make it persistent in memory
+ # jesus i really need to switch to cpp to pass by reference or value at will
+ path_parameter = [0]
+ path_planner = partial(fixedPathPlanner, path_parameter, path2D, rotation)
+
+ CrocoDualArmEEPathFollowingMPC(
+ args, robot, T_absgoal_l, T_absgoal_r, x0, path_planner
+ )
+
+ if args.real:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
diff --git a/examples/optimal_control/path_following/croco_ee_reference_path_following_mpc.py b/examples/optimal_control/path_following/croco_ee_reference_path_following_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..93f624e50e777e461f1d0599969c09167d6c33fc
--- /dev/null
+++ b/examples/optimal_control/path_following/croco_ee_reference_path_following_mpc.py
@@ -0,0 +1,143 @@
+from os.path import exists
+from smc import getRobotFromArgs
+from smc import getMinimalArgParser
+from smc.control.optimal_control.util import get_OCP_args
+from smc.control.cartesian_space import getClikArgs
+from smc.control.optimal_control.croco_point_to_point.mpc.single_arm_reference_mpc import (
+ CrocoEEP2PMPC,
+)
+from smc.control.optimal_control.croco_path_following.mpc.single_arm_reference_mpc import (
+ CrocoEEPathFollowingMPC,
+)
+from smc.path_generation.planner import starPlanner, getPlanningArgs
+from smc.path_generation.maps.premade_maps import createSampleStaticMap
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.multiprocessing import ProcessManager
+
+from pinocchio import SE3
+import numpy as np
+from functools import partial
+import argparse
+import matplotlib
+from smc.util.draw_path import drawPath
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ parser = getClikArgs(parser) # literally just for goal error
+ parser = getPlanningArgs(parser)
+ parser.add_argument(
+ "--planner",
+ action=argparse.BooleanOptionalAction,
+ help="if on, you're in a pre-set map and a planner produce a plan to navigate. if off, you draw the path to be followed",
+ default=True,
+ )
+ parser.add_argument(
+ "--draw-new",
+ action=argparse.BooleanOptionalAction,
+ help="are you drawing a new path or reusing the previous one",
+ default=False,
+ )
+ parser.add_argument(
+ "--map-width",
+ type=float,
+ help="width of the map in meters (x-axis) - only used for drawing of the path",
+ default=3.0,
+ )
+ parser.add_argument(
+ "--map-height",
+ type=float,
+ help="height of the map in meters (y-axis) - only used for drawing of the path",
+ default=3.0,
+ )
+ # TODO: rename argument here and in path following mpc
+ parser.add_argument(
+ "--handlebar-height",
+ type=float,
+ default=0.5,
+ help="heigh of handlebar of the cart to be pulled",
+ )
+ args = parser.parse_args()
+ return args
+
+
+# NOTE: lil bit of evil with passing the integer by reference -> make it a list
+# to make this non-evil this should be a method in a class but i ain't gonna bother with that now
+def fixedPathPlanner(
+ path_parameter: list[int], path2D: np.ndarray, rotation: np.ndarray, T_w_e: SE3
+) -> list[SE3]:
+ p_ee = T_w_e.translation
+ distances = np.linalg.norm(p_ee - path2D, axis=1)
+ index = np.argmin(distances)
+ # if index > path_parameter[0]:
+ # NOTE: bullshit heuristic to deal with the path intersecting with itself.
+ # i can't be bothered with anything better
+ if (index - path_parameter[0]) > 0 and (index - path_parameter[0]) < 10:
+ path_parameter[0] += 1
+ path2D = path2D[path_parameter[0] :]
+ # NOTE: this is of course horribly inefficient,
+ # but since it's just for testing who cares
+ path_SE3 = []
+ for translation in path2D:
+ path_SE3.append(SE3(rotation, translation))
+ return path_SE3
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = getRobotFromArgs(args)
+ # TODO: make this go away
+ robot._step()
+ assert issubclass(robot.__class__, SingleArmInterface)
+ if args.planner:
+ robot._q[0] = 9.0
+ robot._q[1] = 4.0
+ robot._step()
+ x0 = np.concatenate([robot.q, robot.v])
+ rotation = SE3.Random().rotation
+ height = 0.5
+ goal = np.array([0.5, 5.5])
+ T_w_e = robot.T_w_e
+
+ if args.planner:
+ planning_function = partial(starPlanner, goal)
+ # here we're following T_w_e reference so that's what we send
+ path_planner = ProcessManager(
+ args, planning_function, T_w_e.translation[:2], 3, None
+ )
+ _, map_as_list = createSampleStaticMap()
+ if args.visualizer:
+ robot.sendRectangular2DMapToVisualizer(map_as_list)
+ # time.sleep(5)
+ else:
+ if not args.draw_new:
+ assert exists("./parameters/path_in_pixels.csv")
+ pixel_path_file_path = "./parameters/path_in_pixels.csv"
+ path2D = np.genfromtxt(pixel_path_file_path, delimiter=",")
+ else:
+ matplotlib.use("tkagg")
+ path2D = drawPath(args)
+ matplotlib.use("qtagg")
+ path2D[:, 0] = path2D[:, 0] * args.map_width
+ path2D[:, 1] = path2D[:, 1] * args.map_height
+ path2D = np.hstack((path2D, height * np.ones((len(path2D), 1))))
+ robot.updateViz({"fixed_path": path2D})
+ T_w_goal = SE3(rotation, path2D[0])
+ CrocoEEP2PMPC(args, robot, T_w_goal)
+ # note: making it a list is a lil'bit of evil to make it persistent in memory
+ # jesus i really need to switch to cpp to pass by reference or value at will
+ path_parameter = [0]
+ path_planner = partial(fixedPathPlanner, path_parameter, path2D, rotation)
+
+ CrocoEEPathFollowingMPC(args, robot, x0, path_planner)
+
+ if args.real:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
diff --git a/examples/optimal_control/path_following/path_following_mpc.py b/examples/optimal_control/path_following/path_following_mpc.py
deleted file mode 100644
index 5ec64e77586c793bed74bbef2ee98c3cf54ef8ae..0000000000000000000000000000000000000000
--- a/examples/optimal_control/path_following/path_following_mpc.py
+++ /dev/null
@@ -1,51 +0,0 @@
-from smc import getRobotFromArgs
-from smc import getMinimalArgParser
-from smc.control.optimal_control.util import get_OCP_args
-from smc.control.cartesian_space import getClikArgs
-from smc.control.optimal_control.croco_mpc_path_following import (
- CrocoEEPathFollowingMPC,
-)
-import numpy as np
-
-
-def path(T_w_e, i):
- # 2) do T_mobile_base_ee_pos to get
- # end-effector reference frame(s)
-
- # generate bullshit just to see it works
- path = []
- t = i * robot.dt
- for i in range(args.n_knots):
- t += 0.01
- new = T_w_e.copy()
- translation = 2 * np.array([np.cos(t / 20), np.sin(t / 20), 0.3])
- new.translation = translation
- path.append(new)
- return path
-
-
-def get_args():
- parser = getMinimalArgParser()
- parser = get_OCP_args(parser)
- parser = getClikArgs(parser) # literally just for goal error
- args = parser.parse_args()
- return args
-
-
-if __name__ == "__main__":
- args = get_args()
- robot = getRobotFromArgs(args)
- x0 = np.concatenate([robot.q, robot.v])
- robot._step()
-
- CrocoEEPathFollowingMPC(args, robot, x0, path)
-
- if args.real:
- robot.stopRobot()
-
- if args.save_log:
- robot._log_manager.saveLog()
- robot._log_manager.plotAllControlLoops()
-
- if args.visualizer:
- robot.killManipulatorVisualizer()
diff --git a/examples/optimal_control/path_following/test_by_running.sh b/examples/optimal_control/path_following/test_by_running.sh
new file mode 100755
index 0000000000000000000000000000000000000000..69cf6d2fa8ac5d401d84a198ec5f64c8985b727d
--- /dev/null
+++ b/examples/optimal_control/path_following/test_by_running.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+# the idea here is to run all the runnable things
+# and test for syntax errors
+# TODO: make these work with different modes by making robot.model a property which outputs truncated models
+
+##########################################################################################################
+# fixed path
+# ################################################################################################
+# single arm - ee reference
+# ###############
+runnable="croco_ee_reference_path_following_mpc.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2 --no-draw-new --no-planner"
+echo $runnable
+python $runnable
+
+
+####################################################################################
+# path from planner
+#################################################################33
+
+runnable="croco_ee_reference_path_following_mpc.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2 --no-draw-new --planner"
+echo $runnable
+python $runnable
diff --git a/examples/optimal_control/point_to_point/croco_base_and_dual_ee_reference_p2p.py b/examples/optimal_control/point_to_point/croco_base_and_dual_ee_reference_p2p.py
index 1f1fa1bb4734f9c625b469ea9e3a4d469daf8108..765206e52fa67e3997dd0653693ddadef44a5c4a 100644
--- a/examples/optimal_control/point_to_point/croco_base_and_dual_ee_reference_p2p.py
+++ b/examples/optimal_control/point_to_point/croco_base_and_dual_ee_reference_p2p.py
@@ -2,7 +2,7 @@ from smc import getRobotFromArgs
from smc import getMinimalArgParser
from smc.control.optimal_control.util import get_OCP_args
from smc.control.cartesian_space import getClikArgs
-from smc.control.optimal_control.croco_mpc_point_to_point import (
+from smc.control.optimal_control.croco_point_to_point.mpc.base_and_dual_arm_reference_mpc import (
CrocoDualEEAndBaseP2PMPC,
)
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
diff --git a/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py b/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
index 8fd3c4c0e98dc6e383d9a30d910ce39c8430c98e..a8321b436c03f390b3260a4d74e733930530c314 100644
--- a/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
+++ b/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
@@ -2,7 +2,9 @@ from smc import getRobotFromArgs
from smc import getMinimalArgParser
from smc.control.optimal_control.util import get_OCP_args
from smc.control.cartesian_space import getClikArgs
-from smc.control.optimal_control.croco_mpc_point_to_point import CrocoEEAndBaseP2PMPC
+from smc.control.optimal_control.croco_point_to_point.mpc.base_and_single_arm_reference_mpc import (
+ CrocoEEAndBaseP2PMPC,
+)
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
diff --git a/examples/optimal_control/point_to_point/croco_dual_ee_reference_p2p_mpc.py b/examples/optimal_control/point_to_point/croco_dual_ee_reference_p2p_mpc.py
index 82a3cc27a7b348efc16945186a6dda93f156a528..35b0f7e067f5dacd64719b518666ca53fa5d9bd2 100644
--- a/examples/optimal_control/point_to_point/croco_dual_ee_reference_p2p_mpc.py
+++ b/examples/optimal_control/point_to_point/croco_dual_ee_reference_p2p_mpc.py
@@ -1,5 +1,5 @@
from smc import getMinimalArgParser, getRobotFromArgs
-from smc.control.optimal_control.croco_mpc_point_to_point import (
+from smc.control.optimal_control.croco_point_to_point.mpc.dual_arm_reference_mpc import (
CrocoDualEEP2PMPC,
)
from smc.control.optimal_control.util import get_OCP_args
diff --git a/examples/optimal_control/point_to_point/croco_ee_reference_p2p_mpc.py b/examples/optimal_control/point_to_point/croco_ee_reference_p2p_mpc.py
index 0b21cd09225fbb2edf85f968b3c938c72e797ede..92f88775e3ab76078bf4430114c3061d614f4184 100644
--- a/examples/optimal_control/point_to_point/croco_ee_reference_p2p_mpc.py
+++ b/examples/optimal_control/point_to_point/croco_ee_reference_p2p_mpc.py
@@ -1,5 +1,5 @@
from smc import getMinimalArgParser, getRobotFromArgs
-from smc.control.optimal_control.croco_mpc_point_to_point import (
+from smc.control.optimal_control.croco_point_to_point.mpc.single_arm_reference_mpc import (
CrocoEEP2PMPC,
)
from smc.control.optimal_control.util import get_OCP_args
diff --git a/examples/optimal_control/point_to_point/croco_ee_reference_p2p_ocp.py b/examples/optimal_control/point_to_point/croco_ee_reference_p2p_ocp.py
index 0b85c48edf9ed32152d87b1bd8f3647a0ed833a9..907ef7a9e8b4c9fb6b731611fc300d2fd59e5fc2 100644
--- a/examples/optimal_control/point_to_point/croco_ee_reference_p2p_ocp.py
+++ b/examples/optimal_control/point_to_point/croco_ee_reference_p2p_ocp.py
@@ -1,7 +1,7 @@
# PYTHON_ARGCOMPLETE_OK
from smc.control.optimal_control.util import get_OCP_args
from smc import getMinimalArgParser, getRobotFromArgs
-from smc.control.optimal_control.point_to_point_croco_ocp import (
+from smc.control.optimal_control.croco_point_to_point.ocp.single_arm_reference_ocp import (
SingleArmIKOCP,
)
from smc.control.joint_space import followKinematicJointTrajP
diff --git a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
index 7b52ccd3af6be7c1d482edd1c2e80d0ee5ba4939..8b71f35dd224d629eeeaeee5dfb72068d7d41ba0 100644
--- a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
+++ b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
@@ -16,6 +16,8 @@ from argparse import Namespace
from collections import deque
from typing import Callable
+from smc.control.cartesian_space.ik_solvers import QPManipMax
+
def controlLoopClik(
# J err_vec v_cmd
@@ -39,7 +41,16 @@ def controlLoopClik(
J = robot.getJacobian()
# compute the joint velocities based on controller you passed
# qd = ik_solver(J, err_vector, past_qd=past_data['dqs_cmd'][-1])
- v_cmd = ik_solver(J, err_vector)
+ if args.ik_solver == "QPManipMax":
+ v_cmd = QPManipMax(
+ J,
+ err_vector,
+ robot.computeManipulabilityIndexQDerivative(),
+ lb=-1 * robot.max_v,
+ ub=robot.max_v,
+ )
+ else:
+ v_cmd = ik_solver(J, err_vector)
if v_cmd is None:
print(
t,
@@ -202,7 +213,17 @@ def controlLoopClikDualArm(
err_vector = np.concatenate((err_vector_left, err_vector_right))
J = robot.getJacobian()
- v_cmd = ik_solver(J, err_vector)
+
+ if args.ik_solver == "QPManipMax":
+ v_cmd = QPManipMax(
+ J,
+ err_vector,
+ robot.computeManipulabilityIndexQDerivative(),
+ lb=-1 * robot.max_v,
+ ub=robot.max_v,
+ )
+ else:
+ v_cmd = ik_solver(J, err_vector)
if v_cmd is None:
print(
t,
diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py
index a24e140df966434b97797d7479e4da77e4d98776..f933638b0a50dff4a7c3ba7d98592662ec46128d 100644
--- a/python/smc/control/cartesian_space/ik_solvers.py
+++ b/python/smc/control/cartesian_space/ik_solvers.py
@@ -40,6 +40,11 @@ def getIKSolver(
lb = -1 * robot.max_v
ub = robot.max_v
return partial(QPquadprog, lb=lb, ub=ub)
+ # via quadprog
+ # if args.ik_solver == "QPManipMax":
+ # lb = -1 * robot.max_v
+ # ub = robot.max_v
+ # return partial(QPManipMax, lb=lb, ub=ub)
if args.ik_solver == "QPproxsuite":
H = np.eye(robot.nv)
g = np.zeros(robot.nv)
@@ -211,26 +216,38 @@ def QPproxsuite(
# HA! found it in a winter school
# use this
# and test it with finite differencing!
-"""
class CostManipulability:
- def __init__(self,jointIndex=None,frameIndex=None):
+ def __init__(self, jointIndex=None, frameIndex=None):
if frameIndex is not None:
jointIndex = robot.model.frames[frameIndex].parent
- self.jointIndex = jointIndex if jointIndex is not None else robot.model.njoints-1
- def calc(self,q):
- J = self.J=pin.computeJointJacobian(robot.model,robot.data,q,self.jointIndex)
- return np.sqrt(det(J@J.T))
- def calcDiff(self,q):
- Jp = pinv(pin.computeJointJacobian(robot.model,robot.data,q,self.jointIndex))
+ self.jointIndex = (
+ jointIndex if jointIndex is not None else robot.model.njoints - 1
+ )
+
+ def calc(self, q):
+ J = self.J = pin.computeJointJacobian(
+ robot.model, robot.data, q, self.jointIndex
+ )
+ return np.sqrt(det(J @ J.T))
+
+ def calcDiff(self, q):
+ Jp = pinv(pin.computeJointJacobian(robot.model, robot.data, q, self.jointIndex))
res = np.zeros(robot.model.nv)
v0 = np.zeros(robot.model.nv)
for k in range(6):
- pin.computeForwardKinematicsDerivatives(robot.model,robot.data,q,Jp[:,k],v0)
- JqJpk = pin.getJointVelocityDerivatives(robot.model,robot.data,self.jointIndex,pin.LOCAL)[0]
- res += JqJpk[k,:]
+ pin.computeForwardKinematicsDerivatives(
+ robot.model, robot.data, q, Jp[:, k], v0
+ )
+ JqJpk = pin.getJointVelocityDerivatives(
+ robot.model, robot.data, self.jointIndex, pin.LOCAL
+ )[0]
+ res += JqJpk[k, :]
res *= self.calc(q)
return res
+
+"""
+
# use this as a starting point for finite differencing
def numdiff(func, x, eps=1e-6):
f0 = copy.copy(func(x))
@@ -255,9 +272,15 @@ Tgn = Tdiffq(costManipulability.calc,q)
"""
-def QPManipMax(J: np.ndarray, err_vector: np.ndarray, lb=None, ub=None) -> np.ndarray:
+def QPManipMax(
+ J: np.ndarray,
+ V_e_e: np.ndarray,
+ secondary_objective_vec: np.ndarray,
+ lb=None,
+ ub=None,
+) -> np.ndarray:
"""
- invKinmQP
+ QPManipMAx
---------
generic QP:
minimize 1/2 x^T P x + q^T x
@@ -267,7 +290,7 @@ def QPManipMax(J: np.ndarray, err_vector: np.ndarray, lb=None, ub=None) -> np.nd
lb <= x <= ub
inverse kinematics QP:
minimize 1/2 qd^T P qd
- + q^T qd (optional secondary objective)
+ + q^T qd (where q is the partial deriviative of the manipulability index w.r.t. q)
subject to
G qd \\leq h (optional)
J qd = b (mandatory)
@@ -276,14 +299,15 @@ def QPManipMax(J: np.ndarray, err_vector: np.ndarray, lb=None, ub=None) -> np.nd
P = np.eye(J.shape[1], dtype="double")
# secondary objective is given via q
# we set it to 0 here, but we should give a sane default here
- q = np.array([0] * J.shape[1], dtype="double")
+ q = secondary_objective_vec
G = None
- b = err_vector
+ V_e_e_norm = np.linalg.norm(V_e_e)
+ max_V_e_e_norm = 0.3
+ if V_e_e_norm < max_V_e_e_norm:
+ b = V_e_e
+ else:
+ b = (V_e_e / V_e_e_norm) * max_V_e_e_norm
A = J
- # TODO: you probably want limits here
- # lb = None
- # ub = None
h = None
- # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
- qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
+ qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=False)
return qd
diff --git a/python/smc/control/cartesian_space/utils.py b/python/smc/control/cartesian_space/utils.py
index 685ed51b98af585f76964fdc5efebb92ea28c5c9..d98d519f0b9c03837ce45cb6892acd60ef2261cc 100644
--- a/python/smc/control/cartesian_space/utils.py
+++ b/python/smc/control/cartesian_space/utils.py
@@ -46,6 +46,7 @@ def getClikArgs(parser: argparse.ArgumentParser) -> argparse.ArgumentParser:
"jacobianTranspose",
"QPquadprog",
"QPproxsuite",
+ "QPManipMax",
],
)
diff --git a/python/smc/control/controller_templates/point_to_point.py b/python/smc/control/controller_templates/point_to_point.py
index 96e334e7f57214ee60136311428c0402011be31f..04d57137387267325676ff10ec4a0acaa7967b40 100644
--- a/python/smc/control/controller_templates/point_to_point.py
+++ b/python/smc/control/controller_templates/point_to_point.py
@@ -215,7 +215,7 @@ def EEAndBaseP2PCtrlLoopTemplate(
ee_err_vector = log6(SEerror).vector
ee_err_vector_norm = np.linalg.norm(ee_err_vector)
- p_base = np.array(list(robot.q[:2]) + [0.0])
+ p_base = robot.T_w_b.translation
base_err_vector_norm = np.linalg.norm(p_basegoal - p_base)
if (ee_err_vector_norm < robot.args.goal_error) and (
diff --git a/python/smc/control/optimal_control/__init__.py b/python/smc/control/optimal_control/__init__.py
index bd3883b08e418e5c1f5c1efb957bd0d1189b5dbc..e121f5ab4f58acf9fafc15611c2c952d5edd1983 100644
--- a/python/smc/control/optimal_control/__init__.py
+++ b/python/smc/control/optimal_control/__init__.py
@@ -6,7 +6,5 @@ import importlib.util
# print("you need to have pinocchio version 3.0.0 or greater to use pinocchio.casadi!")
# exit()
# from .create_pinocchio_casadi_ocp import *
-from .path_following_croco_ocp import *
-from .point_to_point_croco_ocp import *
-from .croco_mpc_path_following import *
-from .croco_mpc_point_to_point import *
+from .croco_path_following import *
+from .croco_point_to_point import *
diff --git a/python/smc/control/optimal_control/croco_mpc_path_following.py b/python/smc/control/optimal_control/croco_mpc_path_following.py
deleted file mode 100644
index 027ace12b08cd5d16748bfea4bb21377685905bd..0000000000000000000000000000000000000000
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ /dev/null
@@ -1,534 +0,0 @@
-from smc.robots.interfaces.single_arm_interface import SingleArmInterface
-from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface, DualArmWholeBodyInterface
-from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
-from smc.control.control_loop_manager import ControlLoopManager
-from smc.multiprocessing.process_manager import ProcessManager
-from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
-from smc.control.optimal_control.path_following_croco_ocp import (
- BasePathFollowingOCP,
- CrocoEEPathFollowingOCP,
- BaseAndEEPathFollowingOCP,
- BaseAndDualArmEEPathFollowingOCP,
-)
-from smc.path_generation.path_math.path2d_to_6d import (
- path2D_to_SE3,
-)
-from smc.path_generation.path_math.cart_pulling_path_math import (
- construct_EE_path,
-)
-from smc.path_generation.path_math.path_to_trajectory import path2D_to_trajectory2D
-from smc.control.controller_templates.path_following_template import (
- PathFollowingFromPlannerCtrllLoopTemplate,
-)
-
-import numpy as np
-from functools import partial
-import types
-from argparse import Namespace
-from pinocchio import SE3, log6
-from collections import deque
-
-
-def CrocoBasePathFollowingFromPlannerMPCControlLoop(
- ocp: CrocoOCP,
- path2D: np.ndarray,
- args: Namespace,
- robot: MobileBaseInterface,
- t: int,
- _: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
-
- p = robot.T_w_b.translation[:2]
- max_base_v = np.linalg.norm(robot._max_v[:2])
- path_base = path2D_to_trajectory2D(args, path2D, max_base_v)
- path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
-
- if args.visualizer:
- if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
- robot.visualizer_manager.sendCommand({"path": path_base})
-
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=(path_base))
- xs = np.array(ocp.solver.xs)
- v_cmd = xs[1, robot.model.nq :]
- # NOTE: we might get stuck
- # TODO: make it more robust, it can still get stuck with this
- # a good idea is to do this for a some number of iterations -
- # you can keep this them in past data
- if np.linalg.norm(v_cmd) < 0.05:
- print(t, "RESOLVING FOR ONLY FINAL PATH POINT")
- last_point_only = np.ones((len(path_base),2))
- last_point_only = np.hstack((last_point_only, np.zeros((len(path_base),1))))
- last_point_only = last_point_only * path_base[-1]
- ocp.warmstartAndReSolve(x0, data=(last_point_only))
- xs = np.array(ocp.solver.xs)
- v_cmd = xs[1, robot.model.nq :]
-
- err_vector_base = np.linalg.norm(p - path_base[0][:2]) # z axis is irrelevant
- log_item = {}
- log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
- return v_cmd, {}, log_item
-
-
-def CrocoBasePathFollowingMPC(
- args: Namespace,
- robot: MobileBaseInterface,
- x0: np.ndarray,
- path_planner: ProcessManager | types.FunctionType,
-run=True) -> None|ControlLoopManager:
- """
- CrocoBasePathFollowingMPC
- -----
- """
-
- ocp = BasePathFollowingOCP(args, robot, x0)
- x0 = np.concatenate([robot.q, robot.v])
- ocp.solveInitialOCP(x0)
-
- get_position = lambda robot: robot.T_w_b.translation[:2]
- controlLoop = partial(
- PathFollowingFromPlannerCtrllLoopTemplate,
- path_planner,
- get_position,
- ocp,
- CrocoBasePathFollowingFromPlannerMPCControlLoop,
- args,
- robot,
- )
- log_item = {
- "qs": np.zeros(robot.nq),
- "dqs": np.zeros(robot.nv),
- "err_norm_base": np.zeros((1,)),
- }
- save_past_item = {}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_item, log_item
- )
- if run:
- loop_manager.run()
- else:
- return loop_manager
-
-
-# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
-def CrocoEEPathFollowingMPCControlLoop(
- args: Namespace,
- robot: SingleArmInterface,
- ocp: CrocoOCP,
- path_planner: types.FunctionType,
- t: int,
- _: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray],dict[str, np.ndarray]]:
- """
- CrocoPathFollowingMPCControlLoop
- -----------------------------
- end-effector(s) follow their path(s).
-
- path planner can either be a function which spits out a list of path points
- or an instance of ProcessManager which spits out path points
- by calling ProcessManager.getData()
- """
- breakFlag = False
- log_item = {}
- save_past_item = {}
-
- q = robot.q
- T_w_e = robot.T_w_e
-
- path_EE_SE3 = path_planner(T_w_e, t)
-
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=path_EE_SE3)
- xs = np.array(ocp.solver.xs)
- us = np.array(ocp.solver.us)
- vel_cmds = xs[1, robot.model.nq :]
-
- robot.sendVelocityCommand(vel_cmds)
-
- # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
- if args.visualizer:
- if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
- robot.visualizer_manager.sendCommand({"frame_path": path_EE_SE3})
-
- err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0]))
- log_item["err_vec_ee"] = err_vector_ee
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.v.reshape((robot.model.nv,))
- return breakFlag, save_past_item, log_item
-
-
-def CrocoEEPathFollowingFromPlannerMPCControlLoop(
- ocp: CrocoOCP,
- path2D: np.ndarray,
- args: Namespace,
- robot: SingleArmInterface,
- t: int,
- _: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- """
- CrocoPathFollowingMPCControlLoop
- -----------------------------
- end-effector(s) follow their path(s).
-
- path planner can either be a function which spits out a list of path points
- or an instance of ProcessManager which spits out path points
- by calling ProcessManager.getData()
- """
- max_base_v = np.linalg.norm(robot._max_v[:2])
- perc_of_max_v = 0.5
- velocity = perc_of_max_v * max_base_v
- trajectory2D = path2D_to_trajectory2D(args, path2D, velocity)
-
- # create a 3D reference out of the path
- path_EE_SE3 = path2D_to_SE3(trajectory2D, args.handlebar_height)
- # for pose in path_EE_SE3:
- # print(pose.translation)
-
- # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
- if args.visualizer:
- if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
- robot.visualizer_manager.sendCommand({"frame_path": path_EE_SE3})
-
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=path_EE_SE3)
- xs = np.array(ocp.solver.xs)
- v_cmd = xs[1, robot.model.nq :]
-
- err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0])).vector
- log_item = {"err_vec_ee": err_vector_ee}
-
- return v_cmd, {}, log_item
-
-
-def CrocoEEPathFollowingMPC(
- args: Namespace,
- robot: SingleArmInterface,
- x0: np.ndarray,
- path_planner: ProcessManager | types.FunctionType,
-run=True) -> None|ControlLoopManager:
- """
- CrocoEndEffectorPathFollowingMPC
- -----
- follow a fixed pre-determined path, or a path received from a planner.
- the path does NOT need to start from your current pose - you need to get to it yourself.
- """
-
- ocp = CrocoEEPathFollowingOCP(args, robot, x0)
- # technically should be done in controlloop because now
- # it's solved 2 times before the first command,
- # but we don't have time for details rn
- x0 = np.concatenate([robot.q, robot.v])
- ocp.solveInitialOCP(x0)
-
- if type(path_planner) == types.FunctionType:
- controlLoop = partial(
- # NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
- CrocoEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
- )
- else:
- get_position = lambda robot: robot.T_w_e.translation[:2]
- controlLoop = partial(
- PathFollowingFromPlannerCtrllLoopTemplate,
- path_planner,
- get_position,
- ocp,
- CrocoEEPathFollowingFromPlannerMPCControlLoop,
- args,
- robot,
- )
- log_item = {
- "qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nv),
- "err_vec_ee": np.zeros(6),
- }
- save_past_item = {}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_item, log_item
- )
- if run:
- loop_manager.run()
- else:
- return loop_manager
-
-# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
-def BaseAndEEPathFollowingMPCControlLoop(
- args: Namespace,
- robot,
- ocp: CrocoOCP,
- path_planner: types.FunctionType,
- t: int,
- _: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray]]:
- """
- CrocoPathFollowingMPCControlLoop
- -----------------------------
- end-effector(s) follow their path(s).
-
- path planner can either be a function which spits out a list of path points
- or an instance of ProcessManager which spits out path points
- by calling ProcessManager.getData()
- """
- breakFlag = False
- log_item = {}
- save_past_dict = {}
-
- q = robot.q
- T_w_e = robot.T_w_e
- path_base, path_EE = path_planner(T_w_e, t)
-
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=(path_base, path_EE))
- xs = np.array(ocp.solver.xs)
- us = np.array(ocp.solver.us)
- vel_cmds = xs[1, robot.model.nq :]
-
- robot.sendVelocityCommand(vel_cmds)
- if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
- path_base = np.array(path_base)
- robot.visualizer_manager.sendCommand({"path": path_base})
- robot.visualizer_manager.sendCommand({"frame_path": path_EE})
-
- err_vector_ee = log6(T_w_e.actInv(path_EE[0]))
- err_vector_base = np.linalg.norm(q[:2] - path_base[0][:2]) # z axis is irrelevant
- log_item["err_vec_ee"] = err_vector_ee
- log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
- log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
- log_item["qs"] = q.reshape((robot.model.nq,))
- log_item["dqs"] = robot.v.reshape((robot.model.nv,))
- return breakFlag, save_past_dict, log_item
-
-
-def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
- ocp: CrocoOCP,
- path2D_base: np.ndarray,
- args: Namespace,
- robot: SingleArmWholeBodyInterface,
- t: int,
- past_data: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- p = robot.q[:2]
-
- # NOTE: this one is obtained as the future path from path planner
- max_base_v = np.linalg.norm(robot._max_v[:2])
- trajectory_base = path2D_to_trajectory2D(args, path2D_base, max_base_v)
- trajectory_base = np.hstack((trajectory_base, np.zeros((len(trajectory_base), 1))))
-
- trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D"])
-
- if args.visualizer:
- if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
- robot.visualizer_manager.sendCommand({"path": trajectory_base})
- robot.visualizer_manager.sendCommand({"frame_path": trajectorySE3_handlebar})
-
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=(trajectory_base, trajectorySE3_handlebar))
- xs = np.array(ocp.solver.xs)
- v_cmd = xs[1, robot.model.nq :]
-
- err_vector_ee = log6(robot.T_w_e.actInv(trajectorySE3_handlebar[0]))
- err_vector_base = np.linalg.norm(p - trajectory_base[0][:2]) # z axis is irrelevant
- log_item = {}
- log_item["err_vec_ee"] = err_vector_ee
- log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
- log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
- save_past_item = {"path2D": p}
- return v_cmd, save_past_item, log_item
-
-
-def initializePastData(
- args: Namespace, T_w_e: SE3, p_base: np.ndarray, max_base_v: float
-) -> np.ndarray:
- # prepopulate past data to make base and cart be on the same path in the past
- # (which didn't actually happen because this just started)
- p_ee = T_w_e.translation[:2]
- straight_line_path = np.linspace(p_ee, p_base, args.past_window_size)
- # straight_line_path_timed = path2D_timed(args, straight_line_path, max_base_v)
- # return straight_line_path_timed # this one is shortened to args.n_knots! and we want the whole buffer
- return straight_line_path
-
-
-def BaseAndEEPathFollowingMPC(
- args: Namespace,
- robot: SingleArmWholeBodyInterface,
- path_planner: ProcessManager | types.FunctionType,
-run=True) -> None|ControlLoopManager:
- """
- BaseAndEEPathFollowingMPC
- -----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq).
- """
-
- robot._mode = SingleArmWholeBodyInterface.control_mode.whole_body
- T_w_e = robot.T_w_e
- x0 = np.concatenate([robot.q, robot.v])
- ocp = BaseAndEEPathFollowingOCP(args, robot, x0)
- ocp.solveInitialOCP(x0)
-
- max_base_v = np.linalg.norm(robot._max_v[:2])
-
- path2D_handlebar = initializePastData(args, T_w_e, robot.q[:2], float(max_base_v))
-
- if type(path_planner) == types.FunctionType:
- controlLoop = partial(
-# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
- BaseAndEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
- )
- else:
- get_position = lambda robot: robot.q[:2]
- controlLoop = partial(
- PathFollowingFromPlannerCtrllLoopTemplate,
- path_planner,
- get_position,
- ocp,
- BaseAndEEPathFollowingFromPlannerMPCControlLoop,
- args,
- robot,
- )
- log_item = {
- "qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nv),
- "err_vec_ee": np.zeros((6,)),
- "err_norm_ee": np.zeros((1,)),
- "err_norm_base": np.zeros((1,)),
- }
- save_past_dict = {"path2D": T_w_e.translation[:2]}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
- )
-
- # actually put past data into the past window
- loop_manager.past_data["path2D"].clear()
- loop_manager.past_data["path2D"].extend(
- path2D_handlebar[i] for i in range(args.past_window_size)
- )
-
- if run:
- loop_manager.run()
- else:
- return loop_manager
-
-
-# TODO: the robot put in is a whole body robot,
-# which you need to implement
-def BaseAndDualEEPathFollowingMPCControlLoop(
- T_absgoal_l: SE3,
- T_absgoal_r: SE3,
- ocp: CrocoOCP,
- path2D_base: np.ndarray,
- args: Namespace,
- robot: DualArmWholeBodyInterface,
- t: int,
- past_data: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- """
- BaseAndDualEEPathFollowingMPCControlLoop
- -----------------------------
- cart pulling dual arm control loop
- """
- p = robot.q[:2]
-
- # NOTE: this one is obtained as the future path from path planner
- max_base_v = np.linalg.norm(robot._max_v[:2])
- trajectory_base = path2D_to_trajectory2D(args, path2D_base, max_base_v)
- trajectory_base = np.hstack((trajectory_base, np.zeros((len(trajectory_base), 1))))
-
- trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D"])
- trajectorySE3_l = []
- trajectorySE3_r = []
- for traj_pose in trajectorySE3_handlebar:
- trajectorySE3_l.append(T_absgoal_l.act(traj_pose))
- trajectorySE3_r.append(T_absgoal_r.act(traj_pose))
-
- if args.visualizer:
- if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
- robot.visualizer_manager.sendCommand({"path": trajectory_base})
- robot.visualizer_manager.sendCommand({"frame_path": trajectorySE3_handlebar})
-
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0, data=(trajectory_base, trajectorySE3_l, trajectorySE3_r))
- xs = np.array(ocp.solver.xs)
- v_cmd = xs[1, robot.model.nq :]
-
- err_vector_ee_l = log6(robot.T_w_l.actInv(trajectorySE3_l[0]))
- err_norm_ee_l = np.linalg.norm(err_vector_ee_l)
- err_vector_ee_r = log6(robot.T_w_l.actInv(trajectorySE3_r[0]))
- err_norm_ee_r = np.linalg.norm(err_vector_ee_r)
- err_vector_base = np.linalg.norm(p - trajectory_base[0][:2]) # z axis is irrelevant
- log_item = {}
- log_item["err_vec_ee_l"] = err_vector_ee_l
- log_item["err_norm_ee_l"] = err_norm_ee_l.reshape((1,))
- log_item["err_vec_ee_r"] = err_vector_ee_r
- log_item["err_norm_ee_r"] = err_norm_ee_r.reshape((1,))
- log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
- save_past_item = {"path2D": p}
- return v_cmd, save_past_item, log_item
-
-def BaseAndDualEEPathFollowingMPC(
- args: Namespace,
- robot: DualArmWholeBodyInterface,
- path_planner: ProcessManager | types.FunctionType,
- T_absgoal_l: SE3,
- T_absgoal_r: SE3,
- run=True,
-) -> None | ControlLoopManager:
- """
- CrocoEndEffectorPathFollowingMPC
- -----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq).
- """
- robot._mode = DualArmWholeBodyInterface.control_mode.whole_body
- # NOTE: i'm shoving these into the class here - bad style,
- # but i don't
- T_w_abs = robot.T_w_abs
- x0 = np.concatenate([robot.q, robot.v])
- ocp = BaseAndDualArmEEPathFollowingOCP(args, robot, x0)
- ocp.solveInitialOCP(x0)
-
- max_base_v = np.linalg.norm(robot._max_v[:2])
-
- path2D_handlebar = initializePastData(args, T_w_abs, robot.q[:2], float(max_base_v))
-
- if type(path_planner) == types.FunctionType:
- raise NotImplementedError
- else:
- get_position = lambda robot: robot.q[:2]
- BaseAndDualEEPathFollowingMPCControlLoop_with_l_r = partial(BaseAndDualEEPathFollowingMPCControlLoop, T_absgoal_l, T_absgoal_r)
- controlLoop = partial(
- PathFollowingFromPlannerCtrllLoopTemplate,
- path_planner,
- get_position,
- ocp,
- BaseAndDualEEPathFollowingMPCControlLoop_with_l_r,
- args,
- robot,
- )
-
- log_item = {
- "qs": np.zeros(robot.model.nq),
- "dqs": np.zeros(robot.model.nv),
- "err_vec_ee_l": np.zeros((6,)),
- "err_norm_ee_l": np.zeros((1,)),
- "err_vec_ee_r": np.zeros((6,)),
- "err_norm_ee_r": np.zeros((1,)),
- "err_norm_base": np.zeros((1,)),
- }
- save_past_dict = {"path2D": T_w_abs.translation[:2]}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
- )
- # actually put past data into the past window
- loop_manager.past_data["path2D"].clear()
- loop_manager.past_data["path2D"].extend(
- path2D_handlebar[i] for i in range(args.past_window_size)
- )
-
- if run:
- loop_manager.run()
- else:
- return loop_manager
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
deleted file mode 100644
index 379a8c4b1e4370d56bdb2f22dc528da211a3b92b..0000000000000000000000000000000000000000
--- a/python/smc/control/optimal_control/croco_mpc_point_to_point.py
+++ /dev/null
@@ -1,355 +0,0 @@
-from smc.control.controller_templates.point_to_point import (
- BaseP2PCtrlLoopTemplate,
- DualEEAndBaseP2PCtrlLoopTemplate,
- DualEEP2PCtrlLoopTemplate,
- EEAndBaseP2PCtrlLoopTemplate,
- EEP2PCtrlLoopTemplate,
-)
-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.control.optimal_control.abstract_croco_ocp import CrocoOCP
-from smc.control.optimal_control.point_to_point_croco_ocp import (
- BaseIKOCP,
- SingleArmIKOCP,
- DualArmIKOCP,
- BaseAndSingleArmIKOCP,
- BaseAndDualArmIKOCP,
-)
-from smc.control.control_loop_manager import ControlLoopManager
-
-import pinocchio as pin
-import numpy as np
-from functools import partial
-from collections import deque
-from argparse import Namespace
-
-
-def CrocoBaseP2PMPCControlLoop(
- ocp: CrocoOCP,
- p_basegoal: np.ndarray,
- args: Namespace,
- robot: MobileBaseInterface,
- _: int,
- __: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- """
- CrocoIKMPCControlLoop
- ---------------------
- """
- # set initial state from sensor
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0)
- xs = np.array(ocp.solver.xs)
- # NOTE: for some reason the first value is always some wild bs
- vel_cmd = xs[1, robot.model.nq :]
- return vel_cmd, {}, {}
-
-
-def CrocoBaseP2PMPC(
- args: Namespace, robot: MobileBaseInterface, p_basegoal: np.ndarray, run=True
-):
- """
- CrocoBaseP2PMPC
- -----
- base point-to-point task
- """
- x0 = np.concatenate([robot.q, robot.v])
- ocp = BaseIKOCP(args, robot, x0, p_basegoal)
- ocp.solveInitialOCP(x0)
-
- controlLoop = partial(
- BaseP2PCtrlLoopTemplate,
- ocp,
- p_basegoal,
- CrocoBaseP2PMPCControlLoop,
- args,
- robot,
- )
- log_item = {
- "qs": np.zeros(robot.nq),
- "base_err_norm": np.zeros(1),
- "dqs": np.zeros(robot.nv),
- "dqs_cmd": np.zeros(robot.nv),
- }
- save_past_dict = {}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
- )
- if run:
- loop_manager.run()
- else:
- return loop_manager
-
-
-def CrocoEEP2PMPCControlLoop(
- ocp: CrocoOCP,
- T_w_goal: pin.SE3,
- args: Namespace,
- robot: SingleArmInterface,
- _: int,
- __: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- """
- CrocoIKMPCControlLoop
- ---------------------
- """
- # set initial state from sensor
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0)
- xs = np.array(ocp.solver.xs)
- # NOTE: for some reason the first value is always some wild bs
- vel_cmd = xs[1, robot.model.nq :]
- return vel_cmd, {}, {}
-
-
-def CrocoEEP2PMPC(
- args: Namespace, robot: SingleArmInterface, T_w_goal: pin.SE3, run=True
-):
- """
- IKMPC
- -----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq)
- """
- x0 = np.concatenate([robot.q, robot.v])
- ocp = SingleArmIKOCP(args, robot, x0, T_w_goal)
- ocp.solveInitialOCP(x0)
-
- controlLoop = partial(
- EEP2PCtrlLoopTemplate, ocp, T_w_goal, CrocoEEP2PMPCControlLoop, args, robot
- )
- log_item = {
- "qs": np.zeros(robot.nq),
- "err_norm": np.zeros(1),
- "dqs": np.zeros(robot.nv),
- "dqs_cmd": np.zeros(robot.nv),
- }
- save_past_dict = {}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
- )
- if run:
- loop_manager.run()
- else:
- return loop_manager
-
-
-def CrocoDualEEP2PMPCControlLoop(
- ocp: CrocoOCP,
- T_w_abseegoal: pin.SE3,
- T_absgoal_lgoal: pin.SE3,
- T_absgoal_rgoal: pin.SE3,
- args: Namespace,
- robot: DualArmInterface,
- t: int,
- past_data: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- """
- CrocoIKMPCControlLoop
- ---------------------
- """
- # set initial state from sensor
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0)
- xs = np.array(ocp.solver.xs)
- # NOTE: for some reason the first value is always some wild bs
- vel_cmd = xs[1, robot.model.nq :]
- return vel_cmd, {}, {}
-
-
-def CrocoDualEEP2PMPC(
- args: Namespace,
- robot: DualArmInterface,
- T_w_absgoal: pin.SE3,
- T_absgoal_l: pin.SE3,
- T_absgoal_r: pin.SE3,
- run=True,
-):
- """
- DualEEP2PMPC
- -----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq)
- """
- x0 = np.concatenate([robot.q, robot.v])
- 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(
- DualEEP2PCtrlLoopTemplate,
- ocp,
- T_w_absgoal,
- T_absgoal_l,
- T_absgoal_r,
- CrocoDualEEP2PMPCControlLoop,
- args,
- robot,
- )
- log_item = {
- "qs": np.zeros(robot.nq),
- "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(
- robot, controlLoop, args, save_past_dict, log_item
- )
- if run:
- loop_manager.run()
- else:
- return loop_manager
-
-
-def CrocoP2PEEAndBaseMPCControlLoop(
- ocp: CrocoOCP,
- T_w_eegoal: pin.SE3,
- p_basegoal: np.ndarray,
- args: Namespace,
- robot: SingleArmWholeBodyInterface,
- t: int,
- past_data: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- """
- CrocoP2PEEAndBaseMPCControlLoop
- ---------------------
- """
- # set initial state from sensor
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0)
- xs = np.array(ocp.solver.xs)
- # NOTE: for some reason the first value is always some wild bs
- vel_cmd = xs[1, robot.model.nq :]
- return vel_cmd, {}, {}
-
-
-def CrocoEEAndBaseP2PMPC(
- args, robot, T_w_eegoal: pin.SE3, p_basegoal: np.ndarray, run=True
-):
- """
- IKMPC
- -----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq)
- """
- x0 = np.concatenate([robot.q, robot.v])
- goal = (T_w_eegoal, p_basegoal)
- ocp = BaseAndSingleArmIKOCP(args, robot, x0, goal)
- ocp.solveInitialOCP(x0)
-
- controlLoop = partial(
- EEAndBaseP2PCtrlLoopTemplate,
- ocp,
- T_w_eegoal,
- p_basegoal,
- CrocoP2PEEAndBaseMPCControlLoop,
- args,
- robot,
- )
- log_item = {
- "qs": np.zeros(robot.nq),
- "dqs": np.zeros(robot.nv),
- "dqs_cmd": np.zeros(robot.nv),
- "ee_err_norm": np.zeros(1),
- "base_err_norm": np.zeros(1),
- }
- save_past_dict = {}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
- )
- if run:
- loop_manager.run()
- else:
- return loop_manager
-
-
-def CrocoP2PDualEEAndBaseMPCControlLoop(
- ocp,
- T_w_absgoal: pin.SE3,
- T_absgoal_l: pin.SE3,
- T_absgoal_r: pin.SE3,
- p_basegoal: np.ndarray,
- args: Namespace,
- robot: DualArmWholeBodyInterface,
- t: int,
- past_data: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- """
- CrocoP2PEEAndBaseMPCControlLoop
- ---------------------
- """
- x0 = np.concatenate([robot.q, robot.v])
- ocp.warmstartAndReSolve(x0)
- xs = np.array(ocp.solver.xs)
- # NOTE: for some reason the first value is always some wild bs
- v_cmd = xs[1, robot.model.nq :]
-
- return v_cmd, {}, {}
-
-
-def CrocoDualEEAndBaseP2PMPC(
- args: Namespace,
- robot: DualArmWholeBodyInterface,
- T_w_absgoal: pin.SE3,
- T_absgoal_l: pin.SE3,
- T_absgoal_r: pin.SE3,
- p_basegoal: np.ndarray,
- run=True,
-) -> None | ControlLoopManager:
- """
- IKMPC
- -----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq)
- """
- x0 = np.concatenate([robot.q, robot.v])
- T_w_lgoal = T_absgoal_l.act(T_w_absgoal)
- T_w_rgoal = T_absgoal_r.act(T_w_absgoal)
- goal = (T_w_lgoal, T_w_rgoal, p_basegoal)
- ocp = BaseAndDualArmIKOCP(args, robot, x0, goal)
- ocp.solveInitialOCP(x0)
-
- controlLoop = partial(
- DualEEAndBaseP2PCtrlLoopTemplate,
- ocp,
- T_w_absgoal,
- T_absgoal_l,
- T_absgoal_r,
- p_basegoal,
- CrocoP2PDualEEAndBaseMPCControlLoop,
- args,
- robot,
- )
- log_item = {
- "qs": np.zeros(robot.nq),
- "dqs": np.zeros(robot.nv),
- "dqs_cmd": np.zeros(robot.nv),
- "l_err_norm": np.zeros(1),
- "r_err_norm": np.zeros(1),
- "base_err_norm": np.zeros(1),
- }
- save_past_dict = {}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
- )
- if run:
- loop_manager.run()
- else:
- return loop_manager
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/base_and_dual_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/base_and_dual_arm_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..4912177eedd7d80f1a09efa7150d8e6a6189f40b
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/base_and_dual_arm_reference_mpc.py
@@ -0,0 +1,171 @@
+from smc.robots.interfaces.whole_body_interface import (
+ DualArmWholeBodyInterface,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.multiprocessing.process_manager import ProcessManager
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.croco_path_following.ocp.base_and_dual_arm_reference_ocp import (
+ BaseAndDualArmEEPathFollowingOCP,
+)
+from smc.path_generation.path_math.path2d_to_6d import (
+ path2D_to_SE3,
+)
+from smc.path_generation.path_math.cart_pulling_path_math import (
+ construct_EE_path,
+)
+from smc.path_generation.path_math.path_to_trajectory import (
+ path2D_to_trajectory2D,
+ pathSE3_to_trajectorySE3,
+)
+from smc.control.controller_templates.path_following_template import (
+ PathFollowingFromPlannerCtrllLoopTemplate,
+)
+
+import numpy as np
+from functools import partial
+import types
+from argparse import Namespace
+from pinocchio import SE3, log6
+from collections import deque
+
+
+def initializePastData(
+ args: Namespace, T_w_e: SE3, p_base: np.ndarray, max_base_v: float
+) -> np.ndarray:
+ # prepopulate past data to make base and cart be on the same path in the past
+ # (which didn't actually happen because this just started)
+ p_ee = T_w_e.translation[:2]
+ straight_line_path = np.linspace(p_ee, p_base, args.past_window_size)
+ # straight_line_path_timed = path2D_timed(args, straight_line_path, max_base_v)
+ # return straight_line_path_timed # this one is shortened to args.n_knots! and we want the whole buffer
+ return straight_line_path
+
+
+# TODO: the robot put in is a whole body robot,
+# which you need to implement
+def BaseAndDualEEPathFollowingMPCControlLoop(
+ T_absgoal_l: SE3,
+ T_absgoal_r: SE3,
+ ocp: CrocoOCP,
+ path2D_base: np.ndarray,
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ t: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ BaseAndDualEEPathFollowingMPCControlLoop
+ -----------------------------
+ cart pulling dual arm control loop
+ """
+ p = robot.q[:2]
+
+ # NOTE: this one is obtained as the future path from path planner
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+ trajectory_base = path2D_to_trajectory2D(args, path2D_base, max_base_v)
+ trajectory_base = np.hstack((trajectory_base, np.zeros((len(trajectory_base), 1))))
+
+ trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D"])
+ trajectorySE3_l = []
+ trajectorySE3_r = []
+ for traj_pose in trajectorySE3_handlebar:
+ trajectorySE3_l.append(T_absgoal_l.act(traj_pose))
+ trajectorySE3_r.append(T_absgoal_r.act(traj_pose))
+
+ if args.visualizer:
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ robot.visualizer_manager.sendCommand({"path": trajectory_base})
+ robot.visualizer_manager.sendCommand(
+ {"frame_path": trajectorySE3_handlebar}
+ )
+
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(
+ x0, data=(trajectory_base, trajectorySE3_l, trajectorySE3_r)
+ )
+ xs = np.array(ocp.solver.xs)
+ v_cmd = xs[1, robot.model.nq :]
+
+ err_vector_ee_l = log6(robot.T_w_l.actInv(trajectorySE3_l[0]))
+ err_norm_ee_l = np.linalg.norm(err_vector_ee_l)
+ err_vector_ee_r = log6(robot.T_w_r.actInv(trajectorySE3_r[0]))
+ err_norm_ee_r = np.linalg.norm(err_vector_ee_r)
+ err_vector_base = np.linalg.norm(p - trajectory_base[0][:2]) # z axis is irrelevant
+ log_item = {}
+ log_item["err_vec_ee_l"] = err_vector_ee_l
+ log_item["err_norm_ee_l"] = err_norm_ee_l.reshape((1,))
+ log_item["err_vec_ee_r"] = err_vector_ee_r
+ log_item["err_norm_ee_r"] = err_norm_ee_r.reshape((1,))
+ log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
+ save_past_item = {"path2D": p}
+ return v_cmd, save_past_item, log_item
+
+
+def BaseAndDualEEPathFollowingMPC(
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ path_planner: ProcessManager | types.FunctionType,
+ T_absgoal_l: SE3,
+ T_absgoal_r: SE3,
+ run=True,
+) -> None | ControlLoopManager:
+ """
+ CrocoEndEffectorPathFollowingMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq).
+ """
+ robot._mode = DualArmWholeBodyInterface.control_mode.whole_body
+ # NOTE: i'm shoving these into the class here - bad style,
+ # but i don't
+ T_w_abs = robot.T_w_abs
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp = BaseAndDualArmEEPathFollowingOCP(args, robot, x0)
+ ocp.solveInitialOCP(x0)
+
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+
+ path2D_handlebar = initializePastData(args, T_w_abs, robot.q[:2], float(max_base_v))
+
+ if type(path_planner) == types.FunctionType:
+ raise NotImplementedError
+ else:
+ get_position = lambda robot: robot.q[:2]
+ BaseAndDualEEPathFollowingMPCControlLoop_with_l_r = partial(
+ BaseAndDualEEPathFollowingMPCControlLoop, T_absgoal_l, T_absgoal_r
+ )
+ controlLoop = partial(
+ PathFollowingFromPlannerCtrllLoopTemplate,
+ path_planner,
+ get_position,
+ ocp,
+ BaseAndDualEEPathFollowingMPCControlLoop_with_l_r,
+ args,
+ robot,
+ )
+
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee_l": np.zeros((6,)),
+ "err_norm_ee_l": np.zeros((1,)),
+ "err_vec_ee_r": np.zeros((6,)),
+ "err_norm_ee_r": np.zeros((1,)),
+ "err_norm_base": np.zeros((1,)),
+ }
+ save_past_dict = {"path2D": T_w_abs.translation[:2]}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ # actually put past data into the past window
+ loop_manager.past_data["path2D"].clear()
+ loop_manager.past_data["path2D"].extend(
+ path2D_handlebar[i] for i in range(args.past_window_size)
+ )
+
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..4ba2af49b880050e09f538c9e8e1475f84c1c42b
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py
@@ -0,0 +1,115 @@
+from smc.robots.interfaces.whole_body_interface import (
+ SingleArmWholeBodyInterface,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.multiprocessing.process_manager import ProcessManager
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.croco_path_following.ocp.base_and_single_arm_reference_ocp import (
+ BaseAndEEPathFollowingOCP,
+)
+from smc.path_generation.path_math.path2d_to_6d import (
+ path2D_to_SE3,
+)
+from smc.path_generation.path_math.cart_pulling_path_math import (
+ construct_EE_path,
+)
+from smc.path_generation.path_math.path_to_trajectory import (
+ path2D_to_trajectory2D,
+ pathSE3_to_trajectorySE3,
+)
+from smc.control.controller_templates.path_following_template import (
+ PathFollowingFromPlannerCtrllLoopTemplate,
+)
+
+import numpy as np
+from functools import partial
+import types
+from argparse import Namespace
+from pinocchio import SE3, log6
+from collections import deque
+
+
+def BaseAndEEPathFollowingMPCControlLoop(
+ ocp: CrocoOCP,
+ path: tuple[np.ndarray, list[SE3]],
+ args: Namespace,
+ robot,
+ t: int,
+ _: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray]]:
+ """
+ BaseAndEEPathFollowingMPCControlLoop
+ -----------------------------
+ both a path for both the base and the end-effector are provided,
+ and both are followed
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.q
+ T_w_e = robot.T_w_e
+ path_base, path_EE = path_planner(T_w_e)
+
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0, data=(path_base, path_EE))
+ xs = np.array(ocp.solver.xs)
+ us = np.array(ocp.solver.us)
+ vel_cmds = xs[1, robot.model.nq :]
+
+ robot.sendVelocityCommand(vel_cmds)
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ path_base = np.array(path_base)
+ robot.visualizer_manager.sendCommand({"path": path_base})
+ robot.visualizer_manager.sendCommand({"frame_path": path_EE})
+
+ err_vector_ee = log6(T_w_e.actInv(path_EE[0]))
+ err_vector_base = np.linalg.norm(q[:2] - path_base[0][:2]) # z axis is irrelevant
+ log_item["err_vec_ee"] = err_vector_ee
+ log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
+ log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.v.reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+
+def BaseAndEEPathFollowingMPC(
+ args: Namespace,
+ robot: SingleArmWholeBodyInterface,
+ path_planner: ProcessManager | types.FunctionType,
+ run=True,
+) -> None | ControlLoopManager:
+ """
+ BaseAndEEPathFollowingMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq).
+ """
+
+ robot._mode = SingleArmWholeBodyInterface.control_mode.whole_body
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp = BaseAndEEPathFollowingOCP(args, robot, x0)
+ ocp.solveInitialOCP(x0)
+
+ controlLoop = partial(
+ BaseAndEEPathFollowingMPCControlLoop,
+ args,
+ robot,
+ ocp,
+ path_planner,
+ )
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee": np.zeros((6,)),
+ "err_norm_ee": np.zeros((1,)),
+ "err_norm_base": np.zeros((1,)),
+ }
+ loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/base_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/base_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..8a1399373896312db1bc69141a2e34ae7880fb7a
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/base_reference_mpc.py
@@ -0,0 +1,98 @@
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.multiprocessing.process_manager import ProcessManager
+from smc.control.optimal_control.croco_path_following.ocp.base_reference_ocp import (
+ BasePathFollowingOCP,
+)
+from smc.control.controller_templates.path_following_template import (
+ PathFollowingFromPlannerCtrllLoopTemplate,
+)
+from smc.path_generation.path_math.path_to_trajectory import path2D_to_trajectory2D
+
+import numpy as np
+from functools import partial
+import types
+from argparse import Namespace
+from collections import deque
+
+
+def CrocoBasePathFollowingMPCControlLoop(
+ ocp: BasePathFollowingOCP,
+ path2D: np.ndarray,
+ args: Namespace,
+ robot: MobileBaseInterface,
+ t: int,
+ _: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+
+ p = robot.T_w_b.translation[:2]
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+ path_base = path2D_to_trajectory2D(args, path2D, max_base_v)
+ path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
+
+ if args.visualizer:
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ robot.visualizer_manager.sendCommand({"path": path_base})
+
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0, data=(path_base))
+ xs = np.array(ocp.solver.xs)
+ v_cmd = xs[1, robot.model.nq :]
+ # NOTE: we might get stuck
+ # TODO: make it more robust, it can still get stuck with this
+ # a good idea is to do this for a some number of iterations -
+ # you can keep this them in past data
+ if np.linalg.norm(v_cmd) < 0.05:
+ print(t, "RESOLVING FOR ONLY FINAL PATH POINT")
+ last_point_only = np.ones((len(path_base), 2))
+ last_point_only = np.hstack((last_point_only, np.zeros((len(path_base), 1))))
+ last_point_only = last_point_only * path_base[-1]
+ ocp.warmstartAndReSolve(x0, data=(last_point_only))
+ xs = np.array(ocp.solver.xs)
+ v_cmd = xs[1, robot.model.nq :]
+
+ err_vector_base = np.linalg.norm(p - path_base[0][:2]) # z axis is irrelevant
+ log_item = {}
+ log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
+ return v_cmd, {}, log_item
+
+
+def CrocoBasePathFollowingMPC(
+ args: Namespace,
+ robot: MobileBaseInterface,
+ x0: np.ndarray,
+ path_planner: ProcessManager | types.FunctionType,
+ run=True,
+) -> None | ControlLoopManager:
+ """
+ CrocoBasePathFollowingMPC
+ -----
+ """
+
+ ocp = BasePathFollowingOCP(args, robot, x0)
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.solveInitialOCP(x0)
+
+ get_position = lambda robot: robot.T_w_b.translation[:2]
+ controlLoop = partial(
+ PathFollowingFromPlannerCtrllLoopTemplate,
+ path_planner,
+ get_position,
+ ocp,
+ CrocoBasePathFollowingMPCControlLoop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
+ "err_norm_base": np.zeros((1,)),
+ }
+ save_past_item = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_item, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/dual_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/dual_arm_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..bdf94dba262227c21efdfa7e7387e60c33f1d539
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/dual_arm_reference_mpc.py
@@ -0,0 +1,135 @@
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from smc.control.optimal_control.croco_path_following.ocp.dual_arm_reference_ocp import (
+ DualArmEEPathFollowingOCP,
+)
+from smc.control.controller_templates.path_following_template import (
+ PathFollowingFromPlannerCtrllLoopTemplate,
+)
+from smc.path_generation.path_math.path2d_to_6d import (
+ path2D_to_SE3,
+)
+from smc.path_generation.path_math.path_to_trajectory import (
+ path2D_to_trajectory2D,
+ pathSE3_to_trajectorySE3,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.multiprocessing.process_manager import ProcessManager
+
+import numpy as np
+from functools import partial
+import types
+from argparse import Namespace
+from pinocchio import SE3, log6
+from collections import deque
+
+
+def CrocoDualArmEEPathFollowingMPCControlLoop(
+ T_absgoal_l: SE3,
+ T_absgoal_r: SE3,
+ ocp: DualArmEEPathFollowingOCP,
+ path: np.ndarray | list[SE3], # either 2D or pose
+ args: Namespace,
+ robot: DualArmInterface,
+ t: int,
+ _: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ CrocoEEPathFollowingMPCControlLoop
+ -----------------------------
+ end-effectors follows the prescribed path.
+ the path is defined with T_w_abs, from which
+ T_w_l and T_w_r are defined via T_absgoal_l and T_absgoal_r.
+
+ path planner can either be a function which spits out a list of path points
+ or an instance of ProcessManager which spits out path points
+ by calling ProcessManager.getData()
+ """
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+ perc_of_max_v = 0.5
+ velocity = perc_of_max_v * max_base_v
+ if type(path) == np.ndarray:
+ trajectory2D = path2D_to_trajectory2D(args, path, velocity)
+ trajectory_T_w_abs = path2D_to_SE3(trajectory2D, args.handlebar_height)
+ else:
+ trajectory_T_w_abs = pathSE3_to_trajectorySE3(args, path, velocity)
+
+ trajectory_T_w_l = []
+ trajectory_T_w_r = []
+ for traj_T_w_abs in trajectory_T_w_abs:
+ trajectory_T_w_l.append(T_absgoal_l.act(traj_T_w_abs))
+ trajectory_T_w_r.append(T_absgoal_r.act(traj_T_w_abs))
+
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ if args.visualizer:
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ robot.visualizer_manager.sendCommand({"frame_path": trajectory_T_w_abs})
+
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0, data=(trajectory_T_w_l, trajectory_T_w_r))
+ xs = np.array(ocp.solver.xs)
+ v_cmd = xs[1, robot.model.nq :]
+
+ err_vector_ee_l = log6(robot.T_w_l.actInv(trajectory_T_w_l[0])).vector
+ err_vector_ee_r = log6(robot.T_w_r.actInv(trajectory_T_w_r[0])).vector
+ log_item = {"err_vec_ee_l": err_vector_ee_l}
+ log_item = {"err_vec_ee_r": err_vector_ee_r}
+
+ return v_cmd, {}, log_item
+
+
+def CrocoDualArmEEPathFollowingMPC(
+ args: Namespace,
+ robot: DualArmInterface,
+ T_absgoal_l: SE3,
+ T_absgoal_r: SE3,
+ x0: np.ndarray,
+ path_planner: ProcessManager | types.FunctionType,
+ run=True,
+) -> None | ControlLoopManager:
+ """
+ CrocoEndEffectorPathFollowingMPC
+ -----
+ follow a fixed pre-determined path, or a path received from a planner.
+ the path does NOT need to start from your current pose - you need to get to it yourself.
+ """
+
+ ocp = DualArmEEPathFollowingOCP(args, robot, x0)
+ # technically should be done in controlloop because now
+ # it's solved 2 times before the first command,
+ # but we don't have time for details rn
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.solveInitialOCP(x0)
+
+ # NOTE: a bit of dirty hacking for now because
+ # i know that the only planner i have gives 2D references.
+ # but of course otherwise the only reasonable
+ # thing is that the planner gives an SE3 reference
+ # to a MPC following an SE3 reference
+ if type(path_planner) == ProcessManager:
+ get_position = lambda robot: robot.T_w_abs.translation[:2]
+ else:
+ get_position = lambda robot: robot.T_w_abs
+ loop = partial(CrocoDualArmEEPathFollowingMPCControlLoop, T_absgoal_l, T_absgoal_r)
+ controlLoop = partial(
+ PathFollowingFromPlannerCtrllLoopTemplate,
+ path_planner,
+ get_position,
+ ocp,
+ loop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee_l": np.zeros(6),
+ "err_vec_ee_r": np.zeros(6),
+ }
+ save_past_item = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_item, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/single_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/single_arm_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..08aae3ec29efd2ac260fdd8e9be60e667c67203e
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/single_arm_reference_mpc.py
@@ -0,0 +1,119 @@
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.optimal_control.croco_path_following.ocp.single_arm_reference_ocp import (
+ CrocoEEPathFollowingOCP,
+)
+from smc.control.controller_templates.path_following_template import (
+ PathFollowingFromPlannerCtrllLoopTemplate,
+)
+from smc.path_generation.path_math.path2d_to_6d import (
+ path2D_to_SE3,
+)
+from smc.path_generation.path_math.path_to_trajectory import (
+ path2D_to_trajectory2D,
+ pathSE3_to_trajectorySE3,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.multiprocessing.process_manager import ProcessManager
+
+import numpy as np
+from functools import partial
+import types
+from argparse import Namespace
+from pinocchio import SE3, log6
+from collections import deque
+
+
+def CrocoEEPathFollowingMPCControlLoop(
+ ocp: CrocoEEPathFollowingOCP,
+ path: np.ndarray | list[SE3], # either 2D or pose
+ args: Namespace,
+ robot: SingleArmInterface,
+ t: int,
+ _: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ CrocoPathFollowingMPCControlLoop
+ -----------------------------
+ end-effector follows the prescribed path.
+
+ path planner can either be a function which spits out a list of path points
+ or an instance of ProcessManager which spits out path points
+ by calling ProcessManager.getData()
+ """
+ max_base_v = np.linalg.norm(robot._max_v[:2])
+ perc_of_max_v = 0.5
+ velocity = perc_of_max_v * max_base_v
+ if type(path) == np.ndarray:
+ trajectory2D = path2D_to_trajectory2D(args, path, velocity)
+ trajectory_EE_SE3 = path2D_to_SE3(trajectory2D, args.handlebar_height)
+ else:
+ trajectory_EE_SE3 = pathSE3_to_trajectorySE3(args, path, velocity)
+
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ if args.visualizer:
+ if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+ robot.visualizer_manager.sendCommand({"frame_path": trajectory_EE_SE3})
+
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0, data=trajectory_EE_SE3)
+ xs = np.array(ocp.solver.xs)
+ v_cmd = xs[1, robot.model.nq :]
+
+ err_vector_ee = log6(robot.T_w_e.actInv(trajectory_EE_SE3[0])).vector
+ log_item = {"err_vec_ee": err_vector_ee}
+
+ return v_cmd, {}, log_item
+
+
+def CrocoEEPathFollowingMPC(
+ args: Namespace,
+ robot: SingleArmInterface,
+ x0: np.ndarray,
+ path_planner: ProcessManager | types.FunctionType,
+ run=True,
+) -> None | ControlLoopManager:
+ """
+ CrocoEndEffectorPathFollowingMPC
+ -----
+ follow a fixed pre-determined path, or a path received from a planner.
+ the path does NOT need to start from your current pose - you need to get to it yourself.
+ """
+
+ ocp = CrocoEEPathFollowingOCP(args, robot, x0)
+ # technically should be done in controlloop because now
+ # it's solved 2 times before the first command,
+ # but we don't have time for details rn
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.solveInitialOCP(x0)
+
+ # NOTE: a bit of dirty hacking for now because
+ # i know that the only planner i have gives 2D references.
+ # but of course otherwise the only reasonable
+ # thing is that the planner gives an SE3 reference
+ # to a MPC following an SE3 reference
+ if type(path_planner) == ProcessManager:
+ get_position = lambda robot: robot.T_w_e.translation[:2]
+ else:
+ get_position = lambda robot: robot.T_w_e
+ controlLoop = partial(
+ PathFollowingFromPlannerCtrllLoopTemplate,
+ path_planner,
+ get_position,
+ ocp,
+ CrocoEEPathFollowingMPCControlLoop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "err_vec_ee": np.zeros(6),
+ }
+ save_past_item = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_item, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_path_following/ocp/base_and_dual_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_path_following/ocp/base_and_dual_arm_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..fd7784023032e67d2cf90f31dac7f57b9fcc50d1
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/ocp/base_and_dual_arm_reference_ocp.py
@@ -0,0 +1,40 @@
+from smc.control.optimal_control.croco_point_to_point.ocp.base_and_dual_arm_reference_ocp import (
+ BaseAndDualArmIKOCP,
+)
+from smc.robots.interfaces.whole_body_interface import (
+ DualArmWholeBodyInterface,
+)
+
+from argparse import Namespace
+
+
+class BaseAndDualArmEEPathFollowingOCP(BaseAndDualArmIKOCP):
+ def __init__(self, args: Namespace, robot: DualArmWholeBodyInterface, x0):
+ goal = robot.T_w_l, robot.T_w_r, robot.T_w_b.translation
+ super().__init__(args, robot, x0, goal)
+
+ def updateCosts(self, data):
+ path_base = data[0]
+ pathSE3_l = data[1]
+ pathSE3_r = data[2]
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "base_translation" + str(i)
+ ].cost.residual.reference = path_base[i]
+ runningModel.differential.costs.costs[
+ "l_ee_pose" + str(i)
+ ].cost.residual.reference = pathSE3_l[i]
+ runningModel.differential.costs.costs[
+ "r_ee_pose" + str(i)
+ ].cost.residual.reference = pathSE3_r[i]
+
+ # idk if that's necessary
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "base_translation" + str(self.args.n_knots)
+ ].cost.residual.reference = path_base[-1]
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "l_ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = pathSE3_l[-1]
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "r_ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = pathSE3_r[-1]
diff --git a/python/smc/control/optimal_control/croco_path_following/ocp/base_and_single_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_path_following/ocp/base_and_single_arm_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..ef4072c25e1e47259cab993752734d63db9869b5
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/ocp/base_and_single_arm_reference_ocp.py
@@ -0,0 +1,41 @@
+from smc.control.optimal_control.croco_point_to_point.ocp.base_and_single_arm_reference_ocp import (
+ BaseAndSingleArmIKOCP,
+)
+from smc.robots.interfaces.whole_body_interface import (
+ SingleArmWholeBodyInterface,
+)
+
+from argparse import Namespace
+
+
+class BaseAndEEPathFollowingOCP(BaseAndSingleArmIKOCP):
+ """
+ createBaseAndEEPathFollowingOCP
+ -------------------------------
+ creates a path following problem.
+ it is instantiated to just to stay at the current position.
+ NOTE: the path MUST be time indexed with the SAME time used between the knots
+ """
+
+ def __init__(self, args: Namespace, robot: SingleArmWholeBodyInterface, x0):
+ goal = (robot.T_w_e, robot.T_w_b.translation.copy())
+ super().__init__(args, robot, x0, goal)
+
+ def updateCosts(self, data):
+ path_base = data[0]
+ pathSE3 = data[1]
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "base_translation" + str(i)
+ ].cost.residual.reference = path_base[i]
+ runningModel.differential.costs.costs[
+ "ee_pose" + str(i)
+ ].cost.residual.reference = pathSE3[i]
+
+ # idk if that's necessary
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "base_translation" + str(self.args.n_knots)
+ ].cost.residual.reference = path_base[-1]
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = pathSE3[-1]
diff --git a/python/smc/control/optimal_control/croco_path_following/ocp/base_reference_ocp.py b/python/smc/control/optimal_control/croco_path_following/ocp/base_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..0de8d699687ed346293980b7d0967116916aa83d
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/ocp/base_reference_ocp.py
@@ -0,0 +1,32 @@
+from smc.control.optimal_control.croco_point_to_point.ocp.base_reference_ocp import (
+ BaseIKOCP,
+)
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+
+from argparse import Namespace
+
+
+class BasePathFollowingOCP(BaseIKOCP):
+ """
+ createBaseAndEEPathFollowingOCP
+ -------------------------------
+ creates a path following problem.
+ it is instantiated to just to stay at the current position.
+ NOTE: the path MUST be time indexed with the SAME time used between the knots
+ """
+
+ def __init__(self, args: Namespace, robot: MobileBaseInterface, x0):
+ goal = robot.T_w_b.translation.copy()
+ super().__init__(args, robot, x0, goal)
+
+ def updateCosts(self, data):
+ path_base = data
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "base_translation" + str(i)
+ ].cost.residual.reference = path_base[i]
+
+ # idk if that's necessary
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "base_translation" + str(self.args.n_knots)
+ ].cost.residual.reference = path_base[-1]
diff --git a/python/smc/control/optimal_control/croco_path_following/ocp/dual_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_path_following/ocp/dual_arm_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..e9edab741ee4aeb947c40d97ca909c471f63eb1c
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/ocp/dual_arm_reference_ocp.py
@@ -0,0 +1,30 @@
+from smc.control.optimal_control.croco_point_to_point.ocp.dual_arm_reference_ocp import (
+ DualArmIKOCP,
+)
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+
+from argparse import Namespace
+
+
+class DualArmEEPathFollowingOCP(DualArmIKOCP):
+ def __init__(self, args: Namespace, robot: DualArmInterface, x0):
+ goal = (robot.T_w_l, robot.T_w_r)
+ super().__init__(args, robot, x0, goal)
+
+ def updateCosts(self, data):
+ pathSE3_l = data[0]
+ pathSE3_r = data[1]
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "l_ee_pose" + str(i)
+ ].cost.residual.reference = pathSE3_l[i]
+ runningModel.differential.costs.costs[
+ "r_ee_pose" + str(i)
+ ].cost.residual.reference = pathSE3_r[i]
+
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "l_ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = pathSE3_l[-1]
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "r_ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = pathSE3_r[-1]
diff --git a/python/smc/control/optimal_control/croco_path_following/ocp/single_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_path_following/ocp/single_arm_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..892553fe4a55a8dd8ffe70338e25bc592fd1f01c
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_path_following/ocp/single_arm_reference_ocp.py
@@ -0,0 +1,31 @@
+from smc.control.optimal_control.croco_point_to_point.ocp.single_arm_reference_ocp import (
+ SingleArmIKOCP,
+)
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+
+import numpy as np
+from argparse import Namespace
+
+
+class CrocoEEPathFollowingOCP(SingleArmIKOCP):
+ """
+ createCrocoEEPathFollowingOCP
+ -------------------------------
+ creates a path following problem with a single end-effector reference.
+ it is instantiated to just to stay at the current position.
+ NOTE: the path MUST be time indexed with the SAME time used between the knots
+ """
+
+ def __init__(self, args: Namespace, robot: SingleArmInterface, x0: np.ndarray):
+ goal = robot.T_w_e
+ super().__init__(args, robot, x0, goal)
+
+ def updateCosts(self, data):
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "ee_pose" + str(i)
+ ].cost.residual.reference = data[i]
+
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = data[-1]
diff --git a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_dual_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_dual_arm_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..9dd11edf7d61e6663d2efc93809a49595d08e631
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_dual_arm_reference_mpc.py
@@ -0,0 +1,94 @@
+from smc.control.controller_templates.point_to_point import (
+ DualEEAndBaseP2PCtrlLoopTemplate,
+)
+from smc.robots.interfaces.whole_body_interface import (
+ DualArmWholeBodyInterface,
+)
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.croco_point_to_point.ocp.base_and_dual_arm_reference_ocp import (
+ BaseAndDualArmIKOCP,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+
+import pinocchio as pin
+import numpy as np
+from functools import partial
+from collections import deque
+from argparse import Namespace
+
+
+def CrocoP2PDualEEAndBaseMPCControlLoop(
+ ocp,
+ T_w_absgoal: pin.SE3,
+ T_absgoal_l: pin.SE3,
+ T_absgoal_r: pin.SE3,
+ p_basegoal: np.ndarray,
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ t: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ CrocoP2PEEAndBaseMPCControlLoop
+ ---------------------
+ """
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0)
+ xs = np.array(ocp.solver.xs)
+ # NOTE: for some reason the first value is always some wild bs
+ v_cmd = xs[1, robot.model.nq :]
+
+ return v_cmd, {}, {}
+
+
+def CrocoDualEEAndBaseP2PMPC(
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ T_w_absgoal: pin.SE3,
+ T_absgoal_l: pin.SE3,
+ T_absgoal_r: pin.SE3,
+ p_basegoal: np.ndarray,
+ run=True,
+) -> None | ControlLoopManager:
+ """
+ IKMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq)
+ """
+ x0 = np.concatenate([robot.q, robot.v])
+ T_w_lgoal = T_absgoal_l.act(T_w_absgoal)
+ T_w_rgoal = T_absgoal_r.act(T_w_absgoal)
+ goal = (T_w_lgoal, T_w_rgoal, p_basegoal)
+ ocp = BaseAndDualArmIKOCP(args, robot, x0, goal)
+ ocp.solveInitialOCP(x0)
+
+ controlLoop = partial(
+ DualEEAndBaseP2PCtrlLoopTemplate,
+ ocp,
+ T_w_absgoal,
+ T_absgoal_l,
+ T_absgoal_r,
+ p_basegoal,
+ CrocoP2PDualEEAndBaseMPCControlLoop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
+ "dqs_cmd": np.zeros(robot.nv),
+ "l_err_norm": np.zeros(1),
+ "r_err_norm": np.zeros(1),
+ "base_err_norm": np.zeros(1),
+ }
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_single_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_single_arm_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..2c6a411c70858c8649601a2a57f0d3605a8dd906
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_single_arm_reference_mpc.py
@@ -0,0 +1,81 @@
+from smc.control.controller_templates.point_to_point import (
+ EEAndBaseP2PCtrlLoopTemplate,
+)
+from smc.robots.interfaces.whole_body_interface import (
+ SingleArmWholeBodyInterface,
+)
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.croco_point_to_point.ocp.base_and_single_arm_reference_ocp import (
+ BaseAndSingleArmIKOCP,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+
+import pinocchio as pin
+import numpy as np
+from functools import partial
+from collections import deque
+from argparse import Namespace
+
+
+def CrocoP2PEEAndBaseMPCControlLoop(
+ ocp: CrocoOCP,
+ T_w_eegoal: pin.SE3,
+ p_basegoal: np.ndarray,
+ args: Namespace,
+ robot: SingleArmWholeBodyInterface,
+ t: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ CrocoP2PEEAndBaseMPCControlLoop
+ ---------------------
+ """
+ # set initial state from sensor
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0)
+ xs = np.array(ocp.solver.xs)
+ # NOTE: for some reason the first value is always some wild bs
+ vel_cmd = xs[1, robot.model.nq :]
+ return vel_cmd, {}, {}
+
+
+def CrocoEEAndBaseP2PMPC(
+ args, robot, T_w_eegoal: pin.SE3, p_basegoal: np.ndarray, run=True
+):
+ """
+ IKMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq)
+ """
+ x0 = np.concatenate([robot.q, robot.v])
+ goal = (T_w_eegoal, p_basegoal)
+ ocp = BaseAndSingleArmIKOCP(args, robot, x0, goal)
+ ocp.solveInitialOCP(x0)
+
+ controlLoop = partial(
+ EEAndBaseP2PCtrlLoopTemplate,
+ ocp,
+ T_w_eegoal,
+ p_basegoal,
+ CrocoP2PEEAndBaseMPCControlLoop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.nq),
+ "dqs": np.zeros(robot.nv),
+ "dqs_cmd": np.zeros(robot.nv),
+ "ee_err_norm": np.zeros(1),
+ "base_err_norm": np.zeros(1),
+ }
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_reference_mpc.py b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..0399371fad1fffe53c10dfc6014f142740cb6da5
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_reference_mpc.py
@@ -0,0 +1,72 @@
+from smc.control.controller_templates.point_to_point import (
+ BaseP2PCtrlLoopTemplate,
+)
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.croco_point_to_point.ocp.base_reference_ocp import (
+ BaseIKOCP,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+
+import pinocchio as pin
+import numpy as np
+from functools import partial
+from collections import deque
+from argparse import Namespace
+
+
+def CrocoBaseP2PMPCControlLoop(
+ ocp: CrocoOCP,
+ p_basegoal: np.ndarray,
+ args: Namespace,
+ robot: MobileBaseInterface,
+ _: int,
+ __: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ CrocoIKMPCControlLoop
+ ---------------------
+ """
+ # set initial state from sensor
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0)
+ xs = np.array(ocp.solver.xs)
+ # NOTE: for some reason the first value is always some wild bs
+ vel_cmd = xs[1, robot.model.nq :]
+ return vel_cmd, {}, {}
+
+
+def CrocoBaseP2PMPC(
+ args: Namespace, robot: MobileBaseInterface, p_basegoal: np.ndarray, run=True
+):
+ """
+ CrocoBaseP2PMPC
+ -----
+ base point-to-point task
+ """
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp = BaseIKOCP(args, robot, x0, p_basegoal)
+ ocp.solveInitialOCP(x0)
+
+ controlLoop = partial(
+ BaseP2PCtrlLoopTemplate,
+ ocp,
+ p_basegoal,
+ CrocoBaseP2PMPCControlLoop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.nq),
+ "base_err_norm": np.zeros(1),
+ "dqs": np.zeros(robot.nv),
+ "dqs_cmd": np.zeros(robot.nv),
+ }
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_point_to_point/mpc/dual_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_point_to_point/mpc/dual_arm_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..8972eb3f51d61a87481e5e564624dadd24712194
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/mpc/dual_arm_reference_mpc.py
@@ -0,0 +1,109 @@
+from smc.control.controller_templates.point_to_point import (
+ DualEEP2PCtrlLoopTemplate,
+)
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.croco_point_to_point.ocp.dual_arm_reference_ocp import (
+ DualArmIKOCP,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+
+import pinocchio as pin
+import numpy as np
+from functools import partial
+from collections import deque
+from argparse import Namespace
+
+from IPython import embed
+
+
+def CrocoDualEEP2PMPCControlLoop(
+ ocp: CrocoOCP,
+ T_w_absgoal: pin.SE3,
+ T_absgoal_l: pin.SE3,
+ T_absgoal_r: pin.SE3,
+ args: Namespace,
+ robot: DualArmInterface,
+ t: int,
+ past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ CrocoDualEEP2PMPCControlLoop
+ ---------------------
+ """
+ # set initial state from sensor
+ x0 = np.concatenate([robot.q, robot.v])
+
+ T_w_lgoal = T_absgoal_l.act(T_w_absgoal)
+ T_w_rgoal = T_absgoal_r.act(T_w_absgoal)
+ SEerror_left = robot.T_w_l.actInv(T_w_lgoal)
+ SEerror_right = robot.T_w_r.actInv(T_w_rgoal)
+
+ # update costs depending on goal proximity
+ err_vector_left = pin.log6(SEerror_left).vector
+ err_vector_right = pin.log6(SEerror_right).vector
+ err_vector_left_norm = np.linalg.norm(err_vector_left)
+ err_vector_right_norm = np.linalg.norm(err_vector_right)
+
+ # completely arbitrary numbers in condition
+ if (err_vector_left_norm < 0.7) and (err_vector_right_norm < 0.7):
+ ocp.terminalCostModel.changeCostStatus("velFinal_l", True)
+ ocp.terminalCostModel.changeCostStatus("velFinal_r", True)
+
+ ocp.warmstartAndReSolve(x0)
+ xs = np.array(ocp.solver.xs)
+ # NOTE: for some reason the first value is always some wild bs
+ vel_cmd = xs[1, robot.model.nq :]
+ return vel_cmd, {}, {}
+
+
+def CrocoDualEEP2PMPC(
+ args: Namespace,
+ robot: DualArmInterface,
+ T_w_absgoal: pin.SE3,
+ T_absgoal_l: pin.SE3,
+ T_absgoal_r: pin.SE3,
+ run=True,
+):
+ """
+ DualEEP2PMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq)
+ """
+ x0 = np.concatenate([robot.q, robot.v])
+ 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.terminalCostModel.changeCostStatus("velFinal_l", False)
+ ocp.terminalCostModel.changeCostStatus("velFinal_r", False)
+ ocp.solveInitialOCP(x0)
+
+ controlLoop = partial(
+ DualEEP2PCtrlLoopTemplate,
+ ocp,
+ T_w_absgoal,
+ T_absgoal_l,
+ T_absgoal_r,
+ CrocoDualEEP2PMPCControlLoop,
+ args,
+ robot,
+ )
+ log_item = {
+ "qs": np.zeros(robot.nq),
+ "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(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_point_to_point/mpc/single_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_point_to_point/mpc/single_arm_reference_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..bc459a1508693f01a2d2998595556bea9da78db6
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/mpc/single_arm_reference_mpc.py
@@ -0,0 +1,70 @@
+from smc.control.controller_templates.point_to_point import (
+ EEP2PCtrlLoopTemplate,
+)
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.croco_point_to_point.ocp.single_arm_reference_ocp import (
+ SingleArmIKOCP,
+)
+from smc.control.control_loop_manager import ControlLoopManager
+
+import pinocchio as pin
+import numpy as np
+from functools import partial
+from collections import deque
+from argparse import Namespace
+
+
+def CrocoEEP2PMPCControlLoop(
+ ocp: CrocoOCP,
+ T_w_goal: pin.SE3,
+ args: Namespace,
+ robot: SingleArmInterface,
+ _: int,
+ __: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+ """
+ CrocoIKMPCControlLoop
+ ---------------------
+ """
+ # set initial state from sensor
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp.warmstartAndReSolve(x0)
+ xs = np.array(ocp.solver.xs)
+ # NOTE: for some reason the first value is always some wild bs
+ vel_cmd = xs[1, robot.model.nq :]
+ return vel_cmd, {}, {}
+
+
+def CrocoEEP2PMPC(
+ args: Namespace, robot: SingleArmInterface, T_w_goal: pin.SE3, run=True
+):
+ """
+ IKMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq)
+ """
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp = SingleArmIKOCP(args, robot, x0, T_w_goal)
+ ocp.solveInitialOCP(x0)
+
+ controlLoop = partial(
+ EEP2PCtrlLoopTemplate, ocp, T_w_goal, CrocoEEP2PMPCControlLoop, args, robot
+ )
+ log_item = {
+ "qs": np.zeros(robot.nq),
+ "err_norm": np.zeros(1),
+ "dqs": np.zeros(robot.nv),
+ "dqs_cmd": np.zeros(robot.nv),
+ }
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_dual_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_dual_arm_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..44c342f6daa0f6500e13b4c38022e763c1a9bc50
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_dual_arm_reference_ocp.py
@@ -0,0 +1,53 @@
+from smc.robots.interfaces.whole_body_interface import (
+ DualArmWholeBodyInterface,
+)
+from smc.control.optimal_control.croco_point_to_point.ocp.base_reference_ocp import (
+ BaseIKOCP,
+)
+from smc.control.optimal_control.croco_point_to_point.ocp.dual_arm_reference_ocp import (
+ DualArmIKOCP,
+)
+
+import numpy as np
+from argparse import Namespace
+
+
+class BaseAndDualArmIKOCP(DualArmIKOCP, BaseIKOCP):
+ def __init__(
+ self,
+ args: Namespace,
+ robot: DualArmWholeBodyInterface,
+ x0: np.ndarray,
+ goal,
+ ):
+ super().__init__(args, robot, x0, goal)
+
+ def constructTaskCostsValues(self):
+ self.base_translation_cost_values = np.linspace(
+ self.args.base_translation_cost,
+ self.args.base_translation_cost
+ * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+ self.ee_pose_cost_values = np.linspace(
+ self.args.ee_pose_cost,
+ self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
+ def constructTaskObjectiveFunction(
+ self,
+ goal,
+ ) -> None:
+ T_w_lgoal, T_w_rgoal, p_basegoal = goal
+ super().constructTaskObjectiveFunction((T_w_lgoal, T_w_rgoal))
+ BaseIKOCP.constructTaskObjectiveFunction(self, p_basegoal)
+
+ # there is nothing to update in a point-to-point task
+ def updateCosts(self, data) -> None:
+ pass
+
+ def updateGoalInModels(self, goal) -> None:
+ T_w_lgoal, T_w_rgoal, p_basegoal = goal
+ super().updateGoalInModels((T_w_lgoal, T_w_rgoal))
+ BaseIKOCP.updateGoalInModels(self, p_basegoal)
diff --git a/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_single_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_single_arm_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..75f4d1c0609628d5369c285ba09248af6b6f35bf
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_single_arm_reference_ocp.py
@@ -0,0 +1,54 @@
+from smc.robots.interfaces.whole_body_interface import (
+ SingleArmWholeBodyInterface,
+)
+from smc.control.optimal_control.croco_point_to_point.ocp.base_reference_ocp import (
+ BaseIKOCP,
+)
+from smc.control.optimal_control.croco_point_to_point.ocp.single_arm_reference_ocp import (
+ SingleArmIKOCP,
+)
+
+import numpy as np
+from argparse import Namespace
+
+
+class BaseAndSingleArmIKOCP(SingleArmIKOCP, BaseIKOCP):
+ def __init__(
+ self,
+ args: Namespace,
+ robot: SingleArmWholeBodyInterface,
+ x0: np.ndarray,
+ goal,
+ ):
+ super().__init__(args, robot, x0, goal)
+
+ def constructTaskCostsValues(self):
+ self.base_translation_cost_values = np.linspace(
+ self.args.base_translation_cost,
+ self.args.base_translation_cost
+ * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+ self.ee_pose_cost_values = np.linspace(
+ self.args.ee_pose_cost,
+ self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
+ def constructTaskObjectiveFunction(
+ self,
+ goal,
+ ) -> None:
+ T_w_eegoal, p_basegoal = goal
+ super().constructTaskObjectiveFunction(T_w_eegoal)
+ BaseIKOCP.constructTaskObjectiveFunction(self, p_basegoal)
+
+ # there is nothing to update in a point-to-point task
+ def updateCosts(self, data):
+ pass
+
+ def updateGoalInModels(self, goal) -> None:
+ # self, T_w_eegoal: pin.SE3, p_basegoal: np.ndarray) -> None:
+ T_w_eegoal, p_basegoal = goal
+ super().updateGoalInModels(T_w_eegoal)
+ BaseIKOCP.updateGoalInModels(self, p_basegoal)
diff --git a/python/smc/control/optimal_control/croco_point_to_point/ocp/base_reference_ocp.py b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..086b5e3684942bcc0c0db4409a17b46ac6fbb282
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_reference_ocp.py
@@ -0,0 +1,60 @@
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+
+import numpy as np
+from pinocchio import SE3
+import crocoddyl
+from argparse import Namespace
+
+
+class BaseIKOCP(CrocoOCP):
+ def __init__(
+ self,
+ args: Namespace,
+ robot: MobileBaseInterface,
+ x0: np.ndarray,
+ p_basegoal: SE3,
+ ):
+ super().__init__(args, robot, x0, p_basegoal)
+
+ def constructTaskCostsValues(self):
+ self.base_translation_cost_values = np.linspace(
+ self.args.base_translation_cost,
+ self.args.base_translation_cost
+ * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
+ def constructTaskObjectiveFunction(self, goal) -> None:
+ p_basegoal = goal
+ for i in range(self.args.n_knots):
+ baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
+ self.state, self.robot.base_frame_id, p_basegoal, self.state.nv
+ )
+ baseTrackingCost = crocoddyl.CostModelResidual(
+ self.state, baseTranslationResidual
+ )
+ self.runningCostModels[i].addCost(
+ "base_translation" + str(i),
+ baseTrackingCost,
+ self.base_translation_cost_values[i],
+ )
+ self.terminalCostModel.addCost(
+ "base_translation" + str(self.args.n_knots),
+ baseTrackingCost,
+ self.base_translation_cost_values[-1],
+ )
+
+ # there is nothing to update in a point-to-point task
+ def updateCosts(self, data):
+ pass
+
+ def updateGoalInModels(self, goal) -> None:
+ p_basegoal = goal
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "base_translation" + str(i)
+ ].cost.residual.reference = p_basegoal
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "base_translation" + str(self.args.n_knots)
+ ].cost.residual.reference = p_basegoal
diff --git a/python/smc/control/optimal_control/croco_point_to_point/ocp/dual_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_point_to_point/ocp/dual_arm_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..dd3f39ae3d22c054ed8a63ca580bfe1bafe57564
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/ocp/dual_arm_reference_ocp.py
@@ -0,0 +1,109 @@
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+
+import numpy as np
+import pinocchio as pin
+import crocoddyl
+from argparse import Namespace
+
+
+class DualArmIKOCP(CrocoOCP):
+ def __init__(
+ self, args: Namespace, robot: DualArmInterface, x0: np.ndarray, goal: pin.SE3
+ ):
+ super().__init__(args, robot, x0, goal)
+
+ def constructTaskCostsValues(self):
+ self.ee_pose_cost_values = np.linspace(
+ self.args.ee_pose_cost,
+ self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
+ def constructTaskObjectiveFunction(self, goal) -> None:
+ T_w_lgoal, T_w_rgoal = goal
+ framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.l_ee_frame_id,
+ T_w_lgoal.copy(),
+ self.state.nv,
+ )
+ framePlacementResidual_r = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.r_ee_frame_id,
+ T_w_rgoal.copy(),
+ self.state.nv,
+ )
+ goalTrackingCost_l = crocoddyl.CostModelResidual(
+ self.state, framePlacementResidual_l
+ )
+ goalTrackingCost_r = crocoddyl.CostModelResidual(
+ self.state, framePlacementResidual_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!
+
+ if self.args.stop_at_final:
+ frameVelocityResidual_l = crocoddyl.ResidualModelFrameVelocity(
+ self.state,
+ self.robot.l_ee_frame_id,
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD,
+ )
+ frameVelocityCost_l = crocoddyl.CostModelResidual(
+ self.state, frameVelocityResidual_l
+ )
+ 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(
+ "l_ee_pose" + str(i),
+ goalTrackingCost_l,
+ self.ee_pose_cost_values[i],
+ )
+ self.runningCostModels[i].addCost(
+ "r_ee_pose" + str(i),
+ goalTrackingCost_r,
+ self.ee_pose_cost_values[i],
+ )
+ self.terminalCostModel.addCost(
+ "l_ee_pose" + str(self.args.n_knots),
+ goalTrackingCost_l,
+ self.ee_pose_cost_values[-1],
+ )
+ self.terminalCostModel.addCost(
+ "r_ee_pose" + str(self.args.n_knots),
+ goalTrackingCost_r,
+ self.ee_pose_cost_values[-1],
+ )
+ if self.args.stop_at_final:
+ 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):
+ pass
+
+ def updateGoalInModels(self, goal) -> None:
+ T_w_lgoal, T_w_rgoal = goal
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "l_ee_pose" + str(i)
+ ].cost.residual.reference = T_w_lgoal
+ runningModel.differential.costs.costs[
+ "r_ee_pose" + str(i)
+ ].cost.residual.reference = T_w_rgoal
+
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "l_ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = T_w_lgoal
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "r_ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = T_w_rgoal
diff --git a/python/smc/control/optimal_control/croco_point_to_point/ocp/single_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_point_to_point/ocp/single_arm_reference_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..08ab02cd9ed2d22e035b78139d2b937862de8da9
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_point_to_point/ocp/single_arm_reference_ocp.py
@@ -0,0 +1,72 @@
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+
+import numpy as np
+import pinocchio as pin
+import crocoddyl
+from argparse import Namespace
+
+
+class SingleArmIKOCP(CrocoOCP):
+ def __init__(
+ self,
+ args: Namespace,
+ robot: SingleArmInterface,
+ x0: np.ndarray,
+ T_w_goal: pin.SE3,
+ ):
+ super().__init__(args, robot, x0, T_w_goal)
+
+ def constructTaskCostsValues(self):
+ self.ee_pose_cost_values = np.linspace(
+ self.args.ee_pose_cost,
+ self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+ self.args.n_knots + 1,
+ )
+
+ def constructTaskObjectiveFunction(self, goal) -> None:
+ T_w_goal = goal
+ framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.ee_frame_id,
+ T_w_goal.copy(),
+ self.state.nv,
+ )
+ goalTrackingCost = crocoddyl.CostModelResidual(
+ self.state, framePlacementResidual
+ )
+ # 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(
+ "ee_pose" + str(i), goalTrackingCost, self.ee_pose_cost_values[i]
+ )
+ self.terminalCostModel.addCost(
+ "ee_pose" + str(self.args.n_knots),
+ goalTrackingCost,
+ self.ee_pose_cost_values[-1],
+ )
+ # self.terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
+
+ # there is nothing to update in a point-to-point task
+ def updateCosts(self, data):
+ pass
+
+ def updateGoalInModels(self, goal) -> None:
+ T_w_goal = goal
+ for i, runningModel in enumerate(self.solver.problem.runningModels):
+ runningModel.differential.costs.costs[
+ "ee_pose" + str(i)
+ ].cost.residual.reference = T_w_goal
+ self.solver.problem.terminalModel.differential.costs.costs[
+ "ee_pose" + str(self.args.n_knots)
+ ].cost.residual.reference = T_w_goal
diff --git a/python/smc/control/optimal_control/path_following_croco_ocp.py b/python/smc/control/optimal_control/path_following_croco_ocp.py
deleted file mode 100644
index 1b9db978f172dad1b4b07a7befc62da6f7f90e7e..0000000000000000000000000000000000000000
--- a/python/smc/control/optimal_control/path_following_croco_ocp.py
+++ /dev/null
@@ -1,159 +0,0 @@
-from smc.control.optimal_control.point_to_point_croco_ocp import (
- BaseIKOCP,
- SingleArmIKOCP,
- DualArmIKOCP,
- BaseAndSingleArmIKOCP,
- BaseAndDualArmIKOCP,
-)
-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,
-)
-
-import numpy as np
-from argparse import Namespace
-
-
-# NOTE: all of these should be rewritten so that it inherits from BaseIKOCP,
-# and then you just override update goal.
-# you'd cut this code in less than half
-class BasePathFollowingOCP(BaseIKOCP):
- """
- createBaseAndEEPathFollowingOCP
- -------------------------------
- creates a path following problem.
- it is instantiated to just to stay at the current position.
- NOTE: the path MUST be time indexed with the SAME time used between the knots
- """
-
- def __init__(self, args, robot: MobileBaseInterface, x0):
- goal = robot.T_w_b.translation.copy()
- super().__init__(args, robot, x0, goal)
-
- def updateCosts(self, data):
- path_base = data
- for i, runningModel in enumerate(self.solver.problem.runningModels):
- runningModel.differential.costs.costs[
- "base_translation" + str(i)
- ].cost.residual.reference = path_base[i]
-
- # idk if that's necessary
- self.solver.problem.terminalModel.differential.costs.costs[
- "base_translation" + str(self.args.n_knots)
- ].cost.residual.reference = path_base[-1]
-
-
-class CrocoEEPathFollowingOCP(SingleArmIKOCP):
- """
- createCrocoEEPathFollowingOCP
- -------------------------------
- creates a path following problem with a single end-effector reference.
- it is instantiated to just to stay at the current position.
- NOTE: the path MUST be time indexed with the SAME time used between the knots
- """
-
- def __init__(self, args: Namespace, robot: SingleArmInterface, x0: np.ndarray):
- goal = robot.T_w_e
- super().__init__(args, robot, x0, goal)
-
- def updateCosts(self, data):
- for i, runningModel in enumerate(self.solver.problem.runningModels):
- runningModel.differential.costs.costs[
- "ee_pose" + str(i)
- ].cost.residual.reference = data[i]
-
- self.solver.problem.terminalModel.differential.costs.costs[
- "ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = data[-1]
-
-
-class BaseAndEEPathFollowingOCP(BaseAndSingleArmIKOCP):
- """
- createBaseAndEEPathFollowingOCP
- -------------------------------
- creates a path following problem.
- it is instantiated to just to stay at the current position.
- NOTE: the path MUST be time indexed with the SAME time used between the knots
- """
-
- def __init__(self, args, robot: SingleArmWholeBodyInterface, x0):
- goal = (robot.T_w_e, robot.T_w_b.translation.copy())
- super().__init__(args, robot, x0, goal)
-
- def updateCosts(self, data):
- path_base = data[0]
- pathSE3 = data[1]
- for i, runningModel in enumerate(self.solver.problem.runningModels):
- runningModel.differential.costs.costs[
- "base_translation" + str(i)
- ].cost.residual.reference = path_base[i]
- runningModel.differential.costs.costs[
- "ee_pose" + str(i)
- ].cost.residual.reference = pathSE3[i]
-
- # idk if that's necessary
- self.solver.problem.terminalModel.differential.costs.costs[
- "base_translation" + str(self.args.n_knots)
- ].cost.residual.reference = path_base[-1]
- self.solver.problem.terminalModel.differential.costs.costs[
- "ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = pathSE3[-1]
-
-
-class DualArmEEPathFollowingOCP(DualArmIKOCP):
- def __init__(self, args, robot: DualArmInterface, x0):
- goal = (robot.T_w_l, robot.T_w_r)
- super().__init__(args, robot, x0, goal)
-
- def updateCosts(self, data):
- pathSE3_l = data[0]
- pathSE3_r = data[1]
- for i, runningModel in enumerate(self.solver.problem.runningModels):
- runningModel.differential.costs.costs[
- "l_ee_pose" + str(i)
- ].cost.residual.reference = pathSE3_l[i]
- runningModel.differential.costs.costs[
- "r_ee_pose" + str(i)
- ].cost.residual.reference = pathSE3_r[i]
-
- self.solver.problem.terminalModel.differential.costs.costs[
- "l_ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = pathSE3_l[-1]
- self.solver.problem.terminalModel.differential.costs.costs[
- "r_ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = pathSE3_r[-1]
-
-
-class BaseAndDualArmEEPathFollowingOCP(BaseAndDualArmIKOCP):
- def __init__(self, args, robot: DualArmWholeBodyInterface, x0):
- goal = robot.T_w_l, robot.T_w_r, robot.T_w_b.translation
- super().__init__(args, robot, x0, goal)
-
- def updateCosts(self, data):
- path_base = data[0]
- pathSE3_l = data[1]
- pathSE3_r = data[2]
- for i, runningModel in enumerate(self.solver.problem.runningModels):
- runningModel.differential.costs.costs[
- "base_translation" + str(i)
- ].cost.residual.reference = path_base[i]
- runningModel.differential.costs.costs[
- "l_ee_pose" + str(i)
- ].cost.residual.reference = pathSE3_l[i]
- runningModel.differential.costs.costs[
- "r_ee_pose" + str(i)
- ].cost.residual.reference = pathSE3_r[i]
-
- # idk if that's necessary
- self.solver.problem.terminalModel.differential.costs.costs[
- "base_translation" + str(self.args.n_knots)
- ].cost.residual.reference = path_base[-1]
- self.solver.problem.terminalModel.differential.costs.costs[
- "l_ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = pathSE3_l[-1]
- self.solver.problem.terminalModel.differential.costs.costs[
- "r_ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = pathSE3_r[-1]
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
deleted file mode 100644
index 1e6fada56b7244c272a1a5190943c7a5d3694d7f..0000000000000000000000000000000000000000
--- a/python/smc/control/optimal_control/point_to_point_croco_ocp.py
+++ /dev/null
@@ -1,313 +0,0 @@
-from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
-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,
-)
-
-import numpy as np
-import pinocchio as pin
-import crocoddyl
-from argparse import Namespace
-
-
-class BaseIKOCP(CrocoOCP):
- def __init__(
- self,
- args: Namespace,
- robot: SingleArmInterface,
- x0: np.ndarray,
- p_basegoal: pin.SE3,
- ):
- super().__init__(args, robot, x0, p_basegoal)
-
- def constructTaskCostsValues(self):
- self.base_translation_cost_values = np.linspace(
- self.args.base_translation_cost,
- self.args.base_translation_cost
- * self.args.linearly_increasing_task_cost_factor,
- self.args.n_knots + 1,
- )
-
- def constructTaskObjectiveFunction(self, goal) -> None:
- p_basegoal = goal
- for i in range(self.args.n_knots):
- baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
- self.state, self.robot.base_frame_id, p_basegoal, self.state.nv
- )
- baseTrackingCost = crocoddyl.CostModelResidual(
- self.state, baseTranslationResidual
- )
- self.runningCostModels[i].addCost(
- "base_translation" + str(i),
- baseTrackingCost,
- self.base_translation_cost_values[i],
- )
- self.terminalCostModel.addCost(
- "base_translation" + str(self.args.n_knots),
- baseTrackingCost,
- self.base_translation_cost_values[-1],
- )
-
- # there is nothing to update in a point-to-point task
- def updateCosts(self, data):
- pass
-
- def updateGoalInModels(self, goal) -> None:
- p_basegoal = goal
- for i, runningModel in enumerate(self.solver.problem.runningModels):
- runningModel.differential.costs.costs[
- "base_translation" + str(i)
- ].cost.residual.reference = p_basegoal
- self.solver.problem.terminalModel.differential.costs.costs[
- "base_translation" + str(self.args.n_knots)
- ].cost.residual.reference = p_basegoal
-
-
-class SingleArmIKOCP(CrocoOCP):
- def __init__(
- self,
- args: Namespace,
- robot: SingleArmInterface,
- x0: np.ndarray,
- T_w_goal: pin.SE3,
- ):
- super().__init__(args, robot, x0, T_w_goal)
-
- def constructTaskCostsValues(self):
- self.ee_pose_cost_values = np.linspace(
- self.args.ee_pose_cost,
- self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
- self.args.n_knots + 1,
- )
-
- def constructTaskObjectiveFunction(self, goal) -> None:
- T_w_goal = goal
- framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.ee_frame_id,
- T_w_goal.copy(),
- self.state.nv,
- )
- goalTrackingCost = crocoddyl.CostModelResidual(
- self.state, framePlacementResidual
- )
- # 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(
- "ee_pose" + str(i), goalTrackingCost, self.ee_pose_cost_values[i]
- )
- self.terminalCostModel.addCost(
- "ee_pose" + str(self.args.n_knots),
- goalTrackingCost,
- self.ee_pose_cost_values[-1],
- )
- # self.terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
-
- # there is nothing to update in a point-to-point task
- def updateCosts(self, data):
- pass
-
- def updateGoalInModels(self, goal) -> None:
- T_w_goal = goal
- for i, runningModel in enumerate(self.solver.problem.runningModels):
- runningModel.differential.costs.costs[
- "ee_pose" + str(i)
- ].cost.residual.reference = T_w_goal
- self.solver.problem.terminalModel.differential.costs.costs[
- "ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = T_w_goal
-
-
-class DualArmIKOCP(CrocoOCP):
- def __init__(
- self, args: Namespace, robot: DualArmInterface, x0: np.ndarray, goal: pin.SE3
- ):
- super().__init__(args, robot, x0, goal)
-
- def constructTaskCostsValues(self):
- self.ee_pose_cost_values = np.linspace(
- self.args.ee_pose_cost,
- self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
- self.args.n_knots + 1,
- )
-
- def constructTaskObjectiveFunction(self, goal) -> None:
- T_w_lgoal, T_w_rgoal = goal
- framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.l_ee_frame_id,
- T_w_lgoal.copy(),
- self.state.nv,
- )
- framePlacementResidual_r = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.r_ee_frame_id,
- T_w_rgoal.copy(),
- self.state.nv,
- )
- goalTrackingCost_l = crocoddyl.CostModelResidual(
- self.state, framePlacementResidual_l
- )
- goalTrackingCost_r = crocoddyl.CostModelResidual(
- self.state, framePlacementResidual_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(
- "l_ee_pose" + str(i),
- goalTrackingCost_l,
- self.ee_pose_cost_values[i],
- )
- self.runningCostModels[i].addCost(
- "r_ee_pose" + str(i),
- goalTrackingCost_r,
- self.ee_pose_cost_values[i],
- )
- self.terminalCostModel.addCost(
- "l_ee_pose" + str(self.args.n_knots),
- goalTrackingCost_l,
- self.ee_pose_cost_values[-1],
- )
- self.terminalCostModel.addCost(
- "r_ee_pose" + str(self.args.n_knots),
- goalTrackingCost_r,
- self.ee_pose_cost_values[-1],
- )
- # 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):
- pass
-
- def updateGoalInModels(self, goal) -> None:
- T_w_lgoal, T_w_rgoal = goal
- for i, runningModel in enumerate(self.solver.problem.runningModels):
- runningModel.differential.costs.costs[
- "l_ee_pose" + str(i)
- ].cost.residual.reference = T_w_lgoal
- runningModel.differential.costs.costs[
- "r_ee_pose" + str(i)
- ].cost.residual.reference = T_w_rgoal
-
- self.solver.problem.terminalModel.differential.costs.costs[
- "l_ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = T_w_lgoal
- self.solver.problem.terminalModel.differential.costs.costs[
- "r_ee_pose" + str(self.args.n_knots)
- ].cost.residual.reference = T_w_rgoal
-
-
-class BaseAndSingleArmIKOCP(SingleArmIKOCP, BaseIKOCP):
- def __init__(
- self,
- args: Namespace,
- robot: SingleArmWholeBodyInterface,
- x0: np.ndarray,
- goal,
- ):
- super().__init__(args, robot, x0, goal)
-
- def constructTaskCostsValues(self):
- self.base_translation_cost_values = np.linspace(
- self.args.base_translation_cost,
- self.args.base_translation_cost
- * self.args.linearly_increasing_task_cost_factor,
- self.args.n_knots + 1,
- )
- self.ee_pose_cost_values = np.linspace(
- self.args.ee_pose_cost,
- self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
- self.args.n_knots + 1,
- )
-
- def constructTaskObjectiveFunction(
- self,
- goal,
- ) -> None:
- T_w_eegoal, p_basegoal = goal
- super().constructTaskObjectiveFunction(T_w_eegoal)
- BaseIKOCP.constructTaskObjectiveFunction(self, p_basegoal)
-
- # there is nothing to update in a point-to-point task
- def updateCosts(self, data):
- pass
-
- def updateGoalInModels(self, goal) -> None:
- # self, T_w_eegoal: pin.SE3, p_basegoal: np.ndarray) -> None:
- T_w_eegoal, p_basegoal = goal
- super().updateGoalInModels(T_w_eegoal)
- BaseIKOCP.updateGoalInModels(self, p_basegoal)
-
-
-class BaseAndDualArmIKOCP(DualArmIKOCP, BaseIKOCP):
- def __init__(
- self,
- args: Namespace,
- robot: DualArmWholeBodyInterface,
- x0: np.ndarray,
- goal,
- ):
- super().__init__(args, robot, x0, goal)
-
- def constructTaskCostsValues(self):
- self.base_translation_cost_values = np.linspace(
- self.args.base_translation_cost,
- self.args.base_translation_cost
- * self.args.linearly_increasing_task_cost_factor,
- self.args.n_knots + 1,
- )
- self.ee_pose_cost_values = np.linspace(
- self.args.ee_pose_cost,
- self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
- self.args.n_knots + 1,
- )
-
- def constructTaskObjectiveFunction(
- self,
- goal,
- ) -> None:
- T_w_lgoal, T_w_rgoal, p_basegoal = goal
- super().constructTaskObjectiveFunction((T_w_lgoal, T_w_rgoal))
- BaseIKOCP.constructTaskObjectiveFunction(self, p_basegoal)
-
- # there is nothing to update in a point-to-point task
- def updateCosts(self, data) -> None:
- pass
-
- def updateGoalInModels(self, goal) -> None:
- T_w_lgoal, T_w_rgoal, p_basegoal = goal
- super().updateGoalInModels((T_w_lgoal, T_w_rgoal))
- BaseIKOCP.updateGoalInModels(self, p_basegoal)
diff --git a/python/smc/path_generation/path_math/path_to_trajectory.py b/python/smc/path_generation/path_math/path_to_trajectory.py
index d92d3065c9d37a87c47a3081cefe3925450f3803..08ef8f9a3498ecbfa74a310ad5e556e6114c70a9 100644
--- a/python/smc/path_generation/path_math/path_to_trajectory.py
+++ b/python/smc/path_generation/path_math/path_to_trajectory.py
@@ -1,5 +1,6 @@
import numpy as np
from argparse import Namespace
+from pinocchio import SE3
def computePath2DLength(path2D):
@@ -41,3 +42,39 @@ def path2D_to_trajectory2D(
]
).T
return trajectory2D
+
+
+def pathSE3_to_trajectorySE3(
+ args: Namespace, pathSE3: np.ndarray, velocity: float
+) -> list[SE3]:
+ path2D = np.zeros((len(pathSE3), 2))
+ for i, pose in enumerate(pathSE3):
+ path2D[i] = pose.translation[:2]
+ path_length = computePath2DLength(path2D)
+ # NOTE: sometimes the path planner gives me the same god damn points
+ # and that's it. can't do much about, expect set those point then.
+ # and i need to return now so that the math doesn't break down the line
+ if path_length < 1e-3:
+ return [pathSE3[0]] * (args.n_knots + 1)
+ total_time = path_length / velocity
+ # NOTE: assuming the path is uniformly sampled
+ t_path = np.linspace(0.0, total_time, len(path2D))
+ t_ocp = np.linspace(0.0, args.ocp_dt * (args.n_knots + 1), args.n_knots + 1)
+
+ trajectorySE3 = []
+ path_index = 0
+ for t_traj in t_ocp:
+ if t_traj > t_path[path_index + 1]:
+ if path_index < len(pathSE3) - 2:
+ path_index += 1
+
+ if path_index < len(pathSE3) - 2:
+ pose_traj = SE3.Interpolate(
+ pathSE3[path_index],
+ pathSE3[path_index + 1],
+ t_traj - t_path[path_index],
+ )
+ trajectorySE3.append(pose_traj)
+ else:
+ trajectorySE3.append(pathSE3[-1])
+ return trajectorySE3
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index b8fff65a0ab2b3941a1909c75f05a1ac9a02ff93..baa81d88320cea0f6982a8f0e25c68dff3cb8507 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -77,9 +77,9 @@ class AbstractRobotManager(abc.ABC):
self._max_v = np.clip(self._MAX_V, 0.0, args.max_v_percentage * self._MAX_V)
# NOTE: make sure you've read pinocchio docs and understand why
# nq and nv are not necessarily the same number
- self._q = np.zeros(self.nq)
- self._v = np.zeros(self.nv)
- self._a = np.zeros(self.nv)
+ self._q = np.zeros(self.model.nq)
+ self._v = np.zeros(self.model.nv)
+ self._a = np.zeros(self.model.nv)
self._comfy_configuration: np.ndarray
@@ -331,6 +331,11 @@ class AbstractRobotManager(abc.ABC):
else:
print("not implemented yet, so nothing is going to happen!")
+
+ def getJacobian(self) -> np.ndarray:
+ ...
+
+
########################################################################################
# visualizer management. ideally transferred elsewhere
###################################################################################
diff --git a/python/smc/robots/implementations/mir.py b/python/smc/robots/implementations/mir.py
index f64f9e20c39341ed9691600beb05926be54eb781..33bb99e3ac092ae2561ddb271bed42806706ab75 100644
--- a/python/smc/robots/implementations/mir.py
+++ b/python/smc/robots/implementations/mir.py
@@ -26,6 +26,14 @@ class SimulatedMirRobotManager(AbstractSimulatedRobotManager, AbstractMirRobotMa
print("SimulatedMirRobotManager init")
super().__init__(args)
+ def setInitialPose(self):
+ self._q = np.zeros(4)
+ self._q[0] = np.random.random()
+ self._q[1] = np.random.random()
+ theta = np.random.random() * 2 * np.pi - np.pi
+ self._q[2] = np.cos(theta)
+ self._q[3] = np.sin(theta)
+
class RealMirRobotManager(AbstractMirRobotManager):
# TODO: implement
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 28af048a563e96f679d7988996ef86faf8b46869..1e76a5c00d73fa81a9a0bb6a088aadd358b3b183 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -30,6 +30,28 @@ class AbstractMobileYuMiRobotManager(DualArmWholeBodyInterface):
self._mode: DualArmWholeBodyInterface.control_mode = (
DualArmWholeBodyInterface.control_mode.whole_body
)
+ self._comfy_configuration = np.array(
+ [
+ 0.0, # x
+ 0.0, # y
+ 1.0, # cos(theta)
+ 0.0, # sin(theta)
+ 0.045,
+ -0.155,
+ -0.394,
+ -0.617,
+ -0.939,
+ -0.343,
+ -1.216,
+ -0.374,
+ -0.249,
+ 0.562,
+ -0.520,
+ 0.934,
+ -0.337,
+ 1.400,
+ ]
+ )
super().__init__(args)
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index a19fbd05f18781300446e2b72ba789ea0d512f66..e6bf2f06a49802347c9ce88b22ebfa3674043471 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -232,3 +232,50 @@ class DualArmInterface(SingleArmInterface):
v_cmd_to_real = np.clip(v_cmd_to_real, -1 * self._max_v, self._max_v)
self.sendVelocityCommandToReal(v_cmd_to_real)
+
+ # NOTE: we almost certainly want to compute this w.r.t. T_w_abs
+ # but i don't have time to write all the math out
+ def computeManipulabilityIndexQDerivative(self) -> np.ndarray:
+
+ def oneArm(joint_id: int) -> np.ndarray:
+ J = pin.computeJointJacobian(self.model, self.data, self._q, joint_id)
+ Jp = J.T @ np.linalg.inv(
+ J @ J.T + np.eye(J.shape[0], J.shape[0]) * self.args.tikhonov_damp
+ )
+ # res = np.zeros(self.nv)
+ # v0 = np.zeros(self.nv)
+ res = np.zeros(self.model.nv)
+ v0 = np.zeros(self.model.nv)
+ for k in range(6):
+ pin.computeForwardKinematicsDerivatives(
+ self.model,
+ self.data,
+ self._q,
+ Jp[:, k],
+ v0,
+ # self.model,
+ # self.data,
+ # self._q,
+ # v0,
+ # np.zeros(self.model.nv),
+ )
+ JqJpk = pin.getJointVelocityDerivatives(
+ self.model, self.data, joint_id, pin.LOCAL
+ )[0]
+ res += JqJpk[k, :]
+ res *= self.computeManipulabilityIndex()
+ return res
+
+ l_joint_index = self.model.frames[self._l_ee_frame_id].parent
+ r_joint_index = self.model.frames[self._r_ee_frame_id].parent
+
+ # TODO: joint_ids obviously have to be defined per robot, this is a dirty hack
+ # because i know i'm on yumi
+ res_left = oneArm(l_joint_index)
+ if self._mode == AbstractRobotManager.control_mode.left_arm_only:
+ return res_left[:l_joint_index]
+ res_right = oneArm(r_joint_index)
+ if self._mode == AbstractRobotManager.control_mode.right_arm_only:
+ return res_right[l_joint_index:]
+
+ return res_left + res_right
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index 31f00455a73f723cabaf3d455f751b495c5a87de..fbe85ee2aa44674d8c58a44ed07c0f7fa5741a3b 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -56,3 +56,41 @@ class SingleArmInterface(AbstractRobotManager):
self._updateQ()
self._updateV()
self.forwardKinematics()
+
+ # NOTE: manipulability calculations are here
+ # only because i have no idea where to put them at the moment
+ # TODO: put them in a better place
+
+ def computeManipulabilityIndex(self) -> np.ndarray:
+ J = self.getJacobian()
+ return np.sqrt(np.linalg.det(J @ J.T))
+
+ def computeManipulabilityIndexQDerivative(self) -> np.ndarray:
+ joint_index = self.model.frames[self._ee_frame_id].parent
+ J = pin.computeJointJacobian(self.model, self.data, self._q, joint_index)
+ Jp = J.T @ np.linalg.inv(
+ J @ J.T + np.eye(J.shape[0], J.shape[0]) * self.args.tikhonov_damp
+ )
+ # res = np.zeros(self.nv)
+ # v0 = np.zeros(self.nv)
+ res = np.zeros(self.model.nv)
+ v0 = np.zeros(self.model.nv)
+ for k in range(6):
+ pin.computeForwardKinematicsDerivatives(
+ self.model,
+ self.data,
+ self._q,
+ Jp[:, k],
+ v0,
+ # self.model,
+ # self.data,
+ # self._q,
+ # v0,
+ # np.zeros(self.model.nv),
+ )
+ JqJpk = pin.getJointVelocityDerivatives(
+ self.model, self.data, joint_index, pin.LOCAL
+ )[0]
+ res += JqJpk[k, :]
+ res *= self.computeManipulabilityIndex()
+ return res
diff --git a/python/smc/util/draw_path.py b/python/smc/util/draw_path.py
index 3b3a7f29f4282a470a97a0c4f4c7514958bc081f..a32b614a5df2c1255d5038b4f9341192424365d1 100644
--- a/python/smc/util/draw_path.py
+++ b/python/smc/util/draw_path.py
@@ -52,12 +52,14 @@ class DrawPathManager:
print("pixel path:")
print(self.path)
self.disconnect()
- np.savetxt("./path_in_pixels.csv", self.path, delimiter=",", fmt="%.5f")
+ np.savetxt(
+ "./parameters/path_in_pixels.csv", self.path, delimiter=",", fmt="%.5f"
+ )
# plt.close over exit so that we can call this from elsewhere and not kill the program
plt.close()
-def drawPath(args:Namespace)->np.ndarray:
+def drawPath(args: Namespace) -> np.ndarray:
# normalize both x and y to 0-1 range
# we can multiply however we want later
# idk about the number of points, but it's large enough to draw