From e627d6442e2451421406e9ff351f82182221c2e5 Mon Sep 17 00:00:00 2001 From: Pepijn Date: Thu, 2 Apr 2026 14:25:24 +0200 Subject: [PATCH] feat(umi): add EE replay viewer, URDF meshes, and evaluate script updates - Add replay.py script and replay_viewer.html for browser-based EE trajectory visualization from glannuzel/grabette-dataset - Add viewer.html for interactive URDF inspection - Move OpenArm URDF and meshes into openarm_follower/urdf/ - Add virtual EE target frame (openarm_right_ee_target) at 7cm from link7 - Adapt evaluate.py for single right-arm OpenArm with wrist camera - Update docs with replay viewer usage - Update openarm_follower config, driver, and kinematic processor Made-with: Cursor --- .gitignore | 2 - docs/source/umi_pi0_relative_ee.mdx | 53 ++ examples/umi_pi0_relative_ee/evaluate.py | 38 +- examples/umi_pi0_relative_ee/replay.py | 113 ++++ pyproject.toml | 3 +- .../bi_openarm_follower.py | 4 + .../config_openarm_follower.py | 33 +- .../openarm_follower/openarm_follower.py | 232 ++++--- .../meshes/arm/v10/collision/link0_symp.stl | 3 + .../meshes/arm/v10/collision/link1_symp.stl | 3 + .../meshes/arm/v10/collision/link2_symp.stl | 3 + .../meshes/arm/v10/collision/link3_symp.stl | 3 + .../meshes/arm/v10/collision/link4_symp.stl | 3 + .../meshes/arm/v10/collision/link5_symp.stl | 3 + .../meshes/arm/v10/collision/link6_symp.stl | 3 + .../meshes/arm/v10/collision/link7_symp.stl | 3 + .../urdf/meshes/arm/v10/visual/link0.stl | 3 + .../urdf/meshes/arm/v10/visual/link1.stl | 3 + .../urdf/meshes/arm/v10/visual/link2.stl | 3 + .../urdf/meshes/arm/v10/visual/link3.stl | 3 + .../urdf/meshes/arm/v10/visual/link4.stl | 3 + .../urdf/meshes/arm/v10/visual/link5.stl | 3 + .../urdf/meshes/arm/v10/visual/link6.stl | 3 + .../urdf/meshes/arm/v10/visual/link7.stl | 3 + .../body/v10/collision/body_link0_symp.stl | 3 + .../meshes/body/v10/visual/body_link0.stl | 3 + .../ee/openarm_hand/collision/finger.stl | 3 + .../meshes/ee/openarm_hand/collision/hand.stl | 3 + .../meshes/ee/openarm_hand/visual/finger.stl | 3 + .../urdf/openarm_bimanual_pybullet.urdf | 630 ++++++++++++++++++ .../openarm_follower/urdf/replay_viewer.html | 408 ++++++++++++ .../robots/openarm_follower/urdf/viewer.html | 311 +++++++++ .../so_follower/robot_kinematic_processor.py | 180 +++-- 33 files changed, 1867 insertions(+), 203 deletions(-) create mode 100644 examples/umi_pi0_relative_ee/replay.py create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link0_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link1_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link2_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link3_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link4_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link5_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link6_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link7_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link0.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link1.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link2.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link3.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link4.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link5.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link6.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link7.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/body/v10/collision/body_link0_symp.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/body/v10/visual/body_link0.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/collision/finger.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/collision/hand.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/visual/finger.stl create mode 100644 src/lerobot/robots/openarm_follower/urdf/openarm_bimanual_pybullet.urdf create mode 100644 src/lerobot/robots/openarm_follower/urdf/replay_viewer.html create mode 100644 src/lerobot/robots/openarm_follower/urdf/viewer.html diff --git a/.gitignore b/.gitignore index b47e22cbf..9cc9db4e1 100644 --- a/.gitignore +++ b/.gitignore @@ -173,7 +173,5 @@ outputs/ # Dev folders .cache/* -*.stl -*.urdf *.xml *.part diff --git a/docs/source/umi_pi0_relative_ee.mdx b/docs/source/umi_pi0_relative_ee.mdx index c37637759..efa2daebb 100644 --- a/docs/source/umi_pi0_relative_ee.mdx +++ b/docs/source/umi_pi0_relative_ee.mdx @@ -110,6 +110,59 @@ The inference flow uses pi0's built-in processor pipeline — no custom wrappers 4. **Postprocessor** — `UnnormalizerProcessorStep` unnormalizes, then `AbsoluteActionsProcessorStep` adds the cached state back to get absolute EE targets. 5. **IK → Robot** — `InverseKinematicsEEToJoints` converts absolute EE targets to joint commands. +## Replay Viewer + +Before running on hardware, you can visualize any dataset episode in a browser-based 3D viewer. The viewer shows the EE trajectory overlaid on the OpenArm URDF model, making it easy to sanity-check recorded data or debug unexpected behavior. + +### Quick start + +```bash +python examples/umi_pi0_relative_ee/replay.py +``` + +This extracts the trajectory from episode 0 of the default dataset, starts a local HTTP server, and opens the viewer at [http://localhost:8765/replay_viewer.html](http://localhost:8765/replay_viewer.html). + +### Options + +| Flag | Default | Description | +| ----------- | ---------------------------- | ------------------------------------ | +| `--repo-id` | `glannuzel/grabette-dataset` | HuggingFace dataset repo to load | +| `--episode` | `0` | Episode index to replay | +| `--port` | `8765` | HTTP server port | +| `--force` | off | Re-extract trajectory even if cached | + +Example with a different dataset and episode: + +```bash +python examples/umi_pi0_relative_ee/replay.py \ + --repo-id myuser/my-dataset \ + --episode 3 \ + --port 8766 +``` + +### Viewer controls + +The panel in the top-left corner shows live EE coordinates (x, y, z, ax, ay, az) and gripper state for the current frame. Below that are transport controls: + +- **Play / Pause** — toggle automatic playback. +- **Step buttons** (◀ ▶) — advance or rewind one frame at a time. +- **Reset** (⟳) — jump back to frame 0. +- **Scrubber** — drag to seek to any frame. +- **Speed selector** — 0.25×, 0.5×, 1×, 2×, or 4× playback speed. + +The 3D scene uses orbit controls — click and drag to rotate, scroll to zoom, right-click drag to pan. + +### Color legend + +| Color | Meaning | +| ------------------ | --------------------------------------------- | +| Red sphere | Current EE position | +| Yellow trail | Past trajectory (frames already visited) | +| Dark trail | Future trajectory (frames ahead) | +| Orange ring + axes | URDF `ee_target` frame (zero-joint reference) | + +The trajectory is automatically re-centered so that frame 0 aligns with the robot's `openarm_right_ee_target` link in the zero-joint pose. + ## How the Pieces Fit Together ``` diff --git a/examples/umi_pi0_relative_ee/evaluate.py b/examples/umi_pi0_relative_ee/evaluate.py index e3e73029a..687a77241 100644 --- a/examples/umi_pi0_relative_ee/evaluate.py +++ b/examples/umi_pi0_relative_ee/evaluate.py @@ -86,11 +86,12 @@ HF_MODEL_ID = "pepijn223/grabette-umi-pi0" # E.g. at 10Hz with ~200ms total system latency: ceil(200 / 100) = 2. LATENCY_SKIP_STEPS = 0 -# EE feature keys produced by ForwardKinematicsJointsToEE -EE_KEYS = ["x", "y", "z", "wx", "wy", "wz", "gripper_pos"] +# EE feature keys produced by ForwardKinematicsJointsToEE (arm pose only). +# Gripper joints use absolute position control, not EE-relative. +EE_KEYS = ["x", "y", "z", "wx", "wy", "wz"] -URDF_PATH = "./SO101/so101_new_calib.urdf" -URDF_EE_FRAME = "gripper_frame_link" +URDF_PATH = "src/lerobot/robots/openarm_follower/urdf/openarm_bimanual_pybullet.urdf" +URDF_EE_FRAME = "openarm_right_link7" def main(): @@ -101,38 +102,47 @@ def main(): side="right", cameras=camera_config, max_relative_target=8.0, + gripper_port="/dev/ttyUSB0", ) robot = OpenArmFollower(robot_config) policy = PI0Policy.from_pretrained(HF_MODEL_ID) policy.config.latency_skip_steps = LATENCY_SKIP_STEPS - motor_names = list(robot.bus.motors.keys()) + arm_motor_names = list(robot.bus.motors.keys()) + gripper_motor_names = list(robot.gripper_bus.motors.keys()) kinematics_solver = RobotKinematics( urdf_path=URDF_PATH, target_frame_name=URDF_EE_FRAME, - joint_names=motor_names, + joint_names=arm_motor_names, ) - # FK: joint observation → EE observation (produces observation.state) + # The policy starts from the robot's current EE pose (via FK below). + # Relative actions are predicted as deltas from that pose, so no manual + # re-centering is needed — the starting point is always the live EE tip. + + # FK: joint observation → EE observation (produces observation.state). + # gripper_names=[] means proximal/distal pass through as absolute positions. robot_joints_to_ee_processor = RobotProcessorPipeline[RobotObservation, RobotObservation]( steps=[ ForwardKinematicsJointsToEE( kinematics=kinematics_solver, - motor_names=motor_names, + motor_names=arm_motor_names, + gripper_names=[], ) ], to_transition=observation_to_transition, to_output=transition_to_observation, ) - # IK: EE action → joint targets + # IK: EE action → joint targets. Gripper actions are absolute and pass through. robot_ee_to_joints_processor = RobotProcessorPipeline[tuple[RobotAction, RobotObservation], RobotAction]( steps=[ InverseKinematicsEEToJoints( kinematics=kinematics_solver, - motor_names=motor_names, + motor_names=arm_motor_names, + gripper_names=[], initial_guess_current_joints=True, ), ], @@ -162,7 +172,13 @@ def main(): aggregate_pipeline_dataset_features( pipeline=make_default_teleop_action_processor(), initial_features=create_initial_features( - action={f"ee.{k}": PolicyFeature(type=FeatureType.ACTION, shape=(1,)) for k in EE_KEYS} + action={ + **{f"ee.{k}": PolicyFeature(type=FeatureType.ACTION, shape=(1,)) for k in EE_KEYS}, + **{ + f"{g}.pos": PolicyFeature(type=FeatureType.ACTION, shape=(1,)) + for g in gripper_motor_names + }, + } ), use_videos=True, ), diff --git a/examples/umi_pi0_relative_ee/replay.py b/examples/umi_pi0_relative_ee/replay.py new file mode 100644 index 000000000..3c17b4236 --- /dev/null +++ b/examples/umi_pi0_relative_ee/replay.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python + +# Copyright 2025 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Replay a dataset episode in EE frame using a browser-based URDF viewer. + +Extracts ``observation.pose`` from the dataset, saves a trajectory JSON file, +then launches a local HTTP server and opens the replay viewer. The trajectory +is re-centered so frame 0 starts at the OpenArm ``openarm_right_ee_target`` +EE tip (zero-joint pose). + +Usage: + python replay.py + python replay.py --episode 3 --repo-id myuser/mydata +""" + +from __future__ import annotations + +import argparse +import http.server +import json +import os +import threading +import webbrowser +from pathlib import Path + +VIEWER_DIR = Path(__file__).resolve().parents[2] / "src/lerobot/robots/openarm_follower/urdf" +TRAJECTORY_FILENAME = "trajectory_ep0.json" + + +def extract_trajectory(repo_id: str, episode: int, output_path: Path) -> dict: + from lerobot.datasets.lerobot_dataset import LeRobotDataset + + dataset = LeRobotDataset(repo_id, episodes=[episode]) + poses = dataset.select_columns("observation.pose") + actions = dataset.select_columns("action") + + frames = [] + for i in range(dataset.num_frames): + p = poses[i]["observation.pose"] + a = actions[i]["action"] + frames.append( + { + "x": float(p[0]), + "y": float(p[1]), + "z": float(p[2]), + "ax": float(p[3]), + "ay": float(p[4]), + "az": float(p[5]), + "proximal": float(a[0]), + "distal": float(a[1]), + } + ) + payload = {"fps": dataset.fps, "num_frames": dataset.num_frames, "frames": frames} + with open(output_path, "w") as f: + json.dump(payload, f) + print(f"Extracted {dataset.num_frames} frames at {dataset.fps} FPS → {output_path}") + return payload + + +# --------------------------------------------------------------------------- +# Viewer mode +# --------------------------------------------------------------------------- + + +def serve_and_open(directory: Path, port: int = 8765): + os.chdir(directory) + handler = http.server.SimpleHTTPRequestHandler + httpd = http.server.HTTPServer(("", port), handler) + url = f"http://localhost:{port}/replay_viewer.html" + print(f"Serving at {url}") + threading.Thread(target=lambda: webbrowser.open(url), daemon=True).start() + try: + httpd.serve_forever() + except KeyboardInterrupt: + print("\nServer stopped.") + httpd.server_close() + + +def run_viewer(args): + trajectory_path = VIEWER_DIR / TRAJECTORY_FILENAME + if not trajectory_path.exists() or args.force: + extract_trajectory(args.repo_id, args.episode, trajectory_path) + else: + print(f"Using cached trajectory at {trajectory_path} (pass --force to re-extract)") + serve_and_open(VIEWER_DIR, args.port) + + +def main(): + parser = argparse.ArgumentParser(description="Replay a dataset episode in EE frame (URDF viewer)") + parser.add_argument("--repo-id", default="glannuzel/grabette-dataset") + parser.add_argument("--episode", type=int, default=0) + parser.add_argument("--port", type=int, default=8765) + parser.add_argument("--force", action="store_true", help="Re-extract trajectory even if cached") + args = parser.parse_args() + run_viewer(args) + + +if __name__ == "__main__": + main() diff --git a/pyproject.toml b/pyproject.toml index 4a1efab30..0ec1fd22d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -306,7 +306,8 @@ default.extend-ignore-identifiers-re = [ "thw", "inpt", "ROBOTIS", - "OT_VALUE" + "OT_VALUE", + "metalness", ] # TODO: Uncomment when ready to use diff --git a/src/lerobot/robots/bi_openarm_follower/bi_openarm_follower.py b/src/lerobot/robots/bi_openarm_follower/bi_openarm_follower.py index 11da75d18..d2f7d68bc 100644 --- a/src/lerobot/robots/bi_openarm_follower/bi_openarm_follower.py +++ b/src/lerobot/robots/bi_openarm_follower/bi_openarm_follower.py @@ -62,6 +62,8 @@ class BiOpenArmFollower(Robot): can_bitrate=config.left_arm_config.can_bitrate, can_data_bitrate=config.left_arm_config.can_data_bitrate, motor_config=config.left_arm_config.motor_config, + gripper_port=config.left_arm_config.gripper_port, + gripper_motor_ids=config.left_arm_config.gripper_motor_ids, position_kd=config.left_arm_config.position_kd, position_kp=config.left_arm_config.position_kp, joint_limits=config.left_arm_config.joint_limits, @@ -80,6 +82,8 @@ class BiOpenArmFollower(Robot): can_bitrate=config.right_arm_config.can_bitrate, can_data_bitrate=config.right_arm_config.can_data_bitrate, motor_config=config.right_arm_config.motor_config, + gripper_port=config.right_arm_config.gripper_port, + gripper_motor_ids=config.right_arm_config.gripper_motor_ids, position_kd=config.right_arm_config.position_kd, position_kp=config.right_arm_config.position_kp, joint_limits=config.right_arm_config.joint_limits, diff --git a/src/lerobot/robots/openarm_follower/config_openarm_follower.py b/src/lerobot/robots/openarm_follower/config_openarm_follower.py index 88d81fd50..599c0c6df 100644 --- a/src/lerobot/robots/openarm_follower/config_openarm_follower.py +++ b/src/lerobot/robots/openarm_follower/config_openarm_follower.py @@ -28,7 +28,8 @@ LEFT_DEFAULT_JOINTS_LIMITS: dict[str, tuple[float, float]] = { "joint_5": (-85.0, 85.0), "joint_6": (-40.0, 40.0), "joint_7": (-80.0, 80.0), - "gripper": (-65.0, 0.0), + "proximal": (0.0, 100.0), + "distal": (0.0, 100.0), } RIGHT_DEFAULT_JOINTS_LIMITS: dict[str, tuple[float, float]] = { @@ -39,7 +40,8 @@ RIGHT_DEFAULT_JOINTS_LIMITS: dict[str, tuple[float, float]] = { "joint_5": (-85.0, 85.0), "joint_6": (-40.0, 40.0), "joint_7": (-80.0, 80.0), - "gripper": (-65.0, 0.0), + "proximal": (0.0, 100.0), + "distal": (0.0, 100.0), } @@ -73,13 +75,8 @@ class OpenArmFollowerConfigBase: # Camera configurations cameras: dict[str, CameraConfig] = field(default_factory=dict) - # Motor configuration for OpenArms (7 DOF per arm) + # Arm motor configuration (7 DOF, Damiao on CAN bus) # Maps motor names to (send_can_id, recv_can_id, motor_type) - # Based on: https://docs.openarm.dev/software/setup/configure-test - # OpenArms uses 4 types of motors: - # - DM8009 (DM-J8009P-2EC) for shoulders (high torque) - # - DM4340P and DM4340 for shoulder rotation and elbow - # - DM4310 (DM-J4310-2EC V1.1) for wrist and gripper motor_config: dict[str, tuple[int, int, str]] = field( default_factory=lambda: { "joint_1": (0x01, 0x11, "dm8009"), # J1 - Shoulder pan (DM8009) @@ -89,19 +86,18 @@ class OpenArmFollowerConfigBase: "joint_5": (0x05, 0x15, "dm4310"), # J5 - Wrist roll (DM4310) "joint_6": (0x06, 0x16, "dm4310"), # J6 - Wrist pitch (DM4310) "joint_7": (0x07, 0x17, "dm4310"), # J7 - Wrist rotation (DM4310) - "gripper": (0x08, 0x18, "dm4310"), # J8 - Gripper (DM4310) } ) - # MIT control parameters for position control (used in send_action) - # List of 8 values: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7, gripper] - position_kp: list[float] = field( - default_factory=lambda: [240.0, 240.0, 240.0, 240.0, 24.0, 31.0, 25.0, 25.0] - ) - position_kd: list[float] = field(default_factory=lambda: [5.0, 5.0, 3.0, 5.0, 0.3, 0.3, 0.3, 0.3]) + # UMI-style gripper (Feetech STS3215 on serial bus) + gripper_port: str = "/dev/ttyUSB0" + gripper_motor_ids: dict[str, int] = field(default_factory=lambda: {"proximal": 1, "distal": 2}) - # Values for joint limits. Can be overridden via CLI (for custom values) or by setting config.side to either 'left' or 'right'. - # If config.side is left set to None and no CLI values are passed, the default joint limit values are small for safety. + # MIT control parameters for the 7 arm joints + position_kp: list[float] = field(default_factory=lambda: [240.0, 240.0, 240.0, 240.0, 24.0, 31.0, 25.0]) + position_kd: list[float] = field(default_factory=lambda: [5.0, 5.0, 3.0, 5.0, 0.3, 0.3, 0.3]) + + # Joint limits. Can be overridden via CLI or by setting config.side to 'left' or 'right'. joint_limits: dict[str, tuple[float, float]] = field( default_factory=lambda: { "joint_1": (-5.0, 5.0), @@ -111,7 +107,8 @@ class OpenArmFollowerConfigBase: "joint_5": (-5.0, 5.0), "joint_6": (-5.0, 5.0), "joint_7": (-5.0, 5.0), - "gripper": (-5.0, 0.0), + "proximal": (0.0, 100.0), + "distal": (0.0, 100.0), } ) diff --git a/src/lerobot/robots/openarm_follower/openarm_follower.py b/src/lerobot/robots/openarm_follower/openarm_follower.py index 99e8b920b..6da9ffab3 100644 --- a/src/lerobot/robots/openarm_follower/openarm_follower.py +++ b/src/lerobot/robots/openarm_follower/openarm_follower.py @@ -22,6 +22,7 @@ from typing import Any from lerobot.cameras.utils import make_cameras_from_configs from lerobot.motors import Motor, MotorCalibration, MotorNormMode from lerobot.motors.damiao import DamiaoMotorsBus +from lerobot.motors.feetech import FeetechMotorsBus, OperatingMode from lerobot.types import RobotAction, RobotObservation from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected @@ -38,8 +39,7 @@ logger = logging.getLogger(__name__) class OpenArmFollower(Robot): """ - OpenArms Follower Robot which uses CAN bus communication to control 7 DOF arm with a gripper. - The arm uses Damiao motors in MIT control mode. + OpenArms Follower Robot: 7 DOF Damiao arm (CAN) + UMI-style Feetech gripper (serial). """ config_class = OpenArmFollowerConfig @@ -49,19 +49,17 @@ class OpenArmFollower(Robot): super().__init__(config) self.config = config - # Arm motors - motors: dict[str, Motor] = {} + # Arm motors (Damiao on CAN bus) + arm_motors: dict[str, Motor] = {} for motor_name, (send_id, recv_id, motor_type_str) in config.motor_config.items(): - motor = Motor( - send_id, motor_type_str, MotorNormMode.DEGREES - ) # Always use degrees for Damiao motors + motor = Motor(send_id, motor_type_str, MotorNormMode.DEGREES) motor.recv_id = recv_id motor.motor_type_str = motor_type_str - motors[motor_name] = motor + arm_motors[motor_name] = motor self.bus = DamiaoMotorsBus( port=self.config.port, - motors=motors, + motors=arm_motors, calibration=self.calibration, can_interface=self.config.can_interface, use_can_fd=self.config.use_can_fd, @@ -69,6 +67,17 @@ class OpenArmFollower(Robot): data_bitrate=self.config.can_data_bitrate if self.config.use_can_fd else None, ) + # Gripper motors (Feetech STS3215 on serial bus) + gripper_motors: dict[str, Motor] = { + name: Motor(motor_id, "sts3215", MotorNormMode.RANGE_0_100) + for name, motor_id in config.gripper_motor_ids.items() + } + self.gripper_bus = FeetechMotorsBus( + port=config.gripper_port, + motors=gripper_motors, + calibration=self.calibration, + ) + if config.side is not None: if config.side == "left": config.joint_limits = LEFT_DEFAULT_JOINTS_LIMITS @@ -84,7 +93,6 @@ class OpenArmFollower(Robot): ) logger.info(f"Values used for joint limits: {config.joint_limits}.") - # Initialize cameras self.cameras = make_cameras_from_configs(config.cameras) @property @@ -93,8 +101,10 @@ class OpenArmFollower(Robot): features: dict[str, type] = {} for motor in self.bus.motors: features[f"{motor}.pos"] = float - features[f"{motor}.vel"] = float # Add this - features[f"{motor}.torque"] = float # Add this + features[f"{motor}.vel"] = float + features[f"{motor}.torque"] = float + for motor in self.gripper_bus.motors: + features[f"{motor}.pos"] = float return features @property @@ -116,8 +126,11 @@ class OpenArmFollower(Robot): @property def is_connected(self) -> bool: - """Check if robot is connected.""" - return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values()) + return ( + self.bus.is_connected + and self.gripper_bus.is_connected + and all(cam.is_connected for cam in self.cameras.values()) + ) @check_if_already_connected def connect(self, calibrate: bool = True) -> None: @@ -127,12 +140,12 @@ class OpenArmFollower(Robot): We assume that at connection time, the arms are in a safe rest position, and torque can be safely disabled to run calibration if needed. """ - - # Connect to CAN bus logger.info(f"Connecting arm on {self.config.port}...") self.bus.connect() - # Run calibration if needed + logger.info(f"Connecting gripper on {self.config.gripper_port}...") + self.gripper_bus.connect() + if not self.is_calibrated and calibrate: logger.info( "Mismatch between calibration values in the motor and the calibration file or no calibration file found" @@ -144,7 +157,7 @@ class OpenArmFollower(Robot): self.configure() - if self.is_calibrated: + if self.bus.is_calibrated: self.bus.set_zero_position() self.bus.enable_torque() @@ -153,47 +166,39 @@ class OpenArmFollower(Robot): @property def is_calibrated(self) -> bool: - """Check if robot is calibrated.""" - return self.bus.is_calibrated + return self.bus.is_calibrated and self.gripper_bus.is_calibrated def calibrate(self) -> None: """ - Run calibration procedure for OpenArms robot. + Run calibration for both the Damiao arm and Feetech gripper. - The calibration procedure: - 1. Disable torque - 2. Ask user to position arms in hanging position with grippers closed - 3. Set this as zero position - 4. Record range of motion for each joint - 5. Save calibration + Arm calibration: set zero position with arm hanging, ±90° default range. + Gripper calibration: SO100-style half-turn homing + range recording. """ if self.calibration: - # Calibration file exists, ask user whether to use it or run new calibration user_input = input( f"Press ENTER to use provided calibration file associated with the id {self.id}, or type 'c' and press ENTER to run calibration: " ) if user_input.strip().lower() != "c": logger.info(f"Writing calibration file associated with the id {self.id} to the motors") self.bus.write_calibration(self.calibration) + self.gripper_bus.write_calibration(self.calibration) return logger.info(f"\nRunning calibration for {self}") - self.bus.disable_torque() - # Step 1: Set zero position + # --- Arm calibration (Damiao) --- + self.bus.disable_torque() input( - "\nCalibration: Set Zero Position)\n" + "\nCalibration: Set Zero Position\n" "Position the arm in the following configuration:\n" " - Arm hanging straight down\n" " - Gripper closed\n" "Press ENTER when ready..." ) - - # Set current position as zero for all motors self.bus.set_zero_position() logger.info("Arm zero position set.") - logger.info("Setting range: -90° to +90° for safety by default for all joints") for motor_name, motor in self.bus.motors.items(): self.calibration[motor_name] = MotorCalibration( id=motor.id, @@ -202,17 +207,52 @@ class OpenArmFollower(Robot): range_min=-90, range_max=90, ) - self.bus.write_calibration(self.calibration) + + # --- Gripper calibration (Feetech) --- + self.gripper_bus.disable_torque() + for motor in self.gripper_bus.motors: + self.gripper_bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) + + input("Move gripper to the middle of its range of motion and press ENTER....") + homing_offsets = self.gripper_bus.set_half_turn_homings() + + gripper_motor_names = list(self.gripper_bus.motors.keys()) + print( + f"Move gripper joints ({', '.join(gripper_motor_names)}) through their " + "entire ranges of motion.\nRecording positions. Press ENTER to stop..." + ) + range_mins, range_maxes = self.gripper_bus.record_ranges_of_motion(gripper_motor_names) + + for motor_name, m in self.gripper_bus.motors.items(): + self.calibration[motor_name] = MotorCalibration( + id=m.id, + drive_mode=0, + homing_offset=homing_offsets[motor_name], + range_min=range_mins[motor_name], + range_max=range_maxes[motor_name], + ) + self.gripper_bus.write_calibration(self.calibration) + self._save_calibration() print(f"Calibration saved to {self.calibration_fpath}") def configure(self) -> None: - """Configure motors with appropriate settings.""" - # TODO(Steven, Pepijn): Slightly different from what it is happening in the leader + """Configure both arm (Damiao) and gripper (Feetech) motors.""" with self.bus.torque_disabled(): self.bus.configure_motors() + with self.gripper_bus.torque_disabled(): + self.gripper_bus.configure_motors() + for motor in self.gripper_bus.motors: + self.gripper_bus.write("Operating_Mode", motor, OperatingMode.POSITION.value) + self.gripper_bus.write("P_Coefficient", motor, 16) + self.gripper_bus.write("I_Coefficient", motor, 0) + self.gripper_bus.write("D_Coefficient", motor, 32) + self.gripper_bus.write("Max_Torque_Limit", motor, 500) + self.gripper_bus.write("Protection_Current", motor, 250) + self.gripper_bus.write("Overload_Torque", motor, 25) + def setup_motors(self) -> None: raise NotImplementedError( "Motor ID configuration is typically done via manufacturer tools for CAN motors." @@ -220,25 +260,23 @@ class OpenArmFollower(Robot): @check_if_not_connected def get_observation(self) -> RobotObservation: - """ - Get current observation from robot including position, velocity, and torque. - - Reads all motor states (pos/vel/torque) in one CAN refresh cycle - instead of 3 separate reads. - """ + """Read all motor states from arm (CAN) and gripper (serial), plus cameras.""" start = time.perf_counter() - obs_dict: dict[str, Any] = {} + # Arm motors (Damiao) — pos/vel/torque in one CAN refresh cycle states = self.bus.sync_read_all_states() - for motor in self.bus.motors: state = states.get(motor, {}) obs_dict[f"{motor}.pos"] = state.get("position", 0.0) obs_dict[f"{motor}.vel"] = state.get("velocity", 0.0) obs_dict[f"{motor}.torque"] = state.get("torque", 0.0) - # Capture images from cameras + # Gripper motors (Feetech) — position only + gripper_positions = self.gripper_bus.sync_read("Present_Position") + for motor, val in gripper_positions.items(): + obs_dict[f"{motor}.pos"] = val + for cam_key, cam in self.cameras.items(): start = time.perf_counter() obs_dict[cam_key] = cam.read_latest() @@ -258,86 +296,76 @@ class OpenArmFollower(Robot): custom_kd: dict[str, float] | None = None, ) -> RobotAction: """ - Send action command to robot. - - The action magnitude may be clipped based on safety limits. + Send action command to robot. Arm joints go to Damiao CAN bus, + gripper joints go to Feetech serial bus. Args: - action: Dictionary with motor positions (e.g., "joint_1.pos", "joint_2.pos") - custom_kp: Optional custom kp gains per motor (e.g., {"joint_1": 120.0, "joint_2": 150.0}) - custom_kd: Optional custom kd gains per motor (e.g., {"joint_1": 1.5, "joint_2": 2.0}) + action: Dictionary with motor positions (e.g., "joint_1.pos", "proximal.pos") + custom_kp: Optional custom kp gains per arm motor + custom_kd: Optional custom kd gains per arm motor Returns: The action actually sent (potentially clipped) """ - goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} - # Apply joint limit clipping to arm + # Apply joint limit clipping for motor_name, position in goal_pos.items(): if motor_name in self.config.joint_limits: min_limit, max_limit = self.config.joint_limits[motor_name] clipped_position = max(min_limit, min(max_limit, position)) if clipped_position != position: - logger.debug(f"Clipped {motor_name} from {position:.2f}° to {clipped_position:.2f}°") + logger.debug(f"Clipped {motor_name} from {position:.2f} to {clipped_position:.2f}") goal_pos[motor_name] = clipped_position - # Cap goal position when too far away from present position. - # /!\ Slower fps expected due to reading from the follower. - if self.config.max_relative_target is not None: + # Split into arm and gripper actions + arm_motors = set(self.bus.motors.keys()) + gripper_motors = set(self.gripper_bus.motors.keys()) + arm_goal = {k: v for k, v in goal_pos.items() if k in arm_motors} + gripper_goal = {k: v for k, v in goal_pos.items() if k in gripper_motors} + + # Cap arm goal position when too far away from present position + if self.config.max_relative_target is not None and arm_goal: present_pos = self.bus.sync_read("Present_Position") - goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in goal_pos.items()} - goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) + goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in arm_goal.items()} + arm_goal = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target) - # TODO(Steven, Pepijn): Refactor writing - # Motor name to index mapping for gains - motor_index = { - "joint_1": 0, - "joint_2": 1, - "joint_3": 2, - "joint_4": 3, - "joint_5": 4, - "joint_6": 5, - "joint_7": 6, - "gripper": 7, - } + # Arm: batch MIT control (Damiao) + if arm_goal: + arm_motor_names = list(self.bus.motors.keys()) + commands = {} + for motor_name, position_degrees in arm_goal.items(): + idx = arm_motor_names.index(motor_name) if motor_name in arm_motor_names else 0 + if custom_kp is not None and motor_name in custom_kp: + kp = custom_kp[motor_name] + else: + kp = ( + self.config.position_kp[idx] + if isinstance(self.config.position_kp, list) + else self.config.position_kp + ) + if custom_kd is not None and motor_name in custom_kd: + kd = custom_kd[motor_name] + else: + kd = ( + self.config.position_kd[idx] + if isinstance(self.config.position_kd, list) + else self.config.position_kd + ) + commands[motor_name] = (kp, kd, position_degrees, 0.0, 0.0) + self.bus._mit_control_batch(commands) - # Use batch MIT control for arm (sends all commands, then collects responses) - commands = {} - for motor_name, position_degrees in goal_pos.items(): - idx = motor_index.get(motor_name, 0) - # Use custom gains if provided, otherwise use config defaults - if custom_kp is not None and motor_name in custom_kp: - kp = custom_kp[motor_name] - else: - kp = ( - self.config.position_kp[idx] - if isinstance(self.config.position_kp, list) - else self.config.position_kp - ) - if custom_kd is not None and motor_name in custom_kd: - kd = custom_kd[motor_name] - else: - kd = ( - self.config.position_kd[idx] - if isinstance(self.config.position_kd, list) - else self.config.position_kd - ) - commands[motor_name] = (kp, kd, position_degrees, 0.0, 0.0) - - self.bus._mit_control_batch(commands) + # Gripper: position control (Feetech) + if gripper_goal: + self.gripper_bus.sync_write("Goal_Position", gripper_goal) + goal_pos.update(arm_goal) return {f"{motor}.pos": val for motor, val in goal_pos.items()} @check_if_not_connected def disconnect(self): - """Disconnect from robot.""" - - # Disconnect CAN bus self.bus.disconnect(self.config.disable_torque_on_disconnect) - - # Disconnect cameras + self.gripper_bus.disconnect(self.config.disable_torque_on_disconnect) for cam in self.cameras.values(): cam.disconnect() - logger.info(f"{self} disconnected.") diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link0_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link0_symp.stl new file mode 100644 index 000000000..303161fd4 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link0_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:baf52578e1d9e6225f3818cae82b6074a0b948d3cef8e9a3e6dfafca78507590 +size 40284 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link1_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link1_symp.stl new file mode 100644 index 000000000..f4cb78f2b --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link1_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:066113d13d5cc85098609003bc7ebb73c570015350877f5ed7162ef1b6601852 +size 17784 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link2_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link2_symp.stl new file mode 100644 index 000000000..85d0cde79 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link2_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:382ab32e4ae0880e8a1512e7a6ca6ce1f478a6c125db4efa977429ffb1d6b02a +size 13384 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link3_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link3_symp.stl new file mode 100644 index 000000000..48d72950d --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link3_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:00c908cefab152c00416a570a48bf9aafed1549085f19ff2d882dc3f355d9f59 +size 156984 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link4_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link4_symp.stl new file mode 100644 index 000000000..cb96fbfc5 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link4_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b54883b8c7c96268a68a5879f95998a53ad0b0c4fe74325fad63a6caef669c73 +size 1139984 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link5_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link5_symp.stl new file mode 100644 index 000000000..d165f4621 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link5_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:678a2802906eff7b45a836d2f34a2d8e51def50b6599376968f888e05c72739e +size 751484 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link6_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link6_symp.stl new file mode 100644 index 000000000..16e233661 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link6_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:95529bec23733476dfdbbb266c7db0d25a473a568de73c8337a82440fe4a9ac3 +size 30084 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link7_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link7_symp.stl new file mode 100644 index 000000000..96f68d285 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/collision/link7_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:434f207f21f75f5f0bd604e390b8e5bc7b62b619265222846770e06b3f9b5cfb +size 23884 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link0.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link0.stl new file mode 100644 index 000000000..ce7f518db --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c77bdc9419947088e1dfc452e29c6092cc7b02b239ff4f2f5be3d77e393af185 +size 4148234 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link1.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link1.stl new file mode 100644 index 000000000..41bee1e4f --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2a182c7fd4d25226aff6d9e2dd9d1008a85fce7df8e16525722a5c5053f8b055 +size 5741534 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link2.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link2.stl new file mode 100644 index 000000000..beebd74d0 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:de1b190a56c16dea14546fb8e22c86eccabc2ca5e054819630c1932592381745 +size 4543534 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link3.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link3.stl new file mode 100644 index 000000000..7ca572291 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link3.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d47d75d0c47a65021708ba63cb73162bf7c3b1d14e9cfc70dfa47336034ac76 +size 4978834 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link4.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link4.stl new file mode 100644 index 000000000..a12a14ece --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link4.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ca8149f2ce8b1b102270ec0b1a4b75c3e6f98c09b084430a450adce808607e1 +size 4944684 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link5.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link5.stl new file mode 100644 index 000000000..e1fac73ba --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link5.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:30ea7abfdd3661b315f897bb82a5f34fd966357b358e890e155e928e931ea975 +size 6322984 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link6.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link6.stl new file mode 100644 index 000000000..066a5ca28 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link6.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e49f91279f109baecb9ff54f5041eeb4514f757ba6daa65c3ea01fb1991967e4 +size 4818434 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link7.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link7.stl new file mode 100644 index 000000000..d0e853e53 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/arm/v10/visual/link7.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a91593b67d2dec16d1dfb6f1305df3ddcd214cdef02e97d5aba30bc633e775b2 +size 5114784 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/body/v10/collision/body_link0_symp.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/body/v10/collision/body_link0_symp.stl new file mode 100644 index 000000000..dad873332 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/body/v10/collision/body_link0_symp.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6c18bbf7e86b03e3faf802e61e8eb438b38dcbcf146d97cffe6e808c65e9a72a +size 293284 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/body/v10/visual/body_link0.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/body/v10/visual/body_link0.stl new file mode 100644 index 000000000..ec9fc424c --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/body/v10/visual/body_link0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7ab0afa9a26bebbf5c6149894c41d96622caa649555ae7c5f0f2548f86148d91 +size 7955034 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/collision/finger.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/collision/finger.stl new file mode 100644 index 000000000..ad504060a --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/collision/finger.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8e96e1314618cf434908f70df78f68dd2b049c03538964e8d41fc99abe41564d +size 13284 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/collision/hand.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/collision/hand.stl new file mode 100644 index 000000000..381740b7d --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/collision/hand.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8e5d373ebbd3fd001b506058644062ad71a68f1ced5ca5d5ed0f6de20137956b +size 18284 diff --git a/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/visual/finger.stl b/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/visual/finger.stl new file mode 100644 index 000000000..e3868451c --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/meshes/ee/openarm_hand/visual/finger.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:25c115c7c55a422f30ad11581dc21576dc8fc4e09e659890772d86fe82ec04d7 +size 432484 diff --git a/src/lerobot/robots/openarm_follower/urdf/openarm_bimanual_pybullet.urdf b/src/lerobot/robots/openarm_follower/urdf/openarm_bimanual_pybullet.urdf new file mode 100644 index 000000000..7a734a609 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/openarm_bimanual_pybullet.urdf @@ -0,0 +1,630 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/lerobot/robots/openarm_follower/urdf/replay_viewer.html b/src/lerobot/robots/openarm_follower/urdf/replay_viewer.html new file mode 100644 index 000000000..ad0eec7d9 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/replay_viewer.html @@ -0,0 +1,408 @@ + + + + +Dataset Replay — EE Frame Viewer + + + + + +
+

DATASET REPLAY — EE FRAME

+
glannuzel/grabette-dataset · episode 0
+ +
EE target (dataset)
+
Trajectory (past)
+
Trajectory (future)
+ +
—
+
—
+
—
+
—
+
—
+
—
+
—
+ +
+ + + + +
+ +
+ + +
+
Frame 0 / 0 · 0.00s
+
+ + + + + + diff --git a/src/lerobot/robots/openarm_follower/urdf/viewer.html b/src/lerobot/robots/openarm_follower/urdf/viewer.html new file mode 100644 index 000000000..931bfa0b9 --- /dev/null +++ b/src/lerobot/robots/openarm_follower/urdf/viewer.html @@ -0,0 +1,311 @@ + + + + +OpenArm URDF Viewer + + + + + +
+

OpenArm Right Arm — EE Frame Options

+
openarm_right_link7 (wrist output)
+
openarm_right_ee_target (+5cm)
+
openarm_right_hand (+10cm)
+
openarm_right_hand_tcp (+18cm)
+
+ + + + +
+
Loading URDF...
+

Drag to orbit · Scroll to zoom · Right-drag to pan

+
+ + + + + + diff --git a/src/lerobot/robots/so_follower/robot_kinematic_processor.py b/src/lerobot/robots/so_follower/robot_kinematic_processor.py index 2aa60e12a..71b700d60 100644 --- a/src/lerobot/robots/so_follower/robot_kinematic_processor.py +++ b/src/lerobot/robots/so_follower/robot_kinematic_processor.py @@ -255,19 +255,16 @@ class InverseKinematicsEEToJoints(RobotActionProcessorStep): """ Computes desired joint positions from a target end-effector pose using inverse kinematics (IK). - This step translates a Cartesian command (position and orientation of the end-effector) into - the corresponding joint-space commands for each motor. - Attributes: kinematics: The robot's kinematic model for inverse kinematics. - motor_names: A list of motor names for which to compute joint positions. - q_curr: Internal state storing the last joint positions, used as an initial guess for the IK solver. + motor_names: Arm joint names for IK computation. + gripper_names: Gripper joint name(s). ee.gripper_pos is written to all of them. initial_guess_current_joints: If True, use the robot's current joint state as the IK guess. - If False, use the solution from the previous step. """ kinematics: RobotKinematics motor_names: list[str] + gripper_names: list[str] = field(default_factory=lambda: ["gripper"]) q_curr: np.ndarray | None = field(default=None, init=False, repr=False) initial_guess_current_joints: bool = True @@ -278,63 +275,73 @@ class InverseKinematicsEEToJoints(RobotActionProcessorStep): wx = action.pop("ee.wx") wy = action.pop("ee.wy") wz = action.pop("ee.wz") - gripper_pos = action.pop("ee.gripper_pos") - if None in (x, y, z, wx, wy, wz, gripper_pos): - raise ValueError( - "Missing required end-effector pose components: ee.x, ee.y, ee.z, ee.wx, ee.wy, ee.wz, ee.gripper_pos must all be present in action" - ) + ee_keys = [x, y, z, wx, wy, wz] + + if self.gripper_names: + gripper_pos = action.pop("ee.gripper_pos") + ee_keys.append(gripper_pos) + if None in ee_keys: + raise ValueError("Missing required end-effector pose components in action") observation = self.transition.get(TransitionKey.OBSERVATION).copy() if observation is None: - raise ValueError("Joints observation is require for computing robot kinematics") + raise ValueError("Joints observation is required for computing robot kinematics") q_raw = np.array( - [float(v) for k, v in observation.items() if isinstance(k, str) and k.endswith(".pos")], + [ + float(v) + for k, v in observation.items() + if isinstance(k, str) and k.endswith(".pos") and k.removesuffix(".pos") in self.motor_names + ], dtype=float, ) - if q_raw is None: - raise ValueError("Joints observation is require for computing robot kinematics") - if self.initial_guess_current_joints: # Use current joints as initial guess + if self.initial_guess_current_joints: self.q_curr = q_raw - else: # Use previous ik solution as initial guess + else: if self.q_curr is None: self.q_curr = q_raw - # Build desired 4x4 transform from pos + rotvec (twist) t_des = np.eye(4, dtype=float) t_des[:3, :3] = Rotation.from_rotvec([wx, wy, wz]).as_matrix() t_des[:3, 3] = [x, y, z] - # Compute inverse kinematics q_target = self.kinematics.inverse_kinematics(self.q_curr, t_des) self.q_curr = q_target - # TODO: This is sentitive to order of motor_names = q_target mapping for i, name in enumerate(self.motor_names): - if name != "gripper": - action[f"{name}.pos"] = float(q_target[i]) - else: - action["gripper.pos"] = float(gripper_pos) + action[f"{name}.pos"] = float(q_target[i]) + if self.gripper_names: + for gname in self.gripper_names: + action[f"{gname}.pos"] = float(gripper_pos) + + # When gripper_names is empty, gripper keys (e.g. proximal.pos, distal.pos) + # are already in the action dict as absolute positions — left untouched. return action def transform_features( self, features: dict[PipelineFeatureType, dict[str, PolicyFeature]] ) -> dict[PipelineFeatureType, dict[str, PolicyFeature]]: - for feat in ["x", "y", "z", "wx", "wy", "wz", "gripper_pos"]: + ee_feats = ["x", "y", "z", "wx", "wy", "wz"] + if self.gripper_names: + ee_feats.append("gripper_pos") + for feat in ee_feats: features[PipelineFeatureType.ACTION].pop(f"ee.{feat}", None) for name in self.motor_names: features[PipelineFeatureType.ACTION][f"{name}.pos"] = PolicyFeature( type=FeatureType.ACTION, shape=(1,) ) + for name in self.gripper_names: + features[PipelineFeatureType.ACTION][f"{name}.pos"] = PolicyFeature( + type=FeatureType.ACTION, shape=(1,) + ) return features def reset(self): - """Resets the initial guess for the IK solver.""" self.q_curr = None @@ -402,24 +409,39 @@ class GripperVelocityToJoint(RobotActionProcessorStep): def compute_forward_kinematics_joints_to_ee( - joints: dict[str, Any], kinematics: RobotKinematics, motor_names: list[str] + joints: dict[str, Any], + kinematics: RobotKinematics, + motor_names: list[str], + gripper_names: list[str] | None = None, ) -> dict[str, Any]: + if gripper_names is None: + gripper_names = ["gripper"] + motor_joint_values = [joints[f"{n}.pos"] for n in motor_names] q = np.array(motor_joint_values, dtype=float) t = kinematics.forward_kinematics(q) pos = t[:3, 3] tw = Rotation.from_matrix(t[:3, :3]).as_rotvec() - gripper_pos = joints["gripper.pos"] + for n in motor_names: joints.pop(f"{n}.pos") + joints["ee.x"] = float(pos[0]) joints["ee.y"] = float(pos[1]) joints["ee.z"] = float(pos[2]) joints["ee.wx"] = float(tw[0]) joints["ee.wy"] = float(tw[1]) joints["ee.wz"] = float(tw[2]) - joints["ee.gripper_pos"] = float(gripper_pos) + + # When gripper_names is non-empty, fold them into ee.gripper_pos (e.g. SO100). + # When empty, gripper joints pass through as-is (absolute position control). + if gripper_names: + gripper_pos = joints[f"{gripper_names[0]}.pos"] + for n in gripper_names: + joints.pop(f"{n}.pos", None) + joints["ee.gripper_pos"] = float(gripper_pos) + return joints @@ -429,27 +451,33 @@ class ForwardKinematicsJointsToEEObservation(ObservationProcessorStep): """ Computes the end-effector pose from joint positions using forward kinematics (FK). - This step is typically used to add the robot's Cartesian pose to the observation space, - which can be useful for visualization or as an input to a policy. - Attributes: kinematics: The robot's kinematic model. + motor_names: Arm joint names used for FK computation. + gripper_names: Gripper joint name(s) to fold into ee.gripper_pos. + Empty list means gripper joints pass through as absolute positions. """ kinematics: RobotKinematics motor_names: list[str] + gripper_names: list[str] = field(default_factory=lambda: ["gripper"]) def observation(self, observation: RobotObservation) -> RobotObservation: - return compute_forward_kinematics_joints_to_ee(observation, self.kinematics, self.motor_names) + return compute_forward_kinematics_joints_to_ee( + observation, self.kinematics, self.motor_names, self.gripper_names + ) def transform_features( self, features: dict[PipelineFeatureType, dict[str, PolicyFeature]] ) -> dict[PipelineFeatureType, dict[str, PolicyFeature]]: - # We only use the ee pose in the dataset, so we don't need the joint positions for n in self.motor_names: features[PipelineFeatureType.OBSERVATION].pop(f"{n}.pos", None) - # We specify the dataset features of this step that we want to be stored in the dataset - for k in ["x", "y", "z", "wx", "wy", "wz", "gripper_pos"]: + ee_keys = ["x", "y", "z", "wx", "wy", "wz"] + if self.gripper_names: + for n in self.gripper_names: + features[PipelineFeatureType.OBSERVATION].pop(f"{n}.pos", None) + ee_keys.append("gripper_pos") + for k in ee_keys: features[PipelineFeatureType.OBSERVATION][f"ee.{k}"] = PolicyFeature( type=FeatureType.STATE, shape=(1,) ) @@ -462,27 +490,33 @@ class ForwardKinematicsJointsToEEAction(RobotActionProcessorStep): """ Computes the end-effector pose from joint positions using forward kinematics (FK). - This step is typically used to add the robot's Cartesian pose to the observation space, - which can be useful for visualization or as an input to a policy. - Attributes: kinematics: The robot's kinematic model. + motor_names: Arm joint names used for FK computation. + gripper_names: Gripper joint name(s) to fold into ee.gripper_pos. + Empty list means gripper joints pass through as absolute positions. """ kinematics: RobotKinematics motor_names: list[str] + gripper_names: list[str] = field(default_factory=lambda: ["gripper"]) def action(self, action: RobotAction) -> RobotAction: - return compute_forward_kinematics_joints_to_ee(action, self.kinematics, self.motor_names) + return compute_forward_kinematics_joints_to_ee( + action, self.kinematics, self.motor_names, self.gripper_names + ) def transform_features( self, features: dict[PipelineFeatureType, dict[str, PolicyFeature]] ) -> dict[PipelineFeatureType, dict[str, PolicyFeature]]: - # We only use the ee pose in the dataset, so we don't need the joint positions for n in self.motor_names: features[PipelineFeatureType.ACTION].pop(f"{n}.pos", None) - # We specify the dataset features of this step that we want to be stored in the dataset - for k in ["x", "y", "z", "wx", "wy", "wz", "gripper_pos"]: + ee_keys = ["x", "y", "z", "wx", "wy", "wz"] + if self.gripper_names: + for n in self.gripper_names: + features[PipelineFeatureType.ACTION].pop(f"{n}.pos", None) + ee_keys.append("gripper_pos") + for k in ee_keys: features[PipelineFeatureType.ACTION][f"ee.{k}"] = PolicyFeature( type=FeatureType.STATE, shape=(1,) ) @@ -494,13 +528,14 @@ class ForwardKinematicsJointsToEEAction(RobotActionProcessorStep): class ForwardKinematicsJointsToEE(ProcessorStep): kinematics: RobotKinematics motor_names: list[str] + gripper_names: list[str] = field(default_factory=lambda: ["gripper"]) def __post_init__(self): self.joints_to_ee_action_processor = ForwardKinematicsJointsToEEAction( - kinematics=self.kinematics, motor_names=self.motor_names + kinematics=self.kinematics, motor_names=self.motor_names, gripper_names=self.gripper_names ) self.joints_to_ee_observation_processor = ForwardKinematicsJointsToEEObservation( - kinematics=self.kinematics, motor_names=self.motor_names + kinematics=self.kinematics, motor_names=self.motor_names, gripper_names=self.gripper_names ) def __call__(self, transition: EnvTransition) -> EnvTransition: @@ -524,13 +559,13 @@ class ForwardKinematicsJointsToEE(ProcessorStep): @dataclass class InverseKinematicsRLStep(ProcessorStep): """ - Computes desired joint positions from a target end-effector pose using inverse kinematics (IK). - - This is modified from the InverseKinematicsEEToJoints step to be used in the RL pipeline. + IK step for the RL pipeline. Same logic as InverseKinematicsEEToJoints but + operates on EnvTransition directly and stores the IK solution. """ kinematics: RobotKinematics motor_names: list[str] + gripper_names: list[str] = field(default_factory=lambda: ["gripper"]) q_curr: np.ndarray | None = field(default=None, init=False, repr=False) initial_guess_current_joints: bool = True @@ -538,7 +573,7 @@ class InverseKinematicsRLStep(ProcessorStep): new_transition = dict(transition) action = new_transition.get(TransitionKey.ACTION) if action is None: - raise ValueError("Action is required for InverseKinematicsEEToJoints") + raise ValueError("Action is required for InverseKinematicsRLStep") action = dict(action) x = action.pop("ee.x") @@ -547,45 +582,46 @@ class InverseKinematicsRLStep(ProcessorStep): wx = action.pop("ee.wx") wy = action.pop("ee.wy") wz = action.pop("ee.wz") - gripper_pos = action.pop("ee.gripper_pos") - if None in (x, y, z, wx, wy, wz, gripper_pos): - raise ValueError( - "Missing required end-effector pose components: ee.x, ee.y, ee.z, ee.wx, ee.wy, ee.wz, ee.gripper_pos must all be present in action" - ) + ee_keys = [x, y, z, wx, wy, wz] + if self.gripper_names: + gripper_pos = action.pop("ee.gripper_pos") + ee_keys.append(gripper_pos) + if None in ee_keys: + raise ValueError("Missing required end-effector pose components in action") observation = new_transition.get(TransitionKey.OBSERVATION).copy() if observation is None: - raise ValueError("Joints observation is require for computing robot kinematics") + raise ValueError("Joints observation is required for computing robot kinematics") q_raw = np.array( - [float(v) for k, v in observation.items() if isinstance(k, str) and k.endswith(".pos")], + [ + float(v) + for k, v in observation.items() + if isinstance(k, str) and k.endswith(".pos") and k.removesuffix(".pos") in self.motor_names + ], dtype=float, ) - if q_raw is None: - raise ValueError("Joints observation is require for computing robot kinematics") - if self.initial_guess_current_joints: # Use current joints as initial guess + if self.initial_guess_current_joints: self.q_curr = q_raw - else: # Use previous ik solution as initial guess + else: if self.q_curr is None: self.q_curr = q_raw - # Build desired 4x4 transform from pos + rotvec (twist) t_des = np.eye(4, dtype=float) t_des[:3, :3] = Rotation.from_rotvec([wx, wy, wz]).as_matrix() t_des[:3, 3] = [x, y, z] - # Compute inverse kinematics q_target = self.kinematics.inverse_kinematics(self.q_curr, t_des) self.q_curr = q_target - # TODO: This is sentitive to order of motor_names = q_target mapping for i, name in enumerate(self.motor_names): - if name != "gripper": - action[f"{name}.pos"] = float(q_target[i]) - else: - action["gripper.pos"] = float(gripper_pos) + action[f"{name}.pos"] = float(q_target[i]) + + if self.gripper_names: + for gname in self.gripper_names: + action[f"{gname}.pos"] = float(gripper_pos) new_transition[TransitionKey.ACTION] = action complementary_data = new_transition.get(TransitionKey.COMPLEMENTARY_DATA, {}) @@ -596,16 +632,22 @@ class InverseKinematicsRLStep(ProcessorStep): def transform_features( self, features: dict[PipelineFeatureType, dict[str, PolicyFeature]] ) -> dict[PipelineFeatureType, dict[str, PolicyFeature]]: - for feat in ["x", "y", "z", "wx", "wy", "wz", "gripper_pos"]: + ee_feats = ["x", "y", "z", "wx", "wy", "wz"] + if self.gripper_names: + ee_feats.append("gripper_pos") + for feat in ee_feats: features[PipelineFeatureType.ACTION].pop(f"ee.{feat}", None) for name in self.motor_names: features[PipelineFeatureType.ACTION][f"{name}.pos"] = PolicyFeature( type=FeatureType.ACTION, shape=(1,) ) + for name in self.gripper_names: + features[PipelineFeatureType.ACTION][f"{name}.pos"] = PolicyFeature( + type=FeatureType.ACTION, shape=(1,) + ) return features def reset(self): - """Resets the initial guess for the IK solver.""" self.q_curr = None