mani-skill-nightly 2025.4.5.444__py3-none-any.whl → 2025.4.5.2022__py3-none-any.whl

mani_skill/agents/base_real_agent.py

@@ -0,0 +1,202 @@
+from dataclasses import dataclass
+from typing import Dict, List, Optional
+
+import numpy as np
+
+from mani_skill.agents.base_agent import BaseAgent
+from mani_skill.envs.sapien_env import BaseEnv
+from mani_skill.sensors.base_sensor import BaseSensorConfig
+from mani_skill.utils.structs.types import Array
+
+
+class BaseRealAgent:
+    """
+    Base agent class for representing real robots, sensors, and controlling them in a real environment. This generally should be used with the :py:class:`mani_skill.envs.sim2real_env.Sim2RealEnv` class for deploying policies learned in simulation
+    to the real world.
+
+    Args:
+        sensor_configs (Dict[str, BaseSensorConfig]): the sensor configs to create the agent with.
+    """
+
+    def __init__(self, sensor_configs: Dict[str, BaseSensorConfig] = dict()):
+        self.sensor_configs = sensor_configs
+
+        self._sim_agent: BaseAgent = None
+
+        @dataclass
+        class RealRobot:
+            agent: BaseRealAgent
+
+            @property
+            def qpos(self):
+                return self.get_qpos()
+
+            def get_qpos(self):
+                return self.agent.get_qpos()
+
+            @property
+            def qvel(self):
+                return self.get_qvel()
+
+            def get_qvel(self):
+                return self.agent.get_qvel()
+
+        self.robot = RealRobot(self)
+        """
+        a reference to a fake robot/articulation used for accessing qpos/qvel values.
+        """
+
+    @property
+    def controller(self):
+        return self._sim_agent.controller
+
+    def start(self):
+        """
+        Start the agent, which include turning on the motors/robot, setting up cameras/sensors etc.
+
+        For sensors you have access to self.sensor_configs which is the requested sensor setup. For e.g. cameras these sensor configs will define the camera resolution.
+
+        For sim2real/real2sim alignment when defining real environment interfaces we instantiate the real agent with the simulation environment's sensor configs.
+        """
+        raise NotImplementedError
+
+    def stop(self):
+        """
+        Stop the agent, which include turning off the motors/robot, stopping cameras/sensors etc.
+        """
+        raise NotImplementedError
+
+    # ---------------------------------------------------------------------------- #
+    # functions for controlling the agent
+    # ---------------------------------------------------------------------------- #
+    def set_target_qpos(self, qpos: Array):
+        """
+        Set the target joint positions of the agent.
+        Args:
+            qpos: the joint positions in radians to set the agent to.
+        """
+        # equivalent to set_drive_targets in simulation
+        raise NotImplementedError
+
+    def set_target_qvel(self, qvel: Array):
+        """
+        Set the target joint velocities of the agent.
+        Args:
+            qvel: the joint velocities in radians/s to set the agent to.
+        """
+        # equivalent to set_drive_velocity_targets in simulation
+        raise NotImplementedError
+
+    def reset(self, qpos: Array):
+        """
+        Reset the agent to a given qpos. For real robots this function should move the robot at a safe and controlled speed to the given qpos and aim to reach it accurately.
+        Args:
+            qpos: the qpos in radians to reset the agent to.
+        """
+        raise NotImplementedError
+
+    # ---------------------------------------------------------------------------- #
+    # data access for e.g. joint position values, sensor observations etc.
+    # All of the def get_x() functions should return numpy arrays and be implemented
+    # ---------------------------------------------------------------------------- #
+    def capture_sensor_data(self, sensor_names: Optional[List[str]] = None):
+        """
+        Capture the sensor data asynchronously from the agent based on the given sensor names. If sensor_names is None then all sensor data should be captured. This should not return anything and should be async if possible.
+        """
+        raise NotImplementedError
+
+    def get_sensor_data(self, sensor_names: Optional[List[str]] = None):
+        """
+        Get the desired sensor observations from the agent based on the given sensor names. If sensor_names is None then all sensor data should be returned. The expected format for cameras is in line with the simulation's
+        format for cameras.
+
+        .. code-block:: python
+
+            {
+                "sensor_name": {
+                    "rgb": torch.uint8 (1, H, W, 3), # red green blue image colors
+                    "depth": torch.int16 (1, H, W, 1), # depth in millimeters
+                }
+            }
+
+        whether rgb or depth is included depends on the real camera and can be omitted if not supported or not used. Note that a batch dimension is expected in the data.
+
+        For more details see https://maniskill.readthedocs.io/en/latest/user_guide/concepts/sensors.html in order to ensure
+        the real data aligns with simulation formats.
+        """
+        raise NotImplementedError
+
+    def get_sensor_params(self, sensor_names: List[str] = None):
+        """
+        Get the parameters of the desired sensors based on the given sensor names. If sensor_names is None then all sensor parameters should be returned. The expected format for cameras is in line with the simulation's
+        format is:
+
+        .. code-block:: python
+
+            {
+                "sensor_name": {
+                    "cam2world_gl": [4, 4], # transformation from the camera frame to the world frame (OpenGL/Blender convention)
+                    "extrinsic_cv": [4, 4], # camera extrinsic (OpenCV convention)
+                        "intrinsic_cv": [3, 3], # camera intrinsic (OpenCV convention)
+                }
+            }
+
+
+        If these numbers are not needed/unavailable it is okay to leave the fields blank. Some observation processing modes may need these fields however such as point clouds in the world frame.
+        """
+        return dict()
+
+    def get_qpos(self):
+        """
+        Get the current joint positions in radians of the agent as a torch tensor. Data should have a batch dimension, the shape should be (1, N) for N joint positions.
+        """
+        raise NotImplementedError(
+            "This real agent does not an implementation for getting joint positions. If you do not need joint positions and are using the Sim2Real env setup you can override the simulation _get_obs_agent function to remove the qpos information."
+        )
+
+    def get_qvel(self):
+        """
+        Get the current joint velocities in radians/s of the agent as a torch tensor. Data should have a batch dimension, the shape should be (1, N) for N joint velocities.
+        """
+        raise NotImplementedError(
+            "This real agent does not an implementation for getting joint velocities. If you do not need joint velocities and are using the Sim2Real env setup you can override the simulation _get_obs_agent function to remove the qvel information."
+        )
+
+    @property
+    def qpos(self):
+        """
+        Get the current joint positions of the agent.
+        """
+        return self.get_qpos()
+
+    @property
+    def qvel(self):
+        """
+        Get the current joint velocities of the agent.
+        """
+        return self.get_qvel()
+
+    def get_proprioception(self):
+        """
+        Get the proprioceptive state of the agent, default is the qpos and qvel of the robot and any controller state.
+
+        Note that if qpos or qvel functions are not implemented they will return None.
+        """
+        obs = dict(qpos=self.get_qpos(), qvel=self.get_qvel())
+        controller_state = self._sim_agent.controller.get_state()
+        if len(controller_state) > 0:
+            obs.update(controller=controller_state)
+        return obs
+
+    def __getattr__(self, name):
+        """
+        Delegate attribute access to self._sim_agent if the attribute doesn't exist in self.
+        This allows accessing sim_agent properties and methods directly from the real agent. Some simulation agent include convenience functions to access e.g. end-effector poses
+        or various properties of the robot.
+        """
+        try:
+            return object.__getattribute__(self, name)
+        except AttributeError:
+            if hasattr(self, "_sim_agent") and hasattr(self._sim_agent, name):
+                return getattr(self._sim_agent, name)
+            raise AttributeError(f"{self.__class__.__name__} has no attribute '{name}'")

mani_skill/agents/controllers/base_controller.py

@@ -1,7 +1,7 @@
 from __future__ import annotations
 
 from dataclasses import dataclass
-from typing import TYPE_CHECKING, Dict, List
+from typing import TYPE_CHECKING, Dict, List, Optional
 
 import numpy as np
 import sapien
@@ -39,12 +39,17 @@ class BaseController:
     _original_single_action_space: spaces.Space
     """The unbatched, original action space without any additional processing like normalization"""
 
+    sets_target_qpos: bool
+    """Whether the controller sets the target qpos of the articulation"""
+    sets_target_qvel: bool
+    """Whether the controller sets the target qvel of the articulation"""
+
     def __init__(
         self,
         config: "ControllerConfig",
         articulation: Articulation,
         control_freq: int,
-        sim_freq: int = None,
+        sim_freq: Optional[int] = None,
         scene: ManiSkillScene = None,
     ):
         self.config = config
@@ -213,6 +218,12 @@ class DictController(BaseController):
                 self.single_action_space, n=self.scene.num_envs
             )
 
+        self.sets_target_qpos = False
+        self.sets_target_qvel = False
+        for controller in self.controllers.values():
+            self.sets_target_qpos = self.sets_target_qpos or controller.sets_target_qpos
+            self.sets_target_qvel = self.sets_target_qvel or controller.sets_target_qvel
+
     def before_simulation_step(self):
         for controller in self.controllers.values():
             controller.before_simulation_step()

mani_skill/agents/controllers/passive_controller.py

@@ -13,6 +13,8 @@ class PassiveController(BaseController):
     """
 
     config: "PassiveControllerConfig"
+    sets_target_qpos = False
+    sets_target_qvel = False
 
     def set_drive_property(self):
         n = len(self.joints)

mani_skill/agents/controllers/pd_joint_pos.py

@@ -16,6 +16,8 @@ class PDJointPosController(BaseController):
     config: "PDJointPosControllerConfig"
     _start_qpos = None
     _target_qpos = None
+    sets_target_qpos = True
+    sets_target_qvel = False
 
     def _get_joint_limits(self):
         qlimits = (

mani_skill/agents/controllers/pd_joint_pos_vel.py

@@ -11,6 +11,8 @@ from .pd_joint_pos import PDJointPosController, PDJointPosControllerConfig
 class PDJointPosVelController(PDJointPosController):
     config: "PDJointPosVelControllerConfig"
     _target_qvel = None
+    sets_target_qpos = True
+    sets_target_qvel = True
 
     def _initialize_action_space(self):
         joint_limits = self._get_joint_limits()

mani_skill/agents/controllers/pd_joint_vel.py

@@ -12,6 +12,8 @@ from .base_controller import BaseController, ControllerConfig
 # TODO (stao): add GPU support here
 class PDJointVelController(BaseController):
     config: "PDJointVelControllerConfig"
+    sets_target_qpos = False
+    sets_target_qvel = True
 
     def _initialize_action_space(self):
         n = len(self.joints)

mani_skill/agents/robots/__init__.py

@@ -7,7 +7,9 @@ from .floating_panda_gripper import FloatingPandaGripper
 from .floating_robotiq_2f_85_gripper import *
 from .googlerobot import *
 from .humanoid import Humanoid
+from .koch import *
 from .panda import *
+from .so100 import *
 from .stompy import Stompy
 from .trifingerpro import TriFingerPro
 from .unitree_g1 import *

mani_skill/agents/robots/koch/__init__.py

@@ -0,0 +1 @@
+from .koch import Koch

mani_skill/agents/robots/koch/koch.py

@@ -0,0 +1,163 @@
+import numpy as np
+import sapien
+import sapien.render
+import torch
+from transforms3d.euler import euler2quat
+
+from mani_skill import PACKAGE_ASSET_DIR
+from mani_skill.agents.base_agent import BaseAgent, Keyframe
+from mani_skill.agents.controllers import *
+from mani_skill.agents.registration import register_agent
+from mani_skill.utils import common
+from mani_skill.utils.structs.actor import Actor
+
+
+@register_agent()
+class Koch(BaseAgent):
+    uid = "koch-v1.1"
+    urdf_path = f"{PACKAGE_ASSET_DIR}/robots/koch/follower_arm_v1.1.urdf"
+    urdf_config = dict(
+        _materials=dict(
+            gripper=dict(static_friction=0.3, dynamic_friction=0.3, restitution=0.0)
+        ),
+        link=dict(
+            link_6=dict(material="gripper", patch_radius=0.1, min_patch_radius=0.1),
+            gripper=dict(material="gripper", patch_radius=0.1, min_patch_radius=0.1),
+        ),
+    )
+
+    keyframes = dict(
+        rest=Keyframe(
+            qpos=np.array([0, 2.2, 3.017, -0.25, 0, 0.6044]),
+            pose=sapien.Pose(q=euler2quat(0, 0, np.pi / 2)),
+        ),
+        elevated_turn=Keyframe(
+            qpos=np.array([0, 2.2, 2.75, -0.25, -np.pi / 2, 1.0]),
+            pose=sapien.Pose(q=euler2quat(0, 0, np.pi / 2)),
+        ),
+        zero=Keyframe(
+            qpos=np.array([0.0] * 6),
+            pose=sapien.Pose(q=euler2quat(0, 0, np.pi / 2)),
+        ),
+    )
+
+    def __init__(
+        self,
+        *args,
+        robot_chassis_colors=[0.95, 0.95, 0.95, 1],
+        robot_motor_colors=[0.05, 0.05, 0.05, 1],
+        **kwargs,
+    ):
+        self.robot_chassis_colors = robot_chassis_colors
+        self.robot_motor_colors = robot_motor_colors
+        """either a RGBA color or a list of RGBA colors for each robot in each parallel environment to then customize the color of the robot chassis"""
+        super().__init__(*args, **kwargs)
+
+    @property
+    def _controller_configs(self):
+        pd_joint_pos = PDJointPosControllerConfig(
+            [joint.name for joint in self.robot.active_joints],
+            lower=None,
+            upper=None,
+            stiffness=[1e3] * 5 + [1e2],
+            damping=[1e2] * 5 + [1e0],
+            force_limit=100,
+            normalize_action=False,
+        )
+
+        pd_joint_delta_pos = PDJointPosControllerConfig(
+            [joint.name for joint in self.robot.active_joints],
+            [-0.05, -0.05, -0.05, -0.05, -0.1, -0.05],
+            [0.05, 0.05, 0.05, 0.05, 0.1, 0.05],
+            stiffness=[123, 50, 102.68, 145, 108.37, 93.3],
+            damping=[15.85, 6, 15.34, 16, 16.31, 16.3],
+            force_limit=100,
+            use_delta=True,
+            use_target=True,
+        )
+
+        controller_configs = dict(
+            pd_joint_delta_pos=pd_joint_delta_pos,
+            pd_joint_pos=pd_joint_pos,
+        )
+        return deepcopy_dict(controller_configs)
+
+    def set_colors(self, base_color=None, motor_color=None):
+        """
+        basecolor:  RGBA length 4
+        motorcolor: RBGA length 4
+        """
+        if base_color is not None:
+            self.robot_chassis_colors = base_color
+        if motor_color is not None:
+            self.robot_motor_colors = motor_color
+        for link in self.robot.links:
+            for i, obj in enumerate(link._objs):
+                rb_comp = obj.entity.find_component_by_type(
+                    sapien.render.RenderBodyComponent
+                )
+                if rb_comp is not None:
+                    rb_comp: sapien.render.RenderBodyComponent
+                    meshes_to_modify = [
+                        x for x in rb_comp.render_shapes if "chassis" in x.name
+                    ]
+                    for mesh in meshes_to_modify:
+                        if isinstance(self.robot_chassis_colors[0], list):
+                            color = self.robot_chassis_colors[i]
+                        else:
+                            color = self.robot_chassis_colors
+                        mesh.material.base_color = color
+
+                    other_meshes_to_modify = [
+                        x for x in rb_comp.render_shapes if "motor" in x.name
+                    ]
+                    for mesh in other_meshes_to_modify:
+                        if isinstance(self.robot_motor_colors[0], list):
+                            color = self.robot_motor_colors[i]
+                        else:
+                            color = self.robot_motor_colors
+                        for part in mesh.parts:
+                            part.material.base_color = color
+
+    def _after_loading_articulation(self):
+        super()._after_loading_articulation()
+        self.set_colors()
+        self.finger1_link = self.robot.links_map["gripper"]
+        self.finger2_link = self.robot.links_map["link_6"]
+        self.tcp = self.robot.links_map["gripper_tcp"]
+        self.tcp2 = self.robot.links_map["gripper_tcp2"]
+        self.back_tcp = self.robot.links_map["back_tcp"]
+
+    def is_grasping(self, object: Actor, min_force=0.5, max_angle=110):
+        """Check if the robot is grasping an object
+
+        Args:
+            object (Actor): The object to check if the robot is grasping
+            min_force (float, optional): Minimum force before the robot is considered to be grasping the object in Newtons. Defaults to 0.5.
+            max_angle (int, optional): Maximum angle of contact to consider grasping. Defaults to 85.
+        """
+        l_contact_forces = self.scene.get_pairwise_contact_forces(
+            self.finger1_link, object
+        )
+        r_contact_forces = self.scene.get_pairwise_contact_forces(
+            self.finger2_link, object
+        )
+        lforce = torch.linalg.norm(l_contact_forces, axis=1)
+        rforce = torch.linalg.norm(r_contact_forces, axis=1)
+
+        # direction to open the gripper
+        ldirection = self.finger1_link.pose.to_transformation_matrix()[..., :3, 1]
+        rdirection = -self.finger2_link.pose.to_transformation_matrix()[..., :3, 1]
+        langle = common.compute_angle_between(ldirection, l_contact_forces)
+        rangle = common.compute_angle_between(rdirection, r_contact_forces)
+        lflag = torch.logical_and(
+            lforce >= min_force, torch.rad2deg(langle) <= max_angle
+        )
+        rflag = torch.logical_and(
+            rforce >= min_force, torch.rad2deg(rangle) <= max_angle
+        )
+        return torch.logical_and(lflag, rflag)
+
+    def is_static(self, threshold: float = 0.2):
+        qvel = self.robot.get_qvel()[..., :-1]
+        return torch.max(torch.abs(qvel), 1)[0] <= threshold

mani_skill/agents/robots/koch/koch_real.py

@@ -0,0 +1,5 @@
+from mani_skill.agents.robots.lerobot.manipulator import LeRobotRealAgent
+
+
+class KochRealAgent(LeRobotRealAgent):
+    pass

mani_skill/agents/robots/so100/__init__.py

@@ -0,0 +1 @@
+from .so_100 import SO100

mani_skill/agents/robots/so100/so_100.py

@@ -0,0 +1,118 @@
+import copy
+
+import numpy as np
+import sapien
+import sapien.render
+import torch
+from transforms3d.euler import euler2quat
+
+from mani_skill import PACKAGE_ASSET_DIR
+from mani_skill.agents.base_agent import BaseAgent, Keyframe
+from mani_skill.agents.controllers import *
+from mani_skill.agents.registration import register_agent
+from mani_skill.utils import common
+from mani_skill.utils.structs.actor import Actor
+
+
+@register_agent()
+class SO100(BaseAgent):
+    uid = "so100"
+    urdf_path = f"{PACKAGE_ASSET_DIR}/robots/so100/SO_5DOF_ARM100_8j/original.urdf"
+    urdf_config = dict(
+        _materials=dict(
+            gripper=dict(static_friction=2, dynamic_friction=2, restitution=0.0)
+        ),
+        link=dict(
+            Fixed_Jaw=dict(material="gripper", patch_radius=0.1, min_patch_radius=0.1),
+            Moving_Jaw=dict(material="gripper", patch_radius=0.1, min_patch_radius=0.1),
+        ),
+    )
+
+    keyframes = dict(
+        rest=Keyframe(
+            qpos=np.array([0, 2.2, 3.017, -0.25, 0, 0.6044]),
+            pose=sapien.Pose(q=euler2quat(0, 0, np.pi / 2)),
+        ),
+        elevated_turn=Keyframe(
+            qpos=np.array([0, 2.2, 2.75, -0.25, -np.pi / 2, 1.0]),
+            pose=sapien.Pose(q=euler2quat(0, 0, np.pi / 2)),
+        ),
+        zero=Keyframe(
+            qpos=np.array([0.0] * 6),
+            pose=sapien.Pose(q=euler2quat(0, 0, np.pi / 2)),
+        ),
+    )
+
+    @property
+    def _controller_configs(self):
+        pd_joint_pos = PDJointPosControllerConfig(
+            [joint.name for joint in self.robot.active_joints],
+            lower=None,
+            upper=None,
+            stiffness=[1e3] * 6,
+            damping=[1e2] * 6,
+            force_limit=100,
+            normalize_action=False,
+        )
+
+        pd_joint_delta_pos = PDJointPosControllerConfig(
+            [joint.name for joint in self.robot.active_joints],
+            -0.1,
+            0.1,
+            stiffness=[1e3] * 6,
+            damping=[1e2] * 6,
+            force_limit=100,
+            use_delta=True,
+            use_target=False,
+        )
+        pd_joint_target_delta_pos = copy.deepcopy(pd_joint_delta_pos)
+        pd_joint_target_delta_pos.use_target = True
+
+        controller_configs = dict(
+            pd_joint_delta_pos=pd_joint_delta_pos,
+            pd_joint_pos=pd_joint_pos,
+            pd_joint_target_delta_pos=pd_joint_target_delta_pos,
+        )
+        return deepcopy_dict(controller_configs)
+
+    def _after_loading_articulation(self):
+        super()._after_loading_articulation()
+        self.finger1_link = self.robot.links_map["Fixed_Jaw"]
+        self.finger2_link = self.robot.links_map["Moving_Jaw"]
+        self.finger1_tip = self.robot.links_map["Fixed_Jaw_tip"]
+        self.finger2_tip = self.robot.links_map["Moving_Jaw_tip"]
+
+    @property
+    def tcp_pos(self):
+        # computes the tool center point as the mid point between the the fixed and moving jaw's tips
+        return (self.finger1_tip.pose.p + self.finger2_tip.pose.p) / 2
+
+    def is_grasping(self, object: Actor, min_force=0.5, max_angle=110):
+        """Check if the robot is grasping an object
+
+        Args:
+            object (Actor): The object to check if the robot is grasping
+            min_force (float, optional): Minimum force before the robot is considered to be grasping the object in Newtons. Defaults to 0.5.
+            max_angle (int, optional): Maximum angle of contact to consider grasping. Defaults to 85.
+        """
+        l_contact_forces = self.scene.get_pairwise_contact_forces(
+            self.finger1_link, object
+        )
+        r_contact_forces = self.scene.get_pairwise_contact_forces(
+            self.finger2_link, object
+        )
+        lforce = torch.linalg.norm(l_contact_forces, axis=1)
+        rforce = torch.linalg.norm(r_contact_forces, axis=1)
+
+        # direction to open the gripper
+        ldirection = self.finger1_link.pose.to_transformation_matrix()[..., :3, 1]
+        rdirection = -self.finger2_link.pose.to_transformation_matrix()[..., :3, 1]
+        langle = common.compute_angle_between(ldirection, l_contact_forces)
+        rangle = common.compute_angle_between(rdirection, r_contact_forces)
+        lflag = torch.logical_and(
+            lforce >= min_force, torch.rad2deg(langle) <= max_angle
+        )
+        rflag = torch.logical_and(
+            rforce >= min_force, torch.rad2deg(rangle) <= max_angle
+        )
+        return torch.logical_and(lflag, rflag)

mani_skill/agents/robots/so100/so_100_real.py

@@ -0,0 +1,5 @@
+from mani_skill.agents.robots.lerobot.manipulator import LeRobotRealAgent
+
+
+class SO100RealAgent(LeRobotRealAgent):
+    pass