From 039543a0f98838186543c569fe0d4e70dc679e82 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Thu, 12 Sep 2024 15:03:24 +0200 Subject: [PATCH 01/40] Disable mujoco gui by default --- deformable_gym/envs/mujoco/base_mjenv.py | 2 +- deformable_gym/envs/mujoco/grasp_env.py | 2 +- deformable_gym/envs/mujoco/mia_grasp.py | 2 +- deformable_gym/envs/mujoco/shadow_grasp.py | 2 +- examples/mj_mia_example.py | 4 ++-- examples/mj_shadow_example.py | 2 -- 6 files changed, 6 insertions(+), 8 deletions(-) diff --git a/deformable_gym/envs/mujoco/base_mjenv.py b/deformable_gym/envs/mujoco/base_mjenv.py index 4a22784..dfe0573 100644 --- a/deformable_gym/envs/mujoco/base_mjenv.py +++ b/deformable_gym/envs/mujoco/base_mjenv.py @@ -61,7 +61,7 @@ def __init__( obj_name: str, observable_object_pos: bool = True, max_sim_time: float = 5, - gui: bool = True, + gui: bool = False, init_frame: str | None = None, ): self.scene = AssetManager().create_scene(robot_name, obj_name) diff --git a/deformable_gym/envs/mujoco/grasp_env.py b/deformable_gym/envs/mujoco/grasp_env.py index 047f162..e1b29ae 100644 --- a/deformable_gym/envs/mujoco/grasp_env.py +++ b/deformable_gym/envs/mujoco/grasp_env.py @@ -20,7 +20,7 @@ def __init__( obj_name: str, observable_object_pos: bool = True, max_sim_time: float = 5, - gui: bool = True, + gui: bool = False, init_frame: Optional[str] = None, **kwargs, ): diff --git a/deformable_gym/envs/mujoco/mia_grasp.py b/deformable_gym/envs/mujoco/mia_grasp.py index 448b373..7753eed 100644 --- a/deformable_gym/envs/mujoco/mia_grasp.py +++ b/deformable_gym/envs/mujoco/mia_grasp.py @@ -9,7 +9,7 @@ def __init__( obj_name: str, observable_object_pos: bool = True, max_sim_time: float = 1, - gui: bool = True, + gui: bool = False, init_frame: str | None = None, **kwargs, ): diff --git a/deformable_gym/envs/mujoco/shadow_grasp.py b/deformable_gym/envs/mujoco/shadow_grasp.py index 72e5c53..bcd94a0 100644 --- a/deformable_gym/envs/mujoco/shadow_grasp.py +++ b/deformable_gym/envs/mujoco/shadow_grasp.py @@ -10,7 +10,7 @@ def __init__( obj_name: str, observable_object_pos: bool = True, max_sim_time: float = 1, - gui: bool = True, + gui: bool = False, init_frame: str | None = None, **kwargs, ): diff --git a/examples/mj_mia_example.py b/examples/mj_mia_example.py index 1a5d57d..619f1b3 100644 --- a/examples/mj_mia_example.py +++ b/examples/mj_mia_example.py @@ -7,11 +7,11 @@ episode = 0 n_episodes = 3 observation, info = env.reset() - env.render() + #env.render() while episode < n_episodes: action = env.action_space.sample() observation, reward, terminate, truncated, info = env.step(action) - env.render() + #env.render() if terminate: episode += 1 print(f"Episode {episode} finished with reward {reward}") diff --git a/examples/mj_shadow_example.py b/examples/mj_shadow_example.py index d77fc5f..cc21c92 100644 --- a/examples/mj_shadow_example.py +++ b/examples/mj_shadow_example.py @@ -7,11 +7,9 @@ episode = 0 n_episodes = 3 observation, info = env.reset() - env.render() while episode < n_episodes: action = env.action_space.sample() observation, reward, terminate, truncated, info = env.step(action) - env.render() if terminate: episode += 1 print(f"Episode {episode} finished with reward {reward}") From f5079975f92ece5af6d83780839f21492ef50434 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Thu, 12 Sep 2024 15:37:31 +0200 Subject: [PATCH 02/40] Add separate requirements for mujoco and dev installations --- setup.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 85eab90..68f79ae 100644 --- a/setup.py +++ b/setup.py @@ -19,6 +19,11 @@ "gymnasium", "numpy>=1.23.5,<2.0.0", "pytransform3d", - "mujoco==3.1.6", ], + extras_require={ + "mujoco": [ + "mujoco==3.1.6", + ], + "dev": ["pytest", "pre-commit"], + }, ) From 5dd7be4dcbe479267fdc57e1a98820fc103e8c60 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Thu, 12 Sep 2024 16:00:50 +0200 Subject: [PATCH 03/40] Add flake8 as dev dependency --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 68f79ae..ca5bf3f 100644 --- a/setup.py +++ b/setup.py @@ -24,6 +24,6 @@ "mujoco": [ "mujoco==3.1.6", ], - "dev": ["pytest", "pre-commit"], + "dev": ["pytest", "pre-commit", "flake8"], }, ) From 3873659ad05da9023b0633862b617d5751096c10 Mon Sep 17 00:00:00 2001 From: Melvin Laux Date: Thu, 12 Sep 2024 16:01:37 +0200 Subject: [PATCH 04/40] Update pipline --- .github/workflows/test.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index b304c3b..0b87bb0 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -27,8 +27,7 @@ jobs: - name: Install dependencies run: | python -m pip install --upgrade pip - pip install flake8 pytest - pip install . --user + pip install .[dev,mujoco] --user - name: Lint with flake8 run: | # stop the build if there are Python syntax errors or undefined names From f0c9defe447e44daae794033c3de6d6f73d722f8 Mon Sep 17 00:00:00 2001 From: Kaixing Xiao Date: Tue, 10 Sep 2024 20:45:32 +0200 Subject: [PATCH 05/40] Change relative body of equality constraints --- assets/objects/mjcf/insole_fixed.xml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/assets/objects/mjcf/insole_fixed.xml b/assets/objects/mjcf/insole_fixed.xml index d953b60..571a4bf 100644 --- a/assets/objects/mjcf/insole_fixed.xml +++ b/assets/objects/mjcf/insole_fixed.xml @@ -5,7 +5,7 @@ - + @@ -17,13 +17,13 @@ - - - - - - - - + + + + + + + + From 452dfeda11c8bfe25b3fa9c869fce24f9c64f007 Mon Sep 17 00:00:00 2001 From: Kaixing Xiao Date: Tue, 10 Sep 2024 20:46:02 +0200 Subject: [PATCH 06/40] Remove initial position --- assets/objects/mjcf/pillow_fixed.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/assets/objects/mjcf/pillow_fixed.xml b/assets/objects/mjcf/pillow_fixed.xml index 7858dbf..2afe79d 100644 --- a/assets/objects/mjcf/pillow_fixed.xml +++ b/assets/objects/mjcf/pillow_fixed.xml @@ -5,7 +5,7 @@ - + From 702589e5eef7b75889b07b7676badf4d094af977 Mon Sep 17 00:00:00 2001 From: Kaixing Xiao Date: Tue, 10 Sep 2024 20:47:38 +0200 Subject: [PATCH 07/40] Add default position and orientation for pose --- deformable_gym/helpers/mj_utils.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/deformable_gym/helpers/mj_utils.py b/deformable_gym/helpers/mj_utils.py index c0ac2f5..fb4242e 100644 --- a/deformable_gym/helpers/mj_utils.py +++ b/deformable_gym/helpers/mj_utils.py @@ -1,4 +1,4 @@ -from dataclasses import dataclass +from dataclasses import dataclass, field from typing import List, Tuple import mujoco @@ -38,8 +38,10 @@ # -------------------------------- DATA CLASSES --------------------------------# @dataclass class Pose: - position: ArrayLike - orientation: ArrayLike + position: ArrayLike = field(default_factory=lambda: np.zeros(3)) + orientation: ArrayLike = field( + default_factory=lambda: np.array([1, 0, 0, 0]) + ) def __post_init__(self): assert len(self.position) == 3, "position should be in shape (3,)." From 07635ca5eb2b54579021e10e938bb30a71c7184e Mon Sep 17 00:00:00 2001 From: Kaixing Xiao Date: Tue, 10 Sep 2024 20:50:44 +0200 Subject: [PATCH 08/40] Refactor asset manager into modules --- deformable_gym/envs/mujoco/base_mjenv.py | 9 +- deformable_gym/helpers/asset_manager.py | 242 +++++++++++------------ 2 files changed, 116 insertions(+), 135 deletions(-) diff --git a/deformable_gym/envs/mujoco/base_mjenv.py b/deformable_gym/envs/mujoco/base_mjenv.py index dfe0573..cdeacae 100644 --- a/deformable_gym/envs/mujoco/base_mjenv.py +++ b/deformable_gym/envs/mujoco/base_mjenv.py @@ -10,8 +10,8 @@ from gymnasium import spaces from numpy.typing import ArrayLike, NDArray +from ...helpers import asset_manager as am from ...helpers import mj_utils as mju -from ...helpers.asset_manager import AssetManager from ...objects.mj_object import ObjectFactory from ...robots.mj_robot import RobotFactory @@ -64,7 +64,7 @@ def __init__( gui: bool = False, init_frame: str | None = None, ): - self.scene = AssetManager().create_scene(robot_name, obj_name) + self.scene = am.create_scene(robot_name, obj_name) self.model, self.data = mju.load_model_from_string(self.scene) self.robot = RobotFactory.create(robot_name) self.object = ObjectFactory.create(obj_name) @@ -133,7 +133,10 @@ def reset( self.model, _ = mju.load_model_from_string(self.scene) mujoco.mj_resetData(self.model, self.data) self.robot.set_pose( - self.model, self.data, self.robot.init_pose[self.object.name] + self.model, self.data, self.robot.init_pose.get(self.object.name) + ) + self.object.set_pose( + self.model, self.data, self.object.init_pose.get(self.robot.name) ) if self.gui and self.viewer is None: self.viewer = mujoco.viewer.launch_passive( diff --git a/deformable_gym/helpers/asset_manager.py b/deformable_gym/helpers/asset_manager.py index 22e2f5a..75ea8fd 100644 --- a/deformable_gym/helpers/asset_manager.py +++ b/deformable_gym/helpers/asset_manager.py @@ -7,146 +7,124 @@ import mujoco +ASSETS_DIR = Path(__file__).parents[2] / "assets" +MESH_DIR = os.path.join(ASSETS_DIR, "meshes") +ROBOT_DIR = os.path.join(ASSETS_DIR, "robots", "mjcf") +OBJECT_DIR = os.path.join(ASSETS_DIR, "objects", "mjcf") + ROBOTS = { - "shadow_hand": "shadow_hand.xml", - "mia_hand": "mia_hand.xml", + "shadow_hand": os.path.join(ROBOT_DIR, "shadow_hand.xml"), + "mia_hand": os.path.join(ROBOT_DIR, "mia_hand.xml"), + "ur5_mia": os.path.join(ROBOT_DIR, "mia_hand_on_ur5.xml"), + "ur10_mia": os.path.join(ROBOT_DIR, "mia_hand_on_ur10.xml"), + "ur10ft_mia": os.path.join(ROBOT_DIR, "mia_hand_on_ur10_ft.xml"), + "ur10e_mia": os.path.join(ROBOT_DIR, "mia_hand_on_ur10e.xml"), + "ur5_shadow": os.path.join(ROBOT_DIR, "shadow_hand_on_ur5.xml"), + "ur10_shadow": os.path.join(ROBOT_DIR, "shadow_hand_on_ur10.xml"), + "ur10e_shadow": os.path.join(ROBOT_DIR, "shadow_hand_on_ur10e.xml"), } OBJECTS = { - "insole_fixed": "insole_fixed.xml", - "pillow_fixed": "pillow_fixed.xml", + "insole_fixed": os.path.join(OBJECT_DIR, "insole_fixed.xml"), + "pillow_fixed": os.path.join(OBJECT_DIR, "pillow_fixed.xml"), } -class AssetManager: +def load_asset(name: str) -> mujoco.MjModel: + """ + Loads a asset (robot or object mujoco Model) based on its name. + + Args: + name (str): The name of the robot or object to be loaded. Must be a key in either + ROBOTS or OBJECTS. + + Returns: + mujoco.MjModel: The loaded MuJoCo model. + + Raises: + AssertionError: If the specified name is not found in either ROBOTS or OBJECTS. """ - The AssetManager class manages the loading and combining of MuJoCo models (robots and objects) - into a single simulation scene. It provides methods to load individual robot or object models, - create complex scenes, and save the resulting XML configuration. + assert ( + name in ROBOTS or name in OBJECTS + ), f"Model {name} not found.\n available: {list(ROBOTS.keys()) + list(OBJECTS.keys())}" + model_path = ROBOTS.get(name, OBJECTS.get(name)) + model = mujoco.MjModel.from_xml_path(model_path) + return model + + +def create_scene(robot_name: str, obj_name: str) -> str: """ + Creates an MJCF string representing a MuJoCo scene that includes a robot and an object. + + Args: + robot_name (str): The name of the robot to include in the scene. Must be a key in ROBOTS. + obj_name (str): The name of the object to include in the scene. Must be a key in OBJECTS. - def __init__(self) -> None: - self.assets_dir = Path(__file__).parents[2] / "assets" - self.meshdir = os.path.join(self.assets_dir, "meshes") - self.robot_dir = os.path.join(self.assets_dir, "robots", "mjcf") - self.object_dir = os.path.join(self.assets_dir, "objects", "mjcf") - self.robots = { - k: os.path.join(self.robot_dir, v) for k, v in ROBOTS.items() - } - self.objects = { - k: os.path.join(self.object_dir, v) for k, v in OBJECTS.items() - } - - def load_asset(self, name: str) -> mujoco.MjModel: - """ - Loads a asset (robot or object mujoco Model) based on its name. - - Args: - name (str): The name of the robot or object to be loaded. Must be a key in either - self.robots or self.objects. - - Returns: - mujoco.MjModel: The loaded MuJoCo model. - - Raises: - AssertionError: If the specified name is not found in either self.robots or self.objects. - """ - assert ( - name in self.robots or name in self.objects - ), f"Model {name} not found.\n available: {list(self.robots.keys()) + list(self.objects.keys())}" - model_path = self.robots.get(name, self.objects.get(name)) - model = mujoco.MjModel.from_xml_path(model_path) - return model - - def create_scene(self, robot_name: str, obj_name: str) -> str: - """ - Creates an MJCF string representing a MuJoCo scene that includes a robot and an object. - - Args: - robot_name (str): The name of the robot to include in the scene. Must be a key in self.robots. - obj_name (str): The name of the object to include in the scene. Must be a key in self.objects. - - Returns: - str: A MJCF string representing the combined MuJoCo scene. - - Raises: - AssertionError: If the specified robot_name is not found in self.robots. - AssertionError: If the specified obj_name is not found in self.objects. - """ - assert ( - robot_name in self.robots - ), f"Robot {robot_name} not found.\n available: {list(self.robots.keys())}" - assert ( - obj_name in self.objects - ), f"Object {obj_name} not found.\n available: {list(self.objects.keys())}" - - floor_path = os.path.join(self.object_dir, "floor.xml") - robot_path = self.robots.get(robot_name) - obj_path = self.objects.get(obj_name) - - scene = self.include_mjcf( - obj_path, [robot_path, floor_path], meshdir=self.meshdir - ) - return scene - - # def create_scene(self, robot_name: str, *obj_name: str) -> str: - # assert ( - # robot_name in self.robots - # ), f"Robot {robot_name} not found.\n available: {list(self.robots.keys())}" - - # robot_path = self._get_full_path(self.robots[robot_name]) - - # for obj in obj_name: - # assert ( - # obj in self.objects - # ), f"Object {obj} not found.\n available: {list(self.objects.keys())}" - # obj_path = [self._get_full_path(self.objects[obj]) for obj in obj_name] - # scene = self.include_mjcf(robot_path, obj_path, meshdir=self.meshdir) - # return scene - - @staticmethod - def include_mjcf( - base_path: str, - include_path: str | Sequence[str], - *, - meshdir: str | None = None, - ) -> str: - """ - Generates an XML string for a MuJoCo scene by including additional MJCF files within a base MJCF file. - - Args: - base_path (str): The file path to the base MJCF file. - include_path (Union[str, Sequence[str]]): A string or list of strings representing file paths - to MJCF files to be included in the base file. - meshdir (Optional[str]): A string representing the path to the directory containing mesh files. - If provided, this path is added to the meshdir attribute of the compiler - element in the MJCF XML. - - Returns: - str: An XML string representing the combined MJCF file. - """ - tree = ET.parse(base_path) - root = tree.getroot() - if isinstance(include_path, list) or isinstance(include_path, tuple): - for path in include_path: - new_elem = ET.Element("include", {"file": path}) - root.insert(0, new_elem) + Returns: + str: A MJCF string representing the combined MuJoCo scene. + + Raises: + AssertionError: If the specified robot_name is not found in ROBOTS. + AssertionError: If the specified obj_name is not found in OBJECTS. + """ + assert ( + robot_name in ROBOTS + ), f"Robot {robot_name} not found.\n available: {list(ROBOTS.keys())}" + assert ( + obj_name in OBJECTS + ), f"Object {obj_name} not found.\n available: {list(OBJECTS.keys())}" + + floor_path = os.path.join(OBJECT_DIR, "floor.xml") + robot_path = ROBOTS.get(robot_name) + obj_path = OBJECTS.get(obj_name) + + scene = include_mjcf(obj_path, [robot_path, floor_path], meshdir=MESH_DIR) + return scene + + +def include_mjcf( + base_path: str, + include_path: str | Sequence[str], + *, + meshdir: str | None = None, +) -> str: + """ + Generates an XML string for a MuJoCo scene by including additional MJCF files within a base MJCF file. + + Args: + base_path (str): The file path to the base MJCF file. + include_path (Union[str, Sequence[str]]): A string or list of strings representing file paths + to MJCF files to be included in the base file. + meshdir (Optional[str]): A string representing the path to the directory containing mesh files. + If provided, this path is added to the meshdir attribute of the compiler + element in the MJCF XML. + + Returns: + str: An XML string representing the combined MJCF file. + """ + tree = ET.parse(base_path) + root = tree.getroot() + if isinstance(include_path, list) or isinstance(include_path, tuple): + for path in include_path: + new_elem = ET.Element("include", {"file": path}) + root.insert(0, new_elem) + else: + new_elem = ET.Element("include", {"file": include_path}) + root.insert(0, new_elem) + if meshdir is not None: + if meshdir[-1] != "/": + meshdir += "/" + elems = root.findall("compiler") + if len(elems) != 0: + for elem in elems: + elem.set("meshdir", meshdir) else: - new_elem = ET.Element("include", {"file": include_path}) + new_elem = ET.Element("compiler", {"meshdir": meshdir}) root.insert(0, new_elem) - if meshdir is not None: - if meshdir[-1] != "/": - meshdir += "/" - elems = root.findall("compiler") - if len(elems) != 0: - for elem in elems: - elem.set("meshdir", meshdir) - else: - new_elem = ET.Element("compiler", {"meshdir": meshdir}) - root.insert(0, new_elem) - new_mjcf = ET.tostring(root, encoding="utf-8").decode("utf-8") - - return new_mjcf - - def save_mjcf_string(self, mjcf: str, path: str) -> None: - with open(path, "w") as f: - f.write(mjcf) + new_mjcf = ET.tostring(root, encoding="utf-8").decode("utf-8") + + return new_mjcf + + +def save_mjcf_string(mjcf: str, path: str) -> None: + with open(path, "w") as f: + f.write(mjcf) From 299e1cb4f2e16af97fec8265f48fe84904e3eedb Mon Sep 17 00:00:00 2001 From: Kaixing Xiao Date: Tue, 10 Sep 2024 20:51:27 +0200 Subject: [PATCH 09/40] Add new robots --- deformable_gym/robots/mj_robot.py | 142 ++++++++++++++++++++++++++---- 1 file changed, 123 insertions(+), 19 deletions(-) diff --git a/deformable_gym/robots/mj_robot.py b/deformable_gym/robots/mj_robot.py index 9a04af5..1c52a6e 100644 --- a/deformable_gym/robots/mj_robot.py +++ b/deformable_gym/robots/mj_robot.py @@ -5,8 +5,8 @@ import numpy as np from numpy.typing import ArrayLike, NDArray +from ..helpers import asset_manager as am from ..helpers import mj_utils as mju -from ..helpers.asset_manager import AssetManager from ..helpers.mj_utils import Pose @@ -30,17 +30,32 @@ class MJRobot(ABC): actuators (List[str]): A list of actuator names for the robot. """ - init_pos = {} + init_pose = {} def __init__(self, name: str) -> None: self.name = name - self.model = AssetManager().load_asset(self.name) - self.n_qpos = self.model.nq - self.dof = self.model.nv - self.n_actuator = self.model.nu - self.joint_range = self.model.jnt_range.copy() - self.ctrl_range = self.model.actuator_ctrlrange.copy() + self.model = am.load_asset(self.name) + + @property + def n_qpos(self) -> int: + return self.model.nq + + @property + def dof(self) -> int: + return self.model.nv + + @property + def n_actuator(self) -> int: + return self.model.nu + + @property + def joint_range(self) -> NDArray: + return self.model.jnt_range.copy() + + @property + def ctrl_range(self) -> NDArray: + return self.model.actuator_ctrlrange.copy() @property def joints(self) -> List[str]: @@ -82,7 +97,7 @@ def set_pose( self, model: mujoco.MjModel, data: mujoco.MjData, - pose: Pose, + pose: Pose | None, ) -> None: """ Set the pose (position and orientation) of the robot's base. @@ -95,10 +110,10 @@ def set_pose( data (mujoco.MjData): The MuJoCo data object containing the current simulation state. pose (Pose): A Pose object containing the desired position and orientation for the robot's base. """ - - model.body(self.name).pos[:] = pose.position - model.body(self.name).quat[:] = pose.orientation - mujoco.mj_forward(model, data) + if pose is not None: + model.body(self.name).pos[:] = pose.position + model.body(self.name).quat[:] = pose.orientation + mujoco.mj_forward(model, data) def set_ctrl( self, model: mujoco.MjModel, data: mujoco.MjData, ctrl: ArrayLike @@ -131,8 +146,38 @@ class ShadowHand(MJRobot): "insole_fixed": Pose([-0.35, 0.00, 0.49], [0, np.pi / 2, 0]), } - def __init__(self) -> None: - super().__init__("shadow_hand") + def __init__(self, name: str = "shadow_hand") -> None: + super().__init__(name) + + +class Ur5Shadow(ShadowHand): + init_pose = { + "insole_fixed": Pose([0.0, 0.0, 0.0]), + "pillow_fixed": Pose([0.0, 0.0, 0.0]), + } + + def __init__(self, name: str = "ur5_shadow"): + super().__init__(name) + + +class Ur10Shadow(ShadowHand): + init_pose = { + "insole_fixed": Pose([0.0, 0.0, 0.0]), + "pillow_fixed": Pose([0.0, 0.0, 0.0]), + } + + def __init__(self, name: str = "ur10_shadow"): + super().__init__(name) + + +class Ur10eShadow(ShadowHand): + init_pose = { + "insole_fixed": Pose([0.0, 0.0, 0.0]), + "pillow_fixed": Pose([0.0, 0.0, 0.0]), + } + + def __init__(self, name: str = "ur10e_shadow"): + super().__init__(name) class MiaHand(MJRobot): @@ -147,10 +192,15 @@ class MiaHand(MJRobot): "insole_fixed": Pose([-0.1, 0, 0.49], [0, np.pi, np.pi / 2]), } - def __init__(self) -> None: - super().__init__("mia_hand") - self.n_actuator = self.model.nu - 2 + def __init__(self, name: str = "mia_hand") -> None: + super().__init__(name) + + @property + def n_actuator(self): + return self.model.nu - 2 + @property + def ctrl_range(self): mrl_range = self.model.actuator("j_middle_fle_A").ctrlrange ctrl_range_wo_mrl = np.array( [ @@ -159,7 +209,7 @@ def __init__(self) -> None: if name not in self.mrl_actuators ] ) - self.ctrl_range = np.vstack((ctrl_range_wo_mrl, mrl_range)) + return np.vstack((ctrl_range_wo_mrl, mrl_range)) @property def actuators(self): @@ -187,13 +237,67 @@ def set_ctrl( mju.set_actuator_ctrl(model, data, act, ctrl[i]) +class Ur5Mia(MiaHand): + init_pose = { + "insole_fixed": Pose([0.0, 0.0, 0.0]), + "pillow_fixed": Pose([0.0, 0.0, 0.0]), + } + + def __init__(self, name: str = "ur5_mia"): + super().__init__(name) + + +class Ur10Mia(MiaHand): + init_pose = { + "insole_fixed": Pose([0.0, 0.0, 0.0]), + "pillow_fixed": Pose([0.0, 0.0, 0.0]), + } + + def __init__(self, name: str = "ur10_mia"): + super().__init__(name) + + +class Ur10ftMia(MiaHand): + init_pose = { + "insole_fixed": Pose([0.0, 0.0, 0.0]), + "pillow_fixed": Pose([0.0, 0.0, 0.0]), + } + + def __init__(self, name: str = "ur10ft_mia"): + super().__init__(name) + + +class Ur10eMia(MiaHand): + init_pose = { + "insole_fixed": Pose([0.0, 0.0, 0.0]), + "pillow_fixed": Pose([0.0, 0.0, 0.0]), + } + + def __init__(self, name: str = "ur10e_mia"): + super().__init__(name) + + class RobotFactory: @staticmethod def create(name: str) -> MJRobot: if name == "shadow_hand": return ShadowHand() + elif name == "ur5_shadow": + return Ur5Shadow() + elif name == "ur10_shadow": + return Ur10Shadow() + elif name == "ur10e_shadow": + return Ur10eShadow() elif name == "mia_hand": return MiaHand() + elif name == "ur5_mia": + return Ur5Mia() + elif name == "ur10_mia": + return Ur10Mia() + elif name == "ur10ft_mia": + return Ur10ftMia() + elif name == "ur10e_mia": + return Ur10eMia() else: raise ValueError(f"Robot {name} not found.") From 19a85e9b9f63b5ab97d2f47cde2b3090607734ee Mon Sep 17 00:00:00 2001 From: Kaixing Xiao Date: Tue, 17 Sep 2024 12:40:03 +0200 Subject: [PATCH 10/40] Support mocap body control --- assets/robots/mjcf/include/asset/mia_hand.xml | 19 ++ .../robots/mjcf/include/asset/shadow_hand.xml | 20 ++ assets/robots/mjcf/include/asset/ur10.xml | 11 + assets/robots/mjcf/include/asset/ur10e.xml | 11 + assets/robots/mjcf/include/asset/ur10ft.xml | 13 + assets/robots/mjcf/include/asset/ur5.xml | 11 + .../robots/mjcf/include/default/mia_hand.xml | 16 ++ .../mjcf/include/default/shadow_hand.xml | 66 +++++ assets/robots/mjcf/include/default/ur10.xml | 21 ++ assets/robots/mjcf/include/default/ur10e.xml | 21 ++ assets/robots/mjcf/include/default/ur10ft.xml | 21 ++ assets/robots/mjcf/include/default/ur5.xml | 21 ++ .../mjcf/include/hand/mia_hand_component.xml | 60 +++++ .../mjcf/include/hand/mia_hand_misc.xml | 9 + .../include/hand/shadow_hand_component.xml | 161 +++++++++++ .../mjcf/include/hand/shadow_hand_misc.xml | 44 +++ assets/robots/mjcf/mia_hand.xml | 90 +++---- assets/robots/mjcf/mia_hand_on_ur10.xml | 193 +++++--------- assets/robots/mjcf/mia_hand_on_ur10_ft.xml | 207 ++++++-------- assets/robots/mjcf/mia_hand_on_ur10e.xml | 195 +++++--------- assets/robots/mjcf/mia_hand_on_ur5.xml | 191 +++++-------- assets/robots/mjcf/mocap.xml | 7 + assets/robots/mjcf/shadow_hand.xml | 252 +++++++++--------- assets/robots/mjcf/shadow_hand_on_ur10.xml | 68 +++++ assets/robots/mjcf/shadow_hand_on_ur10e.xml | 68 +++++ assets/robots/mjcf/shadow_hand_on_ur5.xml | 68 +++++ assets/robots/mjcf/ur10.xml | 57 ++++ assets/robots/mjcf/ur10e.xml | 57 ++++ assets/robots/mjcf/ur10ft.xml | 61 +++++ assets/robots/mjcf/ur5.xml | 57 ++++ 30 files changed, 1411 insertions(+), 685 deletions(-) create mode 100644 assets/robots/mjcf/include/asset/mia_hand.xml create mode 100644 assets/robots/mjcf/include/asset/shadow_hand.xml create mode 100644 assets/robots/mjcf/include/asset/ur10.xml create mode 100644 assets/robots/mjcf/include/asset/ur10e.xml create mode 100644 assets/robots/mjcf/include/asset/ur10ft.xml create mode 100644 assets/robots/mjcf/include/asset/ur5.xml create mode 100644 assets/robots/mjcf/include/default/mia_hand.xml create mode 100644 assets/robots/mjcf/include/default/shadow_hand.xml create mode 100644 assets/robots/mjcf/include/default/ur10.xml create mode 100644 assets/robots/mjcf/include/default/ur10e.xml create mode 100644 assets/robots/mjcf/include/default/ur10ft.xml create mode 100644 assets/robots/mjcf/include/default/ur5.xml create mode 100644 assets/robots/mjcf/include/hand/mia_hand_component.xml create mode 100644 assets/robots/mjcf/include/hand/mia_hand_misc.xml create mode 100644 assets/robots/mjcf/include/hand/shadow_hand_component.xml create mode 100644 assets/robots/mjcf/include/hand/shadow_hand_misc.xml create mode 100644 assets/robots/mjcf/mocap.xml create mode 100644 assets/robots/mjcf/shadow_hand_on_ur10.xml create mode 100644 assets/robots/mjcf/shadow_hand_on_ur10e.xml create mode 100644 assets/robots/mjcf/shadow_hand_on_ur5.xml create mode 100644 assets/robots/mjcf/ur10.xml create mode 100644 assets/robots/mjcf/ur10e.xml create mode 100644 assets/robots/mjcf/ur10ft.xml create mode 100644 assets/robots/mjcf/ur5.xml diff --git a/assets/robots/mjcf/include/asset/mia_hand.xml b/assets/robots/mjcf/include/asset/mia_hand.xml new file mode 100644 index 0000000..c8f6d8a --- /dev/null +++ b/assets/robots/mjcf/include/asset/mia_hand.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/asset/shadow_hand.xml b/assets/robots/mjcf/include/asset/shadow_hand.xml new file mode 100644 index 0000000..a9d1d32 --- /dev/null +++ b/assets/robots/mjcf/include/asset/shadow_hand.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/asset/ur10.xml b/assets/robots/mjcf/include/asset/ur10.xml new file mode 100644 index 0000000..bd70a29 --- /dev/null +++ b/assets/robots/mjcf/include/asset/ur10.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/asset/ur10e.xml b/assets/robots/mjcf/include/asset/ur10e.xml new file mode 100644 index 0000000..aae1808 --- /dev/null +++ b/assets/robots/mjcf/include/asset/ur10e.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/asset/ur10ft.xml b/assets/robots/mjcf/include/asset/ur10ft.xml new file mode 100644 index 0000000..28c0506 --- /dev/null +++ b/assets/robots/mjcf/include/asset/ur10ft.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/asset/ur5.xml b/assets/robots/mjcf/include/asset/ur5.xml new file mode 100644 index 0000000..6a67171 --- /dev/null +++ b/assets/robots/mjcf/include/asset/ur5.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/default/mia_hand.xml b/assets/robots/mjcf/include/default/mia_hand.xml new file mode 100644 index 0000000..40d292c --- /dev/null +++ b/assets/robots/mjcf/include/default/mia_hand.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/default/shadow_hand.xml b/assets/robots/mjcf/include/default/shadow_hand.xml new file mode 100644 index 0000000..1dedc98 --- /dev/null +++ b/assets/robots/mjcf/include/default/shadow_hand.xml @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/default/ur10.xml b/assets/robots/mjcf/include/default/ur10.xml new file mode 100644 index 0000000..23d3165 --- /dev/null +++ b/assets/robots/mjcf/include/default/ur10.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/default/ur10e.xml b/assets/robots/mjcf/include/default/ur10e.xml new file mode 100644 index 0000000..4367a17 --- /dev/null +++ b/assets/robots/mjcf/include/default/ur10e.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/default/ur10ft.xml b/assets/robots/mjcf/include/default/ur10ft.xml new file mode 100644 index 0000000..36dc77b --- /dev/null +++ b/assets/robots/mjcf/include/default/ur10ft.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/default/ur5.xml b/assets/robots/mjcf/include/default/ur5.xml new file mode 100644 index 0000000..68326d5 --- /dev/null +++ b/assets/robots/mjcf/include/default/ur5.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/hand/mia_hand_component.xml b/assets/robots/mjcf/include/hand/mia_hand_component.xml new file mode 100644 index 0000000..b1303ce --- /dev/null +++ b/assets/robots/mjcf/include/hand/mia_hand_component.xml @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/hand/mia_hand_misc.xml b/assets/robots/mjcf/include/hand/mia_hand_misc.xml new file mode 100644 index 0000000..90b6176 --- /dev/null +++ b/assets/robots/mjcf/include/hand/mia_hand_misc.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/assets/robots/mjcf/include/hand/shadow_hand_component.xml b/assets/robots/mjcf/include/hand/shadow_hand_component.xml new file mode 100644 index 0000000..5b32c32 --- /dev/null +++ b/assets/robots/mjcf/include/hand/shadow_hand_component.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/include/hand/shadow_hand_misc.xml b/assets/robots/mjcf/include/hand/shadow_hand_misc.xml new file mode 100644 index 0000000..4e300a9 --- /dev/null +++ b/assets/robots/mjcf/include/hand/shadow_hand_misc.xml @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/mia_hand.xml b/assets/robots/mjcf/mia_hand.xml index 3ecc694..1c2a020 100644 --- a/assets/robots/mjcf/mia_hand.xml +++ b/assets/robots/mjcf/mia_hand.xml @@ -1,65 +1,49 @@ + + + - - + + - - + + - + - + - - - - - + + + + + - - + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + @@ -119,17 +103,15 @@ + + + - - - - - - - - - - - + + + + + + diff --git a/assets/robots/mjcf/mia_hand_on_ur10.xml b/assets/robots/mjcf/mia_hand_on_ur10.xml index e94206f..515f311 100644 --- a/assets/robots/mjcf/mia_hand_on_ur10.xml +++ b/assets/robots/mjcf/mia_hand_on_ur10.xml @@ -1,127 +1,70 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/mia_hand_on_ur10_ft.xml b/assets/robots/mjcf/mia_hand_on_ur10_ft.xml index 3d34afe..0674667 100644 --- a/assets/robots/mjcf/mia_hand_on_ur10_ft.xml +++ b/assets/robots/mjcf/mia_hand_on_ur10_ft.xml @@ -1,133 +1,76 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/mia_hand_on_ur10e.xml b/assets/robots/mjcf/mia_hand_on_ur10e.xml index 01a0800..df1dfa2 100644 --- a/assets/robots/mjcf/mia_hand_on_ur10e.xml +++ b/assets/robots/mjcf/mia_hand_on_ur10e.xml @@ -1,127 +1,72 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/mia_hand_on_ur5.xml b/assets/robots/mjcf/mia_hand_on_ur5.xml index a45bbe6..0f5d136 100644 --- a/assets/robots/mjcf/mia_hand_on_ur5.xml +++ b/assets/robots/mjcf/mia_hand_on_ur5.xml @@ -1,127 +1,68 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/robots/mjcf/mocap.xml b/assets/robots/mjcf/mocap.xml new file mode 100644 index 0000000..d0ec6c4 --- /dev/null +++ b/assets/robots/mjcf/mocap.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/assets/robots/mjcf/shadow_hand.xml b/assets/robots/mjcf/shadow_hand.xml index 5a5448d..3eda9f2 100644 --- a/assets/robots/mjcf/shadow_hand.xml +++ b/assets/robots/mjcf/shadow_hand.xml @@ -1,9 +1,9 @@ - +