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" )