diff --git a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
index a728f36b3f9a4df81d1ab1b584895df0ca3104bb..0876a250f66824132c4e5fab1903325e3c285f2d 100644
--- a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
+++ b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py
@@ -1,17 +1,20 @@
-from smc.robots.implementations.mobile_yumi import SimulatedMobileYuMiRobotManager
-from smc import getMinimalArgParser
+from smc.robots.abstract_robotmanager import AbstractRobotManager
+from smc.robots.utils import getRobotFromArgs
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 (
- CrocoDualEEAndBaseP2PMPC,
+from smc.path_generation.planner import starPlanner
+from smc.multiprocessing import ProcessManager
+from smc.control.optimal_control.croco_point_to_point.mpc.base_reference_mpc import (
+ CrocoBaseP2PMPC,
)
-from smc.control.optimal_control.croco_mpc_path_following import (
- BaseAndDualEEPathFollowingMPC,
+from smc.control.joint_space.joint_space_point_to_point import moveJP
+from smc.control.cartesian_space.pink_p2p import (
+ DualArmIKSelfAvoidanceViaEndEffectorSpheres,
)
-from smc.multiprocessing import ProcessManager
+
+from utils import get_args, constructInitialT_w_abs
+from dual_arm_cart_pulling_control_loop import DualArmCartPulling
+from fixed_path_planner import contructPath, fixedPathPlanner
import time
import numpy as np
@@ -19,86 +22,101 @@ 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",
- )
- parser.add_argument(
- "--base-to-handlebar-preferred-distance",
- type=float,
- default=0.5,
- help="prefered path arclength from mobile base position to handlebar",
- )
- args = parser.parse_args()
- return args
-
-
if __name__ == "__main__":
args = get_args()
assert args.robot == "myumi"
- robot = SimulatedMobileYuMiRobotManager(args)
- # TODO: HOW IS IT POSSIBLE THAT T_W_E IS WRONG WITHOUT STEP CALLED HERE?????????????????
+ robot = getRobotFromArgs(args)
T_absgoal_l = pin.SE3.Identity()
- T_absgoal_l.translation[1] = 0.1
+ T_absgoal_l.translation[1] = 0.15
T_absgoal_r = pin.SE3.Identity()
- T_absgoal_r.translation[1] = -0.1
- robot._step()
- 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])
-
- T_w_abs = robot.T_w_abs
- 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_abs.translation[:2], 3, None
- )
- _, map_as_list = createSampleStaticMap()
- if args.visualizer:
- robot.sendRectangular2DMapToVisualizer(map_as_list)
- # time.sleep(5)
-
- T_w_abs = robot.T_w_abs
- data = None
- # get first path
- ##########################################3
- # initialize
- ###########################################
- while data is None:
- path_planner.sendCommand(T_w_abs.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)
+ T_absgoal_r.translation[1] = -0.15
+
+ # TODO: before running on the real robot change this to just reading the current position!!
+ if args.planner:
+ robot._step()
+ 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])
+
+ ######################################################
+ # 1. initialize path planner, or instantiate pre-made path
+ # and make that a path planner, for the base
+ #################################################
+ 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, robot.T_w_b.translation[:2], 3, None
+ )
+ _, map_as_list = createSampleStaticMap()
+ # TODO: put in real lab map
+ if args.visualizer:
+ robot.sendRectangular2DMapToVisualizer(map_as_list)
+
+ data = None
+ while data is None:
+ path_planner.sendCommand(robot.T_w_b.translation[:2])
+ data = path_planner.getData()
+ time.sleep(1)
+
+ _, path_base = data
+ path_base = np.array(path_base).reshape((-1, 2))
+ path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
+ else:
+ path_base = contructPath(args, robot)
+ path_planner = partial(fixedPathPlanner, [0], path_base)
+
+ ################################################
+ # 2. construct initial T_w_abs (starting middle of handlebar pose)
+ ################################################
+ pathSE3 = path2D_to_SE3(path_base, args.handlebar_height)
+ T_w_absgoal = constructInitialT_w_abs(args, path_base, pathSE3[0].rotation)
+
if args.visualizer:
- # TODO: document this somewhere
robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})
- if np.linalg.norm(pin.log6(T_w_abs.actInv(pathSE3[0]))) > 1e-2:
- print("going to initial path position")
- p_base = pathSE3[0].translation.copy()
- p_base[0] -= args.base_to_handlebar_preferred_distance
- p_base[2] = 0.0
- print(pathSE3[0].translation)
- print(p_base)
- CrocoDualEEAndBaseP2PMPC(
- args, robot, pathSE3[0], T_absgoal_l, T_absgoal_r, p_base
- )
- print("initialized!")
- BaseAndDualEEPathFollowingMPC(args, robot, path_planner, T_absgoal_l, T_absgoal_r)
- print("final position:", robot.T_w_e)
+ ##################################################
+ # 3. get arms into comfy position
+ ####################################################
+ print("putting arms to a comfy configuration")
+ robot.mode = AbstractRobotManager.control_mode.upper_body
+ moveJP(robot._comfy_configuration, args, robot)
+
+ ###################################################
+ # 4. get base to starting pose
+ ###################################################
+ print("getting base to start of path")
+ p_basegoal = path_base[0]
+ robot.mode = AbstractRobotManager.control_mode.base_only
+ CrocoBaseP2PMPC(args, robot, p_basegoal)
+
+ # NOTE: alternative: navigate to start point and position arms simulatenously
+ # NOTE: does not work well :( (would need finessing which is too time-consuming for what it's worth)
+ # CrocoDualEEAndBaseP2PMPC(args, robot, pathSE3[0], T_absgoal_l, T_absgoal_r, p_base)
+
+ ###################################################
+ # DualArmMoveL 10 cm above handlebar
+ ###################################################
+ robot.mode = AbstractRobotManager.control_mode.upper_body
+ T_w_absgoal.translation[2] -= 0.1
+ DualArmIKSelfAvoidanceViaEndEffectorSpheres(
+ T_w_absgoal, T_absgoal_l, T_absgoal_r, args, robot
+ )
+ ###################################################
+ # DualArmMoveL down to handlebar
+ ###################################################
+ T_w_absgoal.translation[2] += 0.1
+ DualArmIKSelfAvoidanceViaEndEffectorSpheres(
+ T_w_absgoal, T_absgoal_l, T_absgoal_r, args, robot
+ )
+ ###################################################
+ # TODO: (5) grip handlebar with gripper (sleep before, or wait for keyboard input idk)
+ ###################################################
+ # time.sleep(5)
+
+ DualArmCartPulling(args, robot, path_planner, T_absgoal_l, T_absgoal_r)
if args.real:
robot.stopRobot()
diff --git a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..28f4a868f4ad722e54106b81aa1635e052077d12 100644
--- a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py
+++ b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py
@@ -0,0 +1,173 @@
+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.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 DualArmCartPullingControlLoop(
+ 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.T_w_b.translation[: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 :]
+
+ if np.linalg.norm(v_cmd[:3]) < 0.05:
+ print(t, "RESOLVING FOR ONLY FINAL PATH POINT")
+ last_point_only = np.ones((len(trajectory_base), 2))
+ last_point_only = np.hstack(
+ (last_point_only, np.zeros((len(trajectory_base), 1)))
+ )
+ last_point_only = last_point_only * trajectory_base[-1]
+ ocp.warmstartAndReSolve(x0, data=(last_point_only))
+ 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 DualArmCartPulling(
+ 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))
+
+ get_position = lambda robot: robot.q[:2]
+ DualArmCartPullingControlLoop_with_l_r = partial(
+ DualArmCartPullingControlLoop, T_absgoal_l, T_absgoal_r
+ )
+ controlLoop = partial(
+ PathFollowingFromPlannerCtrllLoopTemplate,
+ path_planner,
+ get_position,
+ ocp,
+ DualArmCartPullingControlLoop_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/examples/cart_pulling/control_sub_problems/fixed_path_planner.py b/examples/cart_pulling/control_sub_problems/fixed_path_planner.py
new file mode 100644
index 0000000000000000000000000000000000000000..0116bc4edd42787726ab00630d8fa55ea765dbda
--- /dev/null
+++ b/examples/cart_pulling/control_sub_problems/fixed_path_planner.py
@@ -0,0 +1,36 @@
+from smc.robots.abstract_robotmanager import AbstractRobotManager
+from smc.util.draw_path import drawPath
+
+from argparse import Namespace
+import matplotlib
+import numpy as np
+
+
+def contructPath(args: Namespace, robot: AbstractRobotManager):
+ if not args.draw_new:
+ pixel_path_file_path = "./parameters/path_in_pixels.csv"
+ path_base = np.genfromtxt(pixel_path_file_path, delimiter=",")
+ else:
+ matplotlib.use("tkagg")
+ path_base = drawPath(args)
+ matplotlib.use("qtagg")
+ path_base[:, 0] = path_base[:, 0] * args.map_width
+ path_base[:, 1] = path_base[:, 1] * args.map_height
+ path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
+ robot.updateViz({"fixed_path": path_base})
+ return path_base
+
+
+def fixedPathPlanner(
+ path_parameter: list[int],
+ path_base: np.ndarray,
+ p_base: np.ndarray,
+) -> np.ndarray:
+ distances = np.linalg.norm(p_base - path_base, axis=1)
+ index = np.argmin(distances)
+ # 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
+ path_base = path_base[path_parameter[0] :]
+ return path_base
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
index 312dbd7faf3eda2fa16bcd5ebd00282350d7537d..c395830be43ca44c49c6fd633c2dc24db5633ad7 100644
--- 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
@@ -66,6 +66,19 @@ def SingleArmCartPullingMPCControlLoop(
xs = np.array(ocp.solver.xs)
v_cmd = xs[1, robot.model.nq :]
+ # NOTE: if the base isn't moving, it's probably stuck in some local minimum
+ # this happens with just the navigation reference
+ if np.linalg.norm(v_cmd[:3]) < 0.05:
+ print(t, "RESOLVING FOR ONLY FINAL PATH POINT")
+ last_point_only = np.ones((len(trajectory_base), 2))
+ last_point_only = np.hstack(
+ (last_point_only, np.zeros((len(trajectory_base), 1)))
+ )
+ last_point_only = last_point_only * trajectory_base[-1]
+ ocp.warmstartAndReSolve(x0, data=(last_point_only))
+ 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 = {}
diff --git a/examples/cart_pulling/control_sub_problems/utils.py b/examples/cart_pulling/control_sub_problems/utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..88c5c23d59196d4339c655ffdf3b367b0abf51e9
--- /dev/null
+++ b/examples/cart_pulling/control_sub_problems/utils.py
@@ -0,0 +1,63 @@
+from smc.control.optimal_control.util import get_OCP_args
+from smc.path_generation.planner import getPlanningArgs
+from smc.control.cartesian_space import getClikArgs
+from smc import getMinimalArgParser
+
+import argparse
+from pinocchio import SE3
+import numpy as np
+
+
+def get_args() -> argparse.Namespace:
+ 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",
+ )
+ parser.add_argument(
+ "--base-to-handlebar-preferred-distance",
+ type=float,
+ default=0.5,
+ help="prefered path arclength from mobile base position to handlebar",
+ )
+ 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,
+ )
+ args = parser.parse_args()
+ return args
+
+
+def constructInitialT_w_abs(
+ args: argparse.Namespace, path_base: np.ndarray, rotation: np.ndarray
+) -> SE3:
+ direction = path_base[1] - path_base[0]
+ handlebar_direction = -1 * direction
+ handlebar_direction = handlebar_direction / np.linalg.norm(handlebar_direction)
+ offset = args.base_to_handlebar_preferred_distance * handlebar_direction
+ return SE3(rotation, path_base[0] + offset)
diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index 7e0f97723cac66e32a71343e3686a4f7503c8983..8ab2d287959307213895169158bf680520c860a1 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -3,15 +3,16 @@ from smc import getMinimalArgParser
from smc.control.control_loop_manager import ControlLoopManager
from smc.robots.interfaces import MobileBaseInterface
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.control.cartesian_space.cartesian_space_trajectory_following import (
cartesianPathFollowingWithPlanner,
)
from smc.path_generation.planner import starPlanner, getPlanningArgs
-from smc.control.optimal_control.croco_mpc_point_to_point import CrocoBaseP2PMPC
-from smc.control.optimal_control.croco_mpc_path_following import (
+from smc.control.optimal_control.croco_point_to_point.mpc.base_reference_mpc import (
+ CrocoBaseP2PMPC,
+)
+from smc.control.optimal_control.croco_path_following.mpc.base_reference_mpc import (
CrocoBasePathFollowingMPC,
)
from smc.multiprocessing import ProcessManager
@@ -57,7 +58,9 @@ def get_args() -> argparse.Namespace:
return args
-def fixedPathPlanner(path2D: np.ndarray, p_base: np.ndarray) -> np.ndarray:
+def fixedPathPlanner(
+ path_parameter: list[int], path2D: np.ndarray, p_base: np.ndarray
+) -> np.ndarray:
"""
fixedPathPlanner
----------------
@@ -75,7 +78,9 @@ def fixedPathPlanner(path2D: np.ndarray, p_base: np.ndarray) -> np.ndarray:
p_base_3 = np.array([p_base[0], p_base[1], 0.0])
distances = np.linalg.norm(p_base_3 - path2D, axis=1)
index = np.argmin(distances)
- path2D = path2D[index:]
+ if (index - path_parameter[0]) > 0 and (index - path_parameter[0]) < 10:
+ path_parameter[0] += 1
+ path2D = path2D[path_parameter[0] :]
return path2D
@@ -154,7 +159,7 @@ if __name__ == "__main__":
path2D = np.hstack((path2D, np.zeros((len(path2D), 1))))
robot.updateViz({"fixed_path": path2D})
CrocoBaseP2PMPC(args, robot, path2D[0])
- path_planner = partial(fixedPathPlanner, path2D)
+ path_planner = partial(fixedPathPlanner, [0], path2D)
CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
# cartesianPathFollowingWithPlanner(args, robot, path_planner)
diff --git a/python/smc/control/cartesian_space/pink_p2p.py b/python/smc/control/cartesian_space/pink_p2p.py
index 87526d5c9dc1b0683757fea1236916f1943d8c00..3322b726f4b385c841d01bbca8ef69e1d73e6720 100644
--- a/python/smc/control/cartesian_space/pink_p2p.py
+++ b/python/smc/control/cartesian_space/pink_p2p.py
@@ -1,5 +1,6 @@
from smc.control.control_loop_manager import ControlLoopManager
from smc.control.controller_templates.point_to_point import DualEEP2PCtrlLoopTemplate
+from smc.robots.abstract_robotmanager import AbstractRobotManager
from smc.robots.interfaces.dual_arm_interface import DualArmInterface
import pink
@@ -12,6 +13,7 @@ import argparse
from functools import partial
from collections import deque
import pinocchio as pin
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
# TODO: butcher pink to avoid redundancies with smc like configuration.
@@ -52,6 +54,10 @@ def DualArmIKSelfAvoidanceViaEndEffectorSpheresCtrlLoop(
# safety_break=True,
safety_break=False,
)
+ # NOTE: this is a temporary solution to deal with the fact that model isn't a property depending on control mode yet
+ # TODO: make model a property depending on control mode to avoid this shitty issue
+ if robot.mode == AbstractRobotManager.control_mode.upper_body:
+ v_cmd[:3] = 0.0
dist_ee = np.linalg.norm(robot.T_w_l.translation - robot.T_w_r.translation)
log_item = {"dist_ees": dist_ee.reshape((1,))}
return v_cmd, {}, log_item
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
index 4912177eedd7d80f1a09efa7150d8e6a6189f40b..88c465bdfb3527be77acf32bb17f76205227288e 100644
--- 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
@@ -3,16 +3,9 @@ from smc.robots.interfaces.whole_body_interface import (
)
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,
@@ -29,25 +22,11 @@ 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,
+ ocp: BaseAndDualArmEEPathFollowingOCP,
+ path: tuple[np.ndarray, list[SE3]],
args: Namespace,
robot: DualArmWholeBodyInterface,
t: int,
@@ -59,25 +38,24 @@ def BaseAndDualEEPathFollowingMPCControlLoop(
cart pulling dual arm control loop
"""
p = robot.q[:2]
+ path_base, path_T_w_abs = path
# 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 = path2D_to_trajectory2D(args, path_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_T_w_abs = pathSE3_to_trajectorySE3(args, path_T_w_abs, max_base_v)
trajectorySE3_l = []
trajectorySE3_r = []
- for traj_pose in trajectorySE3_handlebar:
+ for traj_pose in trajectorySE3_T_w_abs:
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}
- )
+ robot.visualizer_manager.sendCommand({"frame_path": trajectorySE3_T_w_abs})
x0 = np.concatenate([robot.q, robot.v])
ocp.warmstartAndReSolve(
@@ -110,41 +88,31 @@ def BaseAndDualEEPathFollowingMPC(
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).
+ BaseAndDualEEPathFollowingMPC
+ -------------------------------
+ path following with 3 refereces: base, left arm, right arm.
+ the path planner has to provide base path and T_w_abs path,
+ and T_absgoal_l and T_absgoal_r from which the left and right
+ references are constructed
"""
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,
- )
+ 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),
@@ -155,15 +123,7 @@ def BaseAndDualEEPathFollowingMPC(
"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)
- )
+ loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
if run:
loop_manager.run()
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
index a96db7664fcdcf64b95edd11895eb9b90cbef941..fc24213e26eb8f23d8b0f64ba5a1a4b4d18bf14a 100644
--- 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
@@ -34,7 +34,7 @@ def CrocoDualArmEEPathFollowingMPCControlLoop(
_: dict[str, deque[np.ndarray]],
) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
"""
- CrocoEEPathFollowingMPCControlLoop
+ CrocoDualArmEEPathFollowingMPCControlLoop
-----------------------------
end-effectors follows the prescribed path.
the path is defined with T_w_abs, from which
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 1e76a5c00d73fa81a9a0bb6a088aadd358b3b183..8d142e13887393f15f53fdeca0461d39b8db0bab 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -20,9 +20,9 @@ class AbstractMobileYuMiRobotManager(DualArmWholeBodyInterface):
get_mobile_yumi_model()
)
- self._l_ee_frame_id = self.model.getFrameId("robl_tool0")
- self._r_ee_frame_id = self.model.getFrameId("robr_tool0")
- self._base_frame_id = self.model.getFrameId("mobile_base")
+ self._l_ee_frame_id = self._model.getFrameId("robl_tool0")
+ self._r_ee_frame_id = self._model.getFrameId("robr_tool0")
+ self._base_frame_id = self._model.getFrameId("mobile_base")
# TODO: CHANGE THIS TO REAL VALUES
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
@@ -32,10 +32,10 @@ class AbstractMobileYuMiRobotManager(DualArmWholeBodyInterface):
)
self._comfy_configuration = np.array(
[
- 0.0, # x
- 0.0, # y
- 1.0, # cos(theta)
- 0.0, # sin(theta)
+ 0.0, # x
+ 0.0, # y
+ 1.0, # cos(theta)
+ 0.0, # sin(theta)
0.045,
-0.155,
-0.394,
diff --git a/python/smc/robots/interfaces/whole_body_interface.py b/python/smc/robots/interfaces/whole_body_interface.py
index 6e81f41f01890349c1640bf1b2bf6b2ed0eca13d..da3800866567254e26900d7f489a1409fe879de9 100644
--- a/python/smc/robots/interfaces/whole_body_interface.py
+++ b/python/smc/robots/interfaces/whole_body_interface.py
@@ -6,7 +6,7 @@ from smc.robots.interfaces.dual_arm_interface import DualArmInterface
import pinocchio as pin
from argparse import Namespace
import numpy as np
-from enum import Enum
+from copy import deepcopy
# TODO: put back in when python3.12 will be safe to use
# from typing import override
@@ -158,7 +158,20 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
AbstractRobotManager.control_mode.left_arm_only,
AbstractRobotManager.control_mode.right_arm_only,
]
+# self._full_model = deepcopy(self._model)
+# self._base_only_model = pin.buildReducedModel(self._model, [i for i in range(1, self._model.njoints + 1) if i > 1], np.zeros(self._model.nq))
+# self._upper_body_model = pin.buildReducedModel(self._model, [i for i in range(1, self._model.njoints + 1) if i < 2], np.zeros(self._model.nq))
super().__init__(args)
+
+# @property
+# def model(self) -> pin.Model:
+# if self.control_mode == AbstractRobotManager.control_mode.whole_body:
+# return self._full_model
+# if self.control_mode == AbstractRobotManager.control_mode.base_only:
+# return self._base_only_model
+# if self.control_mode == AbstractRobotManager.control_mode.upper_body:
+# return self._upper_body_model
+# return self._full_model
# TODO: override model property to produce the reduced version
# depending on the control mode.
diff --git a/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
index 9a506ea3c939d77d535d7d7a473b60c143560766..90e833ee646d86ad4c0fefbc6304e3afc2032f2d 100644
--- a/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
+++ b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
@@ -43,7 +43,9 @@ class MeshcatVisualizer(PMV):
# which will never be changed
self.n_points = 0
self.n_path_points = 0
+ self.add_path_points_set = set()
self.n_frame_path_points = 0
+ self.add_frame_path_points_set = set()
self.n_obstacles = 0
if robot is not None:
super().__init__(robot.model, robot.collision_model, robot.visual_model)
@@ -109,32 +111,41 @@ class MeshcatVisualizer(PMV):
def addPath(
self, name, path: list[pin.SE3] | np.ndarray, radius=5e-3, color=[0, 0, 1, 0.8]
):
- # who cares about the name
- self.n_path_points = len(path)
+ # NOTE: there's too much of them so the visualization is slow
+ self.n_path_points = len(path) if len(path) < 5 else 5
if name == "fixed_path":
- color=[1, 0, 0, 0.8]
+ color = [1, 0, 0, 0.8]
if type(path) == np.ndarray:
# complete the quartenion
- path = np.hstack((path, np.zeros((len(path), 3))))
- path = np.hstack((path, np.ones((len(path), 1))))
- for i, point in enumerate(path):
- if i < self.n_path_points:
+ path_viz = np.hstack((path, np.zeros((len(path), 3))))
+ path_viz = np.hstack((path_viz, np.ones((len(path), 1))))
+ else:
+ path_viz = path.copy()
+ index_step = len(path_viz) // 5
+ for i, point in enumerate(path_viz):
+ if (i % index_step != 0) and name != "fixed_path":
+ continue
+ if i not in self.add_path_points_set:
self.addSphere(f"world/path_{name}_point_{i}", radius, color)
+ self.add_path_points_set.add(i)
self.applyConfiguration(f"world/path_{name}_point_{i}", point)
- self.n_path_points = max(i, self.n_path_points)
+ # self.n_path_points = max(i, self.n_path_points)
def addFramePath(
self, name, path: list[pin.SE3], radius=5e-3, color=[1, 0, 0, 0.8]
):
- # who cares about the name
- name = "frame_path"
+ self.n_frame_path_points = len(path) if len(path) < 5 else 5
+ index_step = len(path) // 5
for i, point in enumerate(path):
- if i < self.n_frame_path_points:
+ if (i % index_step != 0) and name != "fixed_frame_path":
+ continue
+ if i not in self.add_frame_path_points_set:
meshcat_shapes.frame(
self.viewer[f"world/frame_path_{name}_point_{i}"], opacity=0.3
)
+ self.add_frame_path_points_set.add(i)
self.applyConfiguration(f"world/frame_path_{name}_point_{i}", point)
- self.n_frame_path_points = max(i, self.n_frame_path_points)
+ # self.n_frame_path_points = max(i, self.n_frame_path_points)
def applyConfiguration(self, name, placement):
if isinstance(placement, list) or isinstance(placement, tuple):