diff --git a/assets/scenes/so101_empty_world/assets b/assets/scenes/so101_empty_world/assets
new file mode 120000
index 00000000..bfad3f68
--- /dev/null
+++ b/assets/scenes/so101_empty_world/assets
@@ -0,0 +1 @@
+../../so101/stl
\ No newline at end of file
diff --git a/assets/scenes/so101_empty_world/robot.urdf b/assets/scenes/so101_empty_world/robot.urdf
new file mode 100644
index 00000000..5dd02d24
--- /dev/null
+++ b/assets/scenes/so101_empty_world/robot.urdf
@@ -0,0 +1,460 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
\ No newline at end of file
diff --git a/assets/scenes/so101_empty_world/robot.xml b/assets/scenes/so101_empty_world/robot.xml
new file mode 100644
index 00000000..0b0f4f88
--- /dev/null
+++ b/assets/scenes/so101_empty_world/robot.xml
@@ -0,0 +1,158 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/assets/scenes/so101_empty_world/scene.xml b/assets/scenes/so101_empty_world/scene.xml
new file mode 100644
index 00000000..6dd1be24
--- /dev/null
+++ b/assets/scenes/so101_empty_world/scene.xml
@@ -0,0 +1,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/assets/so101/mjcf/so101.xml b/assets/so101/mjcf/so101.xml
new file mode 100644
index 00000000..0b0f4f88
--- /dev/null
+++ b/assets/so101/mjcf/so101.xml
@@ -0,0 +1,158 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/assets/so101/stl/base_motor_holder_so101_v1.stl b/assets/so101/stl/base_motor_holder_so101_v1.stl
new file mode 100644
index 00000000..cac2b8dd
Binary files /dev/null and b/assets/so101/stl/base_motor_holder_so101_v1.stl differ
diff --git a/assets/so101/stl/base_so101_v2.stl b/assets/so101/stl/base_so101_v2.stl
new file mode 100644
index 00000000..ed10aa61
Binary files /dev/null and b/assets/so101/stl/base_so101_v2.stl differ
diff --git a/assets/so101/stl/motor_holder_so101_base_v1.stl b/assets/so101/stl/motor_holder_so101_base_v1.stl
new file mode 100644
index 00000000..9eaf6724
Binary files /dev/null and b/assets/so101/stl/motor_holder_so101_base_v1.stl differ
diff --git a/assets/so101/stl/motor_holder_so101_wrist_v1.stl b/assets/so101/stl/motor_holder_so101_wrist_v1.stl
new file mode 100644
index 00000000..d0aa18b1
Binary files /dev/null and b/assets/so101/stl/motor_holder_so101_wrist_v1.stl differ
diff --git a/assets/so101/stl/moving_jaw_so101_v1.stl b/assets/so101/stl/moving_jaw_so101_v1.stl
new file mode 100644
index 00000000..7e873668
Binary files /dev/null and b/assets/so101/stl/moving_jaw_so101_v1.stl differ
diff --git a/assets/so101/stl/rotation_pitch_so101_v1.stl b/assets/so101/stl/rotation_pitch_so101_v1.stl
new file mode 100644
index 00000000..5c05dee5
Binary files /dev/null and b/assets/so101/stl/rotation_pitch_so101_v1.stl differ
diff --git a/assets/so101/stl/sts3215_03a_no_horn_v1.stl b/assets/so101/stl/sts3215_03a_no_horn_v1.stl
new file mode 100644
index 00000000..2a895977
Binary files /dev/null and b/assets/so101/stl/sts3215_03a_no_horn_v1.stl differ
diff --git a/assets/so101/stl/sts3215_03a_v1.stl b/assets/so101/stl/sts3215_03a_v1.stl
new file mode 100644
index 00000000..fe05403d
Binary files /dev/null and b/assets/so101/stl/sts3215_03a_v1.stl differ
diff --git a/assets/so101/stl/under_arm_so101_v1.stl b/assets/so101/stl/under_arm_so101_v1.stl
new file mode 100644
index 00000000..480e0583
Binary files /dev/null and b/assets/so101/stl/under_arm_so101_v1.stl differ
diff --git a/assets/so101/stl/upper_arm_so101_v1.stl b/assets/so101/stl/upper_arm_so101_v1.stl
new file mode 100644
index 00000000..9c0f9b27
Binary files /dev/null and b/assets/so101/stl/upper_arm_so101_v1.stl differ
diff --git a/assets/so101/stl/waveshare_mounting_plate_so101_v2.stl b/assets/so101/stl/waveshare_mounting_plate_so101_v2.stl
new file mode 100644
index 00000000..4dba1f22
Binary files /dev/null and b/assets/so101/stl/waveshare_mounting_plate_so101_v2.stl differ
diff --git a/assets/so101/stl/wrist_roll_follower_so101_v1.stl b/assets/so101/stl/wrist_roll_follower_so101_v1.stl
new file mode 100644
index 00000000..123ce374
Binary files /dev/null and b/assets/so101/stl/wrist_roll_follower_so101_v1.stl differ
diff --git a/assets/so101/stl/wrist_roll_pitch_so101_v2.stl b/assets/so101/stl/wrist_roll_pitch_so101_v2.stl
new file mode 100644
index 00000000..ac5172a3
Binary files /dev/null and b/assets/so101/stl/wrist_roll_pitch_so101_v2.stl differ
diff --git a/assets/so101/urdf/so101.urdf b/assets/so101/urdf/so101.urdf
new file mode 100644
index 00000000..5dd02d24
--- /dev/null
+++ b/assets/so101/urdf/so101.urdf
@@ -0,0 +1,460 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/PositionJointInterface
+
+
+ hardware_interface/PositionJointInterface
+ 1
+
+
+
+
\ No newline at end of file
diff --git a/examples/so101/so101_env_cartesian_control.py b/examples/so101/so101_env_cartesian_control.py
new file mode 100644
index 00000000..d571bb22
--- /dev/null
+++ b/examples/so101/so101_env_cartesian_control.py
@@ -0,0 +1,74 @@
+import logging
+
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs.envs.creators import SimEnvCreator
+
+import rcs
+from rcs import sim
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+
+def main():
+
+ robot_cfg = sim.SimRobotConfig()
+ robot_cfg.actuators = ["1", "2", "3", "4", "5"]
+ robot_cfg.joints = [
+ "1",
+ "2",
+ "3",
+ "4",
+ "5",
+ ]
+ robot_cfg.base = "base"
+ robot_cfg.robot_type = rcs.common.RobotType.SO101
+ robot_cfg.attachment_site = "gripper"
+ robot_cfg.arm_collision_geoms = []
+ robot_cfg.mjcf_scene_path = rcs.scenes["so101_empty_world"].mjb
+ robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot
+
+ gripper_cfg = sim.SimGripperConfig()
+ gripper_cfg.min_actuator_width = -0.17453292519943295
+ gripper_cfg.max_actuator_width = 1.7453292519943295
+ gripper_cfg.min_joint_width = -0.17453292519943295
+ gripper_cfg.max_joint_width = 1.7453292519943295
+ gripper_cfg.actuator = "6"
+ gripper_cfg.joint = "6"
+ gripper_cfg.collision_geoms = []
+ gripper_cfg.collision_geoms_fingers = []
+
+ env_rel = SimEnvCreator()(
+ robot_cfg=robot_cfg,
+ control_mode=ControlMode.CARTESIAN_TQuat,
+ collision_guard=False,
+ gripper_cfg=gripper_cfg,
+ max_relative_movement=0.5,
+ relative_to=RelativeTo.LAST_STEP,
+ )
+ env_rel.get_wrapper_attr("sim").open_gui()
+ obs, info = env_rel.reset()
+
+ act = {"tquat": [0.03, 0, 0, 0, 0, 0, 1], "gripper": 1}
+ obs, reward, terminated, truncated, info = env_rel.step(act)
+
+ for _ in range(100):
+ for _ in range(5):
+ # 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)
+ print(info, obs)
+ if truncated or terminated:
+ logger.info("Truncated or terminated!")
+ return
+ for _ in range(5):
+ # 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__":
+ main()
diff --git a/examples/so101/so101_env_joint_control.py b/examples/so101/so101_env_joint_control.py
new file mode 100644
index 00000000..1f60ea75
--- /dev/null
+++ b/examples/so101/so101_env_joint_control.py
@@ -0,0 +1,65 @@
+import logging
+
+import numpy as np
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs.envs.creators import SimEnvCreator
+
+import rcs
+from rcs import sim
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+
+def main():
+
+ robot_cfg = sim.SimRobotConfig()
+ robot_cfg.actuators = ["1", "2", "3", "4", "5"]
+ robot_cfg.joints = [
+ "1",
+ "2",
+ "3",
+ "4",
+ "5",
+ ]
+ robot_cfg.base = "base"
+ robot_cfg.robot_type = rcs.common.RobotType.SO101
+ robot_cfg.attachment_site = "gripper"
+ robot_cfg.arm_collision_geoms = []
+ robot_cfg.mjcf_scene_path = rcs.scenes["so101_empty_world"].mjb
+ robot_cfg.kinematic_model_path = rcs.scenes["so101_empty_world"].mjcf_robot
+
+ gripper_cfg = sim.SimGripperConfig()
+ gripper_cfg.min_actuator_width = -0.17453292519943295
+ gripper_cfg.max_actuator_width = 1.7453292519943295
+ gripper_cfg.min_joint_width = -0.17453292519943295
+ gripper_cfg.max_joint_width = 1.7453292519943295
+ gripper_cfg.actuator = "6"
+ gripper_cfg.joint = "6"
+ gripper_cfg.collision_geoms = []
+ gripper_cfg.collision_geoms_fingers = []
+
+ env_rel = SimEnvCreator()(
+ robot_cfg=robot_cfg,
+ control_mode=ControlMode.JOINTS,
+ collision_guard=False,
+ gripper_cfg=gripper_cfg,
+ max_relative_movement=np.deg2rad(5),
+ relative_to=RelativeTo.LAST_STEP,
+ )
+ env_rel.get_wrapper_attr("sim").open_gui()
+
+ for _ in range(100):
+ obs, info = env_rel.reset()
+ for _ in range(10):
+ # sample random relative action and execute it
+ act = env_rel.action_space.sample()
+ # print(act)
+ 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_realsense/pyproject.toml b/extensions/rcs_realsense/pyproject.toml
index 5fa6615a..93e5208c 100644
--- a/extensions/rcs_realsense/pyproject.toml
+++ b/extensions/rcs_realsense/pyproject.toml
@@ -4,10 +4,10 @@ build-backend = "setuptools.build_meta"
[project]
name = "rcs_realsense"
-version = "0.4.0"
+version = "0.5.0"
description="RCS realsense module"
dependencies = [
- "rcs==0.4.0",
+ "rcs>=0.5.0",
"pyrealsense2~=2.55.1",
"apriltag==0.0.16",
"diskcache",
diff --git a/extensions/rcs_realsense/src/rcs_realsense/utils.py b/extensions/rcs_realsense/src/rcs_realsense/utils.py
index b63842bb..2797fd29 100644
--- a/extensions/rcs_realsense/src/rcs_realsense/utils.py
+++ b/extensions/rcs_realsense/src/rcs_realsense/utils.py
@@ -16,3 +16,13 @@ def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | No
}
calibration_strategy = {name: typing.cast(CalibrationStrategy, FR3BaseArucoCalibration(name)) for name in name2id}
return RealSenseCameraSet(cameras=cameras, calibration_strategy=calibration_strategy)
+
+
+def default_realsense_dummy_calibration(name2id: dict[str, str] | None) -> RealSenseCameraSet | None:
+ if name2id is None:
+ return None
+ cameras = {
+ name: common.BaseCameraConfig(identifier=id, resolution_width=640, resolution_height=480, frame_rate=30)
+ for name, id in name2id.items()
+ }
+ return RealSenseCameraSet(cameras=cameras)
diff --git a/extensions/rcs_so101/pyproject.toml b/extensions/rcs_so101/pyproject.toml
index 059315be..73b9e79b 100644
--- a/extensions/rcs_so101/pyproject.toml
+++ b/extensions/rcs_so101/pyproject.toml
@@ -4,10 +4,10 @@ build-backend = "setuptools.build_meta"
[project]
name = "rcs_so101"
-version = "0.4.0"
+version = "0.5.0"
description="RCS SO101 module"
dependencies = [
- "rcs==0.4.0",
+ "rcs>=0.5.0",
"lerobot @ git+https://github.com/huggingface/lerobot.git",
]
readme = "README.md"
diff --git a/extensions/rcs_so101/src/rcs_so101/creators.py b/extensions/rcs_so101/src/rcs_so101/creators.py
index 44f3bafc..07224b44 100644
--- a/extensions/rcs_so101/src/rcs_so101/creators.py
+++ b/extensions/rcs_so101/src/rcs_so101/creators.py
@@ -1,17 +1,6 @@
import logging
-from os import PathLike
-from pathlib import Path
import gymnasium as gym
-from lerobot.common.robots import make_robot_from_config
-from lerobot.common.robots.so101_follower.config_so101_follower import (
- SO101FollowerConfig,
-)
-from lerobot.common.teleoperators.so101_leader.config_so101_leader import (
- SO101LeaderConfig,
-)
-from lerobot.common.teleoperators.so101_leader.so101_leader import SO101Leader
-from lerobot.common.teleoperators.utils import make_teleoperator_from_config
from rcs.camera.hw import HardwareCameraSet
from rcs.envs.base import (
CameraSetWrapper,
@@ -22,7 +11,9 @@
RobotEnv,
)
from rcs.envs.creators import RCSHardwareEnvCreator
-from rcs_so101.hw import SO101, S0101Gripper
+from rcs_so101.hw import SO101, SO101Config, SO101Gripper
+
+import rcs
logger = logging.getLogger(__name__)
logger.setLevel(logging.INFO)
@@ -31,45 +22,45 @@
class RCSSO101EnvCreator(RCSHardwareEnvCreator):
def __call__( # type: ignore
self,
- id: str,
- port: str,
- urdf_path: str,
- calibration_dir: PathLike | str | None = None,
+ robot_cfg: SO101Config,
+ control_mode: ControlMode,
camera_set: HardwareCameraSet | None = None,
max_relative_movement: float | tuple[float, float] | None = None,
relative_to: RelativeTo = RelativeTo.LAST_STEP,
) -> gym.Env:
- if isinstance(calibration_dir, str):
- calibration_dir = Path(calibration_dir)
- cfg = SO101FollowerConfig(id=id, calibration_dir=calibration_dir, port=port)
- hf_robot = make_robot_from_config(cfg)
- robot = SO101(hf_robot) # TODO add path
- env: gym.Env = RobotEnv(robot, ControlMode.JOINTS)
- # env = FR3HW(env)
+ ik = rcs.common.Pin(
+ robot_cfg.kinematic_model_path,
+ robot_cfg.attachment_site,
+ urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
+ )
+ robot = SO101(robot_cfg=robot_cfg, ik=ik)
+ env: gym.Env = RobotEnv(robot, control_mode, home_on_reset=True)
- gripper = S0101Gripper(hf_robot)
+ gripper = SO101Gripper(robot._hf_robot, robot)
env = GripperWrapper(env, gripper, binary=False)
if camera_set is not None:
camera_set.start()
camera_set.wait_for_frames()
logger.info("CameraSet started")
- env = CameraSetWrapper(env, camera_set)
+ env = CameraSetWrapper(env, camera_set, include_depth=True)
if max_relative_movement is not None:
env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
return env
- @staticmethod
- def teleoperator(
- id: str,
- port: str,
- calibration_dir: PathLike | str | None = None,
- ) -> SO101Leader:
- if isinstance(calibration_dir, str):
- calibration_dir = Path(calibration_dir)
- cfg = SO101LeaderConfig(id=id, calibration_dir=calibration_dir, port=port)
- teleop = make_teleoperator_from_config(cfg)
- teleop.connect()
- return teleop
+ # For now, the leader-follower teleop script uses the leader object directly
+ # and doesn't depend on an RCS-provided class.
+ # @staticmethod
+ # def teleoperator(
+ # id: str,
+ # port: str,
+ # calibration_dir: PathLike | str | None = None,
+ # ) -> SO101Leader:
+ # if isinstance(calibration_dir, str):
+ # calibration_dir = Path(calibration_dir)
+ # cfg = SO101LeaderConfig(id=id, calibration_dir=calibration_dir, port=port)
+ # teleop = make_teleoperator_from_config(cfg)
+ # teleop.connect()
+ # return teleop
diff --git a/extensions/rcs_so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py
index 29135dce..4ab55619 100644
--- a/extensions/rcs_so101/src/rcs_so101/hw.py
+++ b/extensions/rcs_so101/src/rcs_so101/hw.py
@@ -1,50 +1,76 @@
+import threading
import typing
+from pathlib import Path
import numpy as np
-from lerobot.common.robots.so101_follower.so101_follower import SO101Follower
+from lerobot.robots import make_robot_from_config
+from lerobot.robots.so101_follower.config_so101_follower import SO101FollowerConfig
+from lerobot.robots.so101_follower.so101_follower import SO101Follower
+from rcs.utils import SimpleFrameRate
-from rcs import common, scenes
+from rcs import common
+
+
+class SO101Config(common.RobotConfig):
+ id: str = "follower"
+ port: str = "/dev/ttyACM0"
+ calibration_dir: str = "."
class SO101(common.Robot):
- def __init__(self, hf_robot: SO101Follower):
- # TODO: fix the hardcoded path when merged with so101 branch
- self.ik = common.Pin(
- scenes["so101_empty_world"].mjcf_robot,
- "attachment_site",
- )
- # ik = rcs_robotics_library._core.rl.RoboticsLibraryIK(robot_cfg.kinematic_model_path)
- self._hf_robot = hf_robot
+ def __init__(self, robot_cfg: SO101Config, ik: common.Kinematics):
+ super().__init__()
+ self.ik = ik
+ cfg = SO101FollowerConfig(id=robot_cfg.id, calibration_dir=Path(robot_cfg.calibration_dir), port=robot_cfg.port)
+ self._hf_robot = make_robot_from_config(cfg)
self._hf_robot.connect()
+ self._robot_config = robot_cfg
+ self._thread: threading.Thread | None = None
+ self._running = False
+ self._goal = None
+ self._goal_lock = threading.Lock()
+ self._rate_limiter = SimpleFrameRate(30, "teleop readout")
+ self.obs = None
+ self._last_joint = self._get_joint_position()
- # def get_base_pose_in_world_coordinates(self) -> Pose: ...
def get_cartesian_position(self) -> common.Pose:
return self.ik.forward(self.get_joint_position())
def get_ik(self) -> common.Kinematics | None:
return self.ik
- def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore
+ def _get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore
obs = self._hf_robot.get_observation()
- return typing.cast(
- np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]],
- np.array(
- [
- obs["shoulder_pan.pos"],
- obs["shoulder_lift.pos"],
- obs["elbow_flex.pos"],
- obs["wrist_flex.pos"],
- obs["wrist_roll.pos"],
- ],
- dtype=np.float64,
- ),
+ self.obs = obs
+ joints_hf = np.array(
+ [
+ obs["shoulder_pan.pos"],
+ obs["shoulder_lift.pos"],
+ obs["elbow_flex.pos"],
+ obs["wrist_flex.pos"],
+ obs["wrist_roll.pos"],
+ ],
+ dtype=np.float64,
)
+ # print(obs)
+ joints_normalized = (joints_hf + 100) / 200
+ joints_in_rad = (
+ joints_normalized
+ * (
+ common.robots_meta_config(common.RobotType.SO101).joint_limits[1]
+ - common.robots_meta_config(common.RobotType.SO101).joint_limits[0]
+ )
+ + common.robots_meta_config(common.RobotType.SO101).joint_limits[0]
+ )
+ self._last_joint = joints_in_rad
+ return joints_in_rad
+
+ def get_joint_position(self) -> np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]: # type: ignore
+ # return self._last_joint
+ return self._get_joint_position()
def get_config(self):
- a = common.RobotConfig()
- a.robot_platform = common.RobotPlatform.HARDWARE
- a.robot_type = common.RobotType.SO101
- return a
+ return self._robot_config
def get_state(self) -> common.RobotState:
return common.RobotState()
@@ -54,6 +80,7 @@ def move_home(self) -> None:
np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]],
common.robots_meta_config(common.RobotType.SO101).q_home,
)
+ print("move home", home)
self.set_joint_position(home)
def reset(self) -> None:
@@ -63,29 +90,95 @@ def set_cartesian_position(self, pose: common.Pose) -> None:
joints = self.ik.inverse(pose, q0=self.get_joint_position())
if joints is not None:
self.set_joint_position(joints)
+ self._last_cart = pose
- def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore
+ def _set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore
+ self._last_joint = q
+ q_normalized = (q - common.robots_meta_config(common.RobotType.SO101).joint_limits[0]) / (
+ common.robots_meta_config(common.RobotType.SO101).joint_limits[1]
+ - common.robots_meta_config(common.RobotType.SO101).joint_limits[0]
+ )
+ q_hf = (q_normalized * 200) - 100
self._hf_robot.send_action(
{
- "shoulder_pan.pos": q[0],
- "shoulder_lift.pos": q[1],
- "elbow_flex.pos": q[2],
- "wrist_flex.pos": q[3],
- "wrist_roll.pos": q[4],
+ "shoulder_pan.pos": q_hf[0],
+ "shoulder_lift.pos": q_hf[1],
+ "elbow_flex.pos": q_hf[2],
+ "wrist_flex.pos": q_hf[3],
+ "wrist_roll.pos": q_hf[4],
}
)
+ def set_joint_position(self, q: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]]) -> None: # type: ignore
+ self._set_joint_position(q)
+
+ def _controller(self):
+ print("Controller thread started")
+ while self._running:
+
+ with self._goal_lock:
+ goal = self._goal
+ if goal is None:
+ self._rate_limiter()
+ continue
+ current_pos = self._get_joint_position()
+ if np.allclose(current_pos, goal, atol=np.deg2rad(5)):
+ # print("Goal reached, continuing...")
+ self._rate_limiter()
+ continue
+ # interpolate with max 10 degree / s
+ max_step = np.deg2rad(90) * self._rate_limiter.get_frame_time()
+ delta = goal - current_pos
+ # how many steps are needed to reach the goal
+ steps_needed = np.ceil(np.max(np.abs(delta)) / max_step)
+ for i in range(int(steps_needed)):
+ if not self._running:
+ # print("Controller thread stopped")
+ return
+ # calculate the next position
+ step = delta / steps_needed * (i + 1)
+ new_pos = current_pos + step
+ self._set_joint_position(new_pos)
+
+ self._rate_limiter()
+ # check if new goal is set
+ with self._goal_lock:
+ if self._goal is None or not np.allclose(goal, self._goal, atol=np.deg2rad(1)):
+ break
+
+ def start_controller_thread(self):
+ self._running = True
+ self._thread = threading.Thread(target=self._controller, daemon=True)
+ self._thread.start()
+
+ def stop_controller_thread(self):
+ print("Stopping controller thread")
+ self._running = False
+ with self._goal_lock:
+ self._goal = None
+ if self._thread is not None and self._thread.is_alive():
+ self._thread.join()
+
# def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ...
# def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ...
+ def close(self):
+ self.move_home()
+ self.stop_controller_thread()
+ self._hf_robot.disconnect()
+
# TODO: problem when we inherit from gripper then we also need to call init which doesnt exist
-class S0101Gripper(common.Gripper):
- def __init__(self, hf_robot: SO101Follower):
+class SO101Gripper(common.Gripper):
+ def __init__(self, hf_robot: SO101Follower, robot: SO101):
+ super().__init__()
self._hf_robot = hf_robot
+ self._robot = robot
def get_normalized_width(self) -> float:
- obs = self._hf_robot.get_observation()
+ obs = self._robot.obs
+ if obs is None:
+ return 0.0
return obs["gripper.pos"] / 100.0
# def get_config(self) -> GripperConfig: ...
diff --git a/extensions/rcs_usb_cam/pyproject.toml b/extensions/rcs_usb_cam/pyproject.toml
new file mode 100644
index 00000000..5309d2a3
--- /dev/null
+++ b/extensions/rcs_usb_cam/pyproject.toml
@@ -0,0 +1,26 @@
+[build-system]
+requires = ["setuptools"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "rcs_usb_cam"
+version = "0.5.0"
+description="RCS USB Camera module"
+dependencies = [
+ "rcs>=0.5.0",
+ "opencv-python~=4.10.0"
+]
+readme = "README.md"
+maintainers = [
+ { name = "Seongjin Bien", email = "seongjin.bien@utn.de" },
+]
+authors = [
+ { name = "Seongjin Bien", email = "seongjin.bien@utn.de" },
+]
+requires-python = ">=3.10"
+classifiers = [
+ "Development Status :: 3 - Alpha",
+ "License :: OSI Approved :: GNU Affero General Public License v3 (AGPLv3)",
+ "Programming Language :: Python :: 3",
+]
+
diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py
new file mode 100644
index 00000000..a434d01f
--- /dev/null
+++ b/extensions/rcs_usb_cam/src/rcs_usb_cam/camera.py
@@ -0,0 +1,126 @@
+import copy
+import datetime
+import logging
+import threading
+import typing
+
+import cv2
+import numpy as np
+from rcs.camera.hw import CalibrationStrategy, DummyCalibrationStrategy, HardwareCamera
+from rcs.camera.interface import CameraFrame, DataFrame, Frame
+
+from rcs import common
+
+"""
+A generic extension class for handling USB-connected cameras.
+Uses OpenCV to interface with the camera hardware, specifically using cv2.VideoCapture(id).
+The ID can be both a single integer passed as a string, i.e. str(0), str(1), or the full /dev/ path, like /dev/video0.
+
+"""
+
+
+class USBCameraConfig(common.BaseCameraConfig):
+ color_intrinsics: np.ndarray[tuple[typing.Literal[3], typing.Literal[4]], np.dtype[np.float64]] | None = None
+ distortion_coeffs: np.ndarray[tuple[typing.Literal[5]], np.dtype[np.float64]] | None = None
+
+
+class USBCameraSet(HardwareCamera):
+ def __init__(
+ self,
+ cameras: dict[str, USBCameraConfig],
+ calibration_strategy: dict[str, CalibrationStrategy] | None = None,
+ ):
+ self.cameras = cameras
+ self.CALIBRATION_FRAME_SIZE = 30
+ if calibration_strategy is None:
+ calibration_strategy = {camera_name: DummyCalibrationStrategy() for camera_name in cameras}
+ for cam in self.cameras.values():
+ if cam.color_intrinsics is None:
+ cam.color_intrinsics = np.zeros((3, 4), dtype=np.float64) # type: ignore
+ if cam.distortion_coeffs is None:
+ cam.distortion_coeffs = np.zeros((5,), dtype=np.float64) # type: ignore
+ if cam.resolution_height is None:
+ cam.resolution_height = 480
+ if cam.resolution_width is None:
+ cam.resolution_width = 640
+ if cam.frame_rate is None:
+ cam.frame_rate = 30
+ self.calibration_strategy = calibration_strategy
+ self._camera_names = list(self.cameras.keys())
+ self._captures: dict[str, cv2.VideoCapture] = {}
+ self._logger = logging.getLogger(__name__)
+ self._logger.info("USBCamera initialized with cameras: %s", self._camera_names)
+ self._logger.info(
+ "If the camera streams are not correct, try v4l2-ctl --list-devices to see the available cameras."
+ )
+ self._frame_buffer_lock: dict[str, threading.Lock] = {}
+ self._frame_buffer: dict[str, list] = {}
+
+ def open(self):
+ for name, camera in self.cameras.items():
+ self._frame_buffer_lock[name] = threading.Lock()
+ self._frame_buffer[name] = []
+ cap = cv2.VideoCapture(camera.identifier)
+ cap.set(cv2.CAP_PROP_FRAME_WIDTH, camera.resolution_width)
+ cap.set(cv2.CAP_PROP_FRAME_HEIGHT, camera.resolution_height)
+ cap.set(cv2.CAP_PROP_FPS, camera.frame_rate)
+
+ if not cap.isOpened():
+ err = f"Could not open camera {name} with id {camera.identifier}"
+ raise RuntimeError(err)
+ self._captures[name] = cap
+
+ @property
+ def camera_names(self) -> list[str]:
+ return self._camera_names
+
+ def poll_frame(self, camera_name: str) -> Frame:
+ cap = self._captures[camera_name]
+ timestamp = datetime.datetime.now().timestamp()
+ ret, color_frame = cap.read()
+ if not ret:
+ err = f"Failed to read frame from camera {camera_name}"
+ raise RuntimeError(err)
+ with self._frame_buffer_lock[camera_name]:
+ if len(self._frame_buffer[camera_name]) >= self.CALIBRATION_FRAME_SIZE:
+ self._frame_buffer[camera_name].pop(0)
+ self._frame_buffer[camera_name].append(copy.deepcopy(color_frame))
+ color = DataFrame(
+ data=color_frame,
+ timestamp=timestamp,
+ intrinsics=self.cameras[camera_name].color_intrinsics,
+ extrinsics=self.calibration_strategy[camera_name].get_extrinsics(),
+ )
+ depth_frame = np.zeros(
+ (self.cameras[camera_name].resolution_height, self.cameras[camera_name].resolution_width), dtype=np.uint16
+ )
+ depth = DataFrame(
+ data=depth_frame,
+ timestamp=timestamp,
+ intrinsics=self.cameras[camera_name].color_intrinsics,
+ extrinsics=self.calibration_strategy[camera_name].get_extrinsics(),
+ )
+ cf = CameraFrame(color=color, depth=depth)
+ return Frame(camera=cf, avg_timestamp=timestamp)
+
+ def close(self):
+ for cap in self._captures.values():
+ cap.release()
+ self._captures = {}
+
+ def config(self, camera_name: str) -> USBCameraConfig:
+ return self.cameras[camera_name]
+
+ def calibrate(self) -> bool:
+ for camera_name in self.cameras:
+ color_intrinsics = self.cameras[camera_name].color_intrinsics
+ assert color_intrinsics is not None, f"Color intrinsics for camera {camera_name} not found"
+ if not self.calibration_strategy[camera_name].calibrate(
+ intrinsics=color_intrinsics,
+ samples=self._frame_buffer[camera_name],
+ lock=self._frame_buffer_lock[camera_name],
+ ):
+ self._logger.warning(f"Calibration of camera {camera_name} failed.")
+ return False
+ self._logger.info("Calibration successful.")
+ return True
diff --git a/extensions/rcs_usb_cam/src/rcs_usb_cam/webcam.py b/extensions/rcs_usb_cam/src/rcs_usb_cam/webcam.py
new file mode 100644
index 00000000..785e3eee
--- /dev/null
+++ b/extensions/rcs_usb_cam/src/rcs_usb_cam/webcam.py
@@ -0,0 +1,41 @@
+import cv2
+
+"""
+Simple script for opening a webcam using OpenCV and displaying the video feed.
+"""
+
+
+def open_webcam():
+ # Open the default webcam (0).
+ # If you have multiple cameras, you can try 1, 2, etc.
+ cap = cv2.VideoCapture("/dev/video8")
+ cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+ cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+ cap.set(cv2.CAP_PROP_FPS, 30)
+
+ if not cap.isOpened():
+ print("Error: Could not open webcam.")
+ return
+
+ # Loop to continuously get frames
+ while True:
+ ret, frame = cap.read()
+
+ if not ret:
+ print("Failed to grab frame")
+ break
+
+ # Show the frame in a window
+ cv2.imshow("Webcam", frame)
+
+ # Press 'q' to quit
+ if cv2.waitKey(1) & 0xFF == ord("q"):
+ break
+
+ # Release the camera and close windows
+ cap.release()
+ cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+ open_webcam()
diff --git a/extensions/rcs_xarm7/pyproject.toml b/extensions/rcs_xarm7/pyproject.toml
index 10b5285b..956124cc 100644
--- a/extensions/rcs_xarm7/pyproject.toml
+++ b/extensions/rcs_xarm7/pyproject.toml
@@ -4,10 +4,10 @@ build-backend = "setuptools.build_meta"
[project]
name = "rcs_xarm7"
-version = "0.4.0"
+version = "0.5.0"
description="RCS xArm7 module"
dependencies = [
- "rcs==0.4.0",
+ "rcs>=0.5.0",
"xarm-python-sdk==1.17.0",
]
readme = "README.md"
diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h
index 6971b38d..c04b0d03 100644
--- a/include/rcs/Robot.h
+++ b/include/rcs/Robot.h
@@ -80,18 +80,23 @@ static const std::unordered_map robots_meta_config =
{SO101,
RobotMetaConfig{
// q_home (5‐vector):
- (VectorXd(5) << -9.40612320177057, -99.66130397967824,
- 99.9124726477024, 69.96996996996998, -9.095744680851055)
+ // (VectorXd(5) << -9.40612320177057, -99.66130397967824,
+ // 99.9124726477024, 69.96996996996998, -9.095744680851055)
+ // .finished(),
+ (VectorXd(5) << -0.01914898, -1.90521916, 1.56476701, 1.04783839, -1.40323926)
.finished(),
// dof:
5,
// joint_limits (2×5):
(Eigen::Matrix(2, 5) <<
- // low 5‐tuple
- -100.0,
- -100.0, -100.0, -100.0, -100.0,
+ // low 5‐tuple
+ -1.9198621771937616,
+ -1.9198621771937634, -1.7453292519943295, -1.6580627969561903,
+ -2.7925268969992407,
+
// high 5‐tuple
- 100.0, 100.0, 100.0, 100.0, 100.0)
+ 1.9198621771937616, 1.9198621771937634, 1.5707963267948966,
+ 1.6580627969561903, 2.7925268969992407)
.finished()}}}};
struct RobotConfig {
diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py
index 47eef514..3ed420b2 100644
--- a/python/rcs/__init__.py
+++ b/python/rcs/__init__.py
@@ -58,6 +58,13 @@ def get_scene_urdf(scene_name: str) -> str | None:
urdf=get_scene_urdf("xarm7_empty_world"),
robot_type=common.RobotType.FR3,
),
+ "so101_empty_world": Scene(
+ mjb=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "so101_empty_world", "scene.mjb"),
+ mjcf_scene=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "so101_empty_world", "scene.xml"),
+ mjcf_robot=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "so101_empty_world", "robot.xml"),
+ urdf=os.path.join(site.getsitepackages()[0], "rcs", "scenes", "so101_empty_world", "robot.urdf"),
+ robot_type=common.RobotType.FR3,
+ ),
}
# make submodules available