RobotControlStack · juelg · Mar 13, 2026 · Mar 15, 2026 · Mar 15, 2026 · Dec 16, 2025
diff --git a/assets/scenes/fr3_simple_pick_up/scene.xml b/assets/scenes/fr3_simple_pick_up/scene.xml
@@ -28,7 +28,7 @@
     <camera name="side_view" pos="1.760 -1.205 0.621" xyaxes="0.681 0.733 0.000 -0.105 0.097 0.990"/>
 
     <body name="box_geom" pos="0.44 0.1 0.03" quat="0 0 0 1">
-			<geom type="box" size="0.032 0.016 0.0288" rgba="0 0.984 0.373 1" name="box_geom" friction="1 0.3 0.1" density="50" />
+			<geom type="box" size="0.032 0.016 0.0288" rgba="0 0.984 0.373 1" name="box_geom" friction="1 0.3 0.1" density="50" group="1" />
 			<joint type="free" name="box_joint" />
     </body>
   </worldbody>

diff --git a/examples/teleop/quest_iris_dual_arm.py b/examples/teleop/quest_iris_dual_arm.py
@@ -3,8 +3,10 @@
 from time import sleep
 
 import numpy as np
+import rcs
+from rcs._core import common
 from rcs._core.common import RPY, Pose, RobotPlatform
-from rcs._core.sim import SimConfig
+from rcs._core.sim import CameraType, SimCameraConfig, SimConfig
 from rcs.camera.hw import HardwareCameraSet
 from rcs.envs.base import (
     ControlMode,
@@ -19,7 +21,7 @@
 from rcs.utils import SimpleFrameRate
 from rcs_fr3.creators import RCSFR3MultiEnvCreator
 from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
-from rcs_realsense.utils import default_realsense
+# from rcs_realsense.utils import default_realsense
 from simpub.core.simpub_server import SimPublisher
 from simpub.parser.simdata import SimObject, SimScene
 from simpub.sim.mj_publisher import MujocoPublisher
@@ -39,10 +41,14 @@
     # "left": "192.168.102.1",
     "right": "192.168.101.1",
 }
+ROBOT2ID = {
+    "left": "0",
+    "right": "1",
+}
 
 
-# ROBOT_INSTANCE = RobotPlatform.SIMULATION
-ROBOT_INSTANCE = RobotPlatform.HARDWARE
+ROBOT_INSTANCE = RobotPlatform.SIMULATION
+# ROBOT_INSTANCE = RobotPlatform.HARDWARE
 RECORD_FPS = 30
 # set camera dict to none disable cameras
 # CAMERA_DICT = {
@@ -87,7 +93,7 @@ def __init__(self, env: RelativeActionSpace):
         self._reset_lock = threading.Lock()
         self._env = env
 
-        self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ["right"]
+        self.controller_names = ROBOT2IP.keys() if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ROBOT2ID.keys()
         self._trg_btn = {"left": "index_trigger", "right": "index_trigger"}
         self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"}
         self._start_btn = "A"
@@ -100,7 +106,7 @@ def __init__(self, env: RelativeActionSpace):
         self._last_controller_pose = {key: Pose() for key in self.controller_names}
         self._offset_pose = {key: Pose() for key in self.controller_names}
 
-        for robot in ROBOT2IP:
+        for robot in ROBOT2IP if ROBOT_INSTANCE == RobotPlatform.HARDWARE else ROBOT2ID:
             self._env.envs[robot].set_origin_to_current()
 
         self._step_env = False
@@ -326,22 +332,50 @@ def main():
 
     else:
         # FR3
-        robot_cfg = default_sim_robot_cfg("fr3_empty_world")
+        rcs.scenes["rcs_icra_scene"] = rcs.Scene(
+            mjcf_scene="/home/tobi/coding/rcs_clones/prs/models/scenes/rcs_icra_scene/scene.xml",
+            mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot,
+            robot_type=common.RobotType.FR3,
+        )
+        rcs.scenes["pick"] = rcs.Scene(
+            mjcf_scene="/home/tobi/coding/rcs_clones/prs/assets/scenes/fr3_simple_pick_up/scene.xml",
+            mjcf_robot=rcs.scenes["fr3_simple_pick_up"].mjcf_robot,
+            robot_type=common.RobotType.FR3,
+        )
+
+        # robot_cfg = default_sim_robot_cfg("fr3_empty_world")
+        # robot_cfg = default_sim_robot_cfg("fr3_simple_pick_up")
+        robot_cfg = default_sim_robot_cfg("rcs_icra_scene")
+        # robot_cfg = default_sim_robot_cfg("pick")
+
+        resolution = (256, 256)
+        cameras = {
+            cam: SimCameraConfig(
+                identifier=cam,
+                type=CameraType.fixed,
+                resolution_height=resolution[1],
+                resolution_width=resolution[0],
+                frame_rate=0,
+            )
+            for cam in ["side", "wrist"]
+        }
 
         sim_cfg = SimConfig()
         sim_cfg.async_control = True
         env_rel = SimMultiEnvCreator()(
-            name2id=ROBOT2IP,
+            name2id=ROBOT2ID,
             robot_cfg=robot_cfg,
             control_mode=ControlMode.CARTESIAN_TQuat,
             gripper_cfg=default_sim_gripper_cfg(),
-            # cameras=default_mujoco_cameraset_cfg(),
+            # cameras=cameras,
             max_relative_movement=0.5,
             relative_to=RelativeTo.CONFIGURED_ORIGIN,
             sim_cfg=sim_cfg,
         )
-        sim = env_rel.unwrapped.envs[ROBOT2IP.keys().__iter__().__next__()].sim  # type: ignore
+        # sim = env_rel.unwrapped.envs[ROBOT2ID.keys().__iter__().__next__()].sim  # type: ignore
+        sim = env_rel.get_wrapper_attr("sim")
         sim.open_gui()
+        # MySimPublisher(MySimScene(), MQ3_ADDR)
         MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3)))
 
     env_rel.reset()

diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py
@@ -121,7 +121,7 @@ def __call__(  # type: ignore
         #         sim_gui=True,
         #         truncate_on_collision=False,
         #     )
-        if max_relative_movement is not None:
+        if relative_to != RelativeTo.NONE:
             env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
 
         return env
@@ -137,6 +137,7 @@ def __call__(  # type: ignore
         camera_set: HardwareCameraSet | None = None,
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
+        robot2world = None,
     ) -> gym.Env:
 
         ik = rcs.common.Pin(
@@ -163,7 +164,7 @@ def __call__(  # type: ignore
                 env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
             envs[key] = env
 
-        env = MultiRobotWrapper(envs)
+        env = MultiRobotWrapper(envs, robot2world)
         if camera_set is not None:
             camera_set.start()
             camera_set.wait_for_frames()

diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py
@@ -18,7 +18,7 @@
 class Scene:
     """Scene configuration."""
 
-    mjb: str
+    mjb: str | None = None
     """Path to the Mujoco binary scene file."""
     mjcf_scene: str
     """Path to the Mujoco scene XML file."""

diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi
@@ -136,6 +136,8 @@ class SimGripperConfig(rcs._core.common.GripperConfig):
     min_actuator_width: float
     min_joint_width: float
     seconds_between_callbacks: float
+    def __copy__(self) -> SimGripperConfig: ...
+    def __deepcopy__(self, arg0: dict) -> SimGripperConfig: ...
     def __init__(self) -> None: ...
     def add_id(self, id: str) -> None: ...
 
@@ -169,6 +171,8 @@ class SimRobotConfig(rcs._core.common.RobotConfig):
     mjcf_scene_path: str
     seconds_between_callbacks: float
     trajectory_trace: bool
+    def __copy__(self) -> SimRobotConfig: ...
+    def __deepcopy__(self, arg0: dict) -> SimRobotConfig: ...
     def __init__(self) -> None: ...
     def add_id(self, id: str) -> None: ...
 

diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py
@@ -167,6 +167,8 @@ class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): ...
 CartOrJointContType: TypeAlias = TQuatDictType | JointsDictType | TRPYDictType
 LimitedCartOrJointContType: TypeAlias = LimitedTQuatRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType
 
+class ArmWithGripper(TQuatDictType, GripperDictType): ...
+
 
 class ControlMode(Enum):
     JOINTS = auto()
@@ -310,9 +312,26 @@ def close(self):
 class MultiRobotWrapper(gym.Env):
     """Wraps a dictionary of environments to allow for multi robot control."""
 
-    def __init__(self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper]):
+    def __init__(self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper], robot2world: dict[str, common.Pose] | None = None):
         self.envs = envs
         self.unwrapped_multi = cast(dict[str, RobotEnv], {key: env.unwrapped for key, env in envs.items()})
+        if robot2world is None:
+            self.robot2world = {}
+        else:
+            self.robot2world = robot2world
+
+    def _translate_pose(self, key, dic, to_world=True):
+        r2w = self.robot2world.get(key, common.Pose())
+        if not to_world:
+            r2w = r2w.inverse()
+        if "tquat" in dic:
+            p = r2w * common.Pose(translation=dic["tquat"][:3], quaternion=dic["tquat"][3:]) * r2w.inverse()
+            dic["tquat"] = np.concatenate([p.translation(), p.rotation_q()])
+        if "xyzrpy" in dic:
+            p = r2w * common.Pose(translation=dic["xyzrpy"][:3], rpy_vector=dic["xyzrpy"][3:]) * r2w.inverse()
+            dic["xyzrpy"] = p.xyzrpy()
+
+        return dic
 
     def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict[str, Any]]:
         # follows gym env by combinding a dict of envs into a single env
@@ -322,7 +341,11 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo
         truncated = False
         info = {}
         for key, env in self.envs.items():
-            obs[key], r, t, tr, info[key] = env.step(action[key])
+            act = self._translate_pose(key, action[key], to_world=False)
+            ob, r, t, tr, info[key] = env.step(act)
+            obs[key] = self._translate_pose(key, ob, to_world=True)
+            # old
+            # obs[key], r, t, tr, info[key] = env.step(action[key])
             reward += float(r)
             terminated = terminated or t
             truncated = truncated or tr
@@ -358,6 +381,7 @@ def close(self):
 class RelativeTo(Enum):
     LAST_STEP = auto()
     CONFIGURED_ORIGIN = auto()
+    NONE = auto()
 
 
 class RelativeActionSpace(gym.ActionWrapper):

diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py
@@ -1,3 +1,4 @@
+import copy
 import logging
 import typing
 from functools import partial
@@ -146,33 +147,40 @@ def __call__(  # type: ignore
         max_relative_movement: float | tuple[float, float] | None = None,
         relative_to: RelativeTo = RelativeTo.LAST_STEP,
         sim_wrapper: Type[SimWrapper] | None = None,
+        robot2world=None,
     ) -> gym.Env:
 
         simulation = sim.Sim(robot_cfg.mjcf_scene_path, sim_cfg)
+        print(robot_cfg.kinematic_model_path)
+        print(robot_cfg.attachment_site)
         ik = rcs.common.Pin(
             robot_cfg.kinematic_model_path,
-            robot_cfg.attachment_site,
+            robot_cfg.attachment_site + "_0",
             urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
         )
         # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path)
 
         robots: dict[str, rcs.sim.SimRobot] = {}
-        for key in name2id:
-            robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=robot_cfg)
+        for key, mid in name2id.items():
+            cfg = copy.copy(robot_cfg)
+            cfg.add_id(mid)
+            robots[key] = rcs.sim.SimRobot(sim=simulation, ik=ik, cfg=cfg)
 
         envs = {}
-        for key in name2id:
+        for key, mid in name2id.items():
             env: gym.Env = RobotEnv(robots[key], control_mode)
-            env = RobotSimWrapper(env, simulation, sim_wrapper)
             if gripper_cfg is not None:
-                gripper = rcs.sim.SimGripper(simulation, gripper_cfg)
+                cfg = copy.copy(gripper_cfg)
+                cfg.add_id(mid)
+                gripper = rcs.sim.SimGripper(simulation, cfg)
                 env = GripperWrapper(env, gripper, binary=True)
 
-            if max_relative_movement is not None:
+            if relative_to != RelativeTo.NONE:
                 env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
             envs[key] = env
 
-        env = MultiRobotWrapper(envs)
+        env = MultiRobotWrapper(envs, robot2world)
+        env = RobotSimWrapper(env, simulation, sim_wrapper)
         if cameras is not None:
             camera_set = typing.cast(
                 BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True)