diff --git a/development_plan.toml b/development_plan.toml
index 54761b2ccf8b39cbcc6b6a1bbdeb75aa7aed252c..d1c908450d435e98a275dd1669033a377e2c636d 100644
--- a/development_plan.toml
+++ b/development_plan.toml
@@ -235,6 +235,18 @@ purpose = """actually pre-verifying experiments with simulation"""
hours_required = 2
is_done = false
+[[project_plan.action_items.action_items]]
+name = "refactor croco mpc p2p"
+description = """
+you have 4 or 5 literally identical controlloops and it looks (is) stupid. the circle to square is that they need to take in different arguments due to the templates. find how to intelligently package this.
+"""
+priority = 1
+deadline = 2025-01-31
+dependencies = []
+purpose = """actually pre-verifying experiments with simulation"""
+hours_required = 3
+is_done = false
+
[[project_plan.action_items]]
name = "improving extensibility"
diff --git a/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm_control_loop.py b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm_control_loop.py
index 6f2d7e91a5e92536f10dc95b035dd65752d51902..2e5d09dc454bc3390f7afe47c30cbb1072f0c504 100644
--- a/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm_control_loop.py
+++ b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm_control_loop.py
@@ -13,7 +13,7 @@ from smc.path_generation.path_math.cart_pulling_path_math import (
)
from smc.path_generation.path_math.path_to_trajectory import path2D_to_trajectory2D
from smc.control.controller_templates.path_following_template import (
- PathFollowingFromPlannerControlLoop,
+ PathFollowingFromPlannerCtrllLoopTemplate,
)
from smc.control.cartesian_space.ik_solvers import dampedPseudoinverse
from smc.control.optimal_control.croco_mpc_path_following import initializePastData
@@ -48,7 +48,7 @@ def BaseMPCEECLIKPathFollowingFromPlannerMPCControlLoop(
v_cmd = xs[1, robot.model.nq :]
pathSE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
- robot._mode = SingleArmWholeBodyInterface.control_mode.arm_only
+ robot._mode = SingleArmWholeBodyInterface.control_mode.upper_body
T_w_e = robot.T_w_e
# first check whether we're at the goal
@@ -62,10 +62,10 @@ def BaseMPCEECLIKPathFollowingFromPlannerMPCControlLoop(
v_cmd[3:] = v_arm
- if args.visualizer:
- if t % args.viz_update_rate == 0:
- robot.visualizer_manager.sendCommand({"path": path_base})
- robot.visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
+ # if args.visualizer:
+ # if t % args.viz_update_rate == 0:
+ # robot.visualizer_manager.sendCommand({"path": path_base})
+ # robot.visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
err_vector_ee = log6(robot.T_w_e.actInv(pathSE3_handlebar[0]))
err_vector_base = np.linalg.norm(p - path_base[0][:2]) # z axis is irrelevant
@@ -104,7 +104,7 @@ def BaseMPCANDEECLIKCartPulling(
else:
get_position = lambda robot: robot.q[:2]
controlLoop = partial(
- PathFollowingFromPlannerControlLoop,
+ PathFollowingFromPlannerCtrllLoopTemplate,
path_planner,
get_position,
ocp,
diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index 1020e48286c925b829026242ae0c72f70b017c6d..d21838958d38671740f7884b652c861fe20333b1 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -1,59 +1,123 @@
from smc import getRobotFromArgs
from smc import getMinimalArgParser
+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.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 (
CrocoBasePathFollowingMPC,
)
from smc.multiprocessing import ProcessManager
+from smc.util.draw_path import drawPath
-import time
import numpy as np
from functools import partial
import pinocchio as pin
+import argparse
+import matplotlib
+# TODO:
+# try valueing future points on the path more the current ones!!!!
-def get_args():
+
+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(
+ "--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
-# DA BI OVAJ MPC SEX FUNKCIONIRAO, MORAS MU DAT PIN.MODEL SAMO BAZE!!!
-# ili to il ce se vrtit GRO sporo bez razloga
+def fixedPathPlanner(path2D: np.ndarray, p_base: np.ndarray) -> np.ndarray:
+ """
+ fixedPathPlanner
+ ----------------
+ we can assume robot is following this path and that it started the following controller at the first point.
+ to make the controller make sense (without a sense of timing), we need to find the closest point on the path,
+ and delete the already traversed portion of the path.
+ the assumption is that the controller constructs a trajectory from the path, starting with the first point.
+
+ the cleanest way to get this done is associate robot's current position with the path parameter s,
+ but i'll just implement it as quickly as possible, not in the most correct or the most efficient manner.
+
+ NOTE: path is (N,3) in shape, last column is 0, because subotimal code structure
+ """
+
+ 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:]
+ return path2D
+
if __name__ == "__main__":
args = get_args()
robot = getRobotFromArgs(args)
- # TODO: for ocp you want to pass only the mobile base model
- robot._step()
- robot._q[0] = 9.0
- robot._q[1] = 4.0
+ assert issubclass(robot.__class__, MobileBaseInterface)
robot._step()
+ if args.planner:
+ 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_b = robot.T_w_b
- 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_b.translation[:2], 3, None
- )
- _, map_as_list = createSampleStaticMap()
- if args.visualizer:
- robot.sendRectangular2DMapToVisualizer(map_as_list)
- # time.sleep(5)
+ 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_b.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:
+ 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, np.zeros((len(path2D), 1))))
+ robot.updateViz({"fixed_path": path2D})
+ CrocoBaseP2PMPC(args, robot, path2D[0])
+ path_planner = partial(fixedPathPlanner, path2D)
CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
- print("final position:", robot.T_w_b.translation)
-
if args.real:
robot.stopRobot()
diff --git a/python/smc/control/controller_templates/path_following_template.py b/python/smc/control/controller_templates/path_following_template.py
index b33d4ecf1b7642a753b759f988760274553c24b0..1ff64efd77ae58484747607b0932a2a65e17e707 100644
--- a/python/smc/control/controller_templates/path_following_template.py
+++ b/python/smc/control/controller_templates/path_following_template.py
@@ -1,3 +1,4 @@
+from types import FunctionType
from smc.robots.abstract_robotmanager import AbstractRobotManager
from smc.multiprocessing.process_manager import ProcessManager
@@ -10,8 +11,8 @@ global control_loop_return
control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
-def PathFollowingFromPlannerControlLoop(
- path_planner: ProcessManager,
+def PathFollowingFromPlannerCtrllLoopTemplate(
+ path_planner: ProcessManager|FunctionType,
get_position: Callable[[AbstractRobotManager], np.ndarray],
SOLVER: Any,
control_loop: Callable[
@@ -42,23 +43,28 @@ def PathFollowingFromPlannerControlLoop(
p = get_position(robot)
- path_planner.sendCommand(p)
- data = path_planner.getData()
+ if type(path_planner) == ProcessManager:
+ path_planner.sendCommand(p)
+ data = path_planner.getData()
- if data == "done":
- breakFlag = True
+ if data == "done":
+ breakFlag = True
- if data == "done" or data is None:
- robot.sendVelocityCommand(np.zeros(robot.model.nv))
- log_item["qs"] = robot.q
- log_item["dqs"] = robot.v
- return breakFlag, save_past_item, log_item
+ if data == "done" or data is None:
+ robot.sendVelocityCommand(np.zeros(robot.model.nv))
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
+ return breakFlag, save_past_item, log_item
- _, path2D_untimed = data
- path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
+ _, path2D = data
+ path2D = np.array(path2D).reshape((-1, 2))
+ else:
+ path2D = path_planner(p)
+ if len(path2D) < 4:
+ breakFlag = True
v_cmd, past_item_inner, log_item_inner = control_loop(
- SOLVER, path2D_untimed, args, robot, t, past_data
+ SOLVER, path2D, args, robot, t, past_data
)
robot.sendVelocityCommand(v_cmd)
log_item.update(log_item_inner)
diff --git a/python/smc/control/controller_templates/point_to_point.py b/python/smc/control/controller_templates/point_to_point.py
index 113f5e0ad8e11dd74dc369e73d82e931d0ecb0aa..c4937fdf1c0ccf1e3b49d419d40dde088c2ceedf 100644
--- a/python/smc/control/controller_templates/point_to_point.py
+++ b/python/smc/control/controller_templates/point_to_point.py
@@ -1,3 +1,4 @@
+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
@@ -15,6 +16,56 @@ from smc.robots.interfaces.whole_body_interface import (
global control_loop_return
control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
+def BaseP2PCtrlLoopTemplate(
+ SOLVER: Any,
+ p_basegoal: np.ndarray,
+ control_loop: Callable[
+ [
+ Any,
+ np.ndarray,
+ Namespace,
+ MobileBaseInterface,
+ int,
+ dict[str, deque[np.ndarray]],
+ ],
+ tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]],
+ ],
+ args: Namespace,
+ robot: MobileBaseInterface,
+ t: int, # will be float eventually
+ past_data: dict[str, deque[np.ndarray]],
+) -> control_loop_return:
+ """
+ EEAndBaseP2PCtrlLoopTemplate
+ ---------------
+ generic control loop for point to point motion for end-effectors of a dual arm robot
+ (handling error to final point etc).
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_item = {}
+
+ v_cmd, past_item_inner, log_item_inner = control_loop(
+ SOLVER, p_basegoal, args, robot, t, past_data
+ )
+ robot.sendVelocityCommand(v_cmd)
+ log_item.update(log_item_inner)
+ save_past_item.update(past_item_inner)
+
+ p_base = np.array(list(robot.q[:2]) + [0.0])
+ base_err_vector_norm = np.linalg.norm(p_basegoal - p_base)
+
+ if (
+ base_err_vector_norm < robot.args.goal_error
+ ):
+ breakFlag = True
+
+ log_item["qs"] = robot.q
+ log_item["dqs"] = robot.v
+ log_item["dqs_cmd"] = v_cmd.reshape((robot.model.nv,))
+ log_item["base_err_norm"] = base_err_vector_norm.reshape((1,))
+ return breakFlag, save_past_item, log_item
+
def EEP2PCtrlLoopTemplate(
SOLVER: Any,
diff --git a/python/smc/control/optimal_control/croco_mpc_path_following.py b/python/smc/control/optimal_control/croco_mpc_path_following.py
index 7efc59ad659f16c2f51048d06957795585618e2a..6a98cdca86330e4c43d2d91dda00f504cfc2d3bf 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -19,7 +19,7 @@ from smc.path_generation.path_math.cart_pulling_path_math import (
)
from smc.path_generation.path_math.path_to_trajectory import path2D_to_trajectory2D
from smc.control.controller_templates.path_following_template import (
- PathFollowingFromPlannerControlLoop,
+ PathFollowingFromPlannerCtrllLoopTemplate,
)
import numpy as np
@@ -32,7 +32,7 @@ from collections import deque
def CrocoBasePathFollowingFromPlannerMPCControlLoop(
ocp: CrocoOCP,
- path2D_untimed: np.ndarray,
+ path2D: np.ndarray,
args: Namespace,
robot: MobileBaseInterface,
t: int,
@@ -41,7 +41,7 @@ def CrocoBasePathFollowingFromPlannerMPCControlLoop(
p = robot.T_w_b.translation[:2]
max_base_v = np.linalg.norm(robot._max_v[:2])
- path_base = path2D_to_trajectory2D(args, path2D_untimed, max_base_v)
+ 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:
@@ -74,19 +74,16 @@ def CrocoBasePathFollowingMPC(
x0 = np.concatenate([robot.q, robot.v])
ocp.solveInitialOCP(x0)
- if type(path_planner) == types.FunctionType:
- raise NotImplementedError
- else:
- get_position = lambda robot: robot.T_w_b.translation[:2]
- controlLoop = partial(
- PathFollowingFromPlannerControlLoop,
- path_planner,
- get_position,
- ocp,
- CrocoBasePathFollowingFromPlannerMPCControlLoop,
- args,
- robot,
- )
+ 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),
@@ -147,7 +144,7 @@ def CrocoEEPathFollowingMPCControlLoop(
def CrocoEEPathFollowingFromPlannerMPCControlLoop(
ocp: CrocoOCP,
- path2D_untimed: np.ndarray,
+ path2D: np.ndarray,
args: Namespace,
robot: SingleArmInterface,
t: int,
@@ -165,7 +162,7 @@ def CrocoEEPathFollowingFromPlannerMPCControlLoop(
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_untimed, velocity)
+ 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)
@@ -215,7 +212,7 @@ def CrocoEEPathFollowingMPC(
else:
get_position = lambda robot: robot.T_w_e.translation[:2]
controlLoop = partial(
- PathFollowingFromPlannerControlLoop,
+ PathFollowingFromPlannerCtrllLoopTemplate,
path_planner,
get_position,
ocp,
@@ -284,7 +281,7 @@ def BaseAndEEPathFollowingMPCControlLoop(
def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
ocp: CrocoOCP,
- path2D_untimed_base: np.ndarray,
+ path2D_base: np.ndarray,
args: Namespace,
robot: SingleArmWholeBodyInterface,
t: int,
@@ -294,10 +291,10 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
# 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_untimed_base, max_base_v)
+ 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_untimed"])
+ trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D"])
if args.visualizer:
if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
@@ -315,7 +312,7 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
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_untimed": p}
+ save_past_item = {"path2D": p}
return v_cmd, save_past_item, log_item
@@ -362,7 +359,7 @@ def BaseAndEEPathFollowingMPC(
else:
get_position = lambda robot: robot.q[:2]
controlLoop = partial(
- PathFollowingFromPlannerControlLoop,
+ PathFollowingFromPlannerCtrllLoopTemplate,
path_planner,
get_position,
ocp,
@@ -377,14 +374,14 @@ def BaseAndEEPathFollowingMPC(
"err_norm_ee": np.zeros((1,)),
"err_norm_base": np.zeros((1,)),
}
- save_past_dict = {"path2D_untimed": T_w_e.translation[:2]}
+ 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_untimed"].clear()
- loop_manager.past_data["path2D_untimed"].extend(
+ loop_manager.past_data["path2D"].clear()
+ loop_manager.past_data["path2D"].extend(
path2D_handlebar[i] for i in range(args.past_window_size)
)
@@ -397,7 +394,7 @@ def BaseAndDualEEPathFollowingMPCControlLoop(
T_absgoal_l: SE3,
T_absgoal_r: SE3,
ocp: CrocoOCP,
- path2D_untimed_base: np.ndarray,
+ path2D_base: np.ndarray,
args: Namespace,
robot: DualArmWholeBodyInterface,
t: int,
@@ -412,10 +409,10 @@ def BaseAndDualEEPathFollowingMPCControlLoop(
# 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_untimed_base, max_base_v)
+ 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_untimed"])
+ trajectorySE3_handlebar = construct_EE_path(args, p, past_data["path2D"])
trajectorySE3_l = []
trajectorySE3_r = []
for traj_pose in trajectorySE3_handlebar:
@@ -443,7 +440,7 @@ def BaseAndDualEEPathFollowingMPCControlLoop(
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_untimed": p}
+ save_past_item = {"path2D": p}
return v_cmd, save_past_item, log_item
def BaseAndDualEEPathFollowingMPC(
@@ -480,7 +477,7 @@ def BaseAndDualEEPathFollowingMPC(
get_position = lambda robot: robot.q[:2]
BaseAndDualEEPathFollowingMPCControlLoop_with_l_r = partial(BaseAndDualEEPathFollowingMPCControlLoop, T_absgoal_l, T_absgoal_r)
controlLoop = partial(
- PathFollowingFromPlannerControlLoop,
+ PathFollowingFromPlannerCtrllLoopTemplate,
path_planner,
get_position,
ocp,
@@ -498,13 +495,13 @@ def BaseAndDualEEPathFollowingMPC(
"err_norm_ee_r": np.zeros((1,)),
"err_norm_base": np.zeros((1,)),
}
- save_past_dict = {"path2D_untimed": T_w_abs.translation[:2]}
+ 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_untimed"].clear()
- loop_manager.past_data["path2D_untimed"].extend(
+ loop_manager.past_data["path2D"].clear()
+ loop_manager.past_data["path2D"].extend(
path2D_handlebar[i] for i in range(args.past_window_size)
)
diff --git a/python/smc/control/optimal_control/croco_mpc_point_to_point.py b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
index a2cdd9f465ace9f60e30537749b6254e509342ee..061ea5076db5210b04f1997606fe3db56a047e24 100644
--- a/python/smc/control/optimal_control/croco_mpc_point_to_point.py
+++ b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
@@ -1,9 +1,11 @@
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 (
@@ -12,6 +14,7 @@ from smc.robots.interfaces.whole_body_interface import (
)
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,
@@ -25,6 +28,57 @@ 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,
diff --git a/python/smc/control/optimal_control/point_to_point_croco_ocp.py b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
index 519458f91f113b5a3ca333abfdb42752716f83c5..5fec4434f95a06ac6654d218fabb37ed21d8e731 100644
--- a/python/smc/control/optimal_control/point_to_point_croco_ocp.py
+++ b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
@@ -11,6 +11,51 @@ 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 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.args.base_translation_cost,
+ )
+ self.terminalCostModel.addCost(
+ "base_translation" + str(self.args.n_knots),
+ baseTrackingCost,
+ self.args.base_translation_cost,
+ )
+
+ # 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__(
@@ -152,43 +197,23 @@ class DualArmIKOCP(CrocoOCP):
].cost.residual.reference = T_w_rgoal
-class BaseAndSingleArmIKOCP(SingleArmIKOCP):
+class BaseAndSingleArmIKOCP(SingleArmIKOCP, BaseIKOCP):
def __init__(
self,
args: Namespace,
robot: SingleArmWholeBodyInterface,
x0: np.ndarray,
goal,
- # T_w_eegoal: pin.SE3,
- # p_basegoal: np.ndarray,
):
- # T_w_eegoal, p_basegoal = goal
super().__init__(args, robot, x0, goal)
def constructTaskObjectiveFunction(
- # self, T_w_eegoal: pin.SE3, p_basegoal: np.ndarray
self,
goal,
) -> None:
T_w_eegoal, p_basegoal = goal
super().constructTaskObjectiveFunction(T_w_eegoal)
- 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.args.base_translation_cost,
- )
- self.terminalCostModel.addCost(
- "base_translation" + str(self.args.n_knots),
- baseTrackingCost,
- self.args.base_translation_cost,
- )
+ BaseIKOCP.constructTaskObjectiveFunction(self, p_basegoal)
# there is nothing to update in a point-to-point task
def updateCosts(self, data):
@@ -198,16 +223,10 @@ class BaseAndSingleArmIKOCP(SingleArmIKOCP):
# self, T_w_eegoal: pin.SE3, p_basegoal: np.ndarray) -> None:
T_w_eegoal, p_basegoal = goal
super().updateGoalInModels(T_w_eegoal)
- 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
+ BaseIKOCP.updateGoalInModels(self, p_basegoal)
-class BaseAndDualArmIKOCP(DualArmIKOCP):
+class BaseAndDualArmIKOCP(DualArmIKOCP, BaseIKOCP):
def __init__(
self,
args: Namespace,
@@ -215,46 +234,21 @@ class BaseAndDualArmIKOCP(DualArmIKOCP):
x0: np.ndarray,
goal,
):
- # T_w_eegoal, p_basegoal = goal
super().__init__(args, robot, x0, goal)
def constructTaskObjectiveFunction(
- # self, T_w_eegoal: pin.SE3, p_basegoal: np.ndarray
self,
goal,
) -> None:
T_w_lgoal, T_w_rgoal, p_basegoal = goal
super().constructTaskObjectiveFunction((T_w_lgoal, T_w_rgoal))
- 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.args.base_translation_cost,
- )
- self.terminalCostModel.addCost(
- "base_translation" + str(self.args.n_knots),
- baseTrackingCost,
- self.args.base_translation_cost,
- )
+ 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:
- # self, T_w_eegoal: pin.SE3, p_basegoal: np.ndarray) -> None:
T_w_lgoal, T_w_rgoal, p_basegoal = goal
super().updateGoalInModels(T_w_lgoal, T_w_rgoal)
- 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
+ BaseIKOCP.updateGoalInModels(self, p_basegoal)
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index 10bec2124740b1bb72f12b26ed57ceebc7833f96..5be02d26860c1bf61341df3517a5ff6795a2c12d 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -130,8 +130,6 @@ class AbstractRobotManager(abc.ABC):
@mode.setter
def mode(self, mode: control_mode) -> None:
- print(mode)
- print(self._available_modes)
assert mode in self._available_modes
self._mode = mode
diff --git a/python/smc/robots/implementations/mir.py b/python/smc/robots/implementations/mir.py
index a09e2d971eca7e2cfdd07219fe5d3d5eb06bc71a..7bc8742cde806c0fe4c096c4fe8705ade942f502 100644
--- a/python/smc/robots/implementations/mir.py
+++ b/python/smc/robots/implementations/mir.py
@@ -106,7 +106,7 @@ def mir_approximation() -> (
"box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy()
)
- geometry_mobile_base.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
+ geometry_mobile_base.meshColor = np.array([1.0, 0.1, 0.1, 0.3])
geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
# have to add the frame manually
diff --git a/python/smc/robots/interfaces/__init__.py b/python/smc/robots/interfaces/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..bc7ff830cf8a0b67c96dce94e013d1ba55628aa3
--- /dev/null
+++ b/python/smc/robots/interfaces/__init__.py
@@ -0,0 +1,5 @@
+from .dual_arm_interface import *
+from .force_torque_sensor_interface import *
+from .mobile_base_interface import *
+from .single_arm_interface import *
+from .whole_body_interface import *
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index 98045ce47fcfd783b2fb6b51c2743fdc8d7c07e5..a19fbd05f18781300446e2b72ba789ea0d512f66 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -34,6 +34,7 @@ class DualArmInterface(SingleArmInterface):
if not hasattr(self, "_available_modes"):
self._available_modes: list[AbstractRobotManager.control_mode] = [
AbstractRobotManager.control_mode.whole_body,
+ AbstractRobotManager.control_mode.upper_body, # in this case the same as wholebody
AbstractRobotManager.control_mode.left_arm_only,
AbstractRobotManager.control_mode.right_arm_only,
]
diff --git a/python/smc/robots/interfaces/mobile_base_interface.py b/python/smc/robots/interfaces/mobile_base_interface.py
index 58a05ff94ddb77debf45b9f85d3e68f32cb3bfee..022502bda2a72dddd60c2dd3536fc7896cbc206a 100644
--- a/python/smc/robots/interfaces/mobile_base_interface.py
+++ b/python/smc/robots/interfaces/mobile_base_interface.py
@@ -25,6 +25,11 @@ class MobileBaseInterface(AbstractRobotManager):
print("MobileBase init")
self._base_frame_id: int
self._T_w_b: pin.SE3
+ if not hasattr(self, "_available_modes"):
+ self._available_modes: list[AbstractRobotManager.control_mode] = [
+ AbstractRobotManager.control_mode.whole_body,
+ AbstractRobotManager.control_mode.base_only, # in this case the same as wholebody
+ ]
super().__init__(args)
@property
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index 667cd3df0841a2afbf0bcea165a5a6459e50d639..31f00455a73f723cabaf3d455f751b495c5a87de 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -14,6 +14,7 @@ class SingleArmInterface(AbstractRobotManager):
if not hasattr(self, "_available_modes"):
self._available_modes: list[AbstractRobotManager.control_mode] = [
AbstractRobotManager.control_mode.whole_body,
+ AbstractRobotManager.control_mode.upper_body, # in this case the same as wholebody
]
super().__init__(args)
diff --git a/python/smc/util/draw_path.py b/python/smc/util/draw_path.py
index 9e897089c370d964c75190f842b295e3a734f2d8..3b3a7f29f4282a470a97a0c4f4c7514958bc081f 100644
--- a/python/smc/util/draw_path.py
+++ b/python/smc/util/draw_path.py
@@ -10,6 +10,7 @@ possible improvement: make it all bezier curves
(but if that was a parameter that would be ok i guess)
"""
+from argparse import Namespace
import numpy as np
import matplotlib.pyplot as plt
@@ -56,7 +57,7 @@ class DrawPathManager:
plt.close()
-def drawPath(args):
+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
diff --git a/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
index ca2e1fe364e423735eca7434a2d83f800e918ab5..9a506ea3c939d77d535d7d7a473b60c143560766 100644
--- a/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
+++ b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
@@ -107,11 +107,12 @@ class MeshcatVisualizer(PMV):
self.n_points += 1
def addPath(
- self, name, path: list[pin.SE3] | np.ndarray, radius=5e-3, color=[1, 0, 0, 0.8]
+ self, name, path: list[pin.SE3] | np.ndarray, radius=5e-3, color=[0, 0, 1, 0.8]
):
# who cares about the name
- name = "path"
self.n_path_points = len(path)
+ if name == "fixed_path":
+ color=[1, 0, 0, 0.8]
if type(path) == np.ndarray:
# complete the quartenion
path = np.hstack((path, np.zeros((len(path), 3))))
diff --git a/python/smc/visualization/visualizer.py b/python/smc/visualization/visualizer.py
index 041dcbae7d17949db4997530243675b0ba5018e1..3abc3df4232c841325df26899628405c086004cb 100644
--- a/python/smc/visualization/visualizer.py
+++ b/python/smc/visualization/visualizer.py
@@ -71,7 +71,10 @@ def manipulatorVisualizer(
viz.addBoxObstacle(cmd["obstacle_box"][0], cmd["obstacle_box"][1])
if key == "path":
# stupid and evil but there is no time
- viz.addPath("", cmd["path"])
+ viz.addPath("path", cmd["path"])
+ if key == "fixed_path":
+ # stupid and evil but there is no time
+ viz.addPath("fixed_path", cmd["fixed_path"])
if key == "frame_path":
# stupid and evil but there is no time
viz.addFramePath("", cmd["frame_path"])