refactor!: wrapper parameters into config objects

juelg · juelg · commit 3512142d5961 · 2026-04-13T21:23:14.000+02:00
- robot wrapper: home_on_reset -&gt; into robot config
- robot wrapper: home position for home_on_reset configurable in robot config
- gripper wrapper: binary -&gt; into gripper config
diff --git a/Makefile b/Makefile
@@ -30,6 +30,7 @@ stubgen:
 	find ./python -not -path "./python/rcs/_core/*" -name '*.pyi' -delete
 	find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
 	find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
+	sed -i 's/    q_home: numpy\.ndarray\[tuple\[M\], numpy\.dtype\[numpy\.float64\]\] | None/    q_home: numpy.ndarray | None/' python/rcs/_core/common.pyi
 	python scripts/generate_common_typing.py
 	ruff check --fix python/rcs/_core python/rcs/common_typing.py
 	isort python/rcs/_core python/rcs/common_typing.py
diff --git a/examples/fr3/fr3_readme.py b/examples/fr3/fr3_readme.py
@@ -48,7 +48,7 @@
 
     # gripper
     gripper = sim.SimGripper(simulation, gripper_cfg)
-    env = GripperWrapper(env, gripper, binary=True)
+    env = GripperWrapper(env, gripper)
 
     env = RobotSimWrapper(env)
     env = GripperWrapperSim(env)
diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py
@@ -100,7 +100,7 @@ def __call__(  # type: ignore
         if isinstance(gripper_cfg, hw.FHConfig):
             gripper_cfg.ip = ip
             gripper = hw.FrankaHand(gripper_cfg)
-            env = GripperWrapper(env, gripper, binary=True)
+            env = GripperWrapper(env, gripper)
         elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig):
             hand = TilburgHand(gripper_cfg)
             env = HandWrapper(env, hand, binary=True)
@@ -164,7 +164,7 @@ def __call__(  # type: ignore
             if gripper_cfg is not None:
                 gripper_cfg.ip = ip
                 gripper = hw.FrankaHand(gripper_cfg)
-                env = GripperWrapper(env, gripper, binary=True)
+                env = GripperWrapper(env, gripper)
 
             if max_relative_movement is not None:
                 env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
diff --git a/extensions/rcs_panda/src/rcs_panda/creators.py b/extensions/rcs_panda/src/rcs_panda/creators.py
@@ -75,14 +75,13 @@ def __call__(  # type: ignore
             env,
             robot,
             ControlMode.JOINTS if collision_guard is not None else control_mode,
-            home_on_reset=True,
         )
 
         env = PandaHW(env)
         if isinstance(gripper_cfg, hw.FHConfig):
             gripper_cfg.ip = ip
             gripper = hw.FrankaHand(gripper_cfg)
-            env = GripperWrapper(env, gripper, binary=True)
+            env = GripperWrapper(env, gripper)
         elif isinstance(gripper_cfg, rcs.hand.tilburg_hand.THConfig):
             hand = TilburgHand(gripper_cfg)
             env = HandWrapper(env, hand, binary=True)
@@ -144,7 +143,7 @@ def __call__(  # type: ignore
             if gripper_cfg is not None:
                 gripper_cfg.ip = ip
                 gripper = hw.FrankaHand(gripper_cfg)
-                env = GripperWrapper(env, gripper, binary=True)
+                env = GripperWrapper(env, gripper)
 
             if max_relative_movement is not None:
                 env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
diff --git a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py
@@ -1,18 +1,28 @@
+import typing
+
 from rcs._core.common import Gripper, GripperConfig, GripperState
+from rcs.common_typing import GripperConfigKwargs
 from Robotiq2F85Driver.Robotiq2F85Driver import GripperStatus, Robotiq2F85Driver
 
 
 class RobotiQ2F85GripperConfig(GripperConfig):
 
-    def __init__(self, serial_number: str, speed: float = 100, force: float = 50, async_control: bool = True) -> None:
+    def __init__(
+        self,
+        serial_number: str,
+        speed: float = 100,
+        force: float = 50,
+        async_control: bool = True,
+        **kwargs: typing.Unpack[GripperConfigKwargs],
+    ) -> None:
         """
         Args:
             serial_number: Get the serial number with `udevadm info -a -n /dev/ttyUSB0 | grep serial`, make sure you have read/write permissions to the port.
             speed: Speed in mm/s. Must be between 20 and 150 mm/s.
             force: Force in N. Must be between 20 and 235 N.
             async_control: If True, gripper commands return immediately without waiting for the movement to complete. A new command interrupts any ongoing movement.
         """
-        super().__init__()
+        super().__init__(**kwargs)
         self.serial_number = serial_number
         self.speed = speed
         self.force = force
diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py
@@ -36,10 +36,10 @@ def __call__(  # type: ignore
         )
         robot = SO101(cfg=robot_cfg, ik=ik)
         env: gym.Env = HardwareEnv()
-        env = RobotWrapper(env, robot, control_mode, home_on_reset=True)
+        env = RobotWrapper(env, robot, control_mode)
 
         gripper = SO101Gripper(robot._hf_robot, robot)
-        env = GripperWrapper(env, gripper, binary=False)
+        env = GripperWrapper(env, gripper)
 
         if camera_set is not None:
             camera_set.start()
diff --git a/extensions/rcs_so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py
@@ -188,14 +188,17 @@ def __init__(self, hf_robot: SO101Follower, robot: SO101):
         super().__init__()
         self._hf_robot = hf_robot
         self._robot = robot
+        self._cfg = common.GripperConfig(binary=False)
 
     def get_normalized_width(self) -> float:
         obs = self._robot.obs
         if obs is None:
             return 0.0
         return obs["gripper.pos"] / 100.0
 
-    # def get_config(self) -> GripperConfig: ...
+    def get_config(self) -> common.GripperConfig:
+        return self._cfg
+
     # def get_state(self) -> GripperState: ...
 
     def grasp(self) -> None:
diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/creators.py b/extensions/rcs_ur5e/src/rcs_ur5e/creators.py
@@ -38,12 +38,12 @@ def __call__(  # type: ignore
         )
         robot = UR5e(robot_cfg, ik)
         env: gym.Env = HardwareEnv()
-        env = RobotWrapper(env, robot, control_mode, home_on_reset=True)
+        env = RobotWrapper(env, robot, control_mode)
 
         if gripper_cfg is not None:
             gripper = RobotiQGripper(cfg=gripper_cfg)
             # TODO: binary and other things of the wrappers should also be in the config
-            env = GripperWrapper(env, gripper, binary=True)
+            env = GripperWrapper(env, gripper)
 
         if camera_set is not None:
             camera_set.start()
diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py
@@ -11,7 +11,7 @@
 import numpy as np
 import rtde_control
 import rtde_receive
-from rcs.common_typing import RobotConfigKwargs
+from rcs.common_typing import GripperConfigKwargs, RobotConfigKwargs
 from rcs_ur5e import robotiq_gripper
 
 from rcs import common
@@ -64,8 +64,8 @@ def from_dict(self, data: dict[str, typing.Any]) -> None:
 
 
 class RobotiQGripperConfig(common.GripperConfig):
-    def __init__(self, ip: str):
-        super().__init__()
+    def __init__(self, ip: str, **kwargs: typing.Unpack[GripperConfigKwargs]) -> None:
+        super().__init__(**kwargs)
         self.ip = ip
 
 
@@ -326,6 +326,7 @@ def reset(self) -> None:
 class RobotiQGripper(common.Gripper):
     def __init__(self, cfg: RobotiQGripperConfig):
         super().__init__()
+        self._cfg = cfg
         self.gripper = robotiq_gripper.RobotiqGripper()
         try:
             self.gripper.connect(cfg.ip, 63352, socket_timeout=3.0)  # default port for Robotiq gripper
@@ -336,6 +337,9 @@ def __init__(self, cfg: RobotiQGripperConfig):
             self.gripper.activate()
         print("Gripper Connection established.")
 
+    def get_config(self) -> RobotiQGripperConfig:
+        return self._cfg
+
     def get_normalized_width(self) -> float:
         # value between 0 and 1 (0 is closed)
         return (self.gripper.get_max_position() - self.gripper.get_current_position()) / self.gripper.get_max_position()
diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py
@@ -44,7 +44,7 @@ def __call__(  # type: ignore
         )
         robot = XArm7(cfg=robot_cfg, ik=ik)
         env: gym.Env = HardwareEnv()
-        env = RobotWrapper(env, robot, control_mode, home_on_reset=True)
+        env = RobotWrapper(env, robot, control_mode)
 
         if camera_set is not None:
             camera_set.start()
diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h
@@ -121,13 +121,16 @@ struct RobotConfig {
   rcs::common::Pose tcp_offset = rcs::common::Pose::Identity();
   std::string attachment_site = "attachment_site";
   std::string kinematic_model_path = "assets/scenes/fr3_empty_world/robot.xml";
+  bool home_on_reset = true;
+  std::optional<VectorXd> q_home = std::nullopt;
   virtual ~RobotConfig(){};
 };
 struct RobotState {
   virtual ~RobotState(){};
 };
 
 struct GripperConfig {
+  bool binary = true;
   virtual ~GripperConfig(){};
 };
 struct GripperState {
diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi
@@ -107,7 +107,8 @@ class Gripper:
     def shut(self) -> None: ...
 
 class GripperConfig:
-    def __init__(self) -> None: ...
+    binary: bool
+    def __init__(self, binary: bool = True) -> None: ...
 
 class GripperState:
     def __init__(self) -> None: ...
@@ -234,7 +235,9 @@ class Robot:
 
 class RobotConfig:
     attachment_site: str
+    home_on_reset: bool
     kinematic_model_path: str
+    q_home: numpy.ndarray | None
     robot_platform: RobotPlatform
     robot_type: RobotType
     tcp_offset: Pose
@@ -245,6 +248,8 @@ class RobotConfig:
         tcp_offset: Pose = ...,
         attachment_site: str = "attachment_site",
         kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
+        home_on_reset: bool = True,
+        q_home: numpy.ndarray[tuple[M], numpy.dtype[numpy.float64]] | None = None,
     ) -> None: ...
 
 class RobotMetaConfig:
diff --git a/python/rcs/common_typing.py b/python/rcs/common_typing.py
@@ -4,9 +4,10 @@
 
 from typing import TypedDict
 
+import numpy
 from rcs._core import common
 
-__all__ = ["BaseCameraConfigKwargs", "RobotConfigKwargs"]
+__all__ = ["BaseCameraConfigKwargs", "RobotConfigKwargs", "GripperConfigKwargs"]
 
 
 class BaseCameraConfigKwargs(TypedDict, total=False):
@@ -22,3 +23,9 @@ class RobotConfigKwargs(TypedDict, total=False):
     tcp_offset: common.Pose
     attachment_site: str
     kinematic_model_path: str
+    home_on_reset: bool
+    q_home: numpy.ndarray | None
+
+
+class GripperConfigKwargs(TypedDict, total=False):
+    binary: bool
diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py
@@ -284,13 +284,12 @@ class RobotWrapper(ActObsInfoWrapper):
     y
     """
 
-    def __init__(self, env, robot: common.Robot, control_mode: ControlMode, home_on_reset: bool = False):
+    def __init__(self, env, robot: common.Robot, control_mode: ControlMode):
         super().__init__(env)
         self.robot = robot
         self._control_mode_overrides = [control_mode]
         self.action_space: gym.spaces.Dict
         self.observation_space: gym.spaces.Dict
-        self.home_on_reset = home_on_reset
         low, high = get_joint_limits(self.robot)
         if control_mode == ControlMode.JOINTS:
             self.action_space = get_space(JointsDictType, params={"joint_limits": {"low": low, "high": high}})
@@ -382,8 +381,12 @@ def reset(
     ) -> tuple[dict[str, Any], dict[str, Any]]:
         self.prev_action = None
         self.robot.reset()
-        if self.home_on_reset:
-            self.robot.move_home()
+        cfg = self.robot.get_config()
+        if cfg.home_on_reset:
+            if cfg.q_home is not None:
+                self.robot.set_joint_position(cfg.q_home)
+            else:
+                self.robot.move_home()
         return super().reset(seed=seed, options=options)
 
     def close(self):
@@ -883,15 +886,14 @@ class GripperWrapper(ActObsInfoWrapper):
     BINARY_GRIPPER_CLOSED: ClassVar[list[float]] = [0]
     BINARY_GRIPPER_OPEN: ClassVar[list[float]] = [1]
 
-    def __init__(self, env, gripper: common.Gripper, binary: bool = True):
+    def __init__(self, env, gripper: common.Gripper):
         super().__init__(env)
         self.observation_space: gym.spaces.Dict
         self.observation_space.spaces.update(get_space(GripperDictType).spaces)
         self.action_space: gym.spaces.Dict
         self.action_space.spaces.update(get_space(GripperDictType).spaces)
         self.gripper_key = get_space_keys(GripperDictType)[0]
         self.gripper = gripper
-        self.binary = binary
         self._last_gripper_cmd = None
 
     def close(self):
@@ -905,7 +907,7 @@ def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]:
 
     def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]:
         observation = copy.deepcopy(observation)
-        if self.binary:
+        if self.gripper.get_config().binary:
             observation[self.gripper_key] = (
                 self._last_gripper_cmd if self._last_gripper_cmd is not None else self.BINARY_GRIPPER_OPEN
             )
@@ -922,11 +924,11 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         gripper_action = action[self.gripper_key]
         if isinstance(gripper_action, int | float):
             gripper_action = [gripper_action]  # type: ignore
-        if self.binary:
+        if self.gripper.get_config().binary:
             gripper_action = np.round(gripper_action)
         gripper_action = np.clip(gripper_action, 0.0, 1.0)
 
-        if self.binary:
+        if self.gripper.get_config().binary:
             self.gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self.gripper.open()
         else:
             self.gripper.set_normalized_width(gripper_action[0])
diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py
@@ -98,7 +98,7 @@ def __call__(  # type: ignore
 
         if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig):
             gripper = sim.SimGripper(simulation, gripper_cfg)
-            env = GripperWrapper(env, gripper, binary=True)
+            env = GripperWrapper(env, gripper)
         else:
             gripper = None
 
@@ -169,7 +169,7 @@ def __call__(  # type: ignore
                 gripper_cfg_copy = copy.copy(gripper_cfg)
                 gripper_cfg_copy.add_postfix("_" + mid)
                 gripper = rcs.sim.SimGripper(simulation, gripper_cfg_copy)
-                env = GripperWrapper(env, gripper, binary=True)
+                env = GripperWrapper(env, gripper)
 
             env = RobotSimWrapper(env)
 
diff --git a/scripts/generate_common_typing.py b/scripts/generate_common_typing.py
diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp

Original file line number	Diff line number	Diff line change
`@@ -44,7 +44,7 @@ def __call__( # type: ignore`
`44`	`44`	`)`
`45`	`45`	`robot = XArm7(cfg=robot_cfg, ik=ik)`
`46`	`46`	`env: gym.Env = HardwareEnv()`
`47`		`- env = RobotWrapper(env, robot, control_mode, home_on_reset=True)`
	`47`	`+ env = RobotWrapper(env, robot, control_mode)`
`48`	`48`
`49`	`49`	`if camera_set is not None:`
`50`	`50`	`camera_set.start()`