feat: working real setup integration

Author: kennkhr

extensions/rcs_xarm7/src/rcs_xarm7/creators.py

@@ -11,10 +11,12 @@
     CameraSetWrapper,
     ControlMode,
     GripperWrapper,
+    HandWrapper,
     RelativeActionSpace,
     RelativeTo,
     RobotEnv,
 )
+from rcs.hand.tilburg_hand import TilburgHand, THConfig
 from rcs.envs.creators import RCSHardwareEnvCreator
 from rcs.envs.sim import CollisionGuard, GripperWrapperSim, RobotSimWrapper, SimWrapper
 from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig
@@ -34,6 +36,7 @@ def __call__(  # type: ignore
         ip: str,
         calibration_dir: PathLike | str | None = None,
         camera_set: HardwareCameraSet | None = None,
+        hand_cfg: THConfig | None = None,
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
     ) -> gym.Env:
@@ -48,6 +51,9 @@ def __call__(  # type: ignore
             camera_set.wait_for_frames()
             logger.info("CameraSet started")
             env = CameraSetWrapper(env, camera_set)
+        if hand_cfg is not None and isinstance(hand_cfg, THConfig):
+            hand = TilburgHand(cfg=hand_cfg, verbose=True)
+            env = HandWrapper(env, hand, True)
 
         if max_relative_movement is not None:
             env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)

extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py

@@ -6,7 +6,7 @@
 from rcs.envs.creators import SimEnvCreator
 from rcs.envs.utils import get_tcp_offset
 from rcs_xarm7.creators import RCSXArm7EnvCreator, XArm7SimEnvCreator
-
+from rcs.hand.tilburg_hand import THConfig
 import rcs
 from rcs import sim
 
@@ -21,9 +21,15 @@
 def main():
 
     if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
+        hand_cfg = THConfig(
+            calibration_file="/home/ken/tilburg_hand/calibration.json",
+            grasp_percentage=1,
+            hand_orientation="right"
+        )
         env_rel = RCSXArm7EnvCreator()(
             control_mode=ControlMode.CARTESIAN_TQuat,
             ip=ROBOT_IP,
+            hand_cfg=hand_cfg,
             relative_to=RelativeTo.LAST_STEP,
             max_relative_movement=np.deg2rad(3),
         )
@@ -66,21 +72,21 @@ def main():
         env_rel.get_wrapper_attr("sim").open_gui()
 
     env_rel.reset()
-    act = {"tquat": [0.1, 0, 0, 0, 0, 0, 1], "gripper": 0}
-    obs, reward, terminated, truncated, info = env_rel.step(act)
+    # act = {"tquat": [0.1, 0, 0, 0, 0, 0, 1], "gripper": 0}
+    # obs, reward, terminated, truncated, info = env_rel.step(act)
 
     with env_rel:
         for _ in range(10):
             for _ in range(10):
                 # move 1cm in x direction (forward) and close gripper
-                act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0}
+                act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "hand": 1}
                 obs, reward, terminated, truncated, info = env_rel.step(act)
                 if truncated or terminated:
                     logger.info("Truncated or terminated!")
                     return
             for _ in range(10):
                 # move 1cm in negative x direction (backward) and open gripper
-                act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1}
+                act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "hand": 0}
                 obs, reward, terminated, truncated, info = env_rel.step(act)
                 if truncated or terminated:
                     logger.info("Truncated or terminated!")

python/rcs/envs/base.py

@@ -802,6 +802,6 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         self._last_hand_cmd = hand_action
         del action[self.hand_key]
         return action
-
+    
     def close(self):
         self._hand.close()