diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index 1dd1127efce60d9e2315e4ed27133cf5c9783d02..1020e48286c925b829026242ae0c72f70b017c6d 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -1,5 +1,4 @@
from smc import getRobotFromArgs
-from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface
from smc import getMinimalArgParser
from smc.path_generation.maps.premade_maps import createSampleStaticMap
from smc.path_generation.path_math.path2d_to_6d import path2D_to_SE3
@@ -33,7 +32,6 @@ if __name__ == "__main__":
args = get_args()
robot = getRobotFromArgs(args)
# TODO: for ocp you want to pass only the mobile base model
- # robot._mode = SingleArmWholeBodyInterface.control_mode.base_only
robot._step()
robot._q[0] = 9.0
robot._q[1] = 4.0
diff --git a/python/smc/control/control_loop_manager.py b/python/smc/control/control_loop_manager.py
index 5756795d934a21023d5ea2330a2ba59eca575c77..0e3175d715f0477842134342e9566b9eee327b0d 100644
--- a/python/smc/control/control_loop_manager.py
+++ b/python/smc/control/control_loop_manager.py
@@ -1,5 +1,7 @@
from argparse import Namespace
from smc.robots.abstract_robotmanager import AbstractRobotManager
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.multiprocessing.process_manager import ProcessManager
from smc.visualization.plotters import realTimePlotter
@@ -149,19 +151,16 @@ class ControlLoopManager:
"q": self.robot_manager._q,
}
)
- self.robot_manager.visualizer_manager.sendCommand(
- {
- "T_w_e": self.robot_manager.T_w_e,
- }
- )
- if self.robot_manager.robot_name == "heron":
- T_base = self.robot_manager.data.oMi[1]
+ if issubclass(self.robot_manager.__class__, SingleArmInterface):
+ self.robot_manager.visualizer_manager.sendCommand(
+ {
+ "T_w_e": self.robot_manager.T_w_e,
+ }
+ )
+ if issubclass(self.robot_manager.__class__, MobileBaseInterface):
self.robot_manager.visualizer_manager.sendCommand(
- {"T_base": T_base}
+ {"T_base": self.robot_manager.T_w_b}
)
- # if self.robot_manager.robot_name == "yumi":
- # T_base = self.robot_manager.data.oMi[1]
- # self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
if self.args.visualize_collision_approximation:
raise NotImplementedError
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index 26127244403d396c6cbb99b8441428bab036295d..356a068db07c9cc1477da339c8c530efb24deb29 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -7,6 +7,8 @@ import importlib.util
import abc
from typing import Any
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+
if importlib.util.find_spec("mim_solvers"):
try:
import mim_solvers
@@ -127,7 +129,7 @@ class CrocoOCP(abc.ABC):
# TODO: replace this with subclass or whatever call
# to check if the robot has mobilebase interface
- if self.robot.robot_name == "heron" or self.robot.robot_name == "myumi":
+ if issubclass(self.robot.__class__, MobileBaseInterface):
self.xlb = self.xlb[1:]
self.xub = self.xub[1:]
diff --git a/python/smc/robots/implementations/__init__.py b/python/smc/robots/implementations/__init__.py
index 387161300f08b24a1369a690c9ccabd46bb596c5..1eb77ce990f4fc06b5aaacb4f08f920d79eb642f 100644
--- a/python/smc/robots/implementations/__init__.py
+++ b/python/smc/robots/implementations/__init__.py
@@ -1,4 +1,4 @@
-# from .mir import AbstractMirRobotManager
+from .mir import SimulatedMirRobotManager
from .heron import SimulatedHeronRobotManager
from .mobile_yumi import SimulatedMobileYuMiRobotManager
from .ur5e import SimulatedUR5eRobotManager
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index a8b770b3f1aa2e5ebf0d0d2bafa581e816f33dcd..02916b7eb4a567c34b6a61e8e9a1f2d273e5d691 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -281,7 +281,9 @@ class RealHeronRobotManager(AbstractHeronRobotManager, AbstractRealRobotManager)
"""
-def heron_approximation():
+def heron_approximation() -> (
+ tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]
+):
# arm + gripper
model_arm, collision_model_arm, visual_model_arm, _ = get_model(
with_gripper_joints=True
diff --git a/python/smc/robots/implementations/mir.py b/python/smc/robots/implementations/mir.py
index acb22f5e9345f9dc2276c2663a4d7315c505ed0d..a09e2d971eca7e2cfdd07219fe5d3d5eb06bc71a 100644
--- a/python/smc/robots/implementations/mir.py
+++ b/python/smc/robots/implementations/mir.py
@@ -1,11 +1,34 @@
-from smc.robots.abstract_robotmanager import AbstractRobotManager
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.abstract_simulated_robotmanager import AbstractSimulatedRobotManager
+import pinocchio as pin
+import hppfcl as fcl
+import numpy as np
+from argparse import Namespace
-class AbstractMirRobotManager(AbstractRobotManager):
- pass
+
+class AbstractMirRobotManager(MobileBaseInterface):
+ def __init__(self, args: Namespace):
+ if args.debug_prints:
+ print("AbstractMirRobotManager init")
+ self._model, self._collision_model, self._visual_model, self._data = (
+ mir_approximation()
+ )
+ self._base_frame_id = self.model.getFrameId("mobile_base")
+ self._MAX_ACCELERATION = 1.7 # const
+ self._MAX_QD = 3.14 # const
+ super().__init__(args)
+
+
+class SimulatedMirRobotManager(AbstractSimulatedRobotManager, AbstractMirRobotManager):
+ def __init__(self, args: Namespace):
+ if args.debug_prints:
+ print("SimulatedMirRobotManager init")
+ super().__init__(args)
class RealMirRobotManager(AbstractMirRobotManager):
+ # TODO: implement
def sendVelocityCommandToReal(self, v):
"""
sendVelocityCommand
@@ -27,17 +50,22 @@ class RealMirRobotManager(AbstractMirRobotManager):
# self.q = pin.integrate(self.model, self.q, qd * self.dt)
raise NotImplementedError
+ # TODO: implement
def stopRobot(self):
raise NotImplementedError
+ # TODO: implement
def setFreedrive(self):
raise NotImplementedError
+ # TODO: implement
def unSetFreedrive(self):
raise NotImplementedError
-def mir_approximation():
+def mir_approximation() -> (
+ 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()
@@ -47,16 +75,6 @@ def mir_approximation():
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
- # TODO TODO TODO TODO TODO TODO TODO TODO
- # TODO: heron is actually a differential drive,
- # meaning that it is not a planar joint.
- # we could put in a prismatic + revolute joint
- # as the base (both joints being at same position),
- # and that should work for our purposes.
- # this makes sense for initial testing
- # because mobile yumi's base is a planar joint
MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
)
@@ -95,7 +113,7 @@ def mir_approximation():
# it's tool0 because that's used everywhere
model_mobile_base.addFrame(
pin.Frame(
- "tool0",
+ "mobile_base",
MOBILE_BASE_JOINT_ID,
0,
joint_placement.copy(),
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 52361ea37d85f917981d9efc8176fa2d67df0fb1..4aa298ecc6473462b4132502e67cec36e92df848 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -71,6 +71,10 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
self.publisher_joints_cmd.publish(msg)
+# TODO: define a separate mobile base for YuMi here
+# necessary because the dimensions are not the same
+
+
def get_mobile_yumi_model() -> (
tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]
):
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index af934b8b2e3cd67474889c0bc715373cb4dc0cac..ef7b7b832e2310a153bc289e7ac9003ec7e73ccc 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -25,7 +25,7 @@ if find_spec("rtde_control"):
class AbstractUR5eRobotManager(ForceTorqueOnSingleArmWrist):
def __init__(self, args):
if args.debug_prints:
- print("RobotManagerUR5e init")
+ print("AbstractUR5eRobotManager 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
diff --git a/python/smc/robots/interfaces/mobile_base_interface.py b/python/smc/robots/interfaces/mobile_base_interface.py
index 56bd0dc7a7ea70a95f41cca04ad02e52f5d3f279..58a05ff94ddb77debf45b9f85d3e68f32cb3bfee 100644
--- a/python/smc/robots/interfaces/mobile_base_interface.py
+++ b/python/smc/robots/interfaces/mobile_base_interface.py
@@ -42,7 +42,7 @@ class MobileBaseInterface(AbstractRobotManager):
def T_w_b(self):
return self._T_w_b.copy()
- def computeT_w_b(self, q) -> pin.SE3:
+ def computeT_w_b(self, q: np.ndarray) -> pin.SE3:
assert type(q) is np.ndarray
pin.forwardKinematics(
self.model,
@@ -58,19 +58,19 @@ class MobileBaseInterface(AbstractRobotManager):
# just driving around with a full-body robot without using hands.
# not prio right now and I don't want to deal with
# how this interacts with other interfaces
- # def forwardKinematics(self):
- # pin.forwardKinematics(
- # self.model,
- # self.data,
- # self._q,
- # )
- # pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
- # self._T_w_e = self.data.oMf[self._ee_frame_id].copy()
-
- # def _step(self):
- # self._updateQ()
- # self._updateV()
- # self.forwardKinematics()
+ def forwardKinematics(self):
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ self._q,
+ )
+ pin.updateFramePlacement(self.model, self.data, self._base_frame_id)
+ self._T_w_b = self.data.oMf[self._base_frame_id].copy()
+
+ def _step(self):
+ self._updateQ()
+ self._updateV()
+ self.forwardKinematics()
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_interface.py
index 4ee98111938e21424dcfdc035a297375b35405c5..d96826b4381c227827df912572ed8af5bbe7ebd2 100644
--- a/python/smc/robots/interfaces/whole_body_interface.py
+++ b/python/smc/robots/interfaces/whole_body_interface.py
@@ -31,6 +31,12 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
self._mode: SingleArmWholeBodyInterface.control_mode
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
+
@property
def mode(self) -> control_mode:
return self._mode
@@ -149,6 +155,12 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
self._mode: DualArmWholeBodyInterface.control_mode
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
+
# TODO: put back in when python3.12 will be safe to use
# @override
@property
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index e3875df8bcf96133a376a72f7cc43db1d8664598..c53dda8f8f6047346006d905add20871b74320a8 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -28,7 +28,7 @@ def getMinimalArgParser():
type=str,
help="which robot you're running or simulating",
default="ur5e",
- choices=["ur5e", "heron", "yumi", "myumi"],
+ choices=["ur5e", "heron", "yumi", "myumi", "mir"],
)
parser.add_argument(
"--real",
@@ -275,6 +275,13 @@ def getRobotFromArgs(args: argparse.Namespace) -> AbstractRobotManager:
# return RealMobileYuMiRobotManager(args)
else:
return SimulatedMobileYuMiRobotManager(args)
+ if args.robot == "mir":
+ if args.real:
+ pass
+ # TODO: finish it
+ # return RealMirRobotManager(args)
+ else:
+ return SimulatedMirRobotManager(args)
raise NotImplementedError(
f"robot {args.robot} is not supported! run the script you ran with --help to see what's available"
)