Merge pull request #254 from RobotControlStack/juelg/fix-tacto-gripper

juelg · web-flow · commit ec4810625784 · 2026-01-28T08:56:37.000+01:00
fix!(tacto,gripper): fixes required for lerobot integration
diff --git a/README.md b/README.md
@@ -90,7 +90,7 @@ if __name__ == "__main__":
 
     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], "gripper": [0]}
         obs, reward, terminated, truncated, info = env.step(act)
         print(obs)
 ```
diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py
@@ -61,11 +61,11 @@ def main():
     for _ in range(100):
         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], "gripper": [0]}
             obs, reward, terminated, truncated, info = env_rel.step(act)
         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], "gripper": [1]}
             obs, reward, terminated, truncated, info = env_rel.step(act)
 
 
diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py
@@ -67,6 +67,6 @@
 
     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], "gripper": [0]}
         obs, reward, terminated, truncated, info = env.step(act)
         print(obs)
diff --git a/examples/fr3/grasp_demo.py b/examples/fr3/grasp_demo.py
@@ -19,8 +19,8 @@ def __init__(self, env: gym.Env):
         self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped)
         self.home_pose = self.unwrapped.robot.get_cartesian_position()
 
-    def _action(self, pose: Pose, gripper: float) -> dict[str, Any]:
-        return {"xyzrpy": pose.xyzrpy(), "gripper": gripper}
+    def _action(self, pose: Pose, gripper: list[float]) -> dict[str, Any]:
+        return {"xyzrpy": pose.xyzrpy(), "gripper": [gripper]}
 
     def get_object_pose(self, geom_name) -> Pose:
         model = self.env.get_wrapper_attr("sim").model
@@ -50,7 +50,7 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int
         goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0]))  # type: ignore
         return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints)
 
-    def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
+    def execute_motion(self, waypoints: list[Pose], gripper: list[float] = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
         for i in range(len(waypoints)):
             obs = self.step(self._action(waypoints[i], gripper))
         return obs
diff --git a/examples/fr3/grasp_digit_demo.py b/examples/fr3/grasp_digit_demo.py
@@ -19,7 +19,7 @@ def __init__(self, env: gym.Env):
         self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped)
         self.home_pose = self.unwrapped.robot.get_cartesian_position()
 
-    def _action(self, pose: Pose, gripper: float) -> dict[str, Any]:
+    def _action(self, pose: Pose, gripper: list[float]) -> dict[str, Any]:
         return {"xyzrpy": pose.xyzrpy(), "gripper": gripper}
 
     def get_object_pose(self, geom_name) -> Pose:
@@ -48,7 +48,7 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int
         goal_pose *= Pose(translation=np.array([0, 0, delta_up]), quaternion=np.array([1, 0, 0, 0]))  # type: ignore
         return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints)
 
-    def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
+    def execute_motion(self, waypoints: list[Pose], gripper: list[float] = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
         for i in range(len(waypoints)):
             obs = self.step(self._action(waypoints[i], gripper))
         return obs
diff --git a/examples/fr3/grasp_ompl_demo.py b/examples/fr3/grasp_ompl_demo.py
@@ -45,9 +45,9 @@ def __init__(self, env: gym.Env, planner: MjOMPL):
         self.planner = planner
 
     def _action(self, pose: Pose, gripper: float) -> dict[str, Any]:
-        return {"xyzrpy": pose.xyzrpy(), "gripper": gripper}
+        return {"xyzrpy": pose.xyzrpy(), "gripper": [gripper]}
 
-    def _jaction(self, joints: np.ndarray, gripper: float) -> dict[str, Any]:
+    def _jaction(self, joints: np.ndarray, gripper: list[float]) -> dict[str, Any]:
         return {"joints": joints, "gripper": gripper}
 
     def get_object_pose(self, geom_name) -> Pose:
@@ -102,7 +102,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in
     def step(self, action: dict) -> dict:
         return self.env.step(action)[0]
 
-    def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
+    def execute_motion(self, waypoints: list[Pose], gripper: list[float] = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
         for i in range(len(waypoints)):
             obs = self.step(self._jaction(waypoints[i], gripper))  # type: ignore
         return obs
diff --git a/examples/so101/so101_env_cartesian_control.py b/examples/so101/so101_env_cartesian_control.py
@@ -49,21 +49,21 @@ def main():
     env_rel.get_wrapper_attr("sim").open_gui()
     obs, info = env_rel.reset()
 
-    act = {"tquat": [0.03, 0, 0, 0, 0, 0, 1], "gripper": 1}
+    act = {"tquat": [0.03, 0, 0, 0, 0, 0, 1], "gripper": [1]}
     obs, reward, terminated, truncated, info = env_rel.step(act)
 
     for _ in range(100):
         for _ in range(5):
             # 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], "gripper": [0]}
             obs, reward, terminated, truncated, info = env_rel.step(act)
             print(info, obs)
             if truncated or terminated:
                 logger.info("Truncated or terminated!")
                 return
         for _ in range(5):
             # 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], "gripper": [1]}
             obs, reward, terminated, truncated, info = env_rel.step(act)
             if truncated or terminated:
                 logger.info("Truncated or terminated!")
diff --git a/examples/ur5e/ur5e_env_cartesian_control.py b/examples/ur5e/ur5e_env_cartesian_control.py
@@ -54,6 +54,10 @@ def main():
         gripper_config.joints = ["right_driver_joint"]
         gripper_config.collision_geoms = []
         gripper_config.collision_geoms_fingers = []
+        gripper_config.max_actuator_width = 0
+        gripper_config.min_actuator_width = 1
+        gripper_config.max_joint_width = 0.0
+        gripper_config.min_joint_width = 0.8
 
         env_rel = SimEnvCreator()(
             control_mode=ControlMode.CARTESIAN_TQuat,
@@ -70,12 +74,12 @@ def main():
     for _ in range(100):
         for _ in range(10):
             # move 1cm in x direction (forward) and close gripper
-            act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1.0], "gripper": 0}
+            act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1.0], "gripper": [0]}
             obs, reward, terminated, truncated, info = env_rel.step(act)
             sleep(0.6)
         for _ in range(10):
             # move 1cm in negative x direction (backward) and open gripper
-            act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1.0], "gripper": 1}
+            act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1.0], "gripper": [1]}
             obs, reward, terminated, truncated, info = env_rel.step(act)
             sleep(0.6)
 
diff --git a/examples/ur5e/ur5e_env_joint_control.py b/examples/ur5e/ur5e_env_joint_control.py
@@ -54,6 +54,10 @@ def main():
         gripper_config.joints = ["right_driver_joint"]
         gripper_config.collision_geoms = []
         gripper_config.collision_geoms_fingers = []
+        gripper_config.max_actuator_width = 0
+        gripper_config.min_actuator_width = 1
+        gripper_config.max_joint_width = 0.0
+        gripper_config.min_joint_width = 0.8
 
         env_rel = SimEnvCreator()(
             control_mode=ControlMode.JOINTS,
diff --git a/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py b/extensions/rcs_panda/src/rcs_panda/panda_env_cartesian_control.py
@@ -29,14 +29,14 @@ def main():
     for _ in range(100):
         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], "gripper": [0]}
             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], "gripper": [1]}
             obs, reward, terminated, truncated, info = env_rel.step(act)
             if truncated or terminated:
                 logger.info("Truncated or terminated!")
diff --git a/extensions/rcs_tacto/pyproject.toml b/extensions/rcs_tacto/pyproject.toml
@@ -4,10 +4,11 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "rcs_tacto"
-version = "0.5.0"
+version = "0.6.0"
 description="RCS integration of tacto"
 dependencies = [
-    "rcs>=0.5.2",
+    "rcs>=0.6.0",
+    "omegaconf",
     "mujoco-tacto@git+https://github.com/utn-air/mujoco-tacto.git@main"
 ]
 readme = "README.md"
diff --git a/extensions/rcs_tacto/src/rcs_tacto/__init__.py b/extensions/rcs_tacto/src/rcs_tacto/__init__.py
@@ -1 +1 @@
-__version__ = "0.5.2"
+__version__ = "0.6.0"
diff --git a/extensions/rcs_tacto/src/rcs_tacto/tacto_wrapper.py b/extensions/rcs_tacto/src/rcs_tacto/tacto_wrapper.py
@@ -5,8 +5,8 @@
 
 import cv2
 import gymnasium as gym
-import hydra
 import tacto
+from omegaconf import OmegaConf
 
 logger = logging.getLogger(__name__)
 
@@ -51,8 +51,8 @@ def __init__(
         if tacto_bg is None:
             tacto_bg = str(files("tacto") / "assets" / "bg_digit_240_320.jpg")
             logger.warning(f"No tacto_bg provided, using default from package: {tacto_bg}")
-        hydra.initialize_config_dir(tacto_config)
-        t_config = hydra.compose("digit.yaml")
+        config_path = os.path.join(tacto_config, "digit.yaml")
+        t_config = OmegaConf.load(config_path)
         self.tacto_sensor = tacto.Sensor(**t_config.tacto, background=cv2.imread(tacto_bg))
         self.tacto_fps = tacto_fps
         self.tacto_last_render = -1
diff --git a/pyproject.toml b/pyproject.toml
@@ -192,4 +192,8 @@ version_files = [
     "extensions/rcs_ur5e/pyproject.toml:version",
     "extensions/rcs_ur5e/src/rcs_ur5e/__init__.py:__version__",
     "extensions/rcs_ur5e/pyproject.toml:\"rcs>=(.*)\"",
+
+    "extensions/rcs_tacto/pyproject.toml:version",
+    "extensions/rcs_tacto/src/rcs_tacto/__init__.py:__version__",
+    "extensions/rcs_tacto/pyproject.toml:\"rcs>=(.*)\"",
 ]
diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py
@@ -3,7 +3,7 @@
 import copy
 import logging
 from enum import Enum, auto
-from typing import Annotated, Any, Literal, TypeAlias, cast
+from typing import Annotated, Any, ClassVar, Literal, TypeAlias, cast
 
 import gymnasium as gym
 import numpy as np
@@ -684,8 +684,8 @@ def close(self):
 class GripperWrapper(ActObsInfoWrapper):
     # TODO: sticky gripper, like in aloha
 
-    BINARY_GRIPPER_CLOSED = 0
-    BINARY_GRIPPER_OPEN = 1
+    BINARY_GRIPPER_CLOSED: ClassVar[list[float]] = [0]
+    BINARY_GRIPPER_OPEN: ClassVar[list[float]] = [1]
 
     def __init__(self, env, gripper: common.Gripper, binary: bool = True):
         super().__init__(env)
@@ -715,7 +715,7 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl
                 self._last_gripper_cmd if self._last_gripper_cmd is not None else self.BINARY_GRIPPER_OPEN
             )
         else:
-            observation[self.gripper_key] = self.gripper.get_normalized_width()
+            observation[self.gripper_key] = [self.gripper.get_normalized_width()]
 
         return observation, info
 
@@ -724,13 +724,17 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         action = copy.deepcopy(action)
         assert self.gripper_key in action, "Gripper action not found."
 
-        gripper_action = np.round(action[self.gripper_key]) if self.binary else action[self.gripper_key]
+        gripper_action = action[self.gripper_key]
+        if isinstance(gripper_action, int | float):
+            gripper_action = [gripper_action]  # type: ignore
+        if self.binary:
+            gripper_action = np.round(gripper_action)
         gripper_action = np.clip(gripper_action, 0.0, 1.0)
 
         if self.binary:
             self.gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self.gripper.open()
         else:
-            self.gripper.set_normalized_width(gripper_action)
+            self.gripper.set_normalized_width(next(gripper_action))
         self._last_gripper_cmd = gripper_action
         del action[self.gripper_key]
         return action
