RobotControlStack · juelg · Sep 25, 2025 · Sep 25, 2025 · Sep 25, 2025 · Sep 25, 2025
diff --git a/.gitignore b/.gitignore
@@ -17,3 +17,4 @@ config.h
 wheels
 .env
 settings.json
+*.egg-info
diff --git a/README.md b/README.md
@@ -72,7 +72,6 @@ from rcs.envs.utils import (
 from rcs.envs.base import ControlMode, RelativeTo
 env_rel = SimEnvCreator()(
     control_mode=ControlMode.JOINTS,
-    collision_guard=False,
     robot_cfg=default_sim_robot_cfg(),
     gripper_cfg=default_sim_gripper_cfg(),
     cameras=default_mujoco_cameraset_cfg(),
@@ -93,10 +92,11 @@ for _ in range(100):
 
 
 ### Examples
-Checkout the python examples in the [examples](examples) folder:
-- [fr3_direct_control.py](examples/fr3_direct_control.py) shows direct robot control with RCS's python bindings
-- [fr3_env_joint_control.py](examples/env_joint_control.py) and [fr3_env_cartesian_control.py](examples/env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control
-All of these examples work both in the MuJoCo simulation as well as on your hardware FR3.
+Checkout the python examples in the [examples](examples) folder. For example
+[fr3_direct_control.py](examples/fr3/fr3_direct_control.py) shows direct robot control with RCS's python bindings.
+And [fr3_env_joint_control.py](examples/fr3/fr3_env_joint_control.py) and [fr3_env_cartesian_control.py](examples/fr3/fr3_env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control
+Checkout the other sub folders for other robot-specific examples.
+Most of these examples work both in the MuJoCo simulation as well as on hardware.
 
 
 ### Hardware Extensions

diff --git a/assets/scenes/fr3_empty_world/scene.xml b/assets/scenes/fr3_empty_world/scene.xml
@@ -20,9 +20,9 @@
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
     <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
-    <body>
-    <geom type="box" name="isocube" size="0.2 0.2 0.2" rgba="0.153 0.961 0.329 0.2" pos="0.498 0.0 0.226" contype="0" conaffinity="0" group="2"/>
+    <!-- <body>
+      <geom type="box" name="isocube" size="0.2 0.2 0.2" rgba="0.153 0.961 0.329 0.2" pos="0.498 0.0 0.226" contype="0" conaffinity="0" group="2"/>
+    </body> -->
     <camera name="bird_eye_cam" pos="0.271 -0.000 2.080" xyaxes="0.001 -1.000 -0.000 1.000 0.001 0.017" />
-  </body>
   </worldbody>
 </mujoco>
diff --git a/assets/scenes/fr3_simple_pick_up/scene.xml b/assets/scenes/fr3_simple_pick_up/scene.xml
@@ -1,4 +1,4 @@
-<mujoco model="simple pick up task">
+<mujoco model="fr3 simple pick up">
   <include file="fr3_common.xml" />
   <include file="robot.xml" />
 
@@ -12,29 +12,24 @@
 
   <asset>
     <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
-
-    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0 0 0" rgb2="0 0 0" markrgb="0.8 0.8 0.8" width="300" height="300" />
-
-    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0" />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
+      markrgb="0.8 0.8 0.8" width="300" height="300" />
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
   </asset>
 
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
-    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" friction="1 0.005 0.0001" />
-    <!-- <body>
-      <geom type="box" name="isocube" size="0.2 0.2 0.2" rgba="0.153 0.961 0.329 0.2" pos="0.498 0.0 0.226" contype="0" conaffinity="0" group="2"/>
-    </body> -->
-
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
     <camera name="bird_eye_cam" pos="0.271 -0.000 2.080" xyaxes="0.001 -1.000 -0.000 1.000 0.001 0.017" />
     <camera name="openvla_view" pos="1.535 -0.406 0.757" xyaxes="0.246 0.969 0.000 -0.468 0.119 0.876" />
     <camera name="right_side" pos="0.217 -1.738 0.583" xyaxes="1.000 -0.010 0.000 0.001 0.143 0.990"/>
     <camera name="front" pos="2.278 -0.020 0.931" xyaxes="0.003 1.000 0.000 -0.274 0.001 0.962"/>
     <camera name="left_side" pos="0.266 1.832 0.434" xyaxes="-1.000 -0.021 0.000 0.002 -0.092 0.996"/>
     <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_body" pos="0.44 0.1 0.03">
-      <geom type="box" size="0.03 0.03 0.03" rgba="1 1 0 1" name="box_geom" friction="1 0.3 0.1" density="50" />
-      <joint type="free" name="box_joint" />
+    <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" />
+			<joint type="free" name="box_joint" />
     </body>
   </worldbody>
 </mujoco>
diff --git a/examples/__init__.py b/examples/__init__.py
diff --git a/examples/fr3_direct_control.py → examples/fr3/fr3_direct_control.py b/examples/fr3_direct_control.py → examples/fr3/fr3_direct_control.py
diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py
@@ -0,0 +1,73 @@
+import logging
+
+from rcs._core.common import RobotPlatform
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs.envs.creators import SimEnvCreator
+from rcs.envs.utils import (
+    default_mujoco_cameraset_cfg,
+    default_sim_gripper_cfg,
+    default_sim_robot_cfg,
+)
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+"""
+This script demonstrates how to control the FR3 robot in Cartesian position control mode
+using relative movements. The robot (or its simulation) moves 1cm forward and then 1cm backward
+in a loop while opening and closing the gripper.
+
+To control a real FR3 robot, install the rcs_fr3 extension (`pip install extensions/rcs_fr3`),
+and set the FR3_IP variable to the robot's IP address. Make sure to unlock the robot's joints and
+put it into FCI mode before running this script. For a scripted way of unlocking and guiding mode see the
+fr3_direct_control.py example which uses the FCI context manager.
+"""
+
+ROBOT_INSTANCE = RobotPlatform.SIMULATION
+FR3_IP = "192.168.101.1"
+
+
+def main():
+    if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
+        env_rel = SimEnvCreator()(
+            control_mode=ControlMode.CARTESIAN_TQuat,
+            robot_cfg=default_sim_robot_cfg(scene="fr3_empty_world"),
+            gripper_cfg=default_sim_gripper_cfg(),
+            cameras=default_mujoco_cameraset_cfg(),
+            max_relative_movement=0.5,
+            relative_to=RelativeTo.LAST_STEP,
+        )
+        env_rel.get_wrapper_attr("sim").open_gui()
+    else:
+        from rcs_fr3.creators import RCSFR3EnvCreator
+        from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
+
+        env_rel = RCSFR3EnvCreator()(
+            ip=FR3_IP,
+            control_mode=ControlMode.CARTESIAN_TQuat,
+            robot_cfg=default_fr3_hw_robot_cfg(),
+            gripper_cfg=default_fr3_hw_gripper_cfg(),
+            camera_set=None,
+            max_relative_movement=0.5,
+            relative_to=RelativeTo.LAST_STEP,
+        )
+        input("the robot is going to move, press enter whenever you are ready")
+
+    env_rel.reset()
+
+    # access low level robot api to get current cartesian position
+    print(env_rel.unwrapped.robot.get_cartesian_position())  # type: ignore
+
+    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}
+            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}
+            obs, reward, terminated, truncated, info = env_rel.step(act)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/examples/fr3/fr3_env_joint_control.py b/examples/fr3/fr3_env_joint_control.py
@@ -0,0 +1,70 @@
+import logging
+
+import numpy as np
+from rcs._core.common import RobotPlatform
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs.envs.creators import SimEnvCreator
+from rcs.envs.utils import (
+    default_mujoco_cameraset_cfg,
+    default_sim_gripper_cfg,
+    default_sim_robot_cfg,
+)
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+"""
+This script demonstrates how to control the FR3 robot in joint position control mode
+using relative movements. The robot (or its simulation) samples random relative joint movements
+in a loop.
+
+To control a real FR3 robot, install the rcs_fr3 extension (`pip install extensions/rcs_fr3`),
+change the ROBOT_INSTANCE variable to RobotPlatform.HARDWARE
+and set the FR3_IP variable to the robot's IP address. Make sure to unlock the robot's joints and
+put it into FCI mode before running this script. For a scripted way of unlocking and guiding mode see the
+fr3_direct_control.py example which uses the FCI context manager.
+"""
+
+ROBOT_INSTANCE = RobotPlatform.SIMULATION
+FR3_IP = "192.168.101.1"
+
+
+def main():
+    if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
+        env_rel = SimEnvCreator()(
+            control_mode=ControlMode.JOINTS,
+            robot_cfg=default_sim_robot_cfg("fr3_empty_world"),
+            gripper_cfg=default_sim_gripper_cfg(),
+            cameras=default_mujoco_cameraset_cfg(),
+            max_relative_movement=np.deg2rad(5),
+            relative_to=RelativeTo.LAST_STEP,
+        )
+        env_rel.get_wrapper_attr("sim").open_gui()
+    else:
+        from rcs_fr3.creators import RCSFR3EnvCreator
+        from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
+
+        env_rel = RCSFR3EnvCreator()(
+            ip=FR3_IP,
+            control_mode=ControlMode.JOINTS,
+            robot_cfg=default_fr3_hw_robot_cfg(),
+            gripper_cfg=default_fr3_hw_gripper_cfg(),
+            camera_set=None,
+            max_relative_movement=np.deg2rad(5),
+            relative_to=RelativeTo.LAST_STEP,
+        )
+        input("the robot is going to move, press enter whenever you are ready")
+
+    # access low level robot api to get current cartesian position
+    print(env_rel.unwrapped.robot.get_joint_position())  # type: ignore
+
+    for _ in range(100):
+        obs, info = env_rel.reset()
+        for _ in range(10):
+            # sample random relative action and execute it
+            act = env_rel.action_space.sample()
+            obs, reward, terminated, truncated, info = env_rel.step(act)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/examples/grasp_demo.py → examples/fr3/grasp_demo.py b/examples/grasp_demo.py → examples/fr3/grasp_demo.py
@@ -1,11 +1,13 @@
 import logging
+from time import sleep
 from typing import Any, cast
 
 import gymnasium as gym
 import mujoco
 import numpy as np
 from rcs._core.common import Pose
 from rcs.envs.base import GripperWrapper, RobotEnv
+from rcs.envs.creators import FR3SimplePickUpSimEnvCreator
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -25,54 +27,51 @@ def get_object_pose(self, geom_name) -> Pose:
         data = self.env.get_wrapper_attr("sim").data
 
         geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
-        return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3))
+        obj_pose_world_coordinates = Pose(
+            translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3)
+        ) * Pose(
+            rpy_vector=np.array([0, 0, np.pi]), translation=[0, 0, 0]  # type: ignore
+        )
+        return self.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates)
 
     def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]:
         waypoints = []
         for i in range(num_waypoints + 1):
-            t = i / (num_waypoints + 1)
+            t = i / (num_waypoints)
             waypoints.append(start_pose.interpolate(end_pose, t))
-        waypoints.append(end_pose)
         return waypoints
 
     def step(self, action: dict) -> dict:
         return self.env.step(action)[0]
 
     def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]:
         end_eff_pose = self.unwrapped.robot.get_cartesian_position()
-
         goal_pose = self.get_object_pose(geom_name=geom_name)
-        # goal pose is above the object and gripper coordinate must flip z-axis (end effector base rotation is [1, 0, 0, 0])
-        # be careful we define identity quaternion as as [0, 0, 0, 1]
-        # this does not work if the object is flipped
         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:
         for i in range(len(waypoints)):
-            # calculate delta action
-            # delta_action = waypoints[i] * waypoints[i - 1].inverse()
             obs = self.step(self._action(waypoints[i], gripper))
         return obs
 
     def approach(self, geom_name: str):
-        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=5)
+        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60)
         self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
 
     def grasp(self, geom_name: str):
 
-        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0, num_waypoints=15)
+        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.01, num_waypoints=60)
         self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_OPEN)
 
         self.step(self._action(Pose(), GripperWrapper.BINARY_GRIPPER_CLOSED))
 
-        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=20)
+        waypoints = self.plan_linear_motion(geom_name=geom_name, delta_up=0.2, num_waypoints=60)
         self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)
 
     def move_home(self):
         end_eff_pose = self.unwrapped.robot.get_cartesian_position()
-        waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=10)
+        waypoints = self.generate_waypoints(end_eff_pose, self.home_pose, num_waypoints=60)
         self.execute_motion(waypoints=waypoints, gripper=GripperWrapper.BINARY_GRIPPER_CLOSED)
 
     def pickup(self, geom_name: str):
@@ -82,15 +81,17 @@ def pickup(self, geom_name: str):
 
 
 def main():
-    env = gym.make(
-        "rcs/FR3SimplePickUpSim-v0",
+    env = FR3SimplePickUpSimEnvCreator()(
         render_mode="human",
         delta_actions=False,
     )
-    env.reset()
-    print(env.unwrapped.robot.get_cartesian_position().translation())  # type: ignore
-    controller = PickUpDemo(env)
-    controller.pickup("box_geom")
+    # wait for gui to open
+    sleep(3)
+    for _ in range(100):
+        env.reset()
+        print(env.unwrapped.robot.get_cartesian_position().translation())  # type: ignore
+        controller = PickUpDemo(env)
+        controller.pickup("box_geom")
 
 
 if __name__ == "__main__":

diff --git a/examples/fr3_env_cartesian_control.py b/examples/fr3_env_cartesian_control.py
-Original file line number
+Diff line change
@@ Expand Up / @@ -17,3 +17,4 @@ config.h @@
     wheels
     .env
     settings.json
+    *.egg-info