From d04d699986cc49026fc6aa6d2de09707edfcc7d7 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sun, 9 Mar 2025 01:18:16 +0100
Subject: [PATCH] last for today. some debugging necessary for final dual arm
cart pulling version, but the greatest hits are there and it should be fine.
disjoint control is the worst case and that's totally fine. after debugging
this the next step is to bring in localization and start running this on ros
---
.../dual_arm_cart_pulling.py | 182 ++++++++++--------
.../dual_arm_cart_pulling_control_loop.py | 173 +++++++++++++++++
.../fixed_path_planner.py | 36 ++++
.../single_arm_cart_pulling_control_loop.py | 13 ++
.../control_sub_problems/utils.py | 63 ++++++
examples/navigation/mobile_base_navigation.py | 17 +-
.../smc/control/cartesian_space/pink_p2p.py | 6 +
.../mpc/base_and_dual_arm_reference_mpc.py | 94 +++------
.../mpc/dual_arm_reference_mpc.py | 2 +-
.../smc/robots/implementations/mobile_yumi.py | 14 +-
.../robots/interfaces/whole_body_interface.py | 15 +-
.../meshcat_viewer_wrapper/visualizer.py | 35 ++--
12 files changed, 474 insertions(+), 176 deletions(-)
create mode 100644 examples/cart_pulling/control_sub_problems/fixed_path_planner.py
create mode 100644 examples/cart_pulling/control_sub_problems/utils.py
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 a728f36..0876a25 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 e69de29..28f4a86 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 0000000..0116bc4
--- /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 312dbd7..c395830 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 0000000..88c5c23
--- /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 7e0f977..8ab2d28 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 87526d5..3322b72 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 4912177..88c465b 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 a96db76..fc24213 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 1e76a5c..8d142e1 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 6e81f41..da38008 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 9a506ea..90e833e 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):
--
GitLab