mani-skill-nightly 2025.4.3.815__py3-none-any.whl → 2025.4.4.36__py3-none-any.whl

mani_skill/agents/controllers/base_controller.py

@@ -172,6 +172,8 @@ class BaseController:
 @dataclass
 class ControllerConfig:
     joint_names: List[str]
+    """the names of the joints to control. Note that some controller configurations might not actually let you directly control all the given joints
+    and will instead have some other implicit control (e.g. Passive controllers or mimic controllers)."""
     # NOTE(jigu): It is a class variable in this base class,
     # but you can inherit it and overwrite with an instance variable.
     controller_cls = BaseController

mani_skill/agents/controllers/pd_joint_pos.py

@@ -1,11 +1,12 @@
-from dataclasses import dataclass
-from typing import Sequence, Union
+from dataclasses import dataclass, field
+from typing import Dict, Sequence, Union
 
 import numpy as np
 import torch
 from gymnasium import spaces
 
 from mani_skill.utils import common
+from mani_skill.utils.logging_utils import logger
 from mani_skill.utils.structs.types import Array, DriveMode
 
 from .base_controller import BaseController, ControllerConfig
@@ -124,12 +125,119 @@ class PDJointPosControllerConfig(ControllerConfig):
 
 
 class PDJointPosMimicController(PDJointPosController):
+    config: "PDJointPosMimicControllerConfig"
+
+    def _initialize_joints(self):
+        super()._initialize_joints()
+        # do some sanity checks and setup the mimic controller
+
+        self.mimic_joint_indices = []
+        self.mimic_control_joint_indices = []
+        if len(self.config.mimic) == 0:
+            if len(self.config.joint_names) == 2:
+                logger.warning(
+                    f"Mimic targets dictionary is missing for controller config for {self.articulation.name}. Assuming the first joint is the control joint and the second joint is the mimic joint"
+                )
+                self.config.mimic = {
+                    self.config.joint_names[0]: {"joint": self.config.joint_names[1]}
+                }
+            else:
+                raise ValueError(
+                    "Mimic targets dictionary is missing. Please provide a mimic targets dictionary to setup mimic controllers withh more than 2 joints"
+                )
+
+        self._multiplier = torch.ones(
+            len(self.config.mimic), device=self.device
+        )  # this is parallel to self.mimic_joint_indices
+        self._offset = torch.zeros(len(self.config.mimic), device=self.device)
+
+        for i, (mimic_joint_name, mimic_data) in enumerate(self.config.mimic.items()):
+            control_joint_name = mimic_data["joint"]
+            multiplier = mimic_data.get("multiplier", 1.0)
+            offset = mimic_data.get("offset", 0.0)
+            assert (
+                self.articulation.joints_map[mimic_joint_name].active_index is not None
+            ), f"Mimic joint {mimic_joint_name} is not active, cannot be used as a mimic joint in a mimic controller"
+            assert (
+                self.articulation.joints_map[control_joint_name].active_index
+                is not None
+            ), f"Control joint {control_joint_name} is not active, cannot be used as a control joint in a mimic controller"
+
+            # NOTE (stao): we are assuming all the articulations are the exact same structure. At the moment I see very little reason to try and support training/evaluating on different embodiments
+            # simultaneously in one process
+            self.mimic_joint_indices.append(
+                torch.argwhere(
+                    self.active_joint_indices
+                    == self.articulation.joints_map[mimic_joint_name].active_index[0]
+                )
+            )
+            self.mimic_control_joint_indices.append(
+                torch.argwhere(
+                    self.active_joint_indices
+                    == self.articulation.joints_map[control_joint_name].active_index[0]
+                )
+            )
+
+            self._multiplier[i] = multiplier
+            self._offset[i] = offset
+
+        self.mimic_joint_indices = torch.tensor(
+            self.mimic_joint_indices, dtype=torch.int32, device=self.device
+        )
+        self.mimic_control_joint_indices = torch.tensor(
+            self.mimic_control_joint_indices, dtype=torch.int32, device=self.device
+        )
+        """list of control joint indices corresponding to mimic joint indices"""
+        self.control_joint_indices = torch.unique(self.mimic_control_joint_indices).to(
+            torch.int32
+        )
+        """list of all directly controlled joint indices"""
+
     def _get_joint_limits(self):
         joint_limits = super()._get_joint_limits()
-        diff = joint_limits[0:-1] - joint_limits[1:]
-        assert np.allclose(diff, 0), "Mimic joints should have the same limit"
-        return joint_limits[0:1]
+        return joint_limits[self.control_joint_indices]
 
+    def set_action(self, action: Array):
+        action = self._preprocess_action(action)
+        self._step = 0
+        self._start_qpos = self.qpos
+        if self.config.use_delta:
+            if self.config.use_target:
+                self._target_qpos[:, self.control_joint_indices] += action
+            else:
+                self._target_qpos[:, self.control_joint_indices] = (
+                    self._start_qpos[:, self.control_joint_indices] + action
+                )
+        else:
+            self._target_qpos[:, self.control_joint_indices] = action
+        self._target_qpos[:, self.mimic_joint_indices] = (
+            self._target_qpos[:, self.mimic_control_joint_indices]
+            * self._multiplier[None, :]
+            + self._offset[None, :]
+        )
+        if self.config.interpolate:
+            self._step_size = (self._target_qpos - self._start_qpos) / self._sim_steps
+        else:
+            self.set_drive_targets(self._target_qpos)
 
+
+@dataclass
 class PDJointPosMimicControllerConfig(PDJointPosControllerConfig):
+    """
+    PD Joint Position mimic controller configuration. This kind of controller is used to emuluate a mimic joint. For some simulation backends mimic joints are not
+    well simulated and/or are hard to tune and so a mimic controller can be used for better stability as well as alignment with a real robot.
+
+    The `joint_names` field is expected to be a list of all the joints, whether they are the joints to be controlled or the joints that are controlled via mimicing.
+
+    `mimic` is a dictionary that maps the mimic joint name to a dictionary of the format dict(joint: str, multiplier: float, offset: float). `joint` is the controlling joint name and is required. All given joints in
+    `joint_names` must be in the `mimic` dictionary as either a key or a value. The ones that are keys are referred to as the mimic joints and the ones that are values
+    are referred to as the control joints. Mimic joints are the ones directly controlled by the agent/user and control joints are implicitly controlled by following the mimic joints via the following equation:
+
+    q_mimic = q_controlling * multiplier + offset
+
+    By default, the multiplier is 1.0 and the offset is 0.0.
+    """
+
     controller_cls = PDJointPosMimicController
+    mimic: Dict[str, Dict[str, float]] = field(default_factory=dict)
+    """the mimic targets. Maps the actual mimic joint name to a dictionary of the format dict(joint: str, multiplier: float, offset: float)"""

mani_skill/agents/robots/__init__.py

@@ -2,6 +2,7 @@ from .allegro_hand import *
 from .anymal import ANYmalC
 from .dclaw import DClaw
 from .fetch import Fetch
+from .floating_inspire_hand import FloatingInspireHandRight
 from .floating_panda_gripper import FloatingPandaGripper
 from .floating_robotiq_2f_85_gripper import *
 from .googlerobot import *
@@ -14,6 +15,6 @@ from .unitree_go import *
 from .unitree_h1 import *
 from .ur_e import UR10e
 from .widowx import *
-from .xarm6 import *
 from .xarm import XArm7Ability
+from .xarm6 import *
 from .xmate3 import Xmate3Robotiq

mani_skill/agents/robots/floating_inspire_hand/__init__.py

@@ -0,0 +1 @@
+from .floating_inspire_hand import FloatingInspireHandRight

mani_skill/agents/robots/floating_inspire_hand/floating_inspire_hand.py

@@ -0,0 +1,176 @@
+from copy import deepcopy
+
+from mani_skill import PACKAGE_ASSET_DIR
+from mani_skill.agents.base_agent import BaseAgent
+from mani_skill.agents.controllers import *
+from mani_skill.agents.registration import register_agent
+
+
+@register_agent()
+class FloatingInspireHandRight(BaseAgent):
+    uid = "floating_inspire_hand_right"
+    urdf_path = f"{PACKAGE_ASSET_DIR}/robots/inspire_hand/urdf/end_effector/inspire_hand/inspire_hand_right_floating.urdf"
+    urdf_config = dict(
+        _materials=dict(
+            finger=dict(static_friction=2.0, dynamic_friction=2.0, restitution=0.0)
+        ),
+        link=dict(
+            right_hand_hand_base_link=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_thumb_distal=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_thumb_metacarpal=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_thumb_proximal=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_index_proximal=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_index_middle=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_middle_proximal=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_middle_middle=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_ring_proximal=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_ring_middle=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_pinky_proximal=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+            right_hand_pinky_middle=dict(
+                material="finger", patch_radius=0.1, min_patch_radius=0.1
+            ),
+        ),
+    )
+    disable_self_collisions = True
+    # you could model all of the fingers and disable certain impossible self collisions that occur
+    # but it is simpler and faster to just disable all self collisions. It is highly unlikely the hand self-collides to begin with.
+
+    root_joint_names = [
+        "root_x_axis_joint",
+        "root_y_axis_joint",
+        "root_z_axis_joint",
+        "root_x_rot_joint",
+        "root_y_rot_joint",
+        "root_z_rot_joint",
+    ]
+
+    @property
+    def _controller_configs(self):
+        float_pd_joint_pos = PDJointPosControllerConfig(
+            joint_names=self.root_joint_names,
+            lower=None,
+            upper=None,
+            stiffness=1e3,
+            damping=1e2,
+            force_limit=100,
+            normalize_action=False,
+        )
+        hand_joint_pos = PDJointPosControllerConfig(
+            joint_names=[
+                "right_hand_wrist_pitch_joint",
+                "right_hand_wrist_yaw_joint",
+                "right_hand_thumb_CMC_yaw_joint",
+            ],
+            lower=None,
+            upper=None,
+            stiffness=2e4,
+            damping=3e2,
+            force_limit=20,
+            normalize_action=False,
+        )
+        fingers_joint_pos = PDJointPosMimicControllerConfig(
+            joint_names=[
+                "right_hand_thumb_MCP_joint",
+                "right_hand_thumb_IP_joint",
+                "right_hand_index_PIP_joint",
+                "right_hand_middle_PIP_joint",
+                "right_hand_ring_PIP_joint",
+                "right_hand_pinky_PIP_joint",
+                "right_hand_thumb_CMC_pitch_joint",
+                "right_hand_index_MCP_joint",
+                "right_hand_middle_MCP_joint",
+                "right_hand_ring_MCP_joint",
+                "right_hand_pinky_MCP_joint",
+            ],
+            lower=None,
+            upper=None,
+            stiffness=2e4,
+            damping=3e2,
+            force_limit=20,
+            mimic={
+                "right_hand_thumb_MCP_joint": {
+                    "joint": "right_hand_thumb_CMC_pitch_joint",
+                    "multiplier": 1.3333333333333335,
+                    "offset": -0.08144869842640205,
+                },
+                "right_hand_thumb_IP_joint": {
+                    "joint": "right_hand_thumb_CMC_pitch_joint",
+                    "multiplier": 0.6666666666666667,
+                    "offset": -0.040724349213201026,
+                },
+                "right_hand_index_PIP_joint": {
+                    "joint": "right_hand_index_MCP_joint",
+                    "multiplier": 1.06399,
+                    "offset": -0.16734800000000002,
+                },
+                "right_hand_middle_PIP_joint": {
+                    "joint": "right_hand_middle_MCP_joint",
+                    "multiplier": 1.06399,
+                    "offset": -0.16734800000000002,
+                },
+                "right_hand_ring_PIP_joint": {
+                    "joint": "right_hand_ring_MCP_joint",
+                    "multiplier": 1.06399,
+                    "offset": -0.16734800000000002,
+                },
+                "right_hand_pinky_PIP_joint": {
+                    "joint": "right_hand_pinky_MCP_joint",
+                    "multiplier": 1.06399,
+                    "offset": -0.16734800000000002,
+                },
+            },
+            normalize_action=False,
+        )
+
+        hand_joint_delta_pos = deepcopy(hand_joint_pos)
+        hand_joint_delta_pos.use_delta = True
+        hand_joint_delta_pos.normalize_action = True
+        hand_joint_delta_pos.lower = -0.1
+        hand_joint_delta_pos.upper = 0.1
+
+        fingers_joint_delta_pos = deepcopy(fingers_joint_pos)
+        fingers_joint_delta_pos.use_delta = True
+        fingers_joint_delta_pos.normalize_action = True
+        fingers_joint_delta_pos.lower = -0.1
+        fingers_joint_delta_pos.upper = 0.1
+
+        float_pd_joint_delta_pos = deepcopy(float_pd_joint_pos)
+        float_pd_joint_delta_pos.use_delta = True
+        float_pd_joint_delta_pos.normalize_action = True
+        float_pd_joint_delta_pos.lower = -0.1
+        float_pd_joint_delta_pos.upper = 0.1
+
+        return dict(
+            pd_joint_pos=dict(
+                root=float_pd_joint_pos,
+                hand=hand_joint_pos,
+                fingers=fingers_joint_pos,
+            ),
+            pd_joint_delta_pos=dict(
+                root=float_pd_joint_delta_pos,
+                hand=hand_joint_delta_pos,
+                fingers=fingers_joint_delta_pos,
+            ),
+        )

mani_skill/agents/robots/panda/panda.py

@@ -181,6 +181,7 @@ class Panda(BaseAgent):
             stiffness=self.gripper_stiffness,
             damping=self.gripper_damping,
             force_limit=self.gripper_force_limit,
+            mimic={"panda_finger_joint2": {"joint": "panda_finger_joint1"}},
         )
 
         controller_configs = dict(