diff --git a/assets/robots/yam/assets/model2.stl b/assets/robots/yam/assets/model2.stl
new file mode 100644
index 00000000..d597a051
Binary files /dev/null and b/assets/robots/yam/assets/model2.stl differ
diff --git a/assets/robots/yam/assets/model2__10.stl b/assets/robots/yam/assets/model2__10.stl
new file mode 100644
index 00000000..b065de19
Binary files /dev/null and b/assets/robots/yam/assets/model2__10.stl differ
diff --git a/assets/robots/yam/assets/model2__11.stl b/assets/robots/yam/assets/model2__11.stl
new file mode 100644
index 00000000..736495bc
Binary files /dev/null and b/assets/robots/yam/assets/model2__11.stl differ
diff --git a/assets/robots/yam/assets/model2__12.stl b/assets/robots/yam/assets/model2__12.stl
new file mode 100644
index 00000000..356322f5
Binary files /dev/null and b/assets/robots/yam/assets/model2__12.stl differ
diff --git a/assets/robots/yam/assets/model2__13.stl b/assets/robots/yam/assets/model2__13.stl
new file mode 100644
index 00000000..41dcd970
Binary files /dev/null and b/assets/robots/yam/assets/model2__13.stl differ
diff --git a/assets/robots/yam/assets/model2__14.stl b/assets/robots/yam/assets/model2__14.stl
new file mode 100644
index 00000000..d5f7aa0e
Binary files /dev/null and b/assets/robots/yam/assets/model2__14.stl differ
diff --git a/assets/robots/yam/assets/model2__15.stl b/assets/robots/yam/assets/model2__15.stl
new file mode 100644
index 00000000..28cabf6f
Binary files /dev/null and b/assets/robots/yam/assets/model2__15.stl differ
diff --git a/assets/robots/yam/assets/model2__16.stl b/assets/robots/yam/assets/model2__16.stl
new file mode 100644
index 00000000..1b397ba5
Binary files /dev/null and b/assets/robots/yam/assets/model2__16.stl differ
diff --git a/assets/robots/yam/assets/model2__17.stl b/assets/robots/yam/assets/model2__17.stl
new file mode 100644
index 00000000..c6859299
Binary files /dev/null and b/assets/robots/yam/assets/model2__17.stl differ
diff --git a/assets/robots/yam/assets/model2__2.stl b/assets/robots/yam/assets/model2__2.stl
new file mode 100644
index 00000000..2d180f05
Binary files /dev/null and b/assets/robots/yam/assets/model2__2.stl differ
diff --git a/assets/robots/yam/assets/model2__3.stl b/assets/robots/yam/assets/model2__3.stl
new file mode 100644
index 00000000..89c512c0
Binary files /dev/null and b/assets/robots/yam/assets/model2__3.stl differ
diff --git a/assets/robots/yam/assets/model2__4.stl b/assets/robots/yam/assets/model2__4.stl
new file mode 100644
index 00000000..7186d0fa
Binary files /dev/null and b/assets/robots/yam/assets/model2__4.stl differ
diff --git a/assets/robots/yam/assets/model2__5.stl b/assets/robots/yam/assets/model2__5.stl
new file mode 100644
index 00000000..afd1996e
Binary files /dev/null and b/assets/robots/yam/assets/model2__5.stl differ
diff --git a/assets/robots/yam/assets/model2__6.stl b/assets/robots/yam/assets/model2__6.stl
new file mode 100644
index 00000000..71fc6c2f
Binary files /dev/null and b/assets/robots/yam/assets/model2__6.stl differ
diff --git a/assets/robots/yam/assets/model2__7.stl b/assets/robots/yam/assets/model2__7.stl
new file mode 100644
index 00000000..1aef9c14
Binary files /dev/null and b/assets/robots/yam/assets/model2__7.stl differ
diff --git a/assets/robots/yam/assets/model2__8.stl b/assets/robots/yam/assets/model2__8.stl
new file mode 100644
index 00000000..ad2475d4
Binary files /dev/null and b/assets/robots/yam/assets/model2__8.stl differ
diff --git a/assets/robots/yam/assets/model2__9.stl b/assets/robots/yam/assets/model2__9.stl
new file mode 100644
index 00000000..e6287e04
Binary files /dev/null and b/assets/robots/yam/assets/model2__9.stl differ
diff --git a/assets/robots/yam/yam.xml b/assets/robots/yam/yam.xml
new file mode 100644
index 00000000..30caca63
--- /dev/null
+++ b/assets/robots/yam/yam.xml
@@ -0,0 +1,202 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/docs/extensions/index.md b/docs/extensions/index.md
index 67ce5944..89964125 100644
--- a/docs/extensions/index.md
+++ b/docs/extensions/index.md
@@ -10,6 +10,7 @@ rcs_panda
rcs_xarm7
rcs_ur5e
rcs_so101
+rcs_yam
rcs_realsense
rcs_usb_cam
rcs_tacto
diff --git a/docs/extensions/overview.md b/docs/extensions/overview.md
index 8494cba0..2a25fd4d 100644
--- a/docs/extensions/overview.md
+++ b/docs/extensions/overview.md
@@ -33,6 +33,7 @@ RCS comes with several supported extensions:
- **rcs_xarm7**: Support for the xArm7 robot.
- **rcs_ur5e**: Support for the UR5e robot.
- **rcs_so101**: Support for the SO101 robot.
+- **rcs_yam**: Support for the I2RT YAM arm.
- **rcs_realsense**: Support for Intel RealSense cameras.
- **rcs_usb_cam**: Support for generic USB webcams.
- **rcs_tacto**: Integration with the Tacto tactile sensor simulator.
diff --git a/docs/extensions/rcs_yam.md b/docs/extensions/rcs_yam.md
new file mode 100644
index 00000000..d8599e12
--- /dev/null
+++ b/docs/extensions/rcs_yam.md
@@ -0,0 +1,60 @@
+# RCS YAM Extension
+
+This extension provides support for the I2RT YAM arm in RCS, built on the
+[i2rt](https://github.com/i2rt-robotics/i2rt) Python driver.
+
+## Installation
+
+`i2rt` is not published on PyPI and is pinned as a direct git reference, so this extension is
+installed from a checkout:
+
+```shell
+pip install -ve . --no-build-isolation
+pip install -ve extensions/rcs_yam
+```
+
+Bring up the CAN interface of the arm before use:
+
+```shell
+sudo ip link set can0 up type can bitrate 1000000
+```
+
+## Usage
+
+```python
+from rcs.envs.base import ControlMode
+from rcs_yam.configs import DefaultYamHardwareEnv
+
+env_creator = DefaultYamHardwareEnv()
+env_creator.channel = "can0"
+
+cfg = env_creator.config()
+cfg.control_mode = ControlMode.JOINTS
+cfg.robot_cfg.async_control = False
+
+env = env_creator.create_env(cfg)
+obs, info = env.reset()
+```
+
+## Sync and async control
+
+The i2rt driver runs its PD control loop in a background thread. With
+`YamConfig.async_control=True` the RCS setters return as soon as the target is handed over, with
+`async_control=False` they poll the measured state until the target is reached or the timeout hits.
+`move_home` interpolates the motion and always blocks.
+
+## Notes
+
+- Arm and gripper share one motor chain, since i2rt exposes the gripper motor as the last entry of
+ the arm's chain. The gripper is created from the same robot instance.
+- The `linear_4310` gripper calibrates on startup and drives the fingers to both end stops unless
+ `gripper_limits_override` is set.
+- There is no compliant force control and no trajectory generation in the driver. Targets further
+ than `max_joint_step` away are ramped rather than sent as a step.
+- The simulated counterpart is registered as `rcs/yam` in core RCS and shares the kinematics, joint
+ limits and gripper stroke of the hardware.
+
+See `extensions/rcs_yam/README.md` for the full extension documentation and
+`extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py` for a bring-up script. For a maintained
+example, see `examples/yam/yam_env_cartesian_control.py`, which moves the TCP forward and backward in
+synchronous Cartesian mode in simulation or on hardware.
diff --git a/examples/teleop/README.md b/examples/teleop/README.md
index ed070f3a..fd4069f4 100644
--- a/examples/teleop/README.md
+++ b/examples/teleop/README.md
@@ -49,6 +49,14 @@ Note that dual arm is only supported for a aloha like setup where the robot face
9. Start the teleoperation script [`franka.py`](franka.py) and enjoy.
+## Teleoperation of a YAM with Meta Quest 3
+[`yam.py`](yam.py) is the same setup for a single [YAM](https://robotcontrolstack.org/extensions/rcs_yam) arm.
+Follow the quest steps above, they are identical, and align the coordinate axis to the arm base (x front, y left, z up).
+The arm follows the right controller, so keep `ROBOT_NAME = "right"` unless you also change the controller mapping.
+1. install the [YAM extension](https://robotcontrolstack.org/extensions/rcs_yam) and bring up its CAN interface (`sudo ip link set can0 up type can bitrate 1000000`)
+2. go to [`yam.py`](yam.py), set `ROBOT_INSTANCE = RobotPlatform.HARDWARE` and set `CAN_CHANNEL` to your interface, or keep `RobotPlatform.SIMULATION` to try it in sim first
+3. to record RealSense cameras, install the [RealSense extension](https://robotcontrolstack.org/extensions/rcs_realsense) and put your camera serial numbers (`rs-enumerate-devices -s`) into `CAMERA_DICT`, or set it to `None` to run without cameras
+
## Teleoperation with Franka GELLO Duo
Teleoperate your Franka Duo using the [Franka GELLO Duo](https://franka.de/de-de/product-prototypes).
Install dependencies via
diff --git a/examples/teleop/franka.py b/examples/teleop/franka.py
index b543c96d..8c539fb7 100644
--- a/examples/teleop/franka.py
+++ b/examples/teleop/franka.py
@@ -77,6 +77,7 @@
simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION,
switched_left_right=False,
display_cameras=False,
+ read_frequency=90,
)
# config = GelloConfig(
# arms={
diff --git a/examples/teleop/yam.py b/examples/teleop/yam.py
new file mode 100644
index 00000000..81fc1b53
--- /dev/null
+++ b/examples/teleop/yam.py
@@ -0,0 +1,119 @@
+import logging
+
+import numpy as np
+from rcs._core.common import BaseCameraConfig, RobotPlatform
+from rcs._core.sim import SimConfig
+from rcs.envs.configs import EmptyWorldYam
+from rcs.envs.storage_wrapper import StorageWrapper
+from rcs.operator.interface import TeleopLoop
+from rcs.operator.quest import QuestConfig, QuestOperator
+from simpub.sim.mj_publisher import MujocoPublisher
+
+logger = logging.getLogger(__name__)
+
+"""
+Teleoperation of two YAM arms with the Meta Quest 3. See README.md for the setup of the quest and
+the IRIS app; each arm follows its controller while the trigger is held, and the hand trigger
+drives the gripper.
+
+To teleoperate real hardware, install the rcs_yam extension (`pip install -ve extensions/rcs_yam`),
+bring up the CAN interfaces (`sudo ip link set can0 up type can bitrate 1000000`, same for can1)
+and set ROBOT_INSTANCE to RobotPlatform.HARDWARE.
+
+RealSense cameras are recorded on hardware when CAMERA_DICT is set, which needs the rcs_realsense
+extension (`pip install -ve extensions/rcs_realsense`).
+"""
+
+ROBOT_INSTANCE = RobotPlatform.HARDWARE
+# The teleop loop matches robots to controllers by name: each arm is named after the controller
+# that drives it.
+CAN_CHANNELS = {"left": "can0", "right": "can1"}
+
+MQ3_ADDR = "10.42.0.1"
+RECORD_FPS = 30
+
+# Serial numbers of the RealSense cameras, use `rs-enumerate-devices -s` to list them.
+# Set CAMERA_DICT to None to disable cameras.
+CAMERA_DICT = {
+ "right_wrist": "230422272017",
+ "left_wrist": "230422271040",
+}
+# CAMERA_DICT = None
+INCLUDE_DEPTH = False
+
+DATASET_PATH = "yam_teleop"
+INSTRUCTION = "pick up cube"
+
+config = QuestConfig(
+ mq3_addr=MQ3_ADDR,
+ simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION,
+ switched_left_right=True,
+ display_cameras=False,
+ read_frequency=90,
+)
+
+
+def get_env():
+ if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
+ from rcs_yam.configs import DefaultYamDualMultiHardwareEnv
+ from rcs_yam.creators import HardwareCameraCreatorConfig
+
+ env_creator = DefaultYamDualMultiHardwareEnv()
+ env_creator.left_channel = CAN_CHANNELS["left"]
+ env_creator.right_channel = CAN_CHANNELS["right"]
+ # The dual config already enables async_control, so the setters do not wait for the arms.
+ hw_cfg = env_creator.config()
+ camera_cfgs: dict[str, HardwareCameraCreatorConfig] = {}
+ if CAMERA_DICT is not None:
+ camera_cfgs["realsense"] = HardwareCameraCreatorConfig(
+ camera_type_id="realsense",
+ camera_cfgs={
+ name: BaseCameraConfig(
+ identifier=identifier,
+ resolution_width=1280,
+ resolution_height=720,
+ frame_rate=30,
+ )
+ for name, identifier in CAMERA_DICT.items()
+ },
+ )
+ hw_cfg.camera_cfgs = camera_cfgs or None
+ hw_cfg.control_mode = config.operator_class.control_mode[0]
+ hw_cfg.relative_to = config.operator_class.control_mode[1]
+ hw_cfg.max_relative_movement = (0.5, np.deg2rad(90))
+ hw_cfg.wrapper_cfg.binary_gripper = False
+ hw_cfg.wrapper_cfg.include_depth = INCLUDE_DEPTH
+ env_rel = env_creator.create_env(hw_cfg)
+ operator = QuestOperator(config)
+ else:
+ scene = EmptyWorldYam()
+ sim_cfg_data = scene.config()
+ sim_cfg_data.sim_cfg = SimConfig(
+ async_control=True, realtime=True, frequency=RECORD_FPS, max_convergence_steps=500
+ )
+ sim_cfg_data.control_mode = config.operator_class.control_mode[0]
+ sim_cfg_data.relative_to = config.operator_class.control_mode[1]
+ sim_cfg_data.max_relative_movement = (0.5, np.deg2rad(90))
+ sim_cfg_data.wrapper_cfg.binary_gripper = False
+ env_rel = scene.create_env(sim_cfg_data)
+
+ sim = env_rel.get_wrapper_attr("sim")
+ MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3)))
+ operator = QuestOperator(config, sim)
+
+ env_rel = StorageWrapper(
+ env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000
+ )
+ return env_rel, operator
+
+
+def main():
+ env_rel, operator = get_env()
+ env_rel.reset()
+ tele = TeleopLoop(env_rel, operator, env_frequency=RECORD_FPS, robot_platform=ROBOT_INSTANCE)
+ with env_rel, tele: # type: ignore
+ tele.environment_step_loop()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/examples/yam/yam_env_cartesian_control.py b/examples/yam/yam_env_cartesian_control.py
new file mode 100644
index 00000000..ec99726e
--- /dev/null
+++ b/examples/yam/yam_env_cartesian_control.py
@@ -0,0 +1,124 @@
+import logging
+from time import sleep
+
+import gymnasium as gym
+import numpy as np
+from rcs._core.common import RobotPlatform
+from rcs._core.sim import SimConfig
+from rcs.envs.base import (
+ ControlMode,
+ CoverWrapper,
+ GripperWrapper,
+ RelativeActionSpace,
+ RelativeTo,
+ RobotWrapper,
+ SimEnv,
+)
+from rcs.envs.configs import EmptyWorldYam
+from rcs.envs.sim import GripperWrapperSim, RobotSimWrapper
+
+import rcs
+from rcs import sim
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+"""
+This script demonstrates Cartesian position control of the YAM arm in synchronous mode. The arm
+first moves to its home pose, ramped rather than snapped, and then moves 1cm forward and backward
+along the base x axis in a loop. Every step goes through inverse kinematics, so the printed TCP
+positions tracking the commanded ones show that IK works.
+
+To control a real YAM arm, install the rcs_yam extension (`pip install -ve extensions/rcs_yam`),
+bring up its CAN interface (`sudo ip link set can0 up type can bitrate 1000000`) and set
+ROBOT_INSTANCE to RobotPlatform.HARDWARE. Note that the linear_4310 gripper calibrates on startup
+and drives its fingers to both end stops.
+"""
+
+ROBOT_INSTANCE = RobotPlatform.SIMULATION # Change to RobotPlatform.HARDWARE for the real arm
+CAN_CHANNEL = "can0"
+
+STEP_SIZE = 0.01 # meters per step
+STEPS_PER_LEG = 5 # steps forward before reversing
+CYCLES = 100
+
+
+def main():
+ env_rel: gym.Env
+ if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
+ from rcs_yam.configs import DefaultYamHardwareEnv
+
+ env_creator = DefaultYamHardwareEnv()
+ env_creator.channel = CAN_CHANNEL
+ hw_cfg = env_creator.config()
+ hw_cfg.control_mode = ControlMode.CARTESIAN_TQuat
+ # Synchronous mode: every command returns once the arm has reached its target.
+ hw_cfg.robot_cfg.async_control = False
+ # Homing interpolates over this duration instead of stepping to the home pose.
+ hw_cfg.robot_cfg.move_home_duration = 3.0
+ hw_cfg.max_relative_movement = (0.05, np.deg2rad(5))
+ hw_cfg.relative_to = RelativeTo.LAST_STEP
+ env_rel = env_creator.create_env(hw_cfg)
+ input("the arm is going to move, press enter whenever you are ready")
+ else:
+ scene = EmptyWorldYam()
+ sim_cfg_data = scene.prefixed_cfg(scene.config())
+ yam = scene.lead_robot_name(sim_cfg_data)
+
+ robot_cfg = sim_cfg_data.robot_cfgs[yam]
+ gripper_cfg = sim_cfg_data.gripper_cfgs[yam] # type: ignore[index]
+ # Synchronous mode: the simulation steps until the commanded pose is reached.
+ sim_cfg = SimConfig(
+ realtime=False,
+ async_control=False,
+ )
+
+ mjmodel = scene.create_model(sim_cfg_data)
+ simulation = sim.Sim(mjmodel, sim_cfg)
+
+ kinematic_model_path, attachment_site = scene.kinematics_cfg(sim_cfg_data)[yam]
+ ik = rcs.common.Pin(
+ kinematic_model_path,
+ attachment_site,
+ )
+
+ robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
+ env_rel = SimEnv(simulation)
+ env_rel = RobotWrapper(env_rel, robot, ControlMode.CARTESIAN_TQuat)
+
+ gripper = sim.SimGripper(simulation, gripper_cfg)
+ env_rel = GripperWrapper(env_rel, gripper)
+
+ env_rel = RobotSimWrapper(env_rel)
+ env_rel = GripperWrapperSim(env_rel)
+
+ env_rel = RelativeActionSpace(
+ env_rel,
+ max_mov=(0.05, np.deg2rad(5)),
+ relative_to=RelativeTo.LAST_STEP,
+ )
+ env_rel = CoverWrapper(env_rel)
+ env_rel.get_wrapper_attr("sim").open_gui()
+
+ # Homing happens on reset, driving the joints to the home pose.
+ env_rel.reset()
+
+ robot_api = env_rel.get_wrapper_attr("robot")
+ print(f"home TCP: {np.round(robot_api.get_cartesian_position().translation(), 4)}")
+
+ 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)
+ sleep(0.1)
+ 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)
+
+ sleep(0.1)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/scripts/__init__.py b/extensions/rcs_ur5e/src/rcs_ur5e/scripts/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/extensions/rcs_yam/README.md b/extensions/rcs_yam/README.md
new file mode 100644
index 00000000..6be612dc
--- /dev/null
+++ b/extensions/rcs_yam/README.md
@@ -0,0 +1,128 @@
+# RCS YAM Extension
+
+Support for the I2RT YAM arm in RCS, built on the [i2rt](https://github.com/i2rt-robotics/i2rt) Python driver.
+
+This extension depends on [`rcs-core`](https://pypi.org/project/rcs-core/).
+Documentation:
+
+## Installation
+
+`i2rt` is not published on PyPI, so it is pinned as a direct git reference in `pyproject.toml`. As a
+consequence this extension is installable from a checkout but cannot be published to PyPI, and it is
+not part of the wheel build workflow.
+
+```shell
+# i2rt requires ruckig==0.15.3 which has no prebuild wheel, tested to work with ruckig==0.19.4
+pip install --override <(echo ruckig==0.19.4) -e extensions/rcs_yam
+```
+
+The i2rt driver talks to the motors over SocketCAN and needs kernel headers for its build
+dependencies:
+
+```shell
+sudo apt install build-essential python3-dev linux-headers-$(uname -r)
+```
+
+## CAN bus setup
+
+Each arm sits on its own CAN interface, running at 1 Mbit/s:
+
+```shell
+ls -l /sys/class/net/can* # list adapters
+sudo ip link set can0 up type can bitrate 1000000 # bring one up
+```
+
+The i2rt repository ships `devices/install_devices.sh` to enable interfaces on boot and
+`scripts/reset_all_can.sh` to reset an unresponsive adapter.
+
+## Safety notes
+
+- The arm is direct driven and follows a plain PD controller. There is no compliant force control
+ and no trajectory generation in the driver, so avoid commanding large jumps. Targets further than
+ `max_joint_step` away are ramped over `max_joint_step / max_joint_velocity` seconds instead of
+ being sent as a step.
+- The `linear_4310` gripper ships without stored limits and calibrates on startup, which drives the
+ fingers to both end stops the moment the robot is created. Set `gripper_limits_override` on
+ `YamConfig` to skip that run.
+- i2rt widens the joint limits from its model by 0.15 rad for its internal clipping. RCS uses the
+ tighter limits from `assets/robots/yam/yam.xml`.
+- Verify the home pose before homing on real hardware, and keep the workspace clear.
+
+## Sync and async control
+
+The driver runs its control loop in a background thread, so commands are inherently non-blocking.
+`YamConfig.async_control` selects what the RCS setters do on top of that:
+
+- `async_control=True`: `set_joint_position` and the gripper setters return as soon as the target has
+ been handed to the control loop.
+- `async_control=False`: they poll the measured state every 5 ms until every joint is within
+ `joint_tolerance` (or the gripper within `gripper_tolerance`) and return on `command_timeout`
+ (`gripper_timeout` for gripper-only commands) at the latest. A gripper holding an object never
+ reaches its commanded width, which is why it has a shorter timeout of its own.
+
+`move_home` always blocks, since it interpolates the whole motion.
+
+## Usage
+
+```python
+from rcs.envs.base import ControlMode
+from rcs_yam.configs import DefaultYamHardwareEnv
+
+env_creator = DefaultYamHardwareEnv()
+env_creator.channel = "can0"
+
+cfg = env_creator.config()
+cfg.control_mode = ControlMode.JOINTS
+cfg.robot_cfg.async_control = False
+
+env = env_creator.create_env(cfg)
+obs, info = env.reset()
+```
+
+The robot and the gripper share one motor chain, because i2rt exposes the gripper motor as the last
+entry of the arm's chain. `create_env` therefore builds the `YamGripper` from the same `Yam`
+instance, and closing the env closes the chain once.
+
+Without the env wrappers:
+
+```python
+import rcs
+from rcs import common
+from rcs_yam.hw import Yam, YamConfig, YamGripper
+
+cfg = YamConfig(channel="can0", async_control=False, dof=6, ...)
+ik = common.Pin(cfg.kinematic_model_path, cfg.attachment_site)
+robot = Yam(cfg, ik)
+gripper = YamGripper(common.GripperConfig(gripper_type=common.GripperType("Yam")), robot)
+
+robot.move_home()
+gripper.grasp()
+robot.close()
+```
+
+See `src/rcs_yam/scripts/test_robot.py` for a complete bring-up script covering both modes, and
+[examples/yam/yam_env_cartesian_control.py](../../examples/yam/yam_env_cartesian_control.py) for a
+maintained Cartesian control example that runs in simulation and on hardware.
+
+## Bimanual setups
+
+Each arm is a separate env on its own CAN channel, combined with `MultiRobotWrapper`:
+
+```python
+from rcs.envs.base import MultiRobotWrapper
+from rcs_yam.configs import DefaultYamHardwareEnv
+
+envs = {}
+for name, channel in (("left", "can0"), ("right", "can1")):
+ creator = DefaultYamHardwareEnv()
+ creator.channel = channel
+ envs[name] = creator.create_env(creator.config())
+
+env = MultiRobotWrapper(envs)
+```
+
+## Simulation
+
+The matching simulated env is registered as `rcs/yam` in core RCS. The kinematics, joint limits and
+gripper stroke of `assets/robots/yam/yam.xml` match the i2rt `yam` v1 arm with the `linear_4310`
+gripper, so joint and cartesian targets carry over between simulation and hardware.
diff --git a/extensions/rcs_yam/pyproject.toml b/extensions/rcs_yam/pyproject.toml
new file mode 100644
index 00000000..3295c872
--- /dev/null
+++ b/extensions/rcs_yam/pyproject.toml
@@ -0,0 +1,30 @@
+[build-system]
+requires = ["setuptools"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "rcs_yam"
+version = "0.7.2"
+description = "RCS YAM module"
+dependencies = [
+ "rcs-core>=0.7.2",
+ # i2rt is not published on PyPI, so it is pinned to a commit of the upstream repository.
+ # A direct reference makes this project unpublishable to PyPI, see README.md.
+ "i2rt @ git+https://github.com/i2rt-robotics/i2rt@main", # tested commit: b9d8704c593aee4ef129d644f881564b8f2c4f6b
+]
+readme = "README.md"
+license = "AGPL-3.0-or-later"
+maintainers = [
+ { name = "Tobias Juelg", email = "tobias.juelg@utn.de" },
+]
+authors = [
+ { name = "Tobias Juelg", email = "tobias.juelg@utn.de" },
+]
+requires-python = ">=3.11"
+
+[tool.black]
+line-length = 120
+target-version = ["py310"]
+
+[tool.isort]
+profile = "black"
diff --git a/extensions/rcs_yam/src/rcs_yam/__init__.py b/extensions/rcs_yam/src/rcs_yam/__init__.py
new file mode 100644
index 00000000..f8ffe7a2
--- /dev/null
+++ b/extensions/rcs_yam/src/rcs_yam/__init__.py
@@ -0,0 +1,10 @@
+from rcs_yam import configs, creators, hw
+
+__version__ = "0.7.2"
+
+__all__ = [
+ "configs",
+ "creators",
+ "hw",
+ "__version__",
+]
diff --git a/extensions/rcs_yam/src/rcs_yam/configs.py b/extensions/rcs_yam/src/rcs_yam/configs.py
new file mode 100644
index 00000000..c1b134fd
--- /dev/null
+++ b/extensions/rcs_yam/src/rcs_yam/configs.py
@@ -0,0 +1,72 @@
+from rcs._core.common import GripperConfig, GripperType, RobotType
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs_yam.creators import (
+ RCSYamConfigEnvCreator,
+ RCSYamMultiConfigEnvCreator,
+ YamHardwareEnvCreatorConfig,
+ YamMultiHardwareEnvCreatorConfig,
+)
+from rcs_yam.hw import YamConfig
+
+import rcs
+
+
+class DefaultYamHardwareEnv(RCSYamConfigEnvCreator):
+ channel = "can0"
+
+ def config(self) -> YamHardwareEnvCreatorConfig:
+ robot_type = RobotType("Yam")
+ gripper_type = GripperType("Yam")
+ robot_cfg = YamConfig(
+ channel=self.channel,
+ gripper_type_id="linear_4310",
+ async_control=False,
+ robot_type=robot_type,
+ kinematic_model_path=rcs.ROBOTS[robot_type].mjcf_model_path,
+ attachment_site=rcs.ROBOTS[robot_type].attachment_site,
+ dof=rcs.ROBOTS[robot_type].dof,
+ joint_limits=rcs.ROBOTS[robot_type].joint_limits,
+ q_home=rcs.ROBOTS[robot_type].q_home,
+ # The gripper is part of the arm's mjcf, so this is the flange to grasp point offset.
+ tcp_offset=rcs.GRIPPER_TCP_OFFSETS[gripper_type],
+ )
+
+ gripper_cfg = GripperConfig(gripper_type=gripper_type)
+
+ return YamHardwareEnvCreatorConfig(
+ control_mode=ControlMode.JOINTS,
+ robot_cfg=robot_cfg,
+ gripper_cfg=gripper_cfg,
+ max_relative_movement=0.2,
+ relative_to=RelativeTo.LAST_STEP,
+ )
+
+
+class DefaultYamDualMultiHardwareEnv(RCSYamMultiConfigEnvCreator):
+ left_channel = "can0"
+ right_channel = "can1"
+
+ def config(self) -> YamMultiHardwareEnvCreatorConfig:
+ base = DefaultYamHardwareEnv()
+
+ base.channel = self.left_channel
+ left_cfg = base.config()
+ left_cfg.robot_cfg.async_control = True
+
+ base.channel = self.right_channel
+ right_cfg = base.config()
+ right_cfg.robot_cfg.async_control = True
+
+ return YamMultiHardwareEnvCreatorConfig(
+ control_mode=ControlMode.JOINTS,
+ robot_cfgs={
+ "left": left_cfg.robot_cfg,
+ "right": right_cfg.robot_cfg,
+ },
+ gripper_cfgs={
+ "left": left_cfg.gripper_cfg,
+ "right": right_cfg.gripper_cfg,
+ },
+ max_relative_movement=0.2,
+ relative_to=RelativeTo.LAST_STEP,
+ )
diff --git a/extensions/rcs_yam/src/rcs_yam/creators.py b/extensions/rcs_yam/src/rcs_yam/creators.py
new file mode 100644
index 00000000..b3dbf341
--- /dev/null
+++ b/extensions/rcs_yam/src/rcs_yam/creators.py
@@ -0,0 +1,157 @@
+import logging
+import typing
+from dataclasses import dataclass, field
+
+import gymnasium as gym
+from rcs._core.common import BaseCameraConfig, GripperConfig
+from rcs.camera.hw import DummyCalibrationStrategy, HardwareCamera, HardwareCameraSet
+from rcs.envs.base import (
+ CameraSetWrapper,
+ ControlMode,
+ CoverWrapper,
+ GripperWrapper,
+ HardwareEnv,
+ MultiRobotWrapper,
+ RelativeActionSpace,
+ RelativeTo,
+ RobotWrapper,
+)
+from rcs.envs.scenes import RCSEnvCreator, WrapperConfig
+from rcs_yam.hw import Yam, YamConfig, YamGripper
+
+import rcs
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+
+@dataclass(kw_only=True)
+class HardwareCameraCreatorConfig:
+ camera_type_id: str
+ camera_cfgs: dict[str, BaseCameraConfig]
+ kwargs: dict[str, typing.Any] = field(default_factory=dict)
+
+
+def _create_realsense_camera(cfg: HardwareCameraCreatorConfig) -> HardwareCamera:
+ try:
+ from rcs.camera.hw import CalibrationStrategy
+ from rcs_realsense.camera import RealSenseCameraSet
+ except ImportError as e:
+ msg = "RealSense camera support requires the `rcs_realsense` extension to be installed."
+ raise ImportError(msg) from e
+
+ calibration_strategy = {
+ name: typing.cast(CalibrationStrategy, DummyCalibrationStrategy()) for name in cfg.camera_cfgs
+ }
+ return typing.cast(
+ HardwareCamera,
+ RealSenseCameraSet(cameras=cfg.camera_cfgs, calibration_strategy=calibration_strategy, **cfg.kwargs),
+ )
+
+
+HARDWARE_CAMERA_CREATORS: dict[str, typing.Callable[[HardwareCameraCreatorConfig], HardwareCamera]] = {
+ "realsense": _create_realsense_camera,
+}
+
+
+def _create_hardware_camera_set(
+ camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None,
+) -> HardwareCameraSet | None:
+ if camera_cfgs is None:
+ return None
+ cameras: list[HardwareCamera] = []
+ for cfg in camera_cfgs.values():
+ if cfg.camera_type_id not in HARDWARE_CAMERA_CREATORS:
+ msg = f"Unknown hardware camera type id: {cfg.camera_type_id}"
+ raise ValueError(msg)
+ cameras.append(HARDWARE_CAMERA_CREATORS[cfg.camera_type_id](cfg))
+ return HardwareCameraSet(cameras) if cameras else None
+
+
+def _attach_camera_set(
+ env: gym.Env, camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None, include_depth: bool
+) -> gym.Env:
+ camera_set = _create_hardware_camera_set(camera_cfgs)
+ if camera_set is None:
+ return env
+ camera_set.start()
+ camera_set.wait_for_frames()
+ logger.info("CameraSet started")
+ return CameraSetWrapper(env, camera_set, include_depth)
+
+
+@dataclass(kw_only=True)
+class YamHardwareEnvCreatorConfig:
+ robot_cfg: YamConfig
+ control_mode: ControlMode
+ gripper_cfg: GripperConfig | None = None
+ camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None = None
+ max_relative_movement: float | tuple[float, float] | None = None
+ relative_to: RelativeTo = RelativeTo.LAST_STEP
+ wrapper_cfg: WrapperConfig = field(default_factory=WrapperConfig)
+
+
+class RCSYamConfigEnvCreator(RCSEnvCreator[YamHardwareEnvCreatorConfig]):
+ def create_env(self, cfg: YamHardwareEnvCreatorConfig) -> gym.Env:
+ ik = rcs.common.Pin(
+ cfg.robot_cfg.kinematic_model_path,
+ cfg.robot_cfg.attachment_site,
+ urdf=cfg.robot_cfg.kinematic_model_path.endswith(".urdf"),
+ )
+ robot = Yam(cfg.robot_cfg, ik)
+ env: gym.Env = HardwareEnv()
+ env = RobotWrapper(env, robot, cfg.control_mode, home_on_reset=cfg.wrapper_cfg.home_on_reset)
+
+ if cfg.gripper_cfg is not None:
+ # The gripper motor is part of the arm's motor chain, so it shares the robot handle.
+ gripper = YamGripper(cfg.gripper_cfg, robot)
+ env = GripperWrapper(env, gripper, binary=cfg.wrapper_cfg.binary_gripper)
+
+ env = _attach_camera_set(env, cfg.camera_cfgs, cfg.wrapper_cfg.include_depth)
+
+ if cfg.relative_to != RelativeTo.NONE:
+ env = RelativeActionSpace(env, max_mov=cfg.max_relative_movement, relative_to=cfg.relative_to)
+ return CoverWrapper(env)
+
+ def config(self) -> YamHardwareEnvCreatorConfig:
+ msg = "Implement config() in a subclass or pass `cfg=` explicitly."
+ raise NotImplementedError(msg)
+
+
+@dataclass(kw_only=True)
+class YamMultiHardwareEnvCreatorConfig:
+ robot_cfgs: dict[str, YamConfig]
+ control_mode: ControlMode
+ gripper_cfgs: dict[str, GripperConfig | None] | None = None
+ camera_cfgs: dict[str, HardwareCameraCreatorConfig] | None = None
+ max_relative_movement: float | tuple[float, float] | None = None
+ relative_to: RelativeTo = RelativeTo.LAST_STEP
+ robot_to_shared_base_frame: dict[str, rcs.common.Pose] | None = None
+ wrapper_cfg: WrapperConfig = field(default_factory=WrapperConfig)
+
+
+class RCSYamMultiConfigEnvCreator(RCSEnvCreator[YamMultiHardwareEnvCreatorConfig]):
+ def create_env(self, cfg: YamMultiHardwareEnvCreatorConfig) -> gym.Env:
+ envs: dict[str, gym.Env] = {}
+ for robot_name, robot_cfg in cfg.robot_cfgs.items():
+ envs[robot_name] = RCSYamConfigEnvCreator().create_env(
+ YamHardwareEnvCreatorConfig(
+ robot_cfg=robot_cfg,
+ control_mode=cfg.control_mode,
+ gripper_cfg=cfg.gripper_cfgs[robot_name] if cfg.gripper_cfgs is not None else None,
+ # The cameras observe the whole scene, so they are attached once around the
+ # combined env instead of per arm.
+ camera_cfgs=None,
+ max_relative_movement=cfg.max_relative_movement,
+ relative_to=cfg.relative_to,
+ wrapper_cfg=cfg.wrapper_cfg,
+ )
+ )
+
+ env: gym.Env = MultiRobotWrapper(envs, cfg.robot_to_shared_base_frame)
+ env = _attach_camera_set(env, cfg.camera_cfgs, cfg.wrapper_cfg.include_depth)
+ return CoverWrapper(env)
+
+ def config(self) -> YamMultiHardwareEnvCreatorConfig:
+ msg = "Implement config() in a subclass or pass `cfg=` explicitly."
+ raise NotImplementedError(msg)
diff --git a/extensions/rcs_yam/src/rcs_yam/hw.py b/extensions/rcs_yam/src/rcs_yam/hw.py
new file mode 100644
index 00000000..76cd0e5d
--- /dev/null
+++ b/extensions/rcs_yam/src/rcs_yam/hw.py
@@ -0,0 +1,236 @@
+"""Hardware abstraction layer for the I2RT YAM arm.
+
+The i2rt driver exposes the arm and the gripper as a single motor chain with the gripper motor
+as its last entry, and runs its PD control loop in a background thread. `Yam` owns that handle
+and the shared target vector, `YamGripper` writes the gripper entry of the same target through it.
+"""
+
+import threading
+import time
+import typing
+
+import numpy as np
+from rcs.common_typing import RobotConfigKwargs
+
+from rcs import common
+
+
+class YamConfig(common.RobotConfig):
+ """Configuration of a single YAM arm on one CAN bus."""
+
+ def __init__(
+ self,
+ channel: str = "can0",
+ arm_type_id: str = "yam",
+ gripper_type_id: str = "linear_4310",
+ arm_version: int = 1,
+ async_control: bool = True,
+ joint_tolerance: float = 0.01,
+ command_timeout: float = 5.0,
+ gripper_tolerance: float = 0.02,
+ gripper_timeout: float = 2.0,
+ max_joint_step: float = 0.1,
+ max_joint_velocity: float = 0.5,
+ move_home_duration: float = 2.0,
+ gripper_limits_override: np.ndarray | None = None,
+ **kwargs: typing.Unpack[RobotConfigKwargs],
+ ):
+ super().__init__(**kwargs)
+ self.robot_platform = common.RobotPlatform.HARDWARE
+ self.robot_type = common.RobotType("Yam")
+ self.channel = channel
+ self.arm_type_id = arm_type_id
+ self.gripper_type_id = gripper_type_id
+ self.arm_version = arm_version
+ # If False, commands return once the target is reached or the timeout hits.
+ self.async_control = async_control
+ self.joint_tolerance = joint_tolerance
+ self.command_timeout = command_timeout
+ # A blocked gripper never reaches its target, so it gets a shorter timeout of its own.
+ self.gripper_tolerance = gripper_tolerance
+ self.gripper_timeout = gripper_timeout
+ # Targets further away than max_joint_step are ramped instead of sent as a step, the arm
+ # is direct driven and follows a plain PD controller without trajectory generation.
+ self.max_joint_step = max_joint_step
+ self.max_joint_velocity = max_joint_velocity
+ self.move_home_duration = move_home_duration
+ # If set, skips the calibration run that would otherwise drive the fingers to both stops.
+ self.gripper_limits_override = gripper_limits_override
+
+
+class Yam(common.Robot):
+ """RCS robot backed by an i2rt motor chain."""
+
+ POLL_INTERVAL = 0.005
+
+ def __init__(self, cfg: YamConfig, ik: common.Kinematics):
+ super().__init__()
+ from i2rt.robots.get_robot import get_yam_robot
+ from i2rt.robots.utils import ArmType, GripperType
+
+ self._closed = True
+ self.ik = ik
+ self._config = cfg
+ self._dof = int(cfg.dof)
+ self._robot = get_yam_robot(
+ channel=cfg.channel,
+ arm_type=ArmType.from_string_name(cfg.arm_type_id),
+ gripper_type=GripperType.from_string_name(cfg.gripper_type_id),
+ gripper_limits_override=cfg.gripper_limits_override,
+ )
+ self._closed = False
+ self._has_gripper = self._robot.num_dofs() > self._dof
+ self._lock = threading.Lock()
+ # Seeding the target from the measured state avoids a jump on the first partial command.
+ self._target = np.asarray(self._robot.get_joint_pos(), dtype=np.float64).copy()
+ print(f"YAM connected on {cfg.channel} with {self._robot.num_dofs()} motors.")
+
+ def __del__(self):
+ self.close()
+
+ def close(self) -> None:
+ if self._closed:
+ return
+ self._closed = True
+ self._robot.close()
+
+ def get_config(self) -> YamConfig:
+ return self._config
+
+ def set_config(self, robot_cfg: YamConfig) -> None:
+ self._config = robot_cfg
+
+ def get_state(self) -> common.RobotState:
+ return common.RobotState()
+
+ def get_ik(self) -> common.Kinematics | None:
+ return self.ik
+
+ def reset(self) -> None:
+ pass
+
+ def automatic_error_recovery(self) -> None:
+ """Called by `RobotWrapper.reset` when homing raises. The i2rt driver has no recovery hook."""
+
+ def get_joint_position(self) -> np.ndarray[tuple[typing.Any], np.dtype[np.float64]]:
+ return np.asarray(self._robot.get_joint_pos(), dtype=np.float64)[: self._dof]
+
+ def get_gripper_width(self) -> float:
+ """Normalized gripper width, 0 is closed and 1 is open, as mapped by the i2rt joint mapper."""
+ self._assert_gripper()
+ return float(np.asarray(self._robot.get_joint_pos(), dtype=np.float64)[self._dof])
+
+ def get_cartesian_position(self) -> common.Pose:
+ # `Kinematics.forward` applies the inverse of the offset it is handed, so the TCP is composed
+ # here instead, to match the pose `SimRobot::get_cartesian_position` reports in simulation.
+ flange = self.ik.forward(self.get_joint_position(), common.Pose())
+ return flange * self._config.tcp_offset
+
+ def set_cartesian_position(self, pose: common.Pose) -> None:
+ q = self.ik.inverse(pose, self.get_joint_position(), self._config.tcp_offset)
+ if q is None:
+ print("IK failed")
+ return
+ # The kinematic model carries the two finger joints, only the arm joints are commanded.
+ self.set_joint_position(np.asarray(q, dtype=np.float64)[: self._dof])
+
+ def set_joint_position(self, q: np.ndarray[tuple[typing.Any], np.dtype[np.float64]]) -> None:
+ self._command(arm=np.asarray(q, dtype=np.float64))
+
+ def set_gripper_width(self, width: float) -> None:
+ self._assert_gripper()
+ self._command(gripper=float(np.clip(width, 0.0, 1.0)))
+
+ def move_home(self) -> None:
+ if self._config.q_home is None:
+ msg = "No home position configured."
+ raise ValueError(msg)
+ home = np.asarray(self._config.q_home, dtype=np.float64)
+ low, high = self._config.joint_limits
+ if np.any((home < low) | (home > high)):
+ msg = f"Home position {home} is out of joint limits."
+ raise ValueError(msg)
+ print(f"Moving to home position: {home}")
+ with self._lock:
+ self._target[: self._dof] = home
+ target = self._target.copy()
+ self._robot.move_joints(target, time_interval_s=self._config.move_home_duration)
+
+ def _assert_gripper(self) -> None:
+ if not self._has_gripper:
+ msg = f"YAM on {self._config.channel} was created without a gripper."
+ raise RuntimeError(msg)
+
+ def _command(self, arm: np.ndarray | None = None, gripper: float | None = None) -> None:
+ """Write the arm and/or gripper part of the shared target and send it to the motor chain."""
+ with self._lock:
+ if arm is not None:
+ self._target[: self._dof] = arm
+ if gripper is not None:
+ self._target[self._dof] = gripper
+ target = self._target.copy()
+
+ if self._config.async_control:
+ # Streaming mode: always forward the latest goal and return immediately. A blocking ramp
+ # here would stall the caller while new targets keep arriving, which reads as queued lag.
+ self._robot.command_joint_pos(target)
+ return
+
+ current = np.asarray(self._robot.get_joint_pos(), dtype=np.float64)
+ arm_error = float(np.max(np.abs(target[: self._dof] - current[: self._dof])))
+ if arm_error > self._config.max_joint_step:
+ # Ramping blocks, a step of this size would jerk the arm.
+ self._robot.move_joints(target, time_interval_s=arm_error / self._config.max_joint_velocity)
+ return
+
+ self._robot.command_joint_pos(target)
+ self._wait_until_reached(target, wait_arm=arm is not None, wait_gripper=gripper is not None)
+
+ def _wait_until_reached(self, target: np.ndarray, wait_arm: bool, wait_gripper: bool) -> None:
+ timeout = self._config.command_timeout if wait_arm else self._config.gripper_timeout
+ deadline = time.time() + timeout
+ while time.time() < deadline:
+ current = np.asarray(self._robot.get_joint_pos(), dtype=np.float64)
+ arm_reached = np.max(np.abs(target[: self._dof] - current[: self._dof])) < self._config.joint_tolerance
+ gripper_reached = (
+ not self._has_gripper or abs(target[self._dof] - current[self._dof]) < self._config.gripper_tolerance
+ )
+ if (arm_reached or not wait_arm) and (gripper_reached or not wait_gripper):
+ return
+ time.sleep(self.POLL_INTERVAL)
+
+
+class YamGripper(common.Gripper):
+ """RCS gripper sharing the motor chain of a `Yam` instance."""
+
+ def __init__(self, cfg: common.GripperConfig, robot: Yam):
+ super().__init__()
+ self._cfg = cfg
+ self._robot = robot
+
+ def get_config(self) -> common.GripperConfig:
+ return self._cfg
+
+ def get_normalized_width(self) -> float:
+ return self._robot.get_gripper_width()
+
+ def set_normalized_width(self, width: float, force: float = 0) -> None:
+ if not (0 <= width <= 1):
+ msg = f"Width must be between 0 and 1, got {width}."
+ raise ValueError(msg)
+ self._robot.set_gripper_width(width)
+
+ def open(self) -> None:
+ self.set_normalized_width(1.0)
+
+ def grasp(self) -> None:
+ self.set_normalized_width(0.0)
+
+ def shut(self) -> None:
+ self.set_normalized_width(0.0)
+
+ def reset(self) -> None:
+ self.open()
+
+ def close(self) -> None:
+ """The motor chain belongs to the robot, which closes it."""
diff --git a/extensions/rcs_yam/src/rcs_yam/scripts/__init__.py b/extensions/rcs_yam/src/rcs_yam/scripts/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py b/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py
new file mode 100644
index 00000000..75a9908f
--- /dev/null
+++ b/extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py
@@ -0,0 +1,77 @@
+"""Script for testing a YAM arm connection, sync and async joint control and the gripper.
+
+Run with the CAN bus up, see the extension README. Keep the workspace clear, the arm moves.
+"""
+
+import time
+
+import numpy as np
+from rcs_yam.hw import Yam, YamConfig, YamGripper
+
+import rcs
+from rcs import common
+
+CHANNEL = "can0"
+
+robot_type = common.RobotType("Yam")
+gripper_type = common.GripperType("Yam")
+robot_config = YamConfig(
+ channel=CHANNEL,
+ async_control=False,
+ robot_type=robot_type,
+ kinematic_model_path=rcs.ROBOTS[robot_type].mjcf_model_path,
+ attachment_site=rcs.ROBOTS[robot_type].attachment_site,
+ dof=rcs.ROBOTS[robot_type].dof,
+ joint_limits=rcs.ROBOTS[robot_type].joint_limits,
+ q_home=rcs.ROBOTS[robot_type].q_home,
+ tcp_offset=rcs.GRIPPER_TCP_OFFSETS[gripper_type],
+)
+ik = rcs.common.Pin(
+ robot_config.kinematic_model_path,
+ robot_config.attachment_site,
+ urdf=robot_config.kinematic_model_path.endswith(".urdf"),
+)
+robot = Yam(robot_config, ik)
+gripper = YamGripper(common.GripperConfig(gripper_type=gripper_type), robot)
+
+print(f"Joint positions: {robot.get_joint_position()}")
+print(f"Cartesian position: {robot.get_cartesian_position()}")
+print(f"Gripper width: {gripper.get_normalized_width():.3f}")
+
+input("Press Enter to move to the home position...")
+robot.move_home()
+
+input("Press Enter for a small synchronous joint move...")
+target_q = robot.get_joint_position()
+target_q[0] += 0.2
+start = time.time()
+robot.set_joint_position(target_q)
+print(f"sync command returned after {time.time() - start:.3f} s at {robot.get_joint_position()}")
+
+input("Press Enter for the same move asynchronously...")
+cfg = robot.get_config()
+cfg.async_control = True
+robot.set_config(cfg)
+target_q[0] -= 0.2
+start = time.time()
+robot.set_joint_position(target_q)
+print(f"async command returned after {time.time() - start:.3f} s at {robot.get_joint_position()}")
+time.sleep(1.0)
+print(f"one second later: {robot.get_joint_position()}")
+
+input("Press Enter for a small cartesian move...")
+cfg.async_control = False
+robot.set_config(cfg)
+pose = robot.get_cartesian_position()
+robot.set_cartesian_position(
+ common.Pose(translation=pose.translation() + np.array([0.0, 0.0, -0.03]), quaternion=pose.rotation_q())
+)
+print(f"cartesian position now: {robot.get_cartesian_position()}")
+
+input("Press Enter to cycle the gripper...")
+for width in (0.0, 1.0):
+ gripper.set_normalized_width(width)
+ print(f"commanded {width:.1f}, measured {gripper.get_normalized_width():.3f}")
+
+robot.close()
+print("done")
diff --git a/pyproject.toml b/pyproject.toml
index e472ace8..e652022a 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -221,6 +221,10 @@ version_files = [
"extensions/rcs_ur5e/src/rcs_ur5e/__init__.py:__version__",
"extensions/rcs_ur5e/pyproject.toml:\"rcs-core>=(.*)\"",
+ "extensions/rcs_yam/pyproject.toml:version",
+ "extensions/rcs_yam/src/rcs_yam/__init__.py:__version__",
+ "extensions/rcs_yam/pyproject.toml:\"rcs-core>=(.*)\"",
+
"extensions/rcs_tacto/pyproject.toml:version",
"extensions/rcs_tacto/src/rcs_tacto/__init__.py:__version__",
"extensions/rcs_tacto/pyproject.toml:\"rcs-core>=(.*)\"",
diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py
index 730cb8a0..deec5e84 100644
--- a/python/rcs/__init__.py
+++ b/python/rcs/__init__.py
@@ -182,6 +182,18 @@ class RobotMetaConfig:
),
attachment_site="gripper",
),
+ common.RobotType("Yam"): RobotMetaConfig(
+ mjcf_model_path="assets/robots/yam/yam.xml",
+ dof=6,
+ q_home=np.array([0.0, 1.047, 1.047, 0.0, 0.0, 0.0]),
+ joint_limits=np.array(
+ [
+ [-2.61799, 0.0, 0.0, -1.69297, -1.5708, -2.0944],
+ [3.14159, 3.66519, 3.14159, 1.5708, 1.5708, 2.0944],
+ ]
+ ),
+ attachment_site="tcp_site",
+ ),
}
@@ -193,6 +205,9 @@ class RobotMetaConfig:
GRIPPER_TCP_OFFSETS: dict[common.GripperType, common.Pose] = {
common.GripperType.FrankaHand: common.Pose(pose_matrix=common.FrankaHandTCPOffset()),
common.GripperType("Robotiq2F85"): common.Pose(translation=np.array([0, 0.0, 0.1493])),
+ # The yam gripper is part of the robot mjcf, hence it needs no entry in GRIPPER_PATHS
+ # and no mount offset, only the offset from the flange to the point between the fingers.
+ common.GripperType("Yam"): common.Pose(translation=np.array([0.0, 0.0, 0.1347])),
}
GRIPPER_MOUNT_OFFSETS: dict[common.GripperType, common.Pose] = {
diff --git a/python/rcs/envs/configs.py b/python/rcs/envs/configs.py
index d9f23d95..b114c18c 100644
--- a/python/rcs/envs/configs.py
+++ b/python/rcs/envs/configs.py
@@ -443,7 +443,8 @@ def config(self) -> SimEnvCreatorConfig:
class EmptyWorldSO101(EmptyWorldFR3):
- gripper_prefix_template = "gripper"
+ # The gripper is part of the so101 mjcf, so its joint and actuator are prefixed with the robot.
+ gripper_prefix_template = EmptyWorldFR3.robot_prefix_template
def config(self) -> SimEnvCreatorConfig:
rt = RobotType("SO101")
@@ -482,15 +483,58 @@ def config(self) -> SimEnvCreatorConfig:
return cfg
+class EmptyWorldYam(EmptyWorldFR3):
+ # The gripper is part of the yam mjcf, so its joint and actuator are prefixed with the robot.
+ gripper_prefix_template = EmptyWorldFR3.robot_prefix_template
+
+ def config(self) -> SimEnvCreatorConfig:
+ rt = RobotType("Yam")
+ cfg = super().config()
+ lead_robot_name = self.lead_robot_name(cfg)
+
+ robot_cfg = cfg.robot_cfgs[lead_robot_name]
+ robot_cfg.robot_type = rt
+ robot_cfg.tcp_offset = GRIPPER_TCP_OFFSETS[GripperType("Yam")]
+ robot_cfg.attachment_site = rcs.ROBOTS[rt].attachment_site
+ robot_cfg.kinematic_model_path = rcs.ROBOTS[rt].mjcf_model_path
+ robot_cfg.arm_collision_geoms = []
+ robot_cfg.joints = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
+ robot_cfg.actuators = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
+ robot_cfg.dof = rcs.ROBOTS[rt].dof
+ robot_cfg.joint_limits = rcs.ROBOTS[rt].joint_limits
+ robot_cfg.q_home = rcs.ROBOTS[rt].q_home
+ robot_cfg.base = "arm"
+
+ assert cfg.gripper_cfgs is not None
+ gripper_cfg = cfg.gripper_cfgs[lead_robot_name]
+ gripper_cfg.gripper_type = GripperType("Yam")
+ gripper_cfg.actuator = "gripper"
+ # right_finger is driven by an equality constraint, so only the actuated finger is listed
+ gripper_cfg.joints = ["left_finger"]
+ gripper_cfg.collision_geoms = []
+ gripper_cfg.collision_geoms_fingers = []
+ gripper_cfg.min_joint_width = 0.0
+ gripper_cfg.max_joint_width = 0.0475
+ gripper_cfg.min_actuator_width = 0.0
+ gripper_cfg.max_actuator_width = 0.0475
+
+ cfg.camera_cfgs = None
+ cfg.camera_adds = None
+ cfg.gripper_offsets = None
+
+ return cfg
+
+
gym.register(id="rcs/fr3", entry_point=EmptyWorldFR3())
gym.register(id="rcs/duo", entry_point=EmptyWorldFR3Duo())
gym.register(id="rcs/ur5e", entry_point=EmptyWorldUR5e())
gym.register(id="rcs/xarm7", entry_point=EmptyWorldXArm7())
gym.register(id="rcs/so101", entry_point=EmptyWorldSO101())
+gym.register(id="rcs/yam", entry_point=EmptyWorldYam())
if __name__ == "__main__":
- env = gym.make("rcs/duo")
+ env = gym.make("rcs/yam")
obs, info = env.reset()
print(obs)
# Duo
diff --git a/python/rcs/operator/interface.py b/python/rcs/operator/interface.py
index b8d454bc..4bc936c8 100644
--- a/python/rcs/operator/interface.py
+++ b/python/rcs/operator/interface.py
@@ -61,7 +61,7 @@ def close(self):
@dataclass(kw_only=True)
class BaseOperatorConfig:
operator_class: type[BaseOperator]
- read_frequency: int = 30
+ read_frequency: int = 90
simulation: bool = True
diff --git a/python/tests/test_kinematics.py b/python/tests/test_kinematics.py
index 179cc19d..b530303a 100644
--- a/python/tests/test_kinematics.py
+++ b/python/tests/test_kinematics.py
@@ -11,6 +11,7 @@
common.RobotType("XArm7"),
common.RobotType("UR5e"),
common.RobotType("SO101"),
+ common.RobotType("Yam"),
]