fix(gripper): gripper state as list as defined by gym space

gripper wrapper now expects a list of the gripper state as defined by the gym.space.Box

Author: juelg

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)
 ```

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)
 
 

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)

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

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

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

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!")

examples/ur5e/ur5e_env_cartesian_control.py

@@ -70,12 +70,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)
 

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!")

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
 
@@ -730,7 +730,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         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