diff --git a/examples/cart_pulling/MASSIVE_TODO b/examples/cart_pulling/MASSIVE_TODO
new file mode 100644
index 0000000000000000000000000000000000000000..eecf06f159f04179c22031c7d0ed83b9b86d6ad8
--- /dev/null
+++ b/examples/cart_pulling/MASSIVE_TODO
@@ -0,0 +1,8 @@
+1. you have to find a way to keep track of the path for the end-effector. one trillion percent it manages to break itself, you need to save it in a log and replay everything. make change to the log_item thing to make not everything automatically plottable (check value type or something)
+ --> preferably do that while running the disjoint control because it survives the hassle
+2. make proper path-tracking cost in python, that's shouldn't be too much of a hassle
+ 2.1. do it with numdiff first
+ 2.2. put in the analytic derivative
+3. put in progressively higher goal tracking costs.
+4. dynamically change the costs - if the end-effector is too fucked, track just the first point while fixing the base, whatever.
+5. re-initialize previous path if the error is too big.
diff --git a/examples/pinocchio_math_understanding/slerp.py b/examples/pinocchio_math_understanding/slerp.py
new file mode 100644
index 0000000000000000000000000000000000000000..417e167ef69f8681b4faad16b1780b7e1a91978b
--- /dev/null
+++ b/examples/pinocchio_math_understanding/slerp.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.Random()
+A.translation /= 2
+B = pin.SE3.Random()
+B.translation /= 2
+
+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/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index 2aa452fdcb2d1581f55abbba029dbc45b213db8a..e2551af9dd86faa346d107d208d10fe1492a943b 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -84,10 +84,11 @@ class CrocoOCP(abc.ABC):
# put these costs into the running costs
for i in range(self.args.n_knots):
- self.runningCostModels[i].addCost("xReg", self.xRegCost, 1e-3)
- self.runningCostModels[i].addCost("uReg", self.uRegCost, 1e-3)
+ self.runningCostModels[i].addCost("xReg", self.xRegCost, self.args.u_reg_cost)
+ self.runningCostModels[i].addCost("uReg", self.uRegCost, self.args.x_reg_cost)
# and add the terminal cost, which is the distance to the goal
# NOTE: shouldn't there be a final velocity = 0 here?
+ # --> no if you're not stopping at the last knot!
self.terminalCostModel.addCost("uReg", self.uRegCost, 1e3)
def constructStateLimits(self) -> None:
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 d163fb6f5e83a88aab11f7e9f7589b0fb2ef631b..5afb4b5021852fe7d0dd599aaf2225a0f4f52a0a 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -166,8 +166,8 @@ def CrocoEEPathFollowingFromPlannerMPCControlLoop(
# create a 3D reference out of the path
path_EE_SE3 = path2D_to_SE3_fixed(path2D, args.handlebar_height)
- for pose in path_EE_SE3:
- print(pose.translation)
+# for pose in path_EE_SE3:
+# print(pose.translation)
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualizer:
@@ -307,8 +307,8 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
# SETTING ROTATION IS BROKEN ATM
# BUT SETTING DISTANCES (TRANSLATIONS) IS TOO.
# WE DEAL WITH DISTANCES FIRST, UNTIL THAT'S DONE THIS STAYS HERE
- for ppp in pathSE3_handlebar:
- ppp.rotation = robot.T_w_e.rotation
+ #for ppp in pathSE3_handlebar:
+ # ppp.rotation = robot.T_w_e.rotation
###########################################
if args.visualizer:
diff --git a/python/smc/control/optimal_control/util.py b/python/smc/control/optimal_control/util.py
index 177b2cf490781465d61cf999a40efd06aaa6235c..16b220bd6e8bd3f31285d74b18644e6ed3de6a81 100644
--- a/python/smc/control/optimal_control/util.py
+++ b/python/smc/control/optimal_control/util.py
@@ -1,19 +1,52 @@
import argparse
-def get_OCP_args(parser : argparse.ArgumentParser):
- parser.add_argument("--ocp-dt", type=float, default=1e-2, \
- help="time length between state-control-pair decision variables")
- parser.add_argument("--n-knots", type=int, default=100, \
- help="number of state-control-pair decision variables")
- parser.add_argument("--stop-at-final", type=int, default=1, \
- help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
- if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).")
- parser.add_argument("--solver", type=str, default="boxfddp", \
- help="optimal control problem solver you want", choices=["boxfddp", "csqp"])
- parser.add_argument("--max-solver-iter", type=int, default=200, \
- help="number of iterations the ocp solver takes. ~100-500 for just ocp (it can converge before ofc), ~10 for mpc")
- parser.add_argument("--ee-pose-cost", type=float, default=1e2, \
- help="ee pose cost in pulling mpc")
- parser.add_argument("--base-translation-cost", type=float, default=1e2, \
- help="base translation cost in pulling mpc")
+
+def get_OCP_args(parser: argparse.ArgumentParser):
+ parser.add_argument(
+ "--ocp-dt",
+ type=float,
+ default=1e-2,
+ help="time length between state-control-pair decision variables",
+ )
+ parser.add_argument(
+ "--n-knots",
+ type=int,
+ default=100,
+ help="number of state-control-pair decision variables",
+ )
+ parser.add_argument(
+ "--stop-at-final",
+ type=int,
+ default=1,
+ help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
+ if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).",
+ )
+ parser.add_argument(
+ "--solver",
+ type=str,
+ default="boxfddp",
+ help="optimal control problem solver you want",
+ choices=["boxfddp", "csqp"],
+ )
+ parser.add_argument(
+ "--max-solver-iter",
+ type=int,
+ default=200,
+ help="number of iterations the ocp solver takes. ~100-500 for just ocp (it can converge before ofc), ~10 for mpc",
+ )
+ parser.add_argument(
+ "--ee-pose-cost", type=float, default=1e2, help="ee pose cost in pulling mpc"
+ )
+ parser.add_argument(
+ "--base-translation-cost",
+ type=float,
+ default=1e2,
+ help="base translation cost in pulling mpc",
+ )
+ parser.add_argument(
+ "--x-reg-cost", type=float, default=1e-3, help="state regulation cost"
+ )
+ parser.add_argument(
+ "--u-reg-cost", type=float, default=1e-3, help="control input regulation cost"
+ )
return parser
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index 2f740df9eee9ded6fc01d93a577a0a0229821d96..b4a9edb936082608516171a8433dcc84d75aa94f 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -194,7 +194,9 @@ def heron_approximation():
# TODO: look what's done in pink, see if this can be usable
# after you've removed camera joint and similar.
# NOTE: NOT USED, BUT STUFF WILL NEED TO BE EXTRACTED FROM THIS EVENTUALLY
-def get_heron_model_from_full_urdf():
+def get_heron_model_from_full_urdf() -> (
+ tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]
+):
# urdf_path_relative = files('smc.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
urdf_path_absolute = "/home/gospodar/home2/gospodar/lund/praxis/software/ros/ros-containers/home/model.urdf"
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 1842c64bf561760e91401d87f67d809ed5760dcc..40adca68c57d737e610d19cbc7eaed3966fb373c 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -1,5 +1,12 @@
from smc.robots.robotmanager_abstract import AbstractRobotManager
from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from smc.robots.interfaces.mobile_base_interface import (
+ MobileBaseInterface,
+ get_mobile_base_model,
+)
+from smc.robots.implementations.yumi import get_yumi_model
+
+import pinocchio as pin
class MobileYumiRobotManager(DualArmInterface):
@@ -42,84 +49,12 @@ class RealMobileYumiRobotManager(MobileYumiRobotManager):
self.publisher_joints_cmd.publish(msg)
-def get_yumi_model():
-
- urdf_path_relative = files("smc.robots.robot_descriptions").joinpath("yumi.urdf")
- urdf_path_absolute = os.path.abspath(urdf_path_relative)
- # mesh_dir = files('smc')
- # mesh_dir_absolute = os.path.abspath(mesh_dir)
- # mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
- mesh_dir_absolute = None
-
- model = None
- collision_model = None
- visual_model = None
- # this command just calls the ones below it. both are kept here
- # in case pinocchio people decide to change their api.
- # model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
- model_arms = pin.buildModelFromUrdf(urdf_path_absolute)
- visual_model_arms = pin.buildGeomFromUrdf(
- model_arms, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute
- )
- collision_model_arms = pin.buildGeomFromUrdf(
- model_arms,
- urdf_path_absolute,
- pin.GeometryType.COLLISION,
- None,
- mesh_dir_absolute,
- )
-
- data_arms = pin.Data(model_arms)
+def get_mobile_yumi_model() -> (
+ tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]
+):
- # mobile base as planar joint (there's probably a better
- # option but whatever right now)
- model_mobile_base = pin.Model()
- model_mobile_base.name = "mobile_base"
- geom_model_mobile_base = pin.GeometryModel()
- joint_name = "mobile_base_planar_joint"
- parent_id = 0
- # TEST
- joint_placement = pin.SE3.Identity()
- # joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
- # joint_placement.translation[2] = 0.2
- MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
- parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
- )
- # we should immediately set velocity limits.
- # there are no position limit by default and that is what we want.
- model_mobile_base.velocityLimit[0] = 2
- model_mobile_base.velocityLimit[1] = 2
- model_mobile_base.velocityLimit[2] = 2
- model_mobile_base.effortLimit[0] = 200
- model_mobile_base.effortLimit[1] = 2
- model_mobile_base.effortLimit[2] = 200
-
- # pretty much random numbers
- # TODO: find heron (mir) numbers
- body_inertia = pin.Inertia.FromBox(30, 0.5, 0.3, 0.4)
- # maybe change placement to sth else depending on where its grasped
- model_mobile_base.appendBodyToJoint(
- MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity()
- )
- box_shape = fcl.Box(0.5, 0.3, 0.4)
- body_placement = pin.SE3.Identity()
- geometry_mobile_base = pin.GeometryObject(
- "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])
- geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
-
- # have to add the frame manually
- model_mobile_base.addFrame(
- pin.Frame(
- "mobile_base",
- MOBILE_BASE_JOINT_ID,
- 0,
- joint_placement.copy(),
- pin.FrameType.JOINT,
- )
- )
+ model_arms, visual_model_arms, collision_model_arms, _ = get_yumi_model()
+ model_mobile_base, geom_model_mobile_base = get_mobile_base_model(False)
# frame-index should be 1
model, visual_model = pin.appendModel(
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index d50ce5de8b0e343d16d427c5d9a2fd070ecf4f4d..7a3ec9853e16a0202e706c047adc94a0887ad26a 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -185,7 +185,7 @@ class RealUR5eRobotManager(RobotManagerUR5e, AbstractRealRobotManager):
raise NotImplementedError("freedrive function only written for ur5e")
-def get_model():
+def get_model()-> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]:
urdf_path_relative = files("smc.robots.robot_descriptions.urdf").joinpath(
"ur5e_with_robotiq_hande_FIXED_PATHS.urdf"
diff --git a/python/smc/robots/implementations/yumi.py b/python/smc/robots/implementations/yumi.py
new file mode 100644
index 0000000000000000000000000000000000000000..b96ac6939988c210ab3b253e677a35d8dc7d9c42
--- /dev/null
+++ b/python/smc/robots/implementations/yumi.py
@@ -0,0 +1,63 @@
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+
+from importlib.resources import files
+from os import path
+import pinocchio as pin
+
+
+class RobotManagerYuMi(DualArmInterface):
+
+ @property
+ def model(self):
+ return self._model
+
+ @property
+ def data(self):
+ return self._data
+
+ @property
+ def visual_model(self):
+ return self._visual_model
+
+ @property
+ def collision_model(self):
+ return self._collision_model
+
+ def __init__(self, args):
+ if args.debug_prints:
+ print("RobotManagerUR5e init")
+ self._model, self._collision_model, self._visual_model, self._data = get_model()
+ self._ee_frame_id = self.model.getFrameId("tool0")
+ self._MAX_ACCELERATION = 1.7 # const
+ self._MAX_QD = 3.14 # const
+ super().__init__(args)
+
+
+def get_yumi_model() -> (
+ tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]
+):
+
+ urdf_path_relative = files("smc.robots.robot_descriptions").joinpath("yumi.urdf")
+ urdf_path_absolute = path.abspath(urdf_path_relative)
+ # mesh_dir = files('smc')
+ # mesh_dir_absolute = os.path.abspath(mesh_dir)
+ # mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
+ mesh_dir_absolute = None
+
+ # this command just calls the ones below it. both are kept here
+ # in case pinocchio people decide to change their api.
+ # model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
+ model = pin.buildModelFromUrdf(urdf_path_absolute)
+ visual_model = pin.buildGeomFromUrdf(
+ model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute
+ )
+ collision_model = pin.buildGeomFromUrdf(
+ model,
+ urdf_path_absolute,
+ pin.GeometryType.COLLISION,
+ None,
+ mesh_dir_absolute,
+ )
+
+ data = pin.Data(model)
+ return model, collision_model, visual_model, data
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index e9b0d605394cf669a3aa2c03387b4012e159f6e7..f93465e25015fd6390e72073570ee29c656aeb24 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -1,11 +1,12 @@
-from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
import abc
import numpy as np
import pinocchio as pin
+from argparse import Namespace
-class DualArmInterface(AbstractRobotManager):
+class DualArmInterface(SingleArmInterface):
"""
DualArmInterface
----------------
@@ -18,12 +19,37 @@ class DualArmInterface(AbstractRobotManager):
# (better than putting in a magic number)
# self._l_ee_frame_id = self.model.getFrameId("left_ee_name")
# self._r_ee_frame_id = self.model.getFrameId("right_ee_name")
+
+ def __init__(self, args: Namespace):
+ self._T_w_abs: pin.SE3
+ self._T_w_l: pin.SE3
+ self._T_w_r: pin.SE3
+ self._T_l_r: pin.SE3
+ self._l_ee_frame_id: int
+ self._r_ee_frame_id: int
+ if args.debug_prints:
+ print("DualArmInterface init")
+ super().__init__(args)
+
@property
- @abc.abstractmethod
- def l_ee_frame_id(self) -> int: ...
+ def T_w_e(self):
+ return self.T_w_abs
+
@property
- @abc.abstractmethod
- def r_ee_frame_id(self) -> int: ...
+ def T_w_l(self):
+ return self._T_w_l.copy()
+
+ @property
+ def l_ee_frame_id(self) -> int:
+ return self._l_ee_frame_id
+
+ @property
+ def T_w_r(self):
+ return self._T_w_r.copy()
+
+ @property
+ def r_ee_frame_id(self) -> int:
+ return self._r_ee_frame_id
# T_abs_l and T_abs_r are relative transformations between the absolute dual-arm frame,
# and the left and right end-effector frames respectively
@@ -34,29 +60,28 @@ class DualArmInterface(AbstractRobotManager):
@abc.abstractmethod
def T_abs_l(self) -> pin.SE3: ...
- def __init__(self):
- self._T_w_abs: pin.SE3
- self._T_w_l: pin.SE3
- self._T_w_r: pin.SE3
-
- # TODO: you have to have relative frame transformations here
- # (from absolute frame to left frame and to the right frame)
- raise NotImplementedError
-
- def constructT_w_abs(self):
- raise NotImplementedError
-
@property
- def T_w_abs(self, q=None):
+ def T_w_abs(self) -> pin.SE3:
"""
getT_w_abs
-----------
get absolute frame, as seen in base frame, based on the current, or provident joint configuration
- TODO: figure out what's the best way to get this
"""
- raise NotImplementedError("need to add the case where you construct it from q")
+ T_w_abs = pin.SE3.Interpolate(self._T_w_l, self._T_w_r, 0.5)
+ self._T_w_abs = T_w_abs
return self._T_w_abs.copy()
+ def T_l_r(self) -> pin.SE3:
+ T_l_r = self._T_w_l.actInv(self._T_w_r)
+ self._T_l_r = T_l_r
+ return self._T_l_r.copy()
+
+ def getV_w_abs(self, V_w_l: pin.Motion, V_w_r: pin.Motion) -> pin.Motion:
+ return 0.5 * (V_w_l + V_w_r)
+
+ def getV_w_lr(self, V_w_l: pin.Motion, V_w_r: pin.Motion) -> pin.Motion:
+ return V_w_r - V_w_l
+
def getLeftRightT_w_e(self, q=None):
"""
getLeftRightT_w_e
@@ -80,17 +105,40 @@ class DualArmInterface(AbstractRobotManager):
)
# NOTE: this also returns the frame, so less copying possible
# pin.updateFramePlacements(self.model, self.data)
- pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
- pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self._l_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self._r_ee_frame_id)
return (
- self.data.oMf[self.l_ee_frame_id].copy(),
- self.data.oMf[self.r_ee_frame_id].copy(),
+ self.data.oMf[self._l_ee_frame_id].copy(),
+ self.data.oMf[self._r_ee_frame_id].copy(),
)
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
def forwardKinematics(self):
- pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
- pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
- self._T_w_l = self.data.oMf[self.l_ee_frame_id].copy()
- self._T_w_r = self.data.oMf[self.r_ee_frame_id].copy()
- self.constructT_w_abs()
- raise NotImplementedError
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ self._q,
+ )
+ pin.updateFramePlacement(self.model, self.data, self._l_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self._r_ee_frame_id)
+ self._T_w_l = self.data.oMf[self._l_ee_frame_id].copy()
+ self._T_w_r = self.data.oMf[self._r_ee_frame_id].copy()
+ self.T_w_abs # will update _T_w_abs in the process
+
+ # TODO: put back in when python3.12 will be safe to use
+ # @override
+ # NOTE: this isn't useful unless it's in world frame
+ def getJacobian(self) -> np.ndarray:
+ # the other arm filled with zeros
+ J_left = pin.computeFrameJacobian(
+ self.model, self.data, self._q, self._l_ee_frame_id
+ )
+
+ J_right = pin.computeFrameJacobian(
+ self.model, self.data, self._q, self._r_ee_frame_id
+ )
+ J = np.zeros((12, self.nq))
+ J[: self.nq, :6] = J_left
+ J[self.nq :, 6:] = J_right
+ return J