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 0876a250f66824132c4e5fab1903325e3c285f2d..1cabaa0e29c10c4b58049881bc1ae2c6f2c829ff 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
@@ -11,6 +11,7 @@ from smc.control.joint_space.joint_space_point_to_point import moveJP
from smc.control.cartesian_space.pink_p2p import (
DualArmIKSelfAvoidanceViaEndEffectorSpheres,
)
+from smc.control.cartesian_space.cartesian_space_point_to_point import moveL
from utils import get_args, constructInitialT_w_abs
from dual_arm_cart_pulling_control_loop import DualArmCartPulling
@@ -89,25 +90,39 @@ if __name__ == "__main__":
###################################################
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: requires passing a reduced model to work because crocoddyl needs the model to formulate itself
+ # robot.mode = AbstractRobotManager.control_mode.base_only
+ robot.mode = robot.control_mode.whole_body
+ # 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)
+ ##########################################
+ # orient base to look at the goal (yes this should've be handled with basep2p)
+ ##########################################
+ robot.mode = robot.control_mode.base_only
+ # NOTE: T_w_absgoal's x-axis points toward the path.
+ # z-axis points down by an undocumented choice
+ rotation_init_rpy = pin.rpy.matrixToRpy(T_w_absgoal.rotation)
+ rotation_init = pin.rpy.rpyToMatrix(0.0, 0.0, rotation_init_rpy[2] + np.pi / 2)
+ T_w_bgoal = pin.SE3(rotation_init, p_basegoal)
+ moveL(args, robot, T_w_bgoal)
+
###################################################
- # DualArmMoveL 10 cm above handlebar
+ # grasp handlebar
###################################################
+ # DualArmMoveL 10 cm above handlebar
robot.mode = AbstractRobotManager.control_mode.upper_body
- T_w_absgoal.translation[2] -= 0.1
+ T_w_absgoal.translation[2] += 0.1
+ print("going above handlebar")
DualArmIKSelfAvoidanceViaEndEffectorSpheres(
T_w_absgoal, T_absgoal_l, T_absgoal_r, args, robot
)
- ###################################################
# DualArmMoveL down to handlebar
- ###################################################
- T_w_absgoal.translation[2] += 0.1
+ print("going down to handlebar")
+ T_w_absgoal.translation[2] -= 0.1
DualArmIKSelfAvoidanceViaEndEffectorSpheres(
T_w_absgoal, T_absgoal_l, T_absgoal_r, args, robot
)
@@ -116,6 +131,8 @@ if __name__ == "__main__":
###################################################
# time.sleep(5)
+ # TODO: for final demo replace this with disjoint controller
+ robot._mode = robot.control_mode.whole_body
DualArmCartPulling(args, robot, path_planner, T_absgoal_l, T_absgoal_r)
if args.real:
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 28f4a868f4ad722e54106b81aa1635e052077d12..3c0b05cd1047b010d6e258d516331e76ac7fc4a9 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
@@ -1,4 +1,4 @@
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_dual_arm_interface import (
DualArmWholeBodyInterface,
)
from smc.control.control_loop_manager import ControlLoopManager
@@ -23,18 +23,7 @@ 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
+from utils import initializePastData
def DualArmCartPullingControlLoop(
@@ -87,7 +76,9 @@ def DualArmCartPullingControlLoop(
(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))
+ ocp.warmstartAndReSolve(
+ x0, data=(last_point_only, trajectorySE3_l, trajectorySE3_r)
+ )
xs = np.array(ocp.solver.xs)
v_cmd = xs[1, robot.model.nq :]
@@ -122,7 +113,7 @@ def DualArmCartPulling(
a dynamics level, and velocities we command
are actually extracted from the state x(q,dq).
"""
- robot._mode = DualArmWholeBodyInterface.control_mode.whole_body
+ robot._mode = robot.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
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 c395830be43ca44c49c6fd633c2dc24db5633ad7..652e704c3bef0053b35ddd3357b25f3236e9a393 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
@@ -1,4 +1,4 @@
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_single_arm_interface import (
SingleArmWholeBodyInterface,
)
from smc.control.control_loop_manager import ControlLoopManager
diff --git a/examples/cart_pulling/control_sub_problems/utils.py b/examples/cart_pulling/control_sub_problems/utils.py
index 88c5c23d59196d4339c655ffdf3b367b0abf51e9..1f5e999e68d35700a51ea01b116591160f21f8c8 100644
--- a/examples/cart_pulling/control_sub_problems/utils.py
+++ b/examples/cart_pulling/control_sub_problems/utils.py
@@ -60,4 +60,18 @@ def constructInitialT_w_abs(
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)
+ translation = path_base[0] + offset
+ translation[2] = args.handlebar_height
+ return SE3(rotation, translation)
+
+
+def initializePastData(
+ args: argparse.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
diff --git a/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
index 4852ebc06d027ef37ba9b49c2d1aa6e8e8d6de3e..41a1f7c909d53b905a2fdd695ad8dd2355431779 100644
--- a/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
+++ b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py
@@ -5,7 +5,9 @@ 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 CrocoEEAndBaseP2PMPC
+from smc.control.optimal_control.croco_point_to_point.mpc.base_and_single_arm_reference_mpc import (
+ CrocoEEAndBaseP2PMPC,
+)
from smc.multiprocessing import ProcessManager
from mpc_base_clik_single_arm_control_loop import BaseMPCANDEECLIKCartPulling
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 2e5d09dc454bc3390f7afe47c30cbb1072f0c504..1a3a2b74d144bb54de551fff26347b89d0dffee4 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
@@ -1,13 +1,12 @@
-from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface
+from smc.robots.interfaces.whole_body_single_arm_interface import (
+ SingleArmWholeBodyInterface,
+)
from smc.control.control_loop_manager import ControlLoopManager
from smc.multiprocessing.process_manager import ProcessManager
from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
-from smc.control.optimal_control.path_following_croco_ocp import (
+from smc.control.optimal_control.croco_path_following.mpc.base_reference_mpc import (
BasePathFollowingOCP,
)
-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,
)
@@ -16,7 +15,8 @@ from smc.control.controller_templates.path_following_template import (
PathFollowingFromPlannerCtrllLoopTemplate,
)
from smc.control.cartesian_space.ik_solvers import dampedPseudoinverse
-from smc.control.optimal_control.croco_mpc_path_following import initializePastData
+
+from utils import initializePastData
import numpy as np
from functools import partial
diff --git a/examples/cart_pulling/disjoint_control/utils.py b/examples/cart_pulling/disjoint_control/utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..1f5e999e68d35700a51ea01b116591160f21f8c8
--- /dev/null
+++ b/examples/cart_pulling/disjoint_control/utils.py
@@ -0,0 +1,77 @@
+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
+ translation = path_base[0] + offset
+ translation[2] = args.handlebar_height
+ return SE3(rotation, translation)
+
+
+def initializePastData(
+ args: argparse.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
diff --git a/examples/cartesian_space/clik_point_to_point.py b/examples/cartesian_space/clik_point_to_point.py
index db7ef64e84e2f09caa1d2d2c13505b0695a1f460..4d1f33cca7ffc4aeaee0c862d0bb839b5c0b5565 100644
--- a/examples/cartesian_space/clik_point_to_point.py
+++ b/examples/cartesian_space/clik_point_to_point.py
@@ -5,6 +5,7 @@ from smc.robots.utils import defineGoalPointCLI
from smc.control.cartesian_space.cartesian_space_point_to_point import moveL
import argparse
+import numpy as np
def get_args() -> argparse.Namespace:
@@ -34,6 +35,7 @@ if __name__ == "__main__":
print("Ain't no way you're going to a random goal on the real robot!")
print("Look at the current pose, define something appropriate manually")
T_w_goal = defineGoalPointCLI(robot)
+ T_w_goal.rotation = np.eye(3)
# compliantMoveL(args, robot, Mgoal)
print(robot.mode)
moveL(args, robot, T_w_goal)
diff --git a/examples/math_understanding/rotate_around_z.py b/examples/math_understanding/rotate_around_z.py
new file mode 100644
index 0000000000000000000000000000000000000000..0c8b1e8d44d0eb4829184cd2838df71d24c34506
--- /dev/null
+++ b/examples/math_understanding/rotate_around_z.py
@@ -0,0 +1,30 @@
+from smc.visualization.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
+
+import time
+import pinocchio as pin
+import numpy as np
+import meshcat_shapes
+
+A = pin.SE3.Identity()
+B = pin.SE3.Identity()
+B.rotation = pin.rpy.rpyToMatrix(0.0, 0.0, np.random.random())
+B.translation += np.random.random(3)
+
+interpolated = []
+t_line = np.linspace(0, 1, 20)
+for t in t_line:
+ interpolated.append(pin.SE3.Interpolate(A, B, t))
+
+visualizer = MeshcatVisualizer()
+meshcat_shapes.frame(visualizer.viewer["A"], opacity=0.5)
+meshcat_shapes.frame(visualizer.viewer["B"], opacity=0.5)
+# meshcat_shapes.frame(visualizer.viewer["C"], opacity=0.5)
+visualizer.viewer["A"].set_transform(A.homogeneous)
+visualizer.viewer["B"].set_transform(B.homogeneous)
+# visualizer.viewer["C"].set_transform(C.homogeneous)
+
+visualizer.addFramePath("", interpolated)
+visualizer.addFramePath("", interpolated)
+
+
+time.sleep(100)
diff --git a/python/smc/control/cartesian_space/pink_p2p.py b/python/smc/control/cartesian_space/pink_p2p.py
index 3322b726f4b385c841d01bbca8ef69e1d73e6720..7f91b62c28cd22b443f00e6c2f6ac29278f72785 100644
--- a/python/smc/control/cartesian_space/pink_p2p.py
+++ b/python/smc/control/cartesian_space/pink_p2p.py
@@ -30,7 +30,11 @@ def DualArmIKSelfAvoidanceViaEndEffectorSpheresCtrlLoop(
t: int,
past_data: dict[str, deque[np.ndarray]],
) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
- configuration = pink.Configuration(robot.model, robot.data, robot.q)
+
+ # NOTE: for control modes to work, passing a reduced model is required.
+ # this model takes in full q, not the reduced one
+ # TODO: re-write stuff to accomodate new model
+ configuration = pink.Configuration(robot.model, robot.data, robot._q)
# NOTE: there are limits in pink, but they are a class where G matrix is constucted
# combining lb and ub and forming Gx \leq h
# this makes it possible to combine other stuff into this inequality constraint
@@ -57,7 +61,8 @@ def DualArmIKSelfAvoidanceViaEndEffectorSpheresCtrlLoop(
# 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
+ # v_cmd[:3] = 0.0
+ v_cmd = v_cmd[3:]
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
@@ -101,7 +106,10 @@ def DualArmIKSelfAvoidanceViaEndEffectorSpheres(
# NOTE: model and data are shared pointers between configuration and robot.
# this is redundant as hell, but I don't have the time butcher pink right now
- configuration = pink.Configuration(robot.model, robot.data, robot.q)
+ # NOTE: for control modes to work, passing a reduced model is required.
+ # this model takes in full q, not the reduced one
+ # TODO: re-write stuff to accomodate new model
+ configuration = pink.Configuration(robot.model, robot.data, robot._q)
posture_task.set_target_from_configuration(configuration)
left_end_effector_task.set_target(T_absgoal_l.act(T_w_absgoal))
right_end_effector_task.set_target(T_absgoal_r.act(T_w_absgoal))
diff --git a/python/smc/control/controller_templates/point_to_point.py b/python/smc/control/controller_templates/point_to_point.py
index 04d57137387267325676ff10ec4a0acaa7967b40..1cc63d15d3778382a1feb48f98606210d6d4f9d1 100644
--- a/python/smc/control/controller_templates/point_to_point.py
+++ b/python/smc/control/controller_templates/point_to_point.py
@@ -8,11 +8,12 @@ from typing import Any, Callable
import numpy as np
from collections import deque
-from smc.robots.interfaces.whole_body_interface import (
- SingleArmWholeBodyInterface,
+from smc.robots.interfaces.whole_body_dual_arm_interface import (
DualArmWholeBodyInterface,
)
-
+from smc.robots.interfaces.whole_body_single_arm_interface import (
+ SingleArmWholeBodyInterface,
+)
global control_loop_return
control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
diff --git a/python/smc/control/joint_space/joint_space_point_to_point.py b/python/smc/control/joint_space/joint_space_point_to_point.py
index 93e30cfe0f0a02b246b76eb17de0a9459f71da2f..06bc77d001d1f683a152955de5b50d1c9c0d29ad 100644
--- a/python/smc/control/joint_space/joint_space_point_to_point.py
+++ b/python/smc/control/joint_space/joint_space_point_to_point.py
@@ -7,6 +7,8 @@ from collections import deque
from argparse import Namespace
import pinocchio as pin
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+
def moveJControlLoop(
q_desired: np.ndarray,
@@ -22,7 +24,18 @@ def moveJControlLoop(
"""
breakFlag = False
q = robot.q
- q_error = pin.difference(robot.model, q, q_desired)
+ # TODO: make a robot output a different model depending on control mode
+ # to avoid having this issue
+ if (robot.control_mode == AbstractRobotManager.control_mode.whole_body) or (
+ robot.control_mode == AbstractRobotManager.control_mode.base_only
+ ):
+ q_error = pin.difference(robot.model, q, q_desired)
+ else:
+ if issubclass(robot.__class__, MobileBaseInterface):
+ q_desired = q_desired[4:]
+ q_error = q_desired - q
+ # q_error = pin.difference(robot.model, q, q_desired)
+
err_norm = np.linalg.norm(q_error)
if err_norm < 1e-3:
breakFlag = True
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 88c465bdfb3527be77acf32bb17f76205227288e..0686f9b7a3f07fdbc37ea65823adf6dde5d71280 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
@@ -1,4 +1,4 @@
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_dual_arm_interface import (
DualArmWholeBodyInterface,
)
from smc.control.control_loop_manager import ControlLoopManager
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py
index 7124b094212f08b981d972f96c5370374c0d83de..8d3fae0cfdd47358f08eb096011e77f59c3ce52d 100644
--- a/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py
@@ -1,4 +1,4 @@
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_single_arm_interface import (
SingleArmWholeBodyInterface,
)
from smc.control.control_loop_manager import ControlLoopManager
diff --git a/python/smc/control/optimal_control/croco_path_following/ocp/base_and_dual_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_path_following/ocp/base_and_dual_arm_reference_ocp.py
index fd7784023032e67d2cf90f31dac7f57b9fcc50d1..0a887add5dcbc696f232a85a8d80c38bb09802c9 100644
--- a/python/smc/control/optimal_control/croco_path_following/ocp/base_and_dual_arm_reference_ocp.py
+++ b/python/smc/control/optimal_control/croco_path_following/ocp/base_and_dual_arm_reference_ocp.py
@@ -1,7 +1,7 @@
from smc.control.optimal_control.croco_point_to_point.ocp.base_and_dual_arm_reference_ocp import (
BaseAndDualArmIKOCP,
)
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_dual_arm_interface import (
DualArmWholeBodyInterface,
)
diff --git a/python/smc/control/optimal_control/croco_path_following/ocp/base_and_single_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_path_following/ocp/base_and_single_arm_reference_ocp.py
index ef4072c25e1e47259cab993752734d63db9869b5..906d7b4754b7b00696469c68232540962f38696d 100644
--- a/python/smc/control/optimal_control/croco_path_following/ocp/base_and_single_arm_reference_ocp.py
+++ b/python/smc/control/optimal_control/croco_path_following/ocp/base_and_single_arm_reference_ocp.py
@@ -1,7 +1,7 @@
from smc.control.optimal_control.croco_point_to_point.ocp.base_and_single_arm_reference_ocp import (
BaseAndSingleArmIKOCP,
)
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_single_arm_interface import (
SingleArmWholeBodyInterface,
)
diff --git a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_dual_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_dual_arm_reference_mpc.py
index 9dd11edf7d61e6663d2efc93809a49595d08e631..05671ac70557dcdbdfb4df2554668720ee623ac1 100644
--- a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_dual_arm_reference_mpc.py
+++ b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_dual_arm_reference_mpc.py
@@ -1,10 +1,9 @@
from smc.control.controller_templates.point_to_point import (
DualEEAndBaseP2PCtrlLoopTemplate,
)
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_dual_arm_interface import (
DualArmWholeBodyInterface,
)
-from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
from smc.control.optimal_control.croco_point_to_point.ocp.base_and_dual_arm_reference_ocp import (
BaseAndDualArmIKOCP,
)
@@ -18,7 +17,7 @@ from argparse import Namespace
def CrocoP2PDualEEAndBaseMPCControlLoop(
- ocp,
+ ocp: BaseAndDualArmIKOCP,
T_w_absgoal: pin.SE3,
T_absgoal_l: pin.SE3,
T_absgoal_r: pin.SE3,
diff --git a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_single_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_single_arm_reference_mpc.py
index 2c6a411c70858c8649601a2a57f0d3605a8dd906..8ccbc3fdc5a68e27d014f4661bea2a5859523ae8 100644
--- a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_single_arm_reference_mpc.py
+++ b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_and_single_arm_reference_mpc.py
@@ -1,7 +1,7 @@
from smc.control.controller_templates.point_to_point import (
EEAndBaseP2PCtrlLoopTemplate,
)
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_single_arm_interface import (
SingleArmWholeBodyInterface,
)
from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
diff --git a/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_dual_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_dual_arm_reference_ocp.py
index 44c342f6daa0f6500e13b4c38022e763c1a9bc50..d85fefd4298e436dc5b1174337e973b8535488be 100644
--- a/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_dual_arm_reference_ocp.py
+++ b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_dual_arm_reference_ocp.py
@@ -1,4 +1,4 @@
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_dual_arm_interface import (
DualArmWholeBodyInterface,
)
from smc.control.optimal_control.croco_point_to_point.ocp.base_reference_ocp import (
diff --git a/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_single_arm_reference_ocp.py b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_single_arm_reference_ocp.py
index 75f4d1c0609628d5369c285ba09248af6b6f35bf..1ef96450927e85e695b196d8522dfa2fc5b3b397 100644
--- a/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_single_arm_reference_ocp.py
+++ b/python/smc/control/optimal_control/croco_point_to_point/ocp/base_and_single_arm_reference_ocp.py
@@ -1,4 +1,4 @@
-from smc.robots.interfaces.whole_body_interface import (
+from smc.robots.interfaces.whole_body_single_arm_interface import (
SingleArmWholeBodyInterface,
)
from smc.control.optimal_control.croco_point_to_point.ocp.base_reference_ocp import (
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index d322e996bdac355da570fcae6c0bbcf3e3fe11e7..a409401f54292ccd83c760969696c748cf06ef92 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -6,7 +6,7 @@ from smc.robots.interfaces.force_torque_sensor_interface import (
from smc.robots.interfaces.mobile_base_interface import (
get_mobile_base_model,
)
-from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface
+from smc.robots.interfaces.whole_body_single_arm_interface import SingleArmWholeBodyInterface
from smc.robots.implementations.ur5e import get_model
from smc.robots.grippers.robotiq.robotiq_gripper import RobotiqGripper
from smc.robots.grippers.rs485_robotiq.rs485_robotiq import RobotiqHand
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 8d142e13887393f15f53fdeca0461d39b8db0bab..8c62ce22fd990ee51375a7ecfe25ce9f9ca91f1d 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -1,9 +1,12 @@
from smc.robots.abstract_simulated_robotmanager import AbstractSimulatedRobotManager
-from smc.robots.interfaces.whole_body_interface import DualArmWholeBodyInterface
+from smc.robots.interfaces.whole_body_dual_arm_interface import (
+ DualArmWholeBodyInterface,
+)
from smc.robots.interfaces.mobile_base_interface import (
get_mobile_base_model,
)
from smc.robots.implementations.yumi import get_yumi_model
+from smc.robots.abstract_robotmanager import AbstractRobotManager
import pinocchio as pin
from argparse import Namespace
@@ -52,8 +55,26 @@ class AbstractMobileYuMiRobotManager(DualArmWholeBodyInterface):
1.400,
]
)
+ # NOTE: there's a shitton of stuff to re-write for this to work, and i'm not doing it now
+ # self._base_only_model, _ = get_mobile_base_model(underactuated=False)
+ # self._upper_body_model, _, _, _ = get_yumi_model()
+ # print(self._upper_body_model)
+
super().__init__(args)
+ # NOTE: there's a shitton of stuff to re-write for this to work, and i'm not doing it now
+
+
+# @property
+# def model(self) -> pin.Model:
+# if self._mode == AbstractRobotManager.control_mode.whole_body:
+# return self._model
+# if self._mode == AbstractRobotManager.control_mode.base_only:
+# return self._base_only_model
+# if self._mode == AbstractRobotManager.control_mode.upper_body:
+# return self._upper_body_model
+# return self._model
+
class SimulatedMobileYuMiRobotManager(
AbstractSimulatedRobotManager, AbstractMobileYuMiRobotManager
diff --git a/python/smc/robots/interfaces/__init__.py b/python/smc/robots/interfaces/__init__.py
index bc7ff830cf8a0b67c96dce94e013d1ba55628aa3..db32966c5fc9b79640822b5025bd9e418498f946 100644
--- a/python/smc/robots/interfaces/__init__.py
+++ b/python/smc/robots/interfaces/__init__.py
@@ -2,4 +2,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 *
+from .whole_body_dual_arm_interface import *
+from .whole_body_single_arm_interface import *
diff --git a/python/smc/robots/interfaces/mobile_base_interface.py b/python/smc/robots/interfaces/mobile_base_interface.py
index 6a266da4a887624c6720ee2b812406beabbeb502..2bb2a9170a4e3b19b662866dcf99f01cd718b801 100644
--- a/python/smc/robots/interfaces/mobile_base_interface.py
+++ b/python/smc/robots/interfaces/mobile_base_interface.py
@@ -83,10 +83,14 @@ class MobileBaseInterface(AbstractRobotManager):
self.forwardKinematics()
def getJacobian(self) -> np.ndarray:
- J = pin.computeFrameJacobian(
- self.model, self.data, self._q, self._base_frame_id
- )
- return J
+ # J = pin.computeFrameJacobian(
+ # self.model, self.data, self._q, self._base_frame_id
+ # )
+ # return J
+ J_base = np.zeros((6, 3))
+ J_base[:2, :2] = self.T_w_b.rotation[:2, :2]
+ J_base[5, 2] = 1
+ return J_base
def get_mobile_base_model(underactuated: bool) -> tuple[pin.Model, pin.GeometryModel]:
diff --git a/python/smc/robots/interfaces/whole_body_interface.py b/python/smc/robots/interfaces/whole_body_dual_arm_interface.py
similarity index 57%
rename from python/smc/robots/interfaces/whole_body_interface.py
rename to python/smc/robots/interfaces/whole_body_dual_arm_interface.py
index da3800866567254e26900d7f489a1409fe879de9..9ab517bf074d6fb56ae5fc550ffdaca47bad654c 100644
--- a/python/smc/robots/interfaces/whole_body_interface.py
+++ b/python/smc/robots/interfaces/whole_body_dual_arm_interface.py
@@ -1,152 +1,17 @@
from smc.robots.abstract_robotmanager import AbstractRobotManager
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
import pinocchio as pin
from argparse import Namespace
import numpy as np
-from copy import deepcopy
+
+# from copy import deepcopy
# TODO: put back in when python3.12 will be safe to use
# from typing import override
-class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
- """
- SingleArmWholeBodyInterface
- ---------------------------
- exists to provide either:
- 1) whole body values
- 2) base only value
- 3) arm only value
-
- what you get depends on the mode you set - they're enumerate as above
- """
-
- def __init__(self, args: Namespace):
- if args.debug_prints:
- print("SingleArmWholeBodyInterface init")
- self._mode: AbstractRobotManager.control_mode
- self._available_modes: list[AbstractRobotManager.control_mode] = [
- AbstractRobotManager.control_mode.whole_body,
- AbstractRobotManager.control_mode.base_only,
- AbstractRobotManager.control_mode.upper_body,
- ]
- super().__init__(args)
-
- # TODO: override model property to produce the reduced version
- # depending on the control mode.
- # you might want to instantiate all of them in advance for easy switching later
- # NOTE: that this is currently only important for ocp construction,
- # even though it's obviously the correct move either way
-
- # @AbstractRobotManager.mode.setter
- # def mode(self, mode: AbstractRobotManager.control_mode) -> None:
- # assert type(mode) in self._available_modes
- # self._mode = mode
-
- # TODO: put back in when python3.12 will be safe to use
- # @override
- @property
- def q(self) -> np.ndarray:
- if self._mode == self.control_mode.base_only:
- return self._q[:4]
-
- if self._mode == self.control_mode.upper_body:
- return self._q[4:]
-
- return self._q.copy()
-
- @property
- def nq(self):
- if self._mode == self.control_mode.base_only:
- return 4
-
- if self._mode == self.control_mode.upper_body:
- return self.model.nq - 4
-
- return self.model.nq
-
- @property
- def v(self) -> np.ndarray:
- if self._mode == self.control_mode.base_only:
- return self._v[:3]
-
- if self._mode == self.control_mode.upper_body:
- return self._v[3:]
-
- return self._v.copy()
-
- @property
- def nv(self):
- if self._mode == self.control_mode.base_only:
- return 3
-
- if self._mode == self.control_mode.upper_body:
- return self.model.nv - 3
-
- return self.model.nv
-
- # TODO: put back in when python3.12 will be safe to use
- # @override
- @property
- def max_v(self) -> np.ndarray:
- if self._mode == self.control_mode.base_only:
- return self._max_v[:3]
- if self._mode == self.control_mode.upper_body:
- return self._max_v[3:]
- return self._max_v.copy()
-
- # TODO: put back in when python3.12 will be safe to use
- # @override
- def forwardKinematics(self) -> None:
- pin.forwardKinematics(
- self.model,
- self.data,
- self._q,
- )
- pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
- pin.updateFramePlacement(self.model, self.data, self._base_frame_id)
- self._T_w_e = self.data.oMf[self._ee_frame_id].copy()
- self._T_w_b = self.data.oMf[self._base_frame_id].copy()
-
- # TODO: put back in when python3.12 will be safe to use
- # @override
- def getJacobian(self) -> np.ndarray:
- J_full = pin.computeFrameJacobian(
- self.model, self.data, self._q, self._ee_frame_id
- )
- if self._mode == self.control_mode.base_only:
- return J_full[:, :3]
-
- if self._mode == self.control_mode.upper_body:
- return J_full[:, 3:]
-
- return J_full
-
- # TODO: put back in when python3.12 will be safe to use
- # @override
- def sendVelocityCommand(self, v) -> None:
- """
- sendVelocityCommand
- -------------------
- 1) saturate the command to comply with hardware limits or smaller limits you've set
- 2) send it via the particular robot's particular communication interface
- """
- assert type(v) == np.ndarray
-
- if self._mode == self.control_mode.base_only:
- v = np.hstack((v, np.zeros(self.model.nv - 3)))
-
- if self._mode == self.control_mode.upper_body:
- v = np.hstack((np.zeros(3), v))
-
- assert len(v) == self.model.nv
- v = np.clip(v, -1 * self._max_v, self._max_v)
- self.sendVelocityCommandToReal(v)
-
-
class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
def __init__(self, args: Namespace):
if args.debug_prints:
@@ -158,20 +23,23 @@ 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))
+ # 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))
+ # NOTE: if you try to take out the mobile base joint, i.e. the first joint, i.e. a planarJoint, you get a segmentation fault :(
+ # meaning this needs to be done on a case-by-case basis
+ # also there's a shitton of stuff to re-write and i'm not doing it
+ # 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
+
+ # @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.
@@ -260,6 +128,19 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
return self._max_v[3 + (self.model.nv - 3) // 2 :]
return self._max_v.copy()
+ # NOTE: lil' bit of evil to access cartesian controllers for single arm without changing the controller
+ @property
+ def T_w_e(self):
+ if self.mode == self.control_mode.left_arm_only:
+ return self._T_w_l.copy()
+ if self.mode == self.control_mode.right_arm_only:
+ return self._T_w_r.copy()
+ if self.mode == self.control_mode.upper_body:
+ return self._T_w_abs.copy()
+ if self.mode == self.control_mode.base_only:
+ return self._T_w_b.copy()
+ return self._T_w_abs.copy()
+
# TODO: put back in when python3.12 will be safe to use
# @override
def forwardKinematics(self) -> None:
@@ -290,7 +171,11 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
# for efficiency of course it would be best to construct it in place,
# but who cares if it runs on time either way.
if self._mode == self.control_mode.base_only:
- return J_left_with_base[:, :3]
+ J_base = np.zeros((6, 3))
+ J_base[:2, :2] = self.T_w_b.rotation[:2, :2]
+ J_base[5, 2] = 1
+ return J_base
+ # return J_left_with_base[:, :3]
J_right = pin.computeFrameJacobian(
self.model, self.data, self._q, self._r_ee_frame_id
@@ -333,5 +218,4 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
v_cmd_to_real[3 + (self.model.nv - 3) // 2 :] = v_cmd
v_cmd_to_real = np.clip(v_cmd_to_real, -1 * self._max_v, self._max_v)
-
self.sendVelocityCommandToReal(v_cmd_to_real)
diff --git a/python/smc/robots/interfaces/whole_body_single_arm_interface.py b/python/smc/robots/interfaces/whole_body_single_arm_interface.py
new file mode 100644
index 0000000000000000000000000000000000000000..a9c5fd4d6e633ccb8ac10dd4ad929b8b367cbca3
--- /dev/null
+++ b/python/smc/robots/interfaces/whole_body_single_arm_interface.py
@@ -0,0 +1,156 @@
+from smc.robots.abstract_robotmanager import AbstractRobotManager
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+
+import pinocchio as pin
+from argparse import Namespace
+import numpy as np
+
+# from copy import deepcopy
+
+# TODO: put back in when python3.12 will be safe to use
+# from typing import override
+
+
+class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
+ """
+ SingleArmWholeBodyInterface
+ ---------------------------
+ exists to provide either:
+ 1) whole body values
+ 2) base only value
+ 3) arm only value
+
+ what you get depends on the mode you set - they're enumerate as above
+ """
+
+ def __init__(self, args: Namespace):
+ if args.debug_prints:
+ print("SingleArmWholeBodyInterface init")
+ self._mode: AbstractRobotManager.control_mode
+ self._available_modes: list[AbstractRobotManager.control_mode] = [
+ AbstractRobotManager.control_mode.whole_body,
+ AbstractRobotManager.control_mode.base_only,
+ AbstractRobotManager.control_mode.upper_body,
+ ]
+ super().__init__(args)
+
+ # TODO: override model property to produce the reduced version
+ # depending on the control mode.
+ # you might want to instantiate all of them in advance for easy switching later
+ # NOTE: that this is currently only important for ocp construction,
+ # even though it's obviously the correct move either way
+
+ # @AbstractRobotManager.mode.setter
+ # def mode(self, mode: AbstractRobotManager.control_mode) -> None:
+ # assert type(mode) in self._available_modes
+ # self._mode = mode
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ @property
+ def q(self) -> np.ndarray:
+ if self._mode == self.control_mode.base_only:
+ return self._q[:4]
+
+ if self._mode == self.control_mode.upper_body:
+ return self._q[4:]
+
+ return self._q.copy()
+
+ @property
+ def nq(self):
+ if self._mode == self.control_mode.base_only:
+ return 4
+
+ if self._mode == self.control_mode.upper_body:
+ return self.model.nq - 4
+
+ return self.model.nq
+
+ @property
+ def v(self) -> np.ndarray:
+ if self._mode == self.control_mode.base_only:
+ return self._v[:3]
+
+ if self._mode == self.control_mode.upper_body:
+ return self._v[3:]
+
+ return self._v.copy()
+
+ @property
+ def nv(self):
+ if self._mode == self.control_mode.base_only:
+ return 3
+
+ if self._mode == self.control_mode.upper_body:
+ return self.model.nv - 3
+
+ return self.model.nv
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ @property
+ def max_v(self) -> np.ndarray:
+ if self._mode == self.control_mode.base_only:
+ return self._max_v[:3]
+ if self._mode == self.control_mode.upper_body:
+ return self._max_v[3:]
+ return self._max_v.copy()
+
+ # NOTE: lil' bit of evil to access cartesian controllers just for the base without changing the controller
+ @property
+ def T_w_e(self):
+ if self.mode == self.control_mode.upper_body:
+ return self._T_w_e.copy()
+ if self.mode == self.control_mode.base_only:
+ return self._T_w_b.copy()
+ return self._T_w_e.copy()
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ def forwardKinematics(self) -> None:
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ self._q,
+ )
+ pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self._base_frame_id)
+ self._T_w_e = self.data.oMf[self._ee_frame_id].copy()
+ self._T_w_b = self.data.oMf[self._base_frame_id].copy()
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ def getJacobian(self) -> np.ndarray:
+ J_full = pin.computeFrameJacobian(
+ self.model, self.data, self._q, self._ee_frame_id
+ )
+ if self._mode == self.control_mode.base_only:
+ return J_full[:, :3]
+
+ if self._mode == self.control_mode.upper_body:
+ return J_full[:, 3:]
+
+ return J_full
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ def sendVelocityCommand(self, v) -> None:
+ """
+ sendVelocityCommand
+ -------------------
+ 1) saturate the command to comply with hardware limits or smaller limits you've set
+ 2) send it via the particular robot's particular communication interface
+ """
+ assert type(v) == np.ndarray
+
+ if self._mode == self.control_mode.base_only:
+ v = np.hstack((v, np.zeros(self.model.nv - 3)))
+
+ if self._mode == self.control_mode.upper_body:
+ v = np.hstack((np.zeros(3), v))
+
+ assert len(v) == self.model.nv
+ v = np.clip(v, -1 * self._max_v, self._max_v)
+ self.sendVelocityCommandToReal(v)