Merge pull request #208 from utn-mi/tilburg_grasp

Tilburg Hand Sim & Real integration

Author: juelg

examples/simple_tilburg_test.py

@@ -0,0 +1,89 @@
+from time import sleep
+
+import numpy as np
+from rcs._core.common import GraspType
+from rcs.hand.tilburg_hand import THConfig, TilburgHand
+
+
+def sim_real_index_flip(q: np.ndarray):
+    if len(q) == 18:
+        # This is the qpos for the real robot
+        q = q[:16]
+    assert len(q) == 16, "Expected 16 joint positions for the Tilburg hand"
+    q2 = q.copy()
+    for i in range(4):
+        for j in range(4):
+            q2[i * 4 + j] = q[i * 4 + (3 - j)]
+    return q2
+
+
+def sim_real_name_flip(names):
+    names2 = names.copy()
+    for i in range(4):
+        for j in range(4):
+            names2[i * 4 + j] = names[i * 4 + (3 - j)]
+    return names2
+
+
+def pad_qpos(q: np.ndarray):
+    if len(q) == 16:
+        # This is the qpos for the real robot
+        q = np.concatenate((q, np.zeros(2)))
+    assert len(q) == 18, "Expected 18 joint positions for the Tilburg hand"
+    return q
+
+
+if __name__ == "__main__":
+    config = {
+        "motor_calib_min_range_deg": [-5, 0, 0, 0, -5, -5, 0, -25, -5, -5, 0, -25, -5, -5, 0, -25, 0, 0],
+        "motor_calib_max_range_deg": [95, 90, 100, 90, 95, 95, 95, 25, 95, 95, 95, 25, 95, 95, 95, 25, 1, 1],
+    }
+    power_grasp_values = [
+        0.5,
+        0.5,
+        0.5,
+        1.4,  # THUMB_(IP, MCP, ABD, CMC)
+        0.5,
+        0.5,
+        1.0,
+        0.7,  # INDEX_(DIP, PIP, MCP, ABD)
+        0.5,
+        0.5,
+        1.0,
+        0.3,
+        0.5,
+        0.5,
+        1.0,
+        0.0,
+        0,
+        0,
+    ]
+    rads = []
+    min_joints_radians = [np.deg2rad(angle) for angle in config["motor_calib_min_range_deg"]]
+    max_joints_radians = [np.deg2rad(angle) for angle in config["motor_calib_max_range_deg"]]
+
+    for i in range(len(config["motor_calib_min_range_deg"])):
+        joint_angle = (
+            power_grasp_values[i] * (config["motor_calib_max_range_deg"][i] - config["motor_calib_min_range_deg"][i])
+            + config["motor_calib_min_range_deg"][i]
+        )
+        rads.append(np.deg2rad(joint_angle))
+
+        print(f"Motor {i}: {joint_angle:.2f} degrees")
+    print("power_grasp_radians=[", ", ".join(f"{rad:.2f}" for rad in rads), "]")
+    print("min_joints_radians=[", ", ".join(f"{rad:.2f}" for rad in min_joints_radians), "]")
+    print("max_joints_radians=[", ", ".join(f"{rad:.2f}" for rad in max_joints_radians), "]")
+    hand_cfg = THConfig(
+        calibration_file="/home/sbien/Documents/Development/RCS/robot-control-stack/python/rcs/hand/calibration_og.json",
+        grasp_percentage=1,
+        hand_orientation="right",
+    )
+    hand = TilburgHand(cfg=hand_cfg, verbose=True)
+    hand.set_grasp_type(GraspType.POWER_GRASP)
+    hand.grasp()
+    sleep(2)
+    hand.open()
+    sleep(5)
+    hand.reset()
+    hand.close()
+    hand.disconnect()

examples/tilburg_grasp.py

@@ -0,0 +1,42 @@
+import logging
+
+import numpy as np
+from rcs._core.common import RobotPlatform
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs.envs.creators import SimEnvCreator
+from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+ROBOT_INSTANCE = RobotPlatform.SIMULATION
+
+
+def main():
+    mjcf = "/home/sbien/Documents/Development/RCS/models/scenes/fr3_tilburg_empty_world/scene.xml"
+    if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
+        env_rel = SimEnvCreator()(
+            control_mode=ControlMode.JOINTS,
+            collision_guard=False,
+            robot_cfg=default_sim_robot_cfg(),
+            hand_cfg=default_sim_tilburg_hand_cfg(),
+            # cameras=default_mujoco_cameraset_cfg(),
+            mjcf=mjcf,
+            max_relative_movement=np.deg2rad(5),
+            relative_to=RelativeTo.LAST_STEP,
+        )
+        env_rel.get_wrapper_attr("sim").open_gui()
+
+        for _ in range(10):
+            obs, info = env_rel.reset()
+            for _ in range(10):
+                # sample random relative action and execute it
+                act = env_rel.action_space.sample()
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+
+
+if __name__ == "__main__":
+    main()

include/rcs/Robot.h

@@ -103,6 +103,17 @@ struct GripperState {
   virtual ~GripperState(){};
 };
 
+enum GraspType {  POWER_GRASP = 0, 
+                  PRECISION_GRASP,
+                  LATERAL_GRASP,
+                  TRIPOD_GRASP  };
+struct HandConfig {
+  virtual ~HandConfig(){};
+};
+struct HandState {
+  virtual ~HandState(){};
+};
+
 class Robot {
  public:
   virtual ~Robot(){};
@@ -170,6 +181,36 @@ class Gripper {
   virtual void reset() = 0;
 };
 
+class Hand {
+ public:
+  virtual ~Hand(){};
+  // TODO: Add low-level control interface for the hand with internal state updates
+  // Also add an implementation specific set_parameters function that takes
+  // a deduced config type
+  // bool set_parameters(const GConfig& cfg);
+
+  virtual HandConfig* get_parameters() = 0;
+  virtual HandState* get_state() = 0;
+
+  // set width of the hand, 0 is closed, 1 is open
+  virtual void set_normalized_joint_poses(const VectorXd& q) = 0;
+  virtual VectorXd get_normalized_joint_poses() = 0;
+
+  virtual bool is_grasped() = 0;
+
+  // close hand with force, return true if the object is grasped successfully
+  virtual void grasp() = 0;
+
+  // open hand
+  virtual void open() = 0;
+
+  // close hand without applying force
+  virtual void shut() = 0;
+
+  // puts the hand to max position
+  virtual void reset() = 0;
+};
+
 }  // namespace common
 }  // namespace rcs
 #endif  // RCS_ROBOT_H

python/rcs/_core/common.pyi

@@ -13,14 +13,21 @@ __all__ = [
     "BaseCameraConfig",
     "FR3",
     "FrankaHandTCPOffset",
+    "GraspType",
     "Gripper",
     "GripperConfig",
     "GripperState",
     "HARDWARE",
+    "Hand",
+    "HandConfig",
+    "HandState",
     "IK",
     "IdentityRotMatrix",
     "IdentityRotQuatVec",
     "IdentityTranslation",
+    "LATERAL_GRASP",
+    "POWER_GRASP",
+    "PRECISION_GRASP",
     "Pin",
     "Pose",
     "RL",
@@ -33,6 +40,7 @@ __all__ = [
     "RobotType",
     "SIMULATION",
     "SO101",
+    "TRIPOD_GRASP",
     "UR5e",
     "XArm7",
     "robots_meta_config",
@@ -47,6 +55,41 @@ class BaseCameraConfig:
     resolution_width: int
     def __init__(self, identifier: str, frame_rate: int, resolution_width: int, resolution_height: int) -> None: ...
 
+class GraspType:
+    """
+    Members:
+
+      POWER_GRASP
+
+      PRECISION_GRASP
+
+      LATERAL_GRASP
+
+      TRIPOD_GRASP
+    """
+
+    LATERAL_GRASP: typing.ClassVar[GraspType]  # value = <GraspType.LATERAL_GRASP: 2>
+    POWER_GRASP: typing.ClassVar[GraspType]  # value = <GraspType.POWER_GRASP: 0>
+    PRECISION_GRASP: typing.ClassVar[GraspType]  # value = <GraspType.PRECISION_GRASP: 1>
+    TRIPOD_GRASP: typing.ClassVar[GraspType]  # value = <GraspType.TRIPOD_GRASP: 3>
+    __members__: typing.ClassVar[
+        dict[str, GraspType]
+    ]  # value = {'POWER_GRASP': <GraspType.POWER_GRASP: 0>, 'PRECISION_GRASP': <GraspType.PRECISION_GRASP: 1>, 'LATERAL_GRASP': <GraspType.LATERAL_GRASP: 2>, 'TRIPOD_GRASP': <GraspType.TRIPOD_GRASP: 3>}
+    def __eq__(self, other: typing.Any) -> bool: ...
+    def __getstate__(self) -> int: ...
+    def __hash__(self) -> int: ...
+    def __index__(self) -> int: ...
+    def __init__(self, value: int) -> None: ...
+    def __int__(self) -> int: ...
+    def __ne__(self, other: typing.Any) -> bool: ...
+    def __repr__(self) -> str: ...
+    def __setstate__(self, state: int) -> None: ...
+    def __str__(self) -> str: ...
+    @property
+    def name(self) -> str: ...
+    @property
+    def value(self) -> int: ...
+
 class Gripper:
     def get_normalized_width(self) -> float: ...
     def get_parameters(self) -> GripperConfig: ...
@@ -64,6 +107,24 @@ class GripperConfig:
 class GripperState:
     pass
 
+class Hand:
+    def __init__(self) -> None: ...
+    def get_normalized_joint_poses(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
+    def get_parameters(self) -> HandConfig: ...
+    def get_state(self) -> HandState: ...
+    def grasp(self) -> None: ...
+    def is_grasped(self) -> bool: ...
+    def open(self) -> None: ...
+    def reset(self) -> None: ...
+    def set_normalized_joint_poses(self, q: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]) -> None: ...
+    def shut(self) -> None: ...
+
+class HandConfig:
+    def __init__(self) -> None: ...
+
+class HandState:
+    def __init__(self) -> None: ...
+
 class IK:
     def forward(self, q0: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...) -> Pose: ...
     def ik(
@@ -256,7 +317,11 @@ def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ...
 
 FR3: RobotType  # value = <RobotType.FR3: 0>
 HARDWARE: RobotPlatform  # value = <RobotPlatform.HARDWARE: 1>
+LATERAL_GRASP: GraspType  # value = <GraspType.LATERAL_GRASP: 2>
+POWER_GRASP: GraspType  # value = <GraspType.POWER_GRASP: 0>
+PRECISION_GRASP: GraspType  # value = <GraspType.PRECISION_GRASP: 1>
 SIMULATION: RobotPlatform  # value = <RobotPlatform.SIMULATION: 0>
 SO101: RobotType  # value = <RobotType.SO101: 2>
+TRIPOD_GRASP: GraspType  # value = <GraspType.TRIPOD_GRASP: 3>
 UR5e: RobotType  # value = <RobotType.UR5e: 1>
 XArm7: RobotType  # value = <RobotType.XArm7: 3>

python/rcs/_core/sim.pyi

@@ -22,6 +22,9 @@ __all__ = [
     "SimRobot",
     "SimRobotConfig",
     "SimRobotState",
+    "SimTilburgHand",
+    "SimTilburgHandConfig",
+    "SimTilburgHandState",
     "default_free",
     "fixed",
     "free",
@@ -177,6 +180,36 @@ class SimRobotState(rcs._core.common.RobotState):
     @property
     def target_angles(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
 
+class SimTilburgHand(rcs._core.common.Hand):
+    def __init__(self, sim: Sim, cfg: SimTilburgHandConfig) -> None: ...
+    def get_parameters(self) -> SimTilburgHandConfig: ...
+    def get_state(self) -> SimTilburgHandState: ...
+    def set_parameters(self, cfg: SimTilburgHandConfig) -> bool: ...
+
+class SimTilburgHandConfig(rcs._core.common.HandConfig, typing.Generic[M]):
+    actuators: list[str]
+    collision_geoms: list[str]
+    collision_geoms_fingers: list[str]
+    grasp_type: rcs._core.common.GraspType
+    ignored_collision_geoms: list[str]
+    joints: list[str]
+    max_joint_position: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]
+    min_joint_position: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]
+    seconds_between_callbacks: float
+    def __init__(self) -> None: ...
+    def add_id(self, id: str) -> None: ...
+
+class SimTilburgHandState(rcs._core.common.HandState):
+    def __init__(self) -> None: ...
+    @property
+    def collision(self) -> bool: ...
+    @property
+    def is_moving(self) -> bool: ...
+    @property
+    def last_commanded_qpos(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
+    @property
+    def last_qpos(self) -> numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]]: ...
+
 default_free: CameraType  # value = <CameraType.default_free: 3>
 fixed: CameraType  # value = <CameraType.fixed: 2>
 free: CameraType  # value = <CameraType.free: 0>

python/rcs/camera/hw.py

@@ -83,7 +83,7 @@ def calibrate(
         return True
 
     def get_extrinsics(self) -> np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None:
-        return np.eye(4)
+        return np.eye(4)  # type: ignore[return-value]
 
 
 class HardwareCameraSet(BaseCameraSet):

python/rcs/envs/base.py

@@ -7,6 +7,7 @@
 
 import gymnasium as gym
 import numpy as np
+from rcs._core.common import Hand
 from rcs.camera.interface import BaseCameraSet
 from rcs.envs.space_utils import (
     ActObsInfoWrapper,
@@ -18,7 +19,6 @@
     get_space,
     get_space_keys,
 )
-from rcs.hand.interface import BaseHand
 
 from rcs import common
 
@@ -749,7 +749,7 @@ class HandWrapper(ActObsInfoWrapper):
     BINARY_HAND_CLOSED = 0
     BINARY_HAND_OPEN = 1
 
-    def __init__(self, env, hand: BaseHand, binary: bool = True):
+    def __init__(self, env, hand: Hand, binary: bool = True):
         super().__init__(env)
         self.unwrapped: RobotEnv
         self.observation_space: gym.spaces.Dict
@@ -778,7 +778,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl
                 self._last_hand_cmd if self._last_hand_cmd is not None else self.BINARY_HAND_OPEN
             )
         else:
-            observation[self.hand_key] = self._hand.get_normalized_joints_poses()
+            observation[self.hand_key] = self._hand.get_normalized_joint_poses()
 
         info = {}
         return observation, info
@@ -798,7 +798,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
                 else:
                     self._hand.open()
         else:
-            self._hand.set_normalized_joints_poses(hand_action)
+            self._hand.set_normalized_joint_poses(hand_action)
         self._last_hand_cmd = hand_action
         del action[self.hand_key]
         return action