From c0d554f66865ec6c2cc0b512ed2568e5416a3846 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 11:36:08 +0200 Subject: [PATCH 1/9] docs: reorganize example folder --- examples/{ => fr3}/fr3_direct_control.py | 0 examples/{ => fr3}/fr3_env_cartesian_control.py | 0 examples/{ => fr3}/fr3_env_joint_control.py | 0 examples/{ => rpc_server_client}/rpc_run_client.py | 0 examples/{ => rpc_server_client}/rpc_run_server.py | 0 examples/{ => xarm7}/xarm7_env_cartesian_control.py | 0 examples/{ => xarm7}/xarm7_env_joint_control.py | 0 7 files changed, 0 insertions(+), 0 deletions(-) rename examples/{ => fr3}/fr3_direct_control.py (100%) rename examples/{ => fr3}/fr3_env_cartesian_control.py (100%) rename examples/{ => fr3}/fr3_env_joint_control.py (100%) rename examples/{ => rpc_server_client}/rpc_run_client.py (100%) rename examples/{ => rpc_server_client}/rpc_run_server.py (100%) rename examples/{ => xarm7}/xarm7_env_cartesian_control.py (100%) rename examples/{ => xarm7}/xarm7_env_joint_control.py (100%) diff --git a/examples/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py similarity index 100% rename from examples/fr3_direct_control.py rename to examples/fr3/fr3_direct_control.py diff --git a/examples/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py similarity index 100% rename from examples/fr3_env_cartesian_control.py rename to examples/fr3/fr3_env_cartesian_control.py diff --git a/examples/fr3_env_joint_control.py b/examples/fr3/fr3_env_joint_control.py similarity index 100% rename from examples/fr3_env_joint_control.py rename to examples/fr3/fr3_env_joint_control.py diff --git a/examples/rpc_run_client.py b/examples/rpc_server_client/rpc_run_client.py similarity index 100% rename from examples/rpc_run_client.py rename to examples/rpc_server_client/rpc_run_client.py diff --git a/examples/rpc_run_server.py b/examples/rpc_server_client/rpc_run_server.py similarity index 100% rename from examples/rpc_run_server.py rename to examples/rpc_server_client/rpc_run_server.py diff --git a/examples/xarm7_env_cartesian_control.py b/examples/xarm7/xarm7_env_cartesian_control.py similarity index 100% rename from examples/xarm7_env_cartesian_control.py rename to examples/xarm7/xarm7_env_cartesian_control.py diff --git a/examples/xarm7_env_joint_control.py b/examples/xarm7/xarm7_env_joint_control.py similarity index 100% rename from examples/xarm7_env_joint_control.py rename to examples/xarm7/xarm7_env_joint_control.py From 1cc87c59d66ac33e067a5df70ffaf320c21ed56c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 11:37:26 +0200 Subject: [PATCH 2/9] docs: added hw to fr3 examples --- examples/__init__.py | 0 examples/fr3/fr3_env_cartesian_control.py | 56 ++++++++++++++++------- examples/fr3/fr3_env_joint_control.py | 54 ++++++++++++++++------ 3 files changed, 80 insertions(+), 30 deletions(-) delete mode 100644 examples/__init__.py diff --git a/examples/__init__.py b/examples/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py index fecb2a16..b9c070f7 100644 --- a/examples/fr3/fr3_env_cartesian_control.py +++ b/examples/fr3/fr3_env_cartesian_control.py @@ -1,5 +1,6 @@ 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 ( @@ -11,21 +12,48 @@ 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. -def main(): +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" - env_rel = SimEnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - robot_cfg=default_sim_robot_cfg(scene="fr3_empty_world"), - collision_guard=False, - 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() +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): @@ -33,16 +61,10 @@ def main(): # 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) - 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} obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return if __name__ == "__main__": diff --git a/examples/fr3/fr3_env_joint_control.py b/examples/fr3/fr3_env_joint_control.py index ff19708b..853d828c 100644 --- a/examples/fr3/fr3_env_joint_control.py +++ b/examples/fr3/fr3_env_joint_control.py @@ -1,6 +1,7 @@ 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 ( @@ -12,18 +13,48 @@ 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(): - env_rel = SimEnvCreator()( - control_mode=ControlMode.JOINTS, - collision_guard=False, - 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() + 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() @@ -31,9 +62,6 @@ def main(): # sample random relative action and execute it act = env_rel.action_space.sample() obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return if __name__ == "__main__": From 5a77d0ad3bc23a18ce189ea16a20a169120e35d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 11:38:45 +0200 Subject: [PATCH 3/9] chore: remove default collision guard flag from creator calls --- README.md | 1 - examples/rpc_server_client/rpc_run_server.py | 1 - examples/xarm7/xarm7_env_cartesian_control.py | 1 - examples/xarm7/xarm7_env_joint_control.py | 1 - extensions/rcs_fr3/src/rcs_fr3/creators.py | 3 +-- .../src/rcs_so101/env_joint_control.py | 1 - .../rcs_xarm7/src/rcs_xarm7/creators.py | 24 +++++++++---------- .../src/rcs_xarm7/env_cartesian_control.py | 1 - .../rcs_xarm7/src/rcs_xarm7/env_grasp.py | 1 - .../src/rcs_xarm7/env_joint_control.py | 1 - python/tests/test_rpc.py | 1 - 11 files changed, 13 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 30187621..f40c86b0 100644 --- 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(), diff --git a/examples/rpc_server_client/rpc_run_server.py b/examples/rpc_server_client/rpc_run_server.py index 74a81125..b2ef4915 100644 --- a/examples/rpc_server_client/rpc_run_server.py +++ b/examples/rpc_server_client/rpc_run_server.py @@ -11,7 +11,6 @@ def run_server(): env = 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(), diff --git a/examples/xarm7/xarm7_env_cartesian_control.py b/examples/xarm7/xarm7_env_cartesian_control.py index 99b1ff7b..5f8c48cb 100644 --- a/examples/xarm7/xarm7_env_cartesian_control.py +++ b/examples/xarm7/xarm7_env_cartesian_control.py @@ -40,7 +40,6 @@ def main(): env_rel = SimEnvCreator()( robot_cfg=robot_cfg, control_mode=ControlMode.CARTESIAN_TQuat, - collision_guard=False, gripper_cfg=None, # cameras=default_mujoco_cameraset_cfg(), max_relative_movement=0.5, diff --git a/examples/xarm7/xarm7_env_joint_control.py b/examples/xarm7/xarm7_env_joint_control.py index c66e88a5..f2d2433a 100644 --- a/examples/xarm7/xarm7_env_joint_control.py +++ b/examples/xarm7/xarm7_env_joint_control.py @@ -40,7 +40,6 @@ def main(): env_rel = SimEnvCreator()( robot_cfg=robot_cfg, control_mode=ControlMode.JOINTS, - collision_guard=False, gripper_cfg=None, # cameras=default_mujoco_cameraset_cfg(), max_relative_movement=np.deg2rad(5), diff --git a/extensions/rcs_fr3/src/rcs_fr3/creators.py b/extensions/rcs_fr3/src/rcs_fr3/creators.py index 76369ef1..2c05b60e 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/creators.py +++ b/extensions/rcs_fr3/src/rcs_fr3/creators.py @@ -49,8 +49,6 @@ def __call__( # type: ignore robot_cfg (hw.FR3Config): Configuration for the FR3 robot. collision_guard (str | PathLike | None): Key to a built-in scene robot_cfg (hw.FR3Config): Configuration for the FR3 robot. - collision_guard (str | PathLike | None): Key to a scene (requires UTN compatible scene package to be present) - or the path to a mujoco scene for collision guarding. If None, collision guarding is not used. gripper_cfg (hw.FHConfig | None): Configuration for the gripper. If None, no gripper is used. camera_set (BaseHardwareCameraSet | None): Camera set to be used. If None, no cameras are used. max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts @@ -84,6 +82,7 @@ def __call__( # type: ignore logger.info("CameraSet started") env = CameraSetWrapper(env, camera_set) + # TODO collision guard not working atm # if collision_guard is not None: # assert urdf_path is not None # env = CollisionGuard.env_from_xml_paths( diff --git a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py index d7cbbb80..a5e25f0d 100644 --- a/extensions/rcs_so101/src/rcs_so101/env_joint_control.py +++ b/extensions/rcs_so101/src/rcs_so101/env_joint_control.py @@ -43,7 +43,6 @@ def main(): control_mode=ControlMode.JOINTS, urdf_path="/home/tobi/coding/lerobot/so101_new_calib.urdf", robot_cfg=cfg, - collision_guard=False, mjcf="/home/tobi/coding/lerobot/SO-ARM100/Simulation/SO101/scene.xml", gripper_cfg=grp_cfg, # camera_set_cfg=default_mujoco_cameraset_cfg(), diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index b928ed91..49522751 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -114,18 +114,18 @@ def __call__( # type: ignore env = GripperWrapper(env, gripper, binary=False) env = GripperWrapperSim(env, gripper) - if collision_guard: - env = CollisionGuard.env_from_xml_paths( - env, - str(rcs.scenes.get(str(mjcf), mjcf)), - str(urdf_path), - gripper=gripper_cfg is not None, - check_home_collision=False, - control_mode=control_mode, - tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), - sim_gui=True, - truncate_on_collision=True, - ) + # if collision_guard: + # env = CollisionGuard.env_from_xml_paths( + # env, + # str(rcs.scenes.get(str(mjcf), mjcf)), + # str(urdf_path), + # gripper=gripper_cfg is not None, + # check_home_collision=False, + # control_mode=control_mode, + # tcp_offset=rcs.common.Pose(common.FrankaHandTCPOffset()), + # sim_gui=True, + # truncate_on_collision=True, + # ) if max_relative_movement is not None: env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py index bf450c1e..41893b14 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py @@ -48,7 +48,6 @@ def sim_env(): env_rel = SimEnvCreator()( robot_cfg=robot_cfg, control_mode=ControlMode.CARTESIAN_TQuat, - collision_guard=False, gripper_cfg=None, hand_cfg=default_sim_tilburg_hand_cfg(), # cameras=default_mujoco_cameraset_cfg(), diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py index 5acdcfa8..350b0295 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_grasp.py @@ -47,7 +47,6 @@ def sim_env(): env_rel = SimEnvCreator()( robot_cfg=robot_cfg, control_mode=ControlMode.JOINTS, - collision_guard=False, gripper_cfg=None, hand_cfg=default_sim_tilburg_hand_cfg(), # cameras=default_mujoco_cameraset_cfg(), diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py index 9ae8ae5f..7abbaad8 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py @@ -56,7 +56,6 @@ def main(): env_rel = SimEnvCreator()( robot_cfg=robot_cfg, control_mode=ControlMode.JOINTS, - collision_guard=False, gripper_cfg=None, # cameras=default_mujoco_cameraset_cfg(), max_relative_movement=np.deg2rad(5), diff --git a/python/tests/test_rpc.py b/python/tests/test_rpc.py index 10a2efab..83f00caf 100644 --- a/python/tests/test_rpc.py +++ b/python/tests/test_rpc.py @@ -70,7 +70,6 @@ def run_server(host: str, port: int, err_q: multiprocessing.Queue) -> None: try: env = SimEnvCreator()( control_mode=ControlMode.JOINTS, - collision_guard=False, robot_cfg=default_sim_robot_cfg(), gripper_cfg=default_sim_gripper_cfg(), # Disabled to avoid rendering problem in python subprocess. From 656470c9db331161d122e7921b58ca07658f0cb3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 13:55:07 +0200 Subject: [PATCH 4/9] docs: move grasp demo into fr3 folder --- examples/{ => fr3}/grasp_demo.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/{ => fr3}/grasp_demo.py (100%) diff --git a/examples/grasp_demo.py b/examples/fr3/grasp_demo.py similarity index 100% rename from examples/grasp_demo.py rename to examples/fr3/grasp_demo.py From 171414ffa67bcd0ac9a29b3819ae21f7746f37f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 14:02:54 +0200 Subject: [PATCH 5/9] docs(examples): improve grasping demo - clean up scenes - fixed ee rotation and tcp --- assets/scenes/fr3_empty_world/scene.xml | 6 ++-- assets/scenes/fr3_simple_pick_up/scene.xml | 21 +++++------- examples/fr3/grasp_demo.py | 39 +++++++++++----------- python/rcs/envs/creators.py | 4 +++ 4 files changed, 34 insertions(+), 36 deletions(-) diff --git a/assets/scenes/fr3_empty_world/scene.xml b/assets/scenes/fr3_empty_world/scene.xml index 128dc17b..2ed02ba3 100644 --- a/assets/scenes/fr3_empty_world/scene.xml +++ b/assets/scenes/fr3_empty_world/scene.xml @@ -20,9 +20,9 @@ - - + - diff --git a/assets/scenes/fr3_simple_pick_up/scene.xml b/assets/scenes/fr3_simple_pick_up/scene.xml index 716bfce5..88b1f269 100644 --- a/assets/scenes/fr3_simple_pick_up/scene.xml +++ b/assets/scenes/fr3_simple_pick_up/scene.xml @@ -1,4 +1,4 @@ - + @@ -12,19 +12,14 @@ - - - - + + - - - + @@ -32,9 +27,9 @@ - - - + + + \ No newline at end of file diff --git a/examples/fr3/grasp_demo.py b/examples/fr3/grasp_demo.py index d741597c..10e361b7 100644 --- a/examples/fr3/grasp_demo.py +++ b/examples/fr3/grasp_demo.py @@ -1,4 +1,5 @@ import logging +from time import sleep from typing import Any, cast import gymnasium as gym @@ -6,6 +7,7 @@ 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,14 +27,16 @@ 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: @@ -40,39 +44,32 @@ def step(self, action: dict) -> dict: 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 +79,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/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 865b604b..4173c1e3 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -209,6 +209,10 @@ def __call__( # type: ignore for cam in cam_list } robot_cfg = default_sim_robot_cfg(scene="fr3_simple_pick_up") + robot_cfg.tcp_offset = rcs.common.Pose( + translation=np.array([0.0, 0.0, 0.1034]), # type: ignore + rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]), # type: ignore + ) return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras) From 27f2e1c3333c69775f31a142506a4020d951eb67 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 17:15:20 +0200 Subject: [PATCH 6/9] feat(sim): sim config support for async and realtime mode - added frequency flag to sim config - pybind for sim config - support for async sim - fixed realtime mode in sim --- python/rcs/_core/sim.pyi | 10 ++++++++++ python/rcs/envs/creators.py | 11 +++++++++-- python/rcs/envs/sim.py | 20 ++++++++++++++++---- python/rcs/sim/__init__.py | 2 ++ python/rcs/sim/sim.py | 6 ++++-- python/rcs/utils.py | 2 +- src/pybind/rcs.cpp | 16 ++++++++++++++++ src/sim/sim.cpp | 11 ++--------- src/sim/sim.h | 13 +++++++------ src/sim/test.cpp | 2 +- 10 files changed, 68 insertions(+), 25 deletions(-) diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 070a0e0e..07b172c7 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -16,6 +16,7 @@ __all__ = [ "Sim", "SimCameraConfig", "SimCameraSet", + "SimConfig", "SimGripper", "SimGripperConfig", "SimGripperState", @@ -86,8 +87,10 @@ class Sim: def __init__(self, mjmdl: int, mjdata: int) -> None: ... def _start_gui_server(self, id: str) -> None: ... def _stop_gui_server(self) -> None: ... + def get_config(self) -> SimConfig: ... def is_converged(self) -> bool: ... def reset(self) -> None: ... + def set_config(self, cfg: SimConfig) -> bool: ... def step(self, k: int) -> None: ... def step_until_convergence(self) -> None: ... @@ -106,6 +109,13 @@ class SimCameraSet: @property def _sim(self) -> Sim: ... +class SimConfig: + async_control: bool + frequency: int + max_convergence_steps: int + realtime: bool + def __init__(self) -> None: ... + class SimGripper(rcs._core.common.Gripper): def __init__(self, sim: Sim, cfg: SimGripperConfig) -> None: ... def get_parameters(self) -> SimGripperConfig: ... diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 4173c1e3..b3d0ca50 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -47,6 +47,7 @@ def __call__( # type: ignore robot_cfg: rcs.sim.SimRobotConfig, collision_guard: bool = False, gripper_cfg: rcs.sim.SimGripperConfig | None = None, + sim_cfg: rcs.sim.SimConfig | None = None, hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, cameras: dict[str, SimCameraConfig] | None = None, max_relative_movement: float | tuple[float, float] | None = None, @@ -75,7 +76,7 @@ def __call__( # type: ignore Returns: gym.Env: The configured simulation environment for the FR3 robot. """ - simulation = sim.Sim(robot_cfg.mjcf_scene_path) + simulation = sim.Sim(robot_cfg.mjcf_scene_path, sim_cfg) ik = rcs.common.Pin( robot_cfg.kinematic_model_path, @@ -137,6 +138,7 @@ def __call__( # type: ignore cameras: dict[str, SimCameraConfig] | None = None, hand_cfg: rcs.sim.SimTilburgHandConfig | None = None, gripper_cfg: rcs.sim.SimGripperConfig | None = None, + sim_cfg: rcs.sim.SimConfig | None = None, random_pos_args: dict | None = None, ) -> gym.Env: mode = "gripper" @@ -170,6 +172,7 @@ def __call__( # type: ignore collision_guard=False, gripper_cfg=_gripper_cfg, hand_cfg=_hand_cfg, + sim_cfg=sim_cfg, cameras=cameras, max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, relative_to=RelativeTo.LAST_STEP, @@ -213,8 +216,12 @@ def __call__( # type: ignore translation=np.array([0.0, 0.0, 0.1034]), # type: ignore rotation=np.array([[0.707, 0.707, 0], [-0.707, 0.707, 0], [0, 0, 1]]), # type: ignore ) + sim_cfg = sim.SimConfig() + sim_cfg.realtime = False + sim_cfg.async_control = True + sim_cfg.frequency = 30 # in Hz - return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras) + return SimTaskEnvCreator()(robot_cfg, render_mode, control_mode, delta_actions, cameras, sim_cfg=sim_cfg) class FR3LabDigitGripperPickUpSimEnvCreator(EnvCreator): diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index ee36ab60..4655b39a 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -14,7 +14,9 @@ from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg import rcs +import time from rcs import sim +from rcs.utils import SimpleFrameRate logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -33,7 +35,7 @@ def __init__(self, env: gym.Env, simulation: sim.Sim): class RobotSimWrapper(gym.Wrapper): - def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | None = None): + def __init__(self, env, simulation: sim.Sim, sim_wrapper: SimWrapper | None = None): self.sim_wrapper = sim_wrapper if sim_wrapper is not None: env = sim_wrapper(env, simulation) @@ -42,10 +44,20 @@ def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | Non assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) self.sim = simulation + cfg = self.sim.get_config() + self.frame_rate = SimpleFrameRate(1/cfg.frequency, "RobotSimWrapper") def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: _, _, _, _, info = super().step(action) - self.sim.step_until_convergence() + cfg = self.sim.get_config() + if cfg.async_control: + self.sim.step(round(1/cfg.frequency / self.sim.model.opt.timestep)) + if cfg.realtime: + self.frame_rate.frame_rate = 1/cfg.frequency + self.frame_rate() + + else: + self.sim.step_until_convergence() state = self.sim_robot.get_state() info["collision"] = state.collision info["ik_success"] = state.ik_success @@ -348,7 +360,7 @@ def reset( class RandomCubePos(SimWrapper): """Wrapper to randomly place cube in the lab environments.""" - def __init__(self, env: gym.Env, simulation: sim.Sim, include_rotation: bool = False): + def __init__(self, env: gym.Env, simulation: sim.Sim, include_rotation: bool = True): super().__init__(env, simulation) self.include_rotation = include_rotation @@ -361,7 +373,7 @@ def reset( iso_cube = np.array([0.498, 0.0, 0.226]) iso_cube_pose = rcs.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) # type: ignore iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() - pos_z = 0.826 + pos_z = 0.0288 / 2 pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py index 036f5bb5..6331b8bf 100644 --- a/python/rcs/sim/__init__.py +++ b/python/rcs/sim/__init__.py @@ -9,6 +9,7 @@ SimTilburgHand, SimTilburgHandConfig, SimTilburgHandState, + SimConfig, ) from rcs.sim.sim import Sim, gui_loop @@ -25,4 +26,5 @@ "SimTilburgHandState", "gui_loop", "SimCameraConfig", + "SimConfig", ] diff --git a/python/rcs/sim/sim.py b/python/rcs/sim/sim.py index 9fc5feb2..17e252c7 100644 --- a/python/rcs/sim/sim.py +++ b/python/rcs/sim/sim.py @@ -12,7 +12,7 @@ import mujoco.viewer from rcs._core.sim import GuiClient as _GuiClient from rcs._core.sim import Sim as _Sim -from rcs.sim import egl_bootstrap +from rcs.sim import egl_bootstrap, SimConfig from rcs.utils import SimpleFrameRate egl_bootstrap.bootstrap() @@ -42,7 +42,7 @@ def gui_loop(gui_uuid: str, close_event): class Sim(_Sim): - def __init__(self, mjmdl: str | PathLike): + def __init__(self, mjmdl: str | PathLike, cfg: SimConfig = None): mjmdl = Path(mjmdl) if mjmdl.suffix == ".xml": self.model = mj.MjModel.from_xml_path(str(mjmdl)) @@ -58,6 +58,8 @@ def __init__(self, mjmdl: str | PathLike): self._gui_client: Optional[_GuiClient] = None self._gui_process: Optional[mp.context.SpawnProcess] = None self._stop_event: Optional[EventClass] = None + if cfg is not None: + self.set_config(cfg) def close_gui(self): if self._stop_event is not None: diff --git a/python/rcs/utils.py b/python/rcs/utils.py index 704fdd48..e6543377 100644 --- a/python/rcs/utils.py +++ b/python/rcs/utils.py @@ -36,7 +36,7 @@ def __call__(self): sleep(sleep_time) if self._last_print is None or perf_counter() - self._last_print > 10: self._last_print = perf_counter() - logger.info(f"FPS {self.loop_name}: {1 / (perf_counter() - self.t)}") + logger.debug(f"FPS {self.loop_name}: {1 / (perf_counter() - self.t)}") self.t = perf_counter() diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 63ea9e45..e1270058 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -487,6 +487,18 @@ PYBIND11_MODULE(_core, m) { .def_readonly("last_width", &rcs::sim::SimGripperState::last_width) .def_readonly("collision", &rcs::sim::SimGripperState::collision); + py::class_( + sim, "SimConfig") + .def(py::init<>()) + .def_readwrite("async_control", + &rcs::sim::SimConfig::async_control) + .def_readwrite("realtime", + &rcs::sim::SimConfig::realtime) + .def_readwrite("frequency", + &rcs::sim::SimConfig::frequency) + .def_readwrite("max_convergence_steps", + &rcs::sim::SimConfig::max_convergence_steps); + py::class_>(sim, "Sim") .def(py::init([](long m, long d) { return std::make_shared((mjModel *)m, (mjData *)d); @@ -495,10 +507,14 @@ PYBIND11_MODULE(_core, m) { .def("step_until_convergence", &rcs::sim::Sim::step_until_convergence, py::call_guard()) .def("is_converged", &rcs::sim::Sim::is_converged) + .def("set_config", &rcs::sim::Sim::set_config, py::arg("cfg")) + .def("get_config", &rcs::sim::Sim::get_config) .def("step", &rcs::sim::Sim::step, py::arg("k")) .def("reset", &rcs::sim::Sim::reset) .def("_start_gui_server", &rcs::sim::Sim::start_gui_server, py::arg("id")) .def("_stop_gui_server", &rcs::sim::Sim::stop_gui_server); + + py::class_>(sim, "SimGripper") .def(py::init, diff --git a/src/sim/sim.cpp b/src/sim/sim.cpp index b33caefd..8059921e 100644 --- a/src/sim/sim.cpp +++ b/src/sim/sim.cpp @@ -28,16 +28,12 @@ bool get_last_return_value(ConditionCallback cb) { Sim::Sim(mjModel* m, mjData* d) : m(m), d(d), renderer(m){}; -bool Sim::set_config(const Config& cfg) { - if (cfg.async) { - /* Not implemented :) */ - return false; - } +bool Sim::set_config(const SimConfig& cfg) { this->cfg = cfg; return true; } -Config Sim::get_config() { return this->cfg; } +SimConfig Sim::get_config() { return this->cfg; } void Sim::invoke_callbacks() { for (int i = 0; i < std::size(this->callbacks); ++i) { @@ -115,9 +111,6 @@ void Sim::step(size_t k) { this->invoke_callbacks(); mj_step2(this->m, this->d); this->invoke_rendering_callbacks(); - if (this->cfg.realtime) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } } } diff --git a/src/sim/sim.h b/src/sim/sim.h index ac48d603..607cb2f9 100644 --- a/src/sim/sim.h +++ b/src/sim/sim.h @@ -26,10 +26,11 @@ class Renderer { std::unordered_map ctxs; }; -struct Config { - bool async = false; +struct SimConfig { + bool async_control = false; bool realtime = false; - int max_convergence_steps = 5000; + int frequency = 30; // in Hz + int max_convergence_steps = 500; }; struct Callback { @@ -56,7 +57,7 @@ struct RenderingCallback { class Sim { private: - Config cfg; + SimConfig cfg; std::vector callbacks; std::vector any_callbacks; std::vector all_callbacks; @@ -74,8 +75,8 @@ class Sim { mjModel* m; mjData* d; Sim(mjModel* m, mjData* d); - bool set_config(const Config& cfg); - Config get_config(); + bool set_config(const SimConfig& cfg); + SimConfig get_config(); bool is_converged(); void step_until_convergence(); void step(size_t k); diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 602ba8cd..41cf2916 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -123,7 +123,7 @@ int test_sim() { auto sim = std::make_shared(m, d); auto cfg = sim->get_config(); cfg.realtime = true; - cfg.async = false; + cfg.async_control = false; sim->set_config(cfg); std::string id = "0"; From 48fe5428a933513e9cda8a46d04d47e243115efe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 17:16:53 +0200 Subject: [PATCH 7/9] style: py and cpp format --- examples/fr3/fr3_env_cartesian_control.py | 2 ++ examples/fr3/fr3_env_joint_control.py | 2 ++ examples/fr3/grasp_demo.py | 4 +++- python/rcs/envs/sim.py | 10 +++++----- python/rcs/sim/__init__.py | 2 +- python/rcs/sim/sim.py | 2 +- src/pybind/rcs.cpp | 13 ++++--------- src/sim/sim.h | 2 +- 8 files changed, 19 insertions(+), 18 deletions(-) diff --git a/examples/fr3/fr3_env_cartesian_control.py b/examples/fr3/fr3_env_cartesian_control.py index b9c070f7..a34104bf 100644 --- a/examples/fr3/fr3_env_cartesian_control.py +++ b/examples/fr3/fr3_env_cartesian_control.py @@ -26,6 +26,7 @@ ROBOT_INSTANCE = RobotPlatform.SIMULATION FR3_IP = "192.168.101.1" + def main(): if ROBOT_INSTANCE == RobotPlatform.SIMULATION: env_rel = SimEnvCreator()( @@ -40,6 +41,7 @@ def main(): 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, diff --git a/examples/fr3/fr3_env_joint_control.py b/examples/fr3/fr3_env_joint_control.py index 853d828c..3a0887d8 100644 --- a/examples/fr3/fr3_env_joint_control.py +++ b/examples/fr3/fr3_env_joint_control.py @@ -28,6 +28,7 @@ ROBOT_INSTANCE = RobotPlatform.SIMULATION FR3_IP = "192.168.101.1" + def main(): if ROBOT_INSTANCE == RobotPlatform.SIMULATION: env_rel = SimEnvCreator()( @@ -42,6 +43,7 @@ def main(): 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, diff --git a/examples/fr3/grasp_demo.py b/examples/fr3/grasp_demo.py index 10e361b7..d54efc06 100644 --- a/examples/fr3/grasp_demo.py +++ b/examples/fr3/grasp_demo.py @@ -29,7 +29,9 @@ def get_object_pose(self, geom_name) -> Pose: geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name) 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 + ) * 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]: diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index 4655b39a..d4223510 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -1,4 +1,5 @@ import logging +import time from typing import Any, SupportsFloat, Type, cast import gymnasium as gym @@ -12,11 +13,10 @@ ) from rcs.envs.space_utils import ActObsInfoWrapper from rcs.envs.utils import default_sim_robot_cfg, default_sim_tilburg_hand_cfg +from rcs.utils import SimpleFrameRate import rcs -import time from rcs import sim -from rcs.utils import SimpleFrameRate logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -45,15 +45,15 @@ def __init__(self, env, simulation: sim.Sim, sim_wrapper: SimWrapper | None = No self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) self.sim = simulation cfg = self.sim.get_config() - self.frame_rate = SimpleFrameRate(1/cfg.frequency, "RobotSimWrapper") + self.frame_rate = SimpleFrameRate(1 / cfg.frequency, "RobotSimWrapper") def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: _, _, _, _, info = super().step(action) cfg = self.sim.get_config() if cfg.async_control: - self.sim.step(round(1/cfg.frequency / self.sim.model.opt.timestep)) + self.sim.step(round(1 / cfg.frequency / self.sim.model.opt.timestep)) if cfg.realtime: - self.frame_rate.frame_rate = 1/cfg.frequency + self.frame_rate.frame_rate = 1 / cfg.frequency self.frame_rate() else: diff --git a/python/rcs/sim/__init__.py b/python/rcs/sim/__init__.py index 6331b8bf..dab8c835 100644 --- a/python/rcs/sim/__init__.py +++ b/python/rcs/sim/__init__.py @@ -1,5 +1,6 @@ from rcs._core.sim import ( SimCameraConfig, + SimConfig, SimGripper, SimGripperConfig, SimGripperState, @@ -9,7 +10,6 @@ SimTilburgHand, SimTilburgHandConfig, SimTilburgHandState, - SimConfig, ) from rcs.sim.sim import Sim, gui_loop diff --git a/python/rcs/sim/sim.py b/python/rcs/sim/sim.py index 17e252c7..6af3e47e 100644 --- a/python/rcs/sim/sim.py +++ b/python/rcs/sim/sim.py @@ -12,7 +12,7 @@ import mujoco.viewer from rcs._core.sim import GuiClient as _GuiClient from rcs._core.sim import Sim as _Sim -from rcs.sim import egl_bootstrap, SimConfig +from rcs.sim import SimConfig, egl_bootstrap from rcs.utils import SimpleFrameRate egl_bootstrap.bootstrap() diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index e1270058..aa26abe7 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -487,15 +487,11 @@ PYBIND11_MODULE(_core, m) { .def_readonly("last_width", &rcs::sim::SimGripperState::last_width) .def_readonly("collision", &rcs::sim::SimGripperState::collision); - py::class_( - sim, "SimConfig") + py::class_(sim, "SimConfig") .def(py::init<>()) - .def_readwrite("async_control", - &rcs::sim::SimConfig::async_control) - .def_readwrite("realtime", - &rcs::sim::SimConfig::realtime) - .def_readwrite("frequency", - &rcs::sim::SimConfig::frequency) + .def_readwrite("async_control", &rcs::sim::SimConfig::async_control) + .def_readwrite("realtime", &rcs::sim::SimConfig::realtime) + .def_readwrite("frequency", &rcs::sim::SimConfig::frequency) .def_readwrite("max_convergence_steps", &rcs::sim::SimConfig::max_convergence_steps); @@ -514,7 +510,6 @@ PYBIND11_MODULE(_core, m) { .def("_start_gui_server", &rcs::sim::Sim::start_gui_server, py::arg("id")) .def("_stop_gui_server", &rcs::sim::Sim::stop_gui_server); - py::class_>(sim, "SimGripper") .def(py::init, diff --git a/src/sim/sim.h b/src/sim/sim.h index 607cb2f9..62a2fbbf 100644 --- a/src/sim/sim.h +++ b/src/sim/sim.h @@ -29,7 +29,7 @@ class Renderer { struct SimConfig { bool async_control = false; bool realtime = false; - int frequency = 30; // in Hz + int frequency = 30; // in Hz int max_convergence_steps = 500; }; From 30f9521d617154f9ebff4d6094163bb066f06492 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 17:39:38 +0200 Subject: [PATCH 8/9] docs(example): moved xarm examples to example folder - moved xarm examples to example folder - fixed types in linting --- .gitignore | 1 + examples/xarm7/xarm7_env_cartesian_control.py | 104 ++++++++++------- examples/xarm7/xarm7_env_joint_control.py | 97 ++++++++++------ .../rcs_xarm7/src/rcs_xarm7/creators.py | 4 +- .../src/rcs_xarm7/env_cartesian_control.py | 107 ------------------ .../src/rcs_xarm7/env_joint_control.py | 84 -------------- python/rcs/envs/sim.py | 3 +- python/rcs/sim/sim.py | 2 +- 8 files changed, 127 insertions(+), 275 deletions(-) delete mode 100644 extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py delete mode 100644 extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py diff --git a/.gitignore b/.gitignore index 362217ad..3ee6fb06 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,4 @@ config.h wheels .env settings.json +*.egg-info diff --git a/examples/xarm7/xarm7_env_cartesian_control.py b/examples/xarm7/xarm7_env_cartesian_control.py index 5f8c48cb..9bf3e50c 100644 --- a/examples/xarm7/xarm7_env_cartesian_control.py +++ b/examples/xarm7/xarm7_env_cartesian_control.py @@ -1,7 +1,10 @@ import logging +from time import sleep +from rcs._core.common import RobotPlatform from rcs.envs.base import ControlMode, RelativeTo from rcs.envs.creators import SimEnvCreator +from rcs_xarm7.creators import RCSXArm7EnvCreator import rcs from rcs import sim @@ -9,60 +12,77 @@ logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) +""" +The example shows how to create a xArm7 environment with Cartesian control +and a relative action space. The example works both with a real robot and in +simulation. + +To test with a real robot, set ROBOT_INSTANCE to RobotPlatform.HARDWARE, +install the rcs_xarm7 extension (`pip install extensions/rcs_xarm7`) +and set the ROBOT_IP variable to the robot's IP address. +""" + +ROBOT_IP = "192.168.1.245" +ROBOT_INSTANCE = RobotPlatform.SIMULATION +# ROBOT_INSTANCE = RobotPlatform.HARDWARE + def main(): - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ - "act1", - "act2", - "act3", - "act4", - "act5", - "act6", - "act7", - ] - robot_cfg.joints = [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - "joint7", - ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot - env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, - control_mode=ControlMode.CARTESIAN_TQuat, - gripper_cfg=None, - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSXArm7EnvCreator()( + control_mode=ControlMode.CARTESIAN_TQuat, + ip=ROBOT_IP, + relative_to=RelativeTo.LAST_STEP, + max_relative_movement=0.5, + ) + else: + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = [ + "act1", + "act2", + "act3", + "act4", + "act5", + "act6", + "act7", + ] + robot_cfg.joints = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_cfg.attachment_site = "attachment_site" + robot_cfg.arm_collision_geoms = [] + robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.CARTESIAN_TQuat, + gripper_cfg=None, + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=0.5, + relative_to=RelativeTo.LAST_STEP, + ) + sleep(3) # wait for gui to open + env_rel.get_wrapper_attr("sim").open_gui() obs, info = env_rel.reset() 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]} 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]} obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return if __name__ == "__main__": diff --git a/examples/xarm7/xarm7_env_joint_control.py b/examples/xarm7/xarm7_env_joint_control.py index f2d2433a..5b98abdd 100644 --- a/examples/xarm7/xarm7_env_joint_control.py +++ b/examples/xarm7/xarm7_env_joint_control.py @@ -1,8 +1,11 @@ import logging +from time import sleep 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_xarm7.creators import RCSXArm7EnvCreator import rcs from rcs import sim @@ -10,42 +13,65 @@ logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) +""" +The example shows how to create a xArm7 environment with joint control +and a relative action space. The example works both with a real robot and in +simulation. + +To test with a real robot, set ROBOT_INSTANCE to RobotPlatform.HARDWARE, +install the rcs_xarm7 extension (`pip install extensions/rcs_xarm7`) +and set the ROBOT_IP variable to the robot's IP address. +""" + +ROBOT_IP = "192.168.1.245" +ROBOT_INSTANCE = RobotPlatform.SIMULATION +# ROBOT_INSTANCE = RobotPlatform.HARDWARE + def main(): - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ - "act1", - "act2", - "act3", - "act4", - "act5", - "act6", - "act7", - ] - robot_cfg.joints = [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - "joint7", - ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot - env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, - control_mode=ControlMode.JOINTS, - gripper_cfg=None, - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=np.deg2rad(5), - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSXArm7EnvCreator()( + control_mode=ControlMode.JOINTS, + ip=ROBOT_IP, + relative_to=RelativeTo.LAST_STEP, + max_relative_movement=np.deg2rad(5), + ) + else: + robot_cfg = sim.SimRobotConfig() + robot_cfg.actuators = [ + "act1", + "act2", + "act3", + "act4", + "act5", + "act6", + "act7", + ] + robot_cfg.joints = [ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ] + robot_cfg.base = "base" + robot_cfg.robot_type = rcs.common.RobotType.XArm7 + robot_cfg.attachment_site = "attachment_site" + robot_cfg.arm_collision_geoms = [] + robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb + robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot + env_rel = SimEnvCreator()( + robot_cfg=robot_cfg, + control_mode=ControlMode.JOINTS, + gripper_cfg=None, + # cameras=default_mujoco_cameraset_cfg(), + max_relative_movement=np.deg2rad(5), + relative_to=RelativeTo.LAST_STEP, + ) + env_rel.get_wrapper_attr("sim").open_gui() + sleep(3) # wait for gui to open for _ in range(100): obs, info = env_rel.reset() @@ -53,9 +79,6 @@ def main(): # sample random relative action and execute it act = env_rel.action_space.sample() obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return if __name__ == "__main__": diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py index 49522751..7c59e103 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/creators.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/creators.py @@ -19,13 +19,13 @@ RobotEnv, ) from rcs.envs.creators import RCSHardwareEnvCreator -from rcs.envs.sim import CollisionGuard, GripperWrapperSim, RobotSimWrapper, SimWrapper +from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper, SimWrapper from rcs.hand.tilburg_hand import THConfig, TilburgHand from rcs.sim import SimCameraConfig, SimGripperConfig, SimRobotConfig from rcs_xarm7.hw import XArm7 import rcs -from rcs import common, sim +from rcs import sim logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py deleted file mode 100644 index 41893b14..00000000 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_cartesian_control.py +++ /dev/null @@ -1,107 +0,0 @@ -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_sim_tilburg_hand_cfg -from rcs.hand.tilburg_hand import THConfig -from rcs_xarm7.creators import RCSXArm7EnvCreator - -import rcs -from rcs import sim - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - -ROBOT_IP = "192.168.1.245" -# ROBOT_INSTANCE = RobotPlatform.SIMULATION -ROBOT_INSTANCE = RobotPlatform.HARDWARE - - -def sim_env(): - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ - "act1", - "act2", - "act3", - "act4", - "act5", - "act6", - "act7", - ] - robot_cfg.joints = [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - "joint7", - ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot - env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, - control_mode=ControlMode.CARTESIAN_TQuat, - gripper_cfg=None, - hand_cfg=default_sim_tilburg_hand_cfg(), - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - return env_rel - - -def main(): - - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - hand_cfg = THConfig( - calibration_file="/home/ken/tilburg_hand/calibration.json", grasp_percentage=1, hand_orientation="right" - ) - env_rel = RCSXArm7EnvCreator()( - control_mode=ControlMode.CARTESIAN_TQuat, - ip=ROBOT_IP, - hand_cfg=hand_cfg, - relative_to=RelativeTo.LAST_STEP, - max_relative_movement=np.deg2rad(3), - ) - else: - env_rel = sim_env() - - twin_env = sim_env() - twin_robot = twin_env.unwrapped.robot - twin_sim = twin_env.get_wrapper_attr("sim") - - env_rel.reset() - # act = {"tquat": [0.1, 0, 0, 0, 0, 0, 1], "gripper": 0} - # obs, reward, terminated, truncated, info = env_rel.step(act) - - with env_rel: - for _ in range(10): - for _ in range(10): - # move 1cm in x direction (forward) and close gripper - act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "hand": 1} - obs, reward, terminated, truncated, info = env_rel.step(act) - joints = env_rel.unwrapped.robot.get_joint_position() # type: ignore - twin_robot.set_joints_hard(joints) - twin_sim.step(50) - 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], "hand": 0} - obs, reward, terminated, truncated, info = env_rel.step(act) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - - -if __name__ == "__main__": - main() diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py b/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py deleted file mode 100644 index 7abbaad8..00000000 --- a/extensions/rcs_xarm7/src/rcs_xarm7/env_joint_control.py +++ /dev/null @@ -1,84 +0,0 @@ -import logging -from time import sleep - -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_xarm7.creators import RCSXArm7EnvCreator - -import rcs -from rcs import sim - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - -ROBOT_IP = "192.168.1.245" -# ROBOT_INSTANCE = RobotPlatform.SIMULATION -ROBOT_INSTANCE = RobotPlatform.HARDWARE - - -def main(): - - if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - env_rel = RCSXArm7EnvCreator()( - control_mode=ControlMode.JOINTS, - ip=ROBOT_IP, - relative_to=RelativeTo.LAST_STEP, - max_relative_movement=np.deg2rad(3), - ) - else: - robot_cfg = sim.SimRobotConfig() - robot_cfg.actuators = [ - "act1", - "act2", - "act3", - "act4", - "act5", - "act6", - "act7", - ] - robot_cfg.joints = [ - "joint1", - "joint2", - "joint3", - "joint4", - "joint5", - "joint6", - "joint7", - ] - robot_cfg.base = "base" - robot_cfg.robot_type = rcs.common.RobotType.XArm7 - robot_cfg.attachment_site = "attachment_site" - robot_cfg.arm_collision_geoms = [] - robot_cfg.mjcf_scene_path = rcs.scenes["xarm7_empty_world"].mjb - robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot - env_rel = SimEnvCreator()( - robot_cfg=robot_cfg, - control_mode=ControlMode.JOINTS, - gripper_cfg=None, - # cameras=default_mujoco_cameraset_cfg(), - max_relative_movement=np.deg2rad(5), - relative_to=RelativeTo.LAST_STEP, - ) - env_rel.get_wrapper_attr("sim").open_gui() - - with env_rel: - for _ in range(10): - obs, info = env_rel.reset() - for _ in range(3): - # sample random relative action and execute it - act = env_rel.action_space.sample() - print(act) - # act["gripper"] = 1.0 - obs, reward, terminated, truncated, info = env_rel.step(act) - print(obs) - if truncated or terminated: - logger.info("Truncated or terminated!") - return - # logger.info(act["gripper"]) - sleep(2.0) - - -if __name__ == "__main__": - main() diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index d4223510..80b0c4d9 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -1,5 +1,4 @@ import logging -import time from typing import Any, SupportsFloat, Type, cast import gymnasium as gym @@ -35,7 +34,7 @@ def __init__(self, env: gym.Env, simulation: sim.Sim): class RobotSimWrapper(gym.Wrapper): - def __init__(self, env, simulation: sim.Sim, sim_wrapper: SimWrapper | None = None): + def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | None = None): self.sim_wrapper = sim_wrapper if sim_wrapper is not None: env = sim_wrapper(env, simulation) diff --git a/python/rcs/sim/sim.py b/python/rcs/sim/sim.py index 6af3e47e..542e7370 100644 --- a/python/rcs/sim/sim.py +++ b/python/rcs/sim/sim.py @@ -42,7 +42,7 @@ def gui_loop(gui_uuid: str, close_event): class Sim(_Sim): - def __init__(self, mjmdl: str | PathLike, cfg: SimConfig = None): + def __init__(self, mjmdl: str | PathLike, cfg: SimConfig | None = None): mjmdl = Path(mjmdl) if mjmdl.suffix == ".xml": self.model = mj.MjModel.from_xml_path(str(mjmdl)) From 719cd1e3d34a962d0741609342b972389cb42ff3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 25 Sep 2025 18:40:08 +0200 Subject: [PATCH 9/9] docs: adapted readme with new example structure --- README.md | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index f40c86b0..6fd0d549 100644 --- a/README.md +++ b/README.md @@ -92,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