From 154eed1bb8831809c635c932c24fdfa5c70fc957 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 9 Sep 2026 19:53:22 -0400 Subject: [PATCH 1/8] feat(so101_sim): add a mock-hardware SO-101 config with a fake joint source Phase one of the SO-101 demo: a MoveIt Pro config for a stock LeRobot SO-101 follower arm on `mock_components/GenericSystem`, with a digital twin that moves in the UI. No MuJoCo, no serial port, no powered arm required. The description is the upstream LeRobot SO-101 (vendored from danwahl/vla-test, provenance in description/assets/NOTICE.md) wrapped as a xacro with a `hardware_interface: mock | real` switch; the `real` branch is an empty stub until the Feetech bus lands. The MoveIt layer is ported from the SO-101 fork PR with its Onshape-derived joint names replaced by the LeRobot ones. `script/so101_arm_bridge.py` is the joint source. In `--fake` mode it publishes a slow sine into the trajectory controller's topic interface, which the mock hardware echoes back as joint states, so the twin moves with nothing plugged in. The real-bus path is stubbed behind the same interface, and the per-joint sign and offset parameters a physical arm needs are already declared. Three things the trajectory controller forced: - One controller owns all six joints, gripper included, and the planning groups are subsets of it, so `allow_partial_joints_goal` has to be true. There is no GripperActionController; a second claimant on the gripper's command interface would lock the bridge out of the jaw. - Mirroring is off until the Mirror SO101 Follower Objective ticks the bridge's `~/mirror` service, and stops when that Objective stops. A continuous topic stream restarts the controller's trajectory every tick, so a plan's goal is accepted and then hangs or aborts on path tolerance. The bridge also stays quiet while a trajectory goal is live. - The fake sine is centered on an extended, arm-out pose. A wider swing folds the wrist onto the shoulder, and MoveIt then refuses to plan from the twin's own state. The SRDF's collision matrix was derived by sampling 400 configurations against the collision meshes rather than guessed; only always-touching and never-touching pairs are disabled. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01DGyVgFaNyf4fC9uRiKsXbg --- src/so101_sim/.gitattributes | 1 + src/so101_sim/CMakeLists.txt | 31 + src/so101_sim/LICENSE | 25 + src/so101_sim/NOTICE.md | 21 + src/so101_sim/README.md | 99 +++ src/so101_sim/config/config.yaml | 134 ++++ .../config/control/so101.ros2_control.yaml | 68 ++ src/so101_sim/config/initial_positions.yaml | 9 + src/so101_sim/config/moveit/joint_jog.yaml | 6 + src/so101_sim/config/moveit/joint_limits.yaml | 51 ++ src/so101_sim/config/moveit/pose_ik.yaml | 7 + src/so101_sim/config/moveit/pose_jog.yaml | 7 + src/so101_sim/config/moveit/so101.srdf | 103 +++ src/so101_sim/description/assets/LICENSE | 200 ++++++ src/so101_sim/description/assets/NOTICE.md | 43 ++ src/so101_sim/description/assets/arm_base.stl | 3 + .../assets/base_motor_holder_so101_v1.stl | 3 + .../description/assets/base_so101_v2.stl | 3 + .../description/assets/cam_mount_bottom.stl | 3 + .../description/assets/cam_mount_middle.stl | 3 + .../description/assets/cam_mount_top.stl | 3 + .../description/assets/camera_wrist_mount.stl | 3 + .../assets/motor_holder_so101_base_v1.stl | 3 + .../assets/motor_holder_so101_wrist_v1.stl | 3 + .../assets/moving_jaw_so101_v1.stl | 3 + .../assets/rotation_pitch_so101_v1.stl | 3 + .../assets/sts3215_03a_no_horn_v1.stl | 3 + .../description/assets/sts3215_03a_v1.stl | 3 + .../description/assets/under_arm_so101_v1.stl | 3 + .../description/assets/upper_arm_so101_v1.stl | 3 + .../waveshare_mounting_plate_so101_v2.stl | 3 + .../assets/wrist_roll_follower_so101_v1.stl | 3 + .../assets/wrist_roll_pitch_so101_v2.stl | 3 + src/so101_sim/description/so101.urdf.xacro | 589 ++++++++++++++++++ src/so101_sim/launch/runtime.launch.xml | 19 + src/so101_sim/objectives/close_gripper.xml | 39 ++ src/so101_sim/objectives/mirror_follower.xml | 41 ++ .../objectives/move_so101_to_waypoint.xml | 40 ++ src/so101_sim/objectives/open_gripper.xml | 39 ++ src/so101_sim/package.xml | 42 ++ src/so101_sim/script/so101_arm_bridge.py | 268 ++++++++ src/so101_sim/test/test_so101_arm_bridge.py | 246 ++++++++ src/so101_sim/thumbnail.png | 3 + src/so101_sim/waypoints/so101_waypoints.yaml | 194 ++++++ 44 files changed, 2379 insertions(+) create mode 100644 src/so101_sim/.gitattributes create mode 100644 src/so101_sim/CMakeLists.txt create mode 100644 src/so101_sim/LICENSE create mode 100644 src/so101_sim/NOTICE.md create mode 100644 src/so101_sim/README.md create mode 100644 src/so101_sim/config/config.yaml create mode 100644 src/so101_sim/config/control/so101.ros2_control.yaml create mode 100644 src/so101_sim/config/initial_positions.yaml create mode 100644 src/so101_sim/config/moveit/joint_jog.yaml create mode 100644 src/so101_sim/config/moveit/joint_limits.yaml create mode 100644 src/so101_sim/config/moveit/pose_ik.yaml create mode 100644 src/so101_sim/config/moveit/pose_jog.yaml create mode 100644 src/so101_sim/config/moveit/so101.srdf create mode 100644 src/so101_sim/description/assets/LICENSE create mode 100644 src/so101_sim/description/assets/NOTICE.md create mode 100644 src/so101_sim/description/assets/arm_base.stl create mode 100644 src/so101_sim/description/assets/base_motor_holder_so101_v1.stl create mode 100644 src/so101_sim/description/assets/base_so101_v2.stl create mode 100644 src/so101_sim/description/assets/cam_mount_bottom.stl create mode 100644 src/so101_sim/description/assets/cam_mount_middle.stl create mode 100644 src/so101_sim/description/assets/cam_mount_top.stl create mode 100644 src/so101_sim/description/assets/camera_wrist_mount.stl create mode 100644 src/so101_sim/description/assets/motor_holder_so101_base_v1.stl create mode 100644 src/so101_sim/description/assets/motor_holder_so101_wrist_v1.stl create mode 100644 src/so101_sim/description/assets/moving_jaw_so101_v1.stl create mode 100644 src/so101_sim/description/assets/rotation_pitch_so101_v1.stl create mode 100644 src/so101_sim/description/assets/sts3215_03a_no_horn_v1.stl create mode 100644 src/so101_sim/description/assets/sts3215_03a_v1.stl create mode 100644 src/so101_sim/description/assets/under_arm_so101_v1.stl create mode 100644 src/so101_sim/description/assets/upper_arm_so101_v1.stl create mode 100644 src/so101_sim/description/assets/waveshare_mounting_plate_so101_v2.stl create mode 100644 src/so101_sim/description/assets/wrist_roll_follower_so101_v1.stl create mode 100644 src/so101_sim/description/assets/wrist_roll_pitch_so101_v2.stl create mode 100644 src/so101_sim/description/so101.urdf.xacro create mode 100644 src/so101_sim/launch/runtime.launch.xml create mode 100644 src/so101_sim/objectives/close_gripper.xml create mode 100644 src/so101_sim/objectives/mirror_follower.xml create mode 100644 src/so101_sim/objectives/move_so101_to_waypoint.xml create mode 100644 src/so101_sim/objectives/open_gripper.xml create mode 100644 src/so101_sim/package.xml create mode 100755 src/so101_sim/script/so101_arm_bridge.py create mode 100644 src/so101_sim/test/test_so101_arm_bridge.py create mode 100644 src/so101_sim/thumbnail.png create mode 100644 src/so101_sim/waypoints/so101_waypoints.yaml diff --git a/src/so101_sim/.gitattributes b/src/so101_sim/.gitattributes new file mode 100644 index 000000000..7db549b5c --- /dev/null +++ b/src/so101_sim/.gitattributes @@ -0,0 +1 @@ +*.{png,jpg,jpeg,obj,dae,stl,OBJ,DAE,STL} filter=lfs diff=lfs merge=lfs -text diff --git a/src/so101_sim/CMakeLists.txt b/src/so101_sim/CMakeLists.txt new file mode 100644 index 000000000..4cbc558ad --- /dev/null +++ b/src/so101_sim/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.22) +project(so101_sim) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY + config + description + launch + objectives + waypoints + DESTINATION + share/${PROJECT_NAME} +) + +install(PROGRAMS + script/so101_arm_bridge.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_pytest_test( + so101_arm_bridge_test test/test_so101_arm_bridge.py + TIMEOUT 60) +endif() + +ament_package() diff --git a/src/so101_sim/LICENSE b/src/so101_sim/LICENSE new file mode 100644 index 000000000..574ef0790 --- /dev/null +++ b/src/so101_sim/LICENSE @@ -0,0 +1,25 @@ +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/src/so101_sim/NOTICE.md b/src/so101_sim/NOTICE.md new file mode 100644 index 000000000..a59599c5a --- /dev/null +++ b/src/so101_sim/NOTICE.md @@ -0,0 +1,21 @@ +# NOTICE + +## Robot description meshes + +The SO-101 arm and camera-mount STL files under `description/assets/` are +vendored from a third party under the Apache License 2.0. Their provenance, +upstream commit and the list of files are recorded in +[`description/assets/NOTICE.md`](description/assets/NOTICE.md), and the license +text is `description/assets/LICENSE`. + +## MoveIt-layer configuration + +The SRDF, joint limits, IK, jog and `ros2_control` YAML in `config/` are derived +from the SO-101 configuration in the `moveit_pro_example_ws` fork PR at +[`noah-wardlow/moveit_pro_example_ws`](https://github.com/noah-wardlow/moveit_pro_example_ws) +(branch `feat/so101-vla-workflows`), which declares Apache License 2.0. Joint +and link names were changed from that PR's Onshape-derived names for a +parallel-gripper build to the stock LeRobot names used by this arm, and the +controllers it configured for VLA policy execution were removed. + +Everything else in this package is licensed under `LICENSE` (BSD-3-Clause). diff --git a/src/so101_sim/README.md b/src/so101_sim/README.md new file mode 100644 index 000000000..3efb98647 --- /dev/null +++ b/src/so101_sim/README.md @@ -0,0 +1,99 @@ +# so101_sim + +A MoveIt Pro configuration for a stock LeRobot SO-101 follower arm on **mock +hardware**. It brings up a digital twin of the arm in the MoveIt Pro UI, driven +by a joint source outside the Runtime. There is no MuJoCo model and no physics. + +Run it: + +```bash +moveit_pro run --config so101_sim +``` + +Then run the **Mirror SO101 Follower** Objective: the twin starts moving. +`script/so101_arm_bridge.py` runs in `--fake` mode and publishes a slow sine on +the `joint_trajectory_controller` topic, which `mock_components/GenericSystem` +echoes back as `/joint_states`. Planning, teleoperation and the waypoint +Objectives work against that same twin. + +Mirroring is off until that Objective asks for it, and stops when the Objective +is stopped. The trajectory controller has one owner at a time: a stream of topic +messages restarts its trajectory on every tick, so a plan's goal would be +accepted and then hang forever, or abort on a path tolerance the moving twin +violated. The Objective keeps mirroring alive by ticking the bridge's +`~/mirror` `Trigger` service in a loop; one second without a tick and the bridge +goes quiet. As a second guard, the bridge also stays quiet while a +`follow_joint_trajectory` goal is live, so an execution that starts anyway still +wins. + +## Execution to hardware is out of scope + +**This config cannot move a physical SO-101.** MoveIt Pro here owns nothing but +mock hardware; planning and executing drives the mock, and no serial port is +opened anywhere in the Runtime. The `real` branch of +`description/so101.urdf.xacro` is an empty stub until the Feetech STS3215 bus +interface lands. + +When a live joint source is added (phase two), it publishes into the same +`joint_trajectory_controller`, so the same one-owner rule applies: stop the +**Mirror SO101 Follower** Objective before pressing Plan. + +## What is here + +| Path | What it is | +|---|---| +| `description/so101.urdf.xacro` | The arm, with a `hardware_interface: mock \| real` switch. Meshes are the upstream LeRobot description; see `description/assets/NOTICE.md`. | +| `config/control/so101.ros2_control.yaml` | `joint_state_broadcaster` plus **one** `joint_trajectory_controller` over all six joints, gripper included. | +| `config/moveit/` | SRDF, joint limits, IK (`PoseIKPlugin`, `optimize_distance` — the SO-101 is 5-DOF and cannot hit arbitrary 6-DOF poses), and the jog configs. | +| `script/so101_arm_bridge.py` | The joint source. `--fake` publishes a sine; `--real` is a phase-two stub. | +| `objectives/` | `Mirror SO101 Follower`, `Move SO101 to Waypoint`, `Close Gripper`, `Open Gripper`. | + +There is no `GripperActionController`. It would claim the gripper joint's +position command interface, and `ros2_control` would then refuse the trajectory +controller's claim on the same interface, locking the joint source out of the +jaw. `Close Gripper` and `Open Gripper` move the gripper joint group through the +trajectory controller instead. Both Objectives must exist under exactly those +names or the teleoperation gripper controls silently do nothing. + +Jogging is configured (`config/moveit/{pose,joint}_jog.yaml`) but inert: it needs +a `velocity_force_controller` and a `joint_velocity_controller`, and neither is +loaded in this phase. + +## Safe bring-up order + +Lifted from the SO-101 fork PR's own bring-up notes, and still worth following +the day a real arm is attached. MoveIt Pro's Stop control is a cooperative +software stop, not a safety-rated emergency stop. Keep physical power isolation +accessible and clear the robot's workspace before any live test. + +1. Bring up the config with the arm unpowered and confirm the twin appears and + moves under the fake source. +2. Power the arm and confirm joint states without commanding motion. +3. Confirm the camera panes, when cameras are added, without commanding motion. +4. Only after explicit authorization, test bounded gripper, waypoint and jog + motions in that order. + +If a waypoint produces clicking, stop the attempt and inspect the physical joint +and its tracking error. Do not raise path tolerances to make a stalled joint look +like a success. + +## Waypoints are not taught poses + +`waypoints/so101_waypoints.yaml` holds poses picked to be reachable and visible +in simulation. None of them has been validated against a physical arm; re-teach +them on the bench before trusting any of them. + +## Later phases + +- **Phase two** — the real Feetech bus: leader and follower over USB through + LeRobot's `SO101Follower`, behind the same bridge node. Needs udev rules for + stable `/dev/so101_{leader,follower}` names and a `pip install` line in the + workspace `Dockerfile`. The per-joint `joint_signs` / `joint_offsets_deg` + parameters the bridge already declares are the calibration knobs for it. +- **Phase three** — the wrist and top USB cameras. `usb_cam` is already an + `exec_depend` and is installed in the image, but nothing launches it yet. +- **Phase four** — Trainer recording of demonstrations. Note that a named + training config is not a workspace file: the Trainer stores them as JSON under + its own data directory, so `RecordEpisode(config_name="so101_sim")` fails with + `Training config 'so101_sim' not found` until one is created for this + deployment through the Trainer UI or its REST API. diff --git a/src/so101_sim/config/config.yaml b/src/so101_sim/config/config.yaml new file mode 100644 index 000000000..3d8cfb78f --- /dev/null +++ b/src/so101_sim/config/config.yaml @@ -0,0 +1,134 @@ +############################################################### +# +# This configures the robot to work with MoveIt Pro +# +############################################################### + +runtime_launch_file: + package: "so101_sim" + path: "launch/runtime.launch.xml" + +# Baseline hardware configuration parameters for MoveIt Pro. +# [Required] +hardware: + # If the MoveIt Pro Agent should launch the ros2 controller node. + # [Optional, default=True] + launch_control_node: True + + # If the MoveIt Pro Agent should launch the robot state publisher. + # This should be false if you are launching the robot state publisher as part of drivers. + # [Optional, default=True] + launch_robot_state_publisher: True + + # Parameters used to configure the robot description through XACRO. + # A URDF and SRDF are both required. + # [Required] + robot_description: + urdf: + package: "so101_sim" + path: "description/so101.urdf.xacro" + srdf: + package: "so101_sim" + path: "config/moveit/so101.srdf" + # Specify any additional parameters required for the URDF. + # [Optional] + urdf_params: + # Use "mock" or "real". "real" is a stub until the Feetech bus driver + # lands; there is no MuJoCo model for this config. + - hardware_interface: "mock" + +# Sets ROS global params for launch. +# [Optional] +ros_global_params: + # Whether or not to use simulated time. + # [Optional, default=False] + use_sim_time: False + +# Configuration files for MoveIt. +# For more information, refer to https://moveit.picknik.ai/main/doc/how_to_guides/moveit_configuration/moveit_configuration_tutorial.html +# [Required] +moveit_params: + # Used by the Waypoint Manager to save joint states from this joint group. + joint_group_name: "manipulator" + + kinematics: + package: "so101_sim" + path: "config/moveit/pose_ik.yaml" + joint_limits: + package: "so101_sim" + path: "config/moveit/joint_limits.yaml" + pose_jog: + package: "so101_sim" + path: "config/moveit/pose_jog.yaml" + joint_jog: + package: "so101_sim" + path: "config/moveit/joint_jog.yaml" + publish: + planning_scene: True + geometry_updates: True + state_updates: True + transforms_updates: True + + trajectory_execution: + manage_controllers: True + allowed_execution_duration_scaling: 2.0 + allowed_goal_duration_margin: 5.0 + allowed_start_tolerance: 0.01 + +# Configuration for launching ros2_control processes. +# [Required, if using ros2_control] +ros2_control: + config: + package: "so101_sim" + path: "config/control/so101.ros2_control.yaml" + # MoveIt Pro will load and activate these controllers at start up to ensure they are available. + # [Optional, default=[]] + controllers_active_at_startup: + - "joint_state_broadcaster" + - "joint_trajectory_controller" + # Load but do not start these controllers so they can be activated later if needed. + # [Optional, default=[]] + controllers_inactive_at_startup: [] + # Any controllers here will not be spawned by MoveIt Pro. + # [Optional, default=[]] + controllers_not_managed: [] + # Optionally configure remapping rules to let multiple controllers receive commands on the same topic. + # [Optional, default=[]] + controller_shared_topics: [] + +# Configuration for loading behaviors and objectives. +# [Required] +objectives: + # List of plugins for loading custom behaviors. + # [Required] + behavior_loader_plugins: + # This plugin will load the core MoveIt Pro Behaviors. + # Add additional plugin loaders as needed. + core: + - "moveit_pro::behaviors::CoreBehaviorsLoader" + - "moveit_pro::behaviors::MTCCoreBehaviorsLoader" + - "moveit_pro::behaviors::VisionBehaviorsLoader" + - "moveit_pro::behaviors::ConverterBehaviorsLoader" + # Specify source folder for objectives + # [Required] + objective_library_paths: + core_objectives: + package_name: "moveit_pro_objectives" + relative_path: "objectives/core" + motion_objectives: + package_name: "moveit_pro_objectives" + relative_path: "objectives/motion" + perception_objectives: + package_name: "moveit_pro_objectives" + relative_path: "objectives/perception" + visualization_objectives: + package_name: "moveit_pro_objectives" + relative_path: "objectives/visualization" + sim_objectives: + package_name: "so101_sim" + relative_path: "objectives" + # Specify the location of the saved waypoints file. + # [Required] + waypoints_file: + package_name: "so101_sim" + relative_path: "waypoints/so101_waypoints.yaml" diff --git a/src/so101_sim/config/control/so101.ros2_control.yaml b/src/so101_sim/config/control/so101.ros2_control.yaml new file mode 100644 index 000000000..50ab72268 --- /dev/null +++ b/src/so101_sim/config/control/so101.ros2_control.yaml @@ -0,0 +1,68 @@ +controller_manager: + ros__parameters: + update_rate: 50 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_state_broadcaster: + ros__parameters: + use_local_topics: false + joints: + - shoulder_pan + - shoulder_lift + - elbow_flex + - wrist_flex + - wrist_roll + - gripper + interfaces: + - position + - velocity + - effort + +# One trajectory controller owns all six joints, gripper included. A separate +# GripperActionController would claim the gripper's position command interface +# and ros2_control would then refuse this controller's claim on it, so the +# mirroring bridge could no longer drive the jaw. +joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan + - shoulder_lift + - elbow_flex + - wrist_flex + - wrist_roll + - gripper + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + # The `manipulator` planning group is the five arm joints and `gripper` is + # the sixth on its own, so every goal this config sends names a subset of + # the controller's joints. Rejecting partial goals rejects all of them. + allow_partial_joints_goal: true + constraints: + stopped_velocity_tolerance: 0.02 + goal_time: 3.0 + shoulder_pan: + trajectory: 0.15 + goal: 0.06 + shoulder_lift: + trajectory: 0.15 + goal: 0.06 + elbow_flex: + trajectory: 0.15 + goal: 0.08 + wrist_flex: + trajectory: 0.15 + goal: 0.06 + wrist_roll: + trajectory: 0.15 + goal: 0.06 + gripper: + trajectory: 0.20 + goal: 0.08 diff --git a/src/so101_sim/config/initial_positions.yaml b/src/so101_sim/config/initial_positions.yaml new file mode 100644 index 000000000..4dadb0799 --- /dev/null +++ b/src/so101_sim/config/initial_positions.yaml @@ -0,0 +1,9 @@ +# Joint positions the mock hardware starts at, in radians. These match the +# center of the bridge's fake sine, so the twin does not jump when it starts. +initial_positions: + shoulder_pan: 0.0 + shoulder_lift: 0.6 + elbow_flex: -0.6 + wrist_flex: 0.0 + wrist_roll: 0.0 + gripper: 0.7 diff --git a/src/so101_sim/config/moveit/joint_jog.yaml b/src/so101_sim/config/moveit/joint_jog.yaml new file mode 100644 index 000000000..7fcecf831 --- /dev/null +++ b/src/so101_sim/config/moveit/joint_jog.yaml @@ -0,0 +1,6 @@ +# Planning groups to use in JointJog, and their corresponding JVC controllers. +# The number of elements in `planning_groups` and `controllers` must match. +# NOTE: as with pose_jog.yaml, no joint_velocity_controller is loaded in this +# phase, so joint jogging is inert until phase two adds one. +planning_groups: ['manipulator'] +controllers: ['joint_velocity_controller'] diff --git a/src/so101_sim/config/moveit/joint_limits.yaml b/src/so101_sim/config/moveit/joint_limits.yaml new file mode 100644 index 000000000..03467276f --- /dev/null +++ b/src/so101_sim/config/moveit/joint_limits.yaml @@ -0,0 +1,51 @@ +# Positions are the upstream LeRobot calibration limits (radians); velocity and +# acceleration are the conservative values ported from the SO-101 fork PR. +joint_limits: + shoulder_pan: + has_position_limits: true + min_position: -1.91986 + max_position: 1.91986 + has_velocity_limits: true + max_velocity: 1.20 + has_acceleration_limits: true + max_acceleration: 2.50 + shoulder_lift: + has_position_limits: true + min_position: -1.74533 + max_position: 1.74533 + has_velocity_limits: true + max_velocity: 1.20 + has_acceleration_limits: true + max_acceleration: 2.50 + elbow_flex: + has_position_limits: true + min_position: -1.69 + max_position: 1.69 + has_velocity_limits: true + max_velocity: 1.20 + has_acceleration_limits: true + max_acceleration: 2.50 + wrist_flex: + has_position_limits: true + min_position: -1.65806 + max_position: 1.65806 + has_velocity_limits: true + max_velocity: 1.20 + has_acceleration_limits: true + max_acceleration: 2.50 + wrist_roll: + has_position_limits: true + min_position: -2.74385 + max_position: 2.84121 + has_velocity_limits: true + max_velocity: 1.20 + has_acceleration_limits: true + max_acceleration: 2.50 + gripper: + has_position_limits: true + min_position: -0.174533 + max_position: 1.74533 + has_velocity_limits: true + max_velocity: 1.20 + has_acceleration_limits: true + max_acceleration: 2.50 diff --git a/src/so101_sim/config/moveit/pose_ik.yaml b/src/so101_sim/config/moveit/pose_ik.yaml new file mode 100644 index 000000000..1b728b782 --- /dev/null +++ b/src/so101_sim/config/moveit/pose_ik.yaml @@ -0,0 +1,7 @@ +# The SO-101 is a 5-DOF arm and cannot reach arbitrary 6-DOF poses, so the IK +# solver optimizes for the closest reachable pose instead of failing. +manipulator: + kinematics_solver: pose_ik_plugin/PoseIKPlugin + target_tolerance: 0.002 + solve_mode: "optimize_distance" + optimization_timeout: 0.01 diff --git a/src/so101_sim/config/moveit/pose_jog.yaml b/src/so101_sim/config/moveit/pose_jog.yaml new file mode 100644 index 000000000..8a0624c0a --- /dev/null +++ b/src/so101_sim/config/moveit/pose_jog.yaml @@ -0,0 +1,7 @@ +# Planning groups to use in PoseJog, and their corresponding VFC controllers. +# The number of elements in `planning_groups` and `controllers` must match. +# NOTE: this phase ships joint_state_broadcaster + one joint_trajectory_controller +# only, so no velocity_force_controller is loaded and pose jogging is inert until +# phase two adds one. +planning_groups: ['manipulator'] +controllers: ['velocity_force_controller'] diff --git a/src/so101_sim/config/moveit/so101.srdf b/src/so101_sim/config/moveit/so101.srdf new file mode 100644 index 000000000..4adbb23b2 --- /dev/null +++ b/src/so101_sim/config/moveit/so101.srdf @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/so101_sim/description/assets/LICENSE b/src/so101_sim/description/assets/LICENSE new file mode 100644 index 000000000..c39a97ad9 --- /dev/null +++ b/src/so101_sim/description/assets/LICENSE @@ -0,0 +1,200 @@ + Apache License + Version 2.0, January 2004 + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2026 Dan Wahl + + 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. diff --git a/src/so101_sim/description/assets/NOTICE.md b/src/so101_sim/description/assets/NOTICE.md new file mode 100644 index 000000000..a98dcb0c4 --- /dev/null +++ b/src/so101_sim/description/assets/NOTICE.md @@ -0,0 +1,43 @@ +# SO-101 arm and camera-mount meshes + +Source: [`danwahl/vla-test`](https://github.com/danwahl/vla-test/tree/bbbe60b0838c8f796942e857e437b81c2fba7bb2/sim/src/sim/description/assets) + +Upstream commit: `bbbe60b0838c8f796942e857e437b81c2fba7bb2` + +License: Apache License 2.0; see `LICENSE` in this directory, which is the +repository-level license of the source repository. + +`vla-test` in turn vendors these from the upstream SO-ARM100 / LeRobot +description, which the mesh filenames preserve (`base_so101_v2.stl`, +`sts3215_03a_v1.stl`, and so on). + +All 18 STL files in this directory are byte-for-byte unmodified upstream +exports, verified by SHA-256 against the source tree at the commit above: + +- `arm_base.stl` +- `base_motor_holder_so101_v1.stl` +- `base_so101_v2.stl` +- `cam_mount_bottom.stl` +- `cam_mount_middle.stl` +- `cam_mount_top.stl` +- `camera_wrist_mount.stl` +- `motor_holder_so101_base_v1.stl` +- `motor_holder_so101_wrist_v1.stl` +- `moving_jaw_so101_v1.stl` +- `rotation_pitch_so101_v1.stl` +- `sts3215_03a_no_horn_v1.stl` +- `sts3215_03a_v1.stl` +- `under_arm_so101_v1.stl` +- `upper_arm_so101_v1.stl` +- `waveshare_mounting_plate_so101_v2.stl` +- `wrist_roll_follower_so101_v1.stl` +- `wrist_roll_pitch_so101_v2.stl` + +## What was not taken + +Upstream also ships an `assets/coacd/` directory: 72 convex parts produced by a +CoACD decomposition of `wrist_roll_follower_so101_v1.stl` and +`moving_jaw_so101_v1.stl`, for MuJoCo's contact solver. This config has no +MuJoCo model, and MoveIt's collision checker takes the visual STLs directly, so +those parts are omitted and `description/so101.urdf.xacro` uses the visual mesh +for collision on both links. diff --git a/src/so101_sim/description/assets/arm_base.stl b/src/so101_sim/description/assets/arm_base.stl new file mode 100644 index 000000000..f2854b1d9 --- /dev/null +++ b/src/so101_sim/description/assets/arm_base.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:19b2318dc60565950adea9b3bc355c7a3c621dfe02de5b83bcc671e8a96a91e9 +size 86884 diff --git a/src/so101_sim/description/assets/base_motor_holder_so101_v1.stl b/src/so101_sim/description/assets/base_motor_holder_so101_v1.stl new file mode 100644 index 000000000..ac9c38076 --- /dev/null +++ b/src/so101_sim/description/assets/base_motor_holder_so101_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8cd2f241037ea377af1191fffe0dd9d9006beea6dcc48543660ed41647072424 +size 1877084 diff --git a/src/so101_sim/description/assets/base_so101_v2.stl b/src/so101_sim/description/assets/base_so101_v2.stl new file mode 100644 index 000000000..503d30be0 --- /dev/null +++ b/src/so101_sim/description/assets/base_so101_v2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bb12b7026575e1f70ccc7240051f9d943553bf34e5128537de6cd86fae33924d +size 471584 diff --git a/src/so101_sim/description/assets/cam_mount_bottom.stl b/src/so101_sim/description/assets/cam_mount_bottom.stl new file mode 100644 index 000000000..2ee4bab47 --- /dev/null +++ b/src/so101_sim/description/assets/cam_mount_bottom.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:47e47548ccd07c6151abcecca9b2c54a3f49bd09b1eb12f6a861800f2aa577d8 +size 48684 diff --git a/src/so101_sim/description/assets/cam_mount_middle.stl b/src/so101_sim/description/assets/cam_mount_middle.stl new file mode 100644 index 000000000..278d2310c --- /dev/null +++ b/src/so101_sim/description/assets/cam_mount_middle.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1a03cb0b8a62879c83f3269e50c27f63d07387a076f9247aa3d4d254d0ace4df +size 56984 diff --git a/src/so101_sim/description/assets/cam_mount_top.stl b/src/so101_sim/description/assets/cam_mount_top.stl new file mode 100644 index 000000000..a5c468917 --- /dev/null +++ b/src/so101_sim/description/assets/cam_mount_top.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:73f82aa584a50a34d05ddd316c9db9a4b9a783ba2ffa71a83003da10a85d3ec1 +size 69684 diff --git a/src/so101_sim/description/assets/camera_wrist_mount.stl b/src/so101_sim/description/assets/camera_wrist_mount.stl new file mode 100644 index 000000000..890b2f8cb --- /dev/null +++ b/src/so101_sim/description/assets/camera_wrist_mount.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ce2dddbe1d7d27d64dd41a2e243944b6d978565edf0b3ef4282e9fbc05f4e067 +size 173884 diff --git a/src/so101_sim/description/assets/motor_holder_so101_base_v1.stl b/src/so101_sim/description/assets/motor_holder_so101_base_v1.stl new file mode 100644 index 000000000..f8e3d75c0 --- /dev/null +++ b/src/so101_sim/description/assets/motor_holder_so101_base_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:31242ae6fb59d8b15c66617b88ad8e9bded62d57c35d11c0c43a70d2f4caa95b +size 1129384 diff --git a/src/so101_sim/description/assets/motor_holder_so101_wrist_v1.stl b/src/so101_sim/description/assets/motor_holder_so101_wrist_v1.stl new file mode 100644 index 000000000..e55b71946 --- /dev/null +++ b/src/so101_sim/description/assets/motor_holder_so101_wrist_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:887f92e6013cb64ea3a1ab8675e92da1e0beacfd5e001f972523540545e08011 +size 1052184 diff --git a/src/so101_sim/description/assets/moving_jaw_so101_v1.stl b/src/so101_sim/description/assets/moving_jaw_so101_v1.stl new file mode 100644 index 000000000..eb17d253d --- /dev/null +++ b/src/so101_sim/description/assets/moving_jaw_so101_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:785a9dded2f474bc1d869e0d3dae398a3dcd9c0c345640040472210d2861fa9d +size 1413584 diff --git a/src/so101_sim/description/assets/rotation_pitch_so101_v1.stl b/src/so101_sim/description/assets/rotation_pitch_so101_v1.stl new file mode 100644 index 000000000..b536cb410 --- /dev/null +++ b/src/so101_sim/description/assets/rotation_pitch_so101_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9be900cc2a2bf718102841ef82ef8d2873842427648092c8ed2ca1e2ef4ffa34 +size 883684 diff --git a/src/so101_sim/description/assets/sts3215_03a_no_horn_v1.stl b/src/so101_sim/description/assets/sts3215_03a_no_horn_v1.stl new file mode 100644 index 000000000..18e933567 --- /dev/null +++ b/src/so101_sim/description/assets/sts3215_03a_no_horn_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:75ef3781b752e4065891aea855e34dc161a38a549549cd0970cedd07eae6f887 +size 865884 diff --git a/src/so101_sim/description/assets/sts3215_03a_v1.stl b/src/so101_sim/description/assets/sts3215_03a_v1.stl new file mode 100644 index 000000000..a14c57b90 --- /dev/null +++ b/src/so101_sim/description/assets/sts3215_03a_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a37c871fb502483ab96c256baf457d36f2e97afc9205313d9c5ab275ef941cd0 +size 954084 diff --git a/src/so101_sim/description/assets/under_arm_so101_v1.stl b/src/so101_sim/description/assets/under_arm_so101_v1.stl new file mode 100644 index 000000000..47b611ef9 --- /dev/null +++ b/src/so101_sim/description/assets/under_arm_so101_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d01d1f2de365651dcad9d6669e94ff87ff7652b5bb2d10752a66a456a86dbc71 +size 1975884 diff --git a/src/so101_sim/description/assets/upper_arm_so101_v1.stl b/src/so101_sim/description/assets/upper_arm_so101_v1.stl new file mode 100644 index 000000000..8832740f9 --- /dev/null +++ b/src/so101_sim/description/assets/upper_arm_so101_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:475056e03a17e71919b82fd88ab9a0b898ab50164f2a7943652a6b2941bb2d4f +size 1303484 diff --git a/src/so101_sim/description/assets/waveshare_mounting_plate_so101_v2.stl b/src/so101_sim/description/assets/waveshare_mounting_plate_so101_v2.stl new file mode 100644 index 000000000..e0d90d5b6 --- /dev/null +++ b/src/so101_sim/description/assets/waveshare_mounting_plate_so101_v2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e197e24005a07d01bbc06a8c42311664eaeda415bf859f68fa247884d0f1a6e9 +size 62784 diff --git a/src/so101_sim/description/assets/wrist_roll_follower_so101_v1.stl b/src/so101_sim/description/assets/wrist_roll_follower_so101_v1.stl new file mode 100644 index 000000000..9a5fa8fe2 --- /dev/null +++ b/src/so101_sim/description/assets/wrist_roll_follower_so101_v1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b17b410a12d64ec39554abc3e8054d8a97384b2dc4a8d95a5ecb2a93670f5f4 +size 1439884 diff --git a/src/so101_sim/description/assets/wrist_roll_pitch_so101_v2.stl b/src/so101_sim/description/assets/wrist_roll_pitch_so101_v2.stl new file mode 100644 index 000000000..2f531712f --- /dev/null +++ b/src/so101_sim/description/assets/wrist_roll_pitch_so101_v2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6c7ec5525b4d8b9e397a30ab4bb0037156a5d5f38a4adf2c7d943d6c56eda5ae +size 2699784 diff --git a/src/so101_sim/description/so101.urdf.xacro b/src/so101_sim/description/so101.urdf.xacro new file mode 100644 index 000000000..415b35303 --- /dev/null +++ b/src/so101_sim/description/so101.urdf.xacro @@ -0,0 +1,589 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${lower} + ${upper} + + + ${initial_positions[name]} + + + + + + + + + + + mock_components/GenericSystem + + + + + + + + + + + + + + diff --git a/src/so101_sim/launch/runtime.launch.xml b/src/so101_sim/launch/runtime.launch.xml new file mode 100644 index 000000000..c11cfe801 --- /dev/null +++ b/src/so101_sim/launch/runtime.launch.xml @@ -0,0 +1,19 @@ + + + + + + diff --git a/src/so101_sim/objectives/close_gripper.xml b/src/so101_sim/objectives/close_gripper.xml new file mode 100644 index 000000000..37fa29919 --- /dev/null +++ b/src/so101_sim/objectives/close_gripper.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + diff --git a/src/so101_sim/objectives/mirror_follower.xml b/src/so101_sim/objectives/mirror_follower.xml new file mode 100644 index 000000000..1e5ae554c --- /dev/null +++ b/src/so101_sim/objectives/mirror_follower.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/so101_sim/objectives/move_so101_to_waypoint.xml b/src/so101_sim/objectives/move_so101_to_waypoint.xml new file mode 100644 index 000000000..8e85240a0 --- /dev/null +++ b/src/so101_sim/objectives/move_so101_to_waypoint.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + Named SO-101 waypoint to plan and execute. + + + Fraction of the configured joint velocity limit. + + + Fraction of the configured joint acceleration limit. + + + + diff --git a/src/so101_sim/objectives/open_gripper.xml b/src/so101_sim/objectives/open_gripper.xml new file mode 100644 index 000000000..38652afb6 --- /dev/null +++ b/src/so101_sim/objectives/open_gripper.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + diff --git a/src/so101_sim/package.xml b/src/so101_sim/package.xml new file mode 100644 index 000000000..59cd00dbf --- /dev/null +++ b/src/so101_sim/package.xml @@ -0,0 +1,42 @@ + + + so101_sim + 9.5.0 + + + A MoveIt Pro configuration for a stock LeRobot SO-101 follower arm, running + on mock hardware. + + + MoveIt Pro Maintainer + + BSD-3-Clause + Apache-2.0 + + ament_cmake + + action_msgs + joint_state_broadcaster + joint_trajectory_controller + moveit_pro_behavior + moveit_studio_agent + rclpy + std_srvs + trajectory_msgs + xacro + + usb_cam + + ament_cmake_copyright + ament_cmake_lint_cmake + ament_cmake_pytest + ament_flake8 + ament_lint_auto + picknik_ament_copyright + rclpy + + + ament_cmake + + diff --git a/src/so101_sim/script/so101_arm_bridge.py b/src/so101_sim/script/so101_arm_bridge.py new file mode 100755 index 000000000..495c6a489 --- /dev/null +++ b/src/so101_sim/script/so101_arm_bridge.py @@ -0,0 +1,268 @@ +#!/usr/bin/env python3 + +# Copyright 2026 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the PickNik Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Publish SO-101 follower joint positions into the mock joint trajectory controller. + +The MoveIt Pro side of this demo is pure `mock_components/GenericSystem`: it +never opens a serial port. This node is the only thing that knows about the arm, +and it feeds the twin by publishing one-point `JointTrajectory` messages, which +the mock hardware echoes straight back out as `/joint_states`. + +Two sources are foreseen: + +* ``--fake`` (this phase) - a slow sine, so the twin moves with no arm plugged in. +* ``--real`` (phase two) - the Feetech STS3215 bus over USB, read through + LeRobot's ``SO101Follower``. Stubbed out here behind the same interface. + +Mirroring is off until something asks for it. The `Mirror SO101 Follower` +Objective ticks this node's ``~/mirror`` Trigger service in a loop; mirroring +runs while those ticks keep arriving and stops on its own when the Objective is +stopped. That is what keeps mirroring and planning from fighting over the +trajectory controller, which has one owner at a time. +""" + +import argparse +import math +import sys + +import rclpy +from action_msgs.msg import GoalStatus, GoalStatusArray +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from builtin_interfaces.msg import Duration +from std_srvs.srv import Trigger +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +# LeRobot's joint order for the SO-101, and the order every downstream consumer +# (the controller, the dataset, the policy) expects. +JOINT_NAMES = [ + "shoulder_pan", + "shoulder_lift", + "elbow_flex", + "wrist_flex", + "wrist_roll", + "gripper", +] + +# Radians. Comfortably inside the URDF limits so the sine never trips a +# joint-limit rejection, and centered on an extended, arm-out pose: a wider +# swing on shoulder_lift/elbow_flex folds the wrist back onto the shoulder, +# which is a self-collision, and MoveIt then refuses to plan from the twin's +# current state for as long as the sine sits in that region. +FAKE_AMPLITUDE = [0.8, 0.2, 0.25, 0.35, 1.0, 0.6] +FAKE_CENTER = [0.0, 0.6, -0.6, 0.0, 0.0, 0.7] + + +def to_radians(degrees, signs, offsets): + """Convert a bus reading in degrees to URDF radians. + + The Feetech bus reports degrees and does not agree with the URDF on which + way is positive or where zero is, so each joint carries a sign and an offset + that a real arm has to be calibrated for. Offsets are in degrees, applied + before the sign, so they can be read straight off a calibration sheet. + """ + if not (len(degrees) == len(signs) == len(offsets)): + raise ValueError( + f"expected matching lengths, got {len(degrees)}, {len(signs)}, {len(offsets)}" + ) + return [math.radians((d - o) * s) for d, o, s in zip(degrees, offsets, signs)] + + +def order_like(names, values, wanted=None): + """Reorder ``values`` from ``names`` into ``wanted`` (default JOINT_NAMES). + + The bus and the controller both speak in joint names, but not necessarily in + the same order, and a silently transposed pair is the kind of bug that only + shows up as a robot bending the wrong way. + """ + wanted = JOINT_NAMES if wanted is None else wanted + if len(names) != len(values): + raise ValueError(f"{len(names)} names but {len(values)} values") + lookup = dict(zip(names, values)) + missing = [n for n in wanted if n not in lookup] + if missing: + raise KeyError(f"missing joints: {missing}") + return [lookup[n] for n in wanted] + + +def fake_positions(elapsed_s, period_s): + """A slow sine, phase-shifted per joint so the whole arm visibly moves.""" + return [ + center + amplitude * math.sin(2.0 * math.pi * elapsed_s / period_s + i * 0.7) + for i, (center, amplitude) in enumerate(zip(FAKE_CENTER, FAKE_AMPLITUDE)) + ] + + +class So101ArmBridge(Node): + """Publish follower joint positions as single-point trajectories.""" + + def __init__(self, source="fake", parameter_overrides=None): + super().__init__("so101_arm_bridge", parameter_overrides=parameter_overrides) + + self.source = source + publish_rate_hz = self.declare_parameter("publish_rate_hz", 50.0).value + # One controller period of lead time. Too small and the controller + # discards the point as already in the past; too large and the twin + # visibly lags the arm. + self.point_dt_s = self.declare_parameter("point_dt_s", 0.04).value + self.sine_period_s = self.declare_parameter("sine_period_s", 12.0).value + # How long a single ~/mirror tick keeps mirroring alive. Long enough to + # ride out a slow Behavior Tree tick, short enough that stopping the + # Objective visibly stops the twin. + self.mirror_timeout_s = self.declare_parameter("mirror_timeout_s", 1.0).value + topic = self.declare_parameter( + "joint_trajectory_topic", "/joint_trajectory_controller/joint_trajectory" + ).value + status_topic = self.declare_parameter( + "trajectory_status_topic", + "/joint_trajectory_controller/follow_joint_trajectory/_action/status", + ).value + # Calibration knobs for the real bus. Fake mode ignores them, but they + # are declared now because a physical arm always needs them and finding + # that out at bring-up time is a bad afternoon. + self.port = self.declare_parameter("follower_port", "/dev/so101_follower").value + self.joint_signs = list( + self.declare_parameter("joint_signs", [1.0] * len(JOINT_NAMES)).value + ) + self.joint_offsets_deg = list( + self.declare_parameter("joint_offsets_deg", [0.0] * len(JOINT_NAMES)).value + ) + + self.publisher = self.create_publisher(JointTrajectory, topic, 10) + # The trajectory controller has one owner at a time. A stream of topic + # messages restarts its trajectory on every tick, so an action goal + # from a plan would be accepted and then never converge. Yield the + # controller while a goal is live and pick mirroring back up after. + self.goal_active = False + self.create_subscription( + GoalStatusArray, status_topic, self.on_trajectory_status, 10 + ) + self.last_mirror_tick = None + self.create_service(Trigger, "~/mirror", self.on_mirror_tick) + self.start_time = self.get_clock().now() + self.timer = self.create_timer(1.0 / publish_rate_hz, self.publish_once) + self.get_logger().info( + f"so101_arm_bridge ready to publish {self.source} joint states to " + f"{topic} at {publish_rate_hz} Hz; run the Mirror SO101 Follower " + "Objective to start mirroring" + ) + + def read_positions(self): + """Return the six joint positions in JOINT_NAMES order, in radians.""" + if self.source == "fake": + elapsed = (self.get_clock().now() - self.start_time).nanoseconds * 1e-9 + return fake_positions(elapsed, self.sine_period_s) + # Phase two: open the Feetech bus through LeRobot's SO101Follower on + # self.port, read get_observation(), then + # order_like(names, degrees) -> to_radians(..., self.joint_signs, + # self.joint_offsets_deg) + raise NotImplementedError( + "the real Feetech bus source lands in phase two; run with --fake" + ) + + def build_message(self, positions): + message = JointTrajectory() + message.header.stamp = self.get_clock().now().to_msg() + message.joint_names = list(JOINT_NAMES) + point = JointTrajectoryPoint() + point.positions = [float(p) for p in positions] + point.time_from_start = Duration( + sec=int(self.point_dt_s), + nanosec=int((self.point_dt_s % 1.0) * 1e9), + ) + message.points = [point] + return message + + def on_mirror_tick(self, request, response): + del request + self.last_mirror_tick = self.get_clock().now() + response.success = True + response.message = "mirroring" + return response + + def mirroring(self): + if self.last_mirror_tick is None: + return False + age = (self.get_clock().now() - self.last_mirror_tick).nanoseconds * 1e-9 + return age < self.mirror_timeout_s + + def on_trajectory_status(self, message): + live = { + GoalStatus.STATUS_ACCEPTED, + GoalStatus.STATUS_EXECUTING, + GoalStatus.STATUS_CANCELING, + } + active = any(status.status in live for status in message.status_list) + if active != self.goal_active: + self.get_logger().info( + "trajectory goal active, pausing mirroring" + if active + else "trajectory goal finished, resuming mirroring" + ) + self.goal_active = active + + def publish_once(self): + if self.goal_active or not self.mirroring(): + return + self.publisher.publish(self.build_message(self.read_positions())) + + +def main(argv=None): + argv = sys.argv[1:] if argv is None else argv + parser = argparse.ArgumentParser(description=__doc__) + source = parser.add_mutually_exclusive_group() + source.add_argument( + "--fake", + dest="source", + action="store_const", + const="fake", + default="fake", + help="publish a slow sine instead of reading an arm (the default)", + ) + source.add_argument( + "--real", + dest="source", + action="store_const", + const="real", + help="read the Feetech bus (not implemented until phase two)", + ) + args, ros_args = parser.parse_known_args(argv) + + rclpy.init(args=ros_args) + try: + rclpy.spin(So101ArmBridge(source=args.source)) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/so101_sim/test/test_so101_arm_bridge.py b/src/so101_sim/test/test_so101_arm_bridge.py new file mode 100644 index 000000000..38d14c38e --- /dev/null +++ b/src/so101_sim/test/test_so101_arm_bridge.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python3 + +# Copyright 2026 PickNik Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the PickNik Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +"""Check the bridge's fake source and its name/unit conversion helpers.""" + +from pathlib import Path +import math +import sys +import time + +import pytest +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from std_srvs.srv import Trigger +from trajectory_msgs.msg import JointTrajectory + +sys.path.insert(0, str(Path(__file__).parents[1] / "script")) +from so101_arm_bridge import ( # noqa: E402 + JOINT_NAMES, + So101ArmBridge, + fake_positions, + order_like, + to_radians, +) + + +def test_to_radians_applies_offset_then_sign(): + # 90 deg with a 10 deg offset and a flipped sign is -80 deg. + assert to_radians([90.0], [-1.0], [10.0]) == pytest.approx([math.radians(-80.0)]) + assert to_radians([0.0] * 6, [1.0] * 6, [0.0] * 6) == [0.0] * 6 + + +def test_to_radians_rejects_mismatched_calibration(): + with pytest.raises(ValueError): + to_radians([0.0, 0.0], [1.0], [0.0, 0.0]) + + +def test_order_like_reorders_by_name(): + shuffled = list(reversed(JOINT_NAMES)) + values = [float(i) for i in range(len(shuffled))] + ordered = order_like(shuffled, values) + assert ordered == list(reversed(values)) + assert order_like(JOINT_NAMES, values) == values + + +def test_order_like_rejects_a_missing_joint(): + with pytest.raises(KeyError): + order_like(JOINT_NAMES[:-1] + ["jaw"], [0.0] * len(JOINT_NAMES)) + with pytest.raises(ValueError): + order_like(JOINT_NAMES, [0.0]) + + +def test_fake_positions_stay_inside_the_urdf_limits(): + limits = { + "shoulder_pan": (-1.91986, 1.91986), + "shoulder_lift": (-1.74533, 1.74533), + "elbow_flex": (-1.69, 1.69), + "wrist_flex": (-1.65806, 1.65806), + "wrist_roll": (-2.74385, 2.84121), + "gripper": (-0.174533, 1.74533), + } + period = 12.0 + moved = [False] * len(JOINT_NAMES) + first = fake_positions(0.0, period) + for step in range(241): + positions = fake_positions(step * period / 240.0, period) + for index, name in enumerate(JOINT_NAMES): + lower, upper = limits[name] + assert lower <= positions[index] <= upper, name + if abs(positions[index] - first[index]) > 0.1: + moved[index] = True + assert all(moved), "every joint should visibly move over one sine period" + + +def test_fake_positions_stay_out_of_the_self_collision_fold(): + """Guard the envelope that made MoveIt refuse to plan from the twin's state. + + Folding the wrist back over the shoulder is a self-collision, and while the + sine sits in that region every plan from the current state is rejected. The + fold needs shoulder_lift and elbow_flex to swing to the same side; keeping + them on opposite sides keeps the arm reaching outward. + """ + period = 12.0 + for step in range(241): + positions = fake_positions(step * period / 240.0, period) + shoulder_lift = positions[JOINT_NAMES.index("shoulder_lift")] + elbow_flex = positions[JOINT_NAMES.index("elbow_flex")] + assert shoulder_lift > 0.1, shoulder_lift + assert elbow_flex < -0.1, elbow_flex + + +@pytest.fixture +def ros_context(): + rclpy.init() + yield + rclpy.try_shutdown() + + +def tick_mirroring(bridge): + """Stand in for the Mirror Objective's ~/mirror service tick.""" + bridge.on_mirror_tick(Trigger.Request(), Trigger.Response()) + + +def test_the_bridge_is_silent_until_mirroring_is_requested(ros_context): + received = [] + bridge = So101ArmBridge(source="fake") + listener = Node("test_silence_listener") + listener.create_subscription( + JointTrajectory, + "/joint_trajectory_controller/joint_trajectory", + received.append, + 10, + ) + executor = SingleThreadedExecutor() + executor.add_node(bridge) + executor.add_node(listener) + deadline = time.monotonic() + 1.0 + while time.monotonic() < deadline: + executor.spin_once(timeout_sec=0.02) + executor.shutdown() + listener.destroy_node() + bridge.destroy_node() + assert received == [], "mirroring must be off until the Objective asks for it" + + +def test_fake_mode_publishes_a_usable_trajectory(ros_context): + received = [] + bridge = So101ArmBridge(source="fake") + listener = Node("test_listener") + listener.create_subscription( + JointTrajectory, + "/joint_trajectory_controller/joint_trajectory", + received.append, + 10, + ) + executor = SingleThreadedExecutor() + executor.add_node(bridge) + executor.add_node(listener) + + deadline = time.monotonic() + 5.0 + while time.monotonic() < deadline and len(received) < 2: + tick_mirroring(bridge) + executor.spin_once(timeout_sec=0.05) + + executor.shutdown() + listener.destroy_node() + bridge.destroy_node() + + assert len(received) >= 2, "the bridge should publish at its configured rate" + message = received[0] + assert message.joint_names == JOINT_NAMES + assert len(message.points) == 1 + assert len(message.points[0].positions) == len(JOINT_NAMES) + # A zero lead time would land the point in the past and the controller + # would drop it. + lead = message.points[0].time_from_start + assert lead.sec + lead.nanosec * 1e-9 > 0.0 + + +def test_bridge_yields_while_a_trajectory_goal_is_active(ros_context): + """A live action goal must silence the topic stream, or the goal never ends. + + The controller restarts its trajectory on every topic message, so a plan's + goal is accepted and then hangs forever while the bridge keeps publishing. + """ + from action_msgs.msg import GoalStatus, GoalStatusArray + + received = [] + bridge = So101ArmBridge(source="fake") + listener = Node("test_yield_listener") + listener.create_subscription( + JointTrajectory, + "/joint_trajectory_controller/joint_trajectory", + received.append, + 10, + ) + status_publisher = listener.create_publisher( + GoalStatusArray, + "/joint_trajectory_controller/follow_joint_trajectory/_action/status", + 10, + ) + executor = SingleThreadedExecutor() + executor.add_node(bridge) + executor.add_node(listener) + + def spin(seconds): + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + tick_mirroring(bridge) + executor.spin_once(timeout_sec=0.02) + + def publish_status(status): + message = GoalStatusArray() + entry = GoalStatus() + entry.status = status + message.status_list = [entry] + status_publisher.publish(message) + + tick_mirroring(bridge) + publish_status(GoalStatus.STATUS_EXECUTING) + spin(0.5) + received.clear() + spin(0.5) + assert received == [], "the bridge must not publish while a goal is executing" + + publish_status(GoalStatus.STATUS_SUCCEEDED) + spin(0.5) + assert received, "the bridge must resume once the goal finishes" + + executor.shutdown() + listener.destroy_node() + bridge.destroy_node() + + +def test_real_source_is_still_a_stub(ros_context): + bridge = So101ArmBridge(source="real") + with pytest.raises(NotImplementedError): + bridge.read_positions() + bridge.destroy_node() diff --git a/src/so101_sim/thumbnail.png b/src/so101_sim/thumbnail.png new file mode 100644 index 000000000..a1b937a0b --- /dev/null +++ b/src/so101_sim/thumbnail.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e7258ed600bee525ce1911ee64ea69719889eaabe51e3e726ad596c0b50ea884 +size 37339 diff --git a/src/so101_sim/waypoints/so101_waypoints.yaml b/src/so101_sim/waypoints/so101_waypoints.yaml new file mode 100644 index 000000000..07fb316e0 --- /dev/null +++ b/src/so101_sim/waypoints/so101_waypoints.yaml @@ -0,0 +1,194 @@ +- description: Folded rest pose. Values are the mock hardware start state, not a taught + hardware pose. + favorite: true + joint_group_names: + - gripper + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - shoulder_pan + - shoulder_lift + - elbow_flex + - wrist_flex + - wrist_roll + - gripper + position: + - 0.0 + - 0.6 + - -0.6 + - 0.0 + - 0.0 + - 0.5 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Home +- description: Arm up and gripper open, clear of the bench. Re-teach on the physical + arm before trusting it. + favorite: true + joint_group_names: + - gripper + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - shoulder_pan + - shoulder_lift + - elbow_flex + - wrist_flex + - wrist_roll + - gripper + position: + - 0.3 + - 0.75 + - -0.4 + - 0.2 + - 0.0 + - 1.2 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Ready +- description: Reaching out over the bench. Sim-only pose. + favorite: false + joint_group_names: + - gripper + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - shoulder_pan + - shoulder_lift + - elbow_flex + - wrist_flex + - wrist_roll + - gripper + position: + - 0.0 + - 0.9 + - -0.75 + - 0.3 + - 0.0 + - 1.2 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Reach Forward +- description: Jaw fully open. Used by the Open Gripper Objective; only the gripper + joint is planned for. + favorite: false + joint_group_names: + - gripper + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - shoulder_pan + - shoulder_lift + - elbow_flex + - wrist_flex + - wrist_roll + - gripper + position: + - 0.0 + - 0.6 + - -0.6 + - 0.0 + - 0.0 + - 1.5 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Gripper Open +- description: Jaw closed. Used by the Close Gripper Objective; only the gripper joint + is planned for. + favorite: false + joint_group_names: + - gripper + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - shoulder_pan + - shoulder_lift + - elbow_flex + - wrist_flex + - wrist_roll + - gripper + position: + - 0.0 + - 0.6 + - -0.6 + - 0.0 + - 0.0 + - 0.0 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Gripper Closed From 2b2611facbd5eaa1597ca67f64cabfce86a62120 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 9 Sep 2026 19:53:57 -0400 Subject: [PATCH 2/8] docs: record worktree image tagging and single-JTC contention in AGENTS.md Both cost a debugging cycle while building so101_sim and apply to any config in this workspace, not just that one. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01DGyVgFaNyf4fC9uRiKsXbg --- AGENTS.md | 99 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ CLAUDE.md | 64 ++--------------------------------- 2 files changed, 101 insertions(+), 62 deletions(-) create mode 100644 AGENTS.md diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 000000000..427b77919 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,99 @@ +# AI Code Assistant Instructions for MoveIt Pro Example Workspace + +## MuJoCo Scene Files + +### Keyframe qpos must match model DOF count + +When editing `scene.xml` files (adding/removing bodies with joints), the `` section's `qpos` attribute must have exactly the number of values matching the model's total degrees of freedom. A mismatch causes `ros2_control_node` to crash with: + +``` +Error: keyframe 0: invalid qpos size, expected length +``` + +Each joint type contributes to qpos: +- **freejoint**: 7 values (x, y, z, qw, qx, qy, qz) +- **hinge/slide**: 1 value each +- **ball**: 4 values (quaternion) + +After adding or removing bodies with joints, **remove the keyframe** and let MuJoCo use body `pos=` attributes for initial positions. + +### Velocity actuators: `armature/kv` time-constant must stay below the timestep + +A `` actuator on a joint with `armature="..."` behaves like a first-order servo with time-constant `τ = armature / kv`. If `τ` is larger than the scene `timestep`, the servo cannot inject enough velocity correction per step to overcome external load, and the joint effectively **stops responding to commands** — it stays pinned near zero even at full command. The per-step velocity correction scales as `kv · timestep / armature`, so halving the timestep halves the authority. + +This bit `hangar_sim`'s mecanum base: the wheels had `armature="1.0"`, `kv="50"` → `τ = 0.02 s`. It worked only because the timestep was `0.025 s` (above τ). Standardizing the timestep to `0.003 s` dropped it well below τ, the wheel servos lost authority, the wheels pinned at ~0 rad/s, and the base would not drive (the whole-body `ExecuteTrajectory` then hung forever waiting for the base to reach goal). Fix was `kv: 50 → 500` (τ → 0.002 s, below the new timestep), verified in standalone MuJoCo to be stable across `timestep` 0.025→0.002. Lowering `armature` instead also raises authority but went unstable at small timesteps — prefer raising `kv`. (hangar's scene ultimately runs `timestep="0.008"`, coarser than the 0.003 s the other configs use: at 0.003 the CI runner overran ~47% of sim steps and starved controller mode-switching. `kv=500` keeps the wheels valid there too — τ=0.002 s < 0.008 s.) + +The two coupled numbers live in different files: the actuator `kv` is in the `` blocks of `hangar_sim/description/ur5e_ridgeback.xml` (~line 1709), and the joint `armature` is in the per-wheel includes (`hangar_sim/description/{front,rear}_{left,right}_wheel_link.xml`, the wheel ``). + +Rule of thumb when changing a sim `timestep`: for every velocity actuator, check `armature/kv < timestep`. The symptom of violation is a joint that ignores commands (pinned), not one that oscillates. + +### MuJoCo documentation + +Refer to [docs.picknik.ai](https://docs.picknik.ai) for MuJoCo configuration guides: + +- [Physics Simulator Setup](https://docs.picknik.ai/how_to/configuration_tutorials/migrate_to_mujoco_config/) — creating scene.xml from URDF, camera/sensor setup, mesh conversion, MuJoCo Interactive Viewer +- [config.yaml Reference](https://docs.picknik.ai/how_to/configuration_tutorials/config_yaml_reference/) — `hardware` section for `picknik_mujoco_ros/MujocoSystem` plugin configuration +- [Simulator Keyframes Setup](https://docs.picknik.ai/how_to/configuration_tutorials/create_robot_sim_config/configure_keyframes/) — defining keyframes in scene.xml, `ResetMujocoKeyframe` Behavior +- [Optimize Model Meshes](https://docs.picknik.ai/how_to/configuration_tutorials/optimizing_robot_model_meshes/) — MuJoCo enforces 1-200,000 faces per STL +- [Simulation Troubleshooting](https://docs.picknik.ai/troubleshooting/Simulation%20Troubleshooting/) — physics parameters, grip stability, mass/inertia errors, rendering issues + +## Objective XML Files + +### MetadataFields required for CI + +Every objective XML file must include a `MetadataFields` block inside the `TreeNodesModel` section. The `validate_objectives` CI check will fail without it. + +```xml + + + + + + + + +``` + +- `runnable` — set to `"true"` for top-level objectives the user can run, `"false"` for subtrees only called by other objectives +- `subcategory` — groups the objective in the UI (e.g., `"AprilTag"`, `"Grasping"`, `"MuJoCo Simulation"`) + +### A gripper config needs `close_gripper.xml` / `open_gripper.xml`, or teleop gripper silently fails + +Teleoperation drives the gripper by looking up Objectives named exactly `"Close Gripper"` / `"Open Gripper"` (the `Request Teleoperation` SubTree in moveit_pro core). If a config package doesn't provide those overrides in its `objectives/` directory, the lookup falls back to moveit_pro's core placeholder, which logs `[ERROR] LogMessage Error: This robot configuration does not have a \`Close Gripper\` Objective configured to override this default.` on every BT tick for as long as the control is held, and the gripper never moves — even if some other Objective in the same config already drives the gripper directly via `MoveGripperAction` (that path bypasses the named-Objective lookup entirely). Any new config with a gripper needs both files; see `moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/{close,open}_gripper.xml` for the reference pattern. + +## Running MoveIt Pro from a git worktree + +The user image tag is `moveit-pro-:--${MOVEIT_HOST_USER_WORKSPACE_NAME}`, +and that variable defaults to the workspace directory's basename. Every worktree +of this repo shares that basename, so a plain `moveit_pro build` from a worktree +overwrites the images built from the primary checkout. Set +`MOVEIT_HOST_USER_WORKSPACE_NAME` to something unique for the worktree, and pass +`-w "$PWD"` to `build` and `run`, which also keeps the CLI from repointing the +user's global config at the worktree. + +Inside the containers, `ros2 node list` and friends return nothing until you run +`ros2 daemon stop` once: the daemon that survives from an earlier deployment +holds a participant that finds nothing on the current graph. + +## One trajectory controller, several planning groups + +When a config puts every joint on a single `joint_trajectory_controller` (the +right call when something other than MoveIt also drives the arm, since a second +controller claiming a joint's command interface locks the first one out), every +goal a planning group sends names a subset of the controller's joints and +`allow_partial_joints_goal` must be true, or the controller rejects all of them +with "Joints on incoming trajectory don't match the controller joints." + +That controller has one owner at a time. A node publishing on its topic +interface restarts the trajectory on every message, so an action goal from a +plan is accepted and then never converges — or aborts on a path tolerance the +still-moving robot violated. Such a publisher has to yield: gate it on a +heartbeat the driving Objective ticks, and on the controller's +`follow_joint_trajectory/_action/status`. `so101_sim` does both. + +## Maintaining this file + +Keep this file for knowledge useful to almost every future agent session in this project. +Do not repeat what the codebase already shows; point to the authoritative file or command instead. +Prefer rewriting or pruning existing entries over appending new ones. +When updating this file, preserve this bar for all agents and keep entries concise. diff --git a/CLAUDE.md b/CLAUDE.md index dc3de155c..a9d4d2694 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -1,62 +1,2 @@ -# AI Code Assistant Instructions for MoveIt Pro Example Workspace - -## MuJoCo Scene Files - -### Keyframe qpos must match model DOF count - -When editing `scene.xml` files (adding/removing bodies with joints), the `` section's `qpos` attribute must have exactly the number of values matching the model's total degrees of freedom. A mismatch causes `ros2_control_node` to crash with: - -``` -Error: keyframe 0: invalid qpos size, expected length -``` - -Each joint type contributes to qpos: -- **freejoint**: 7 values (x, y, z, qw, qx, qy, qz) -- **hinge/slide**: 1 value each -- **ball**: 4 values (quaternion) - -After adding or removing bodies with joints, **remove the keyframe** and let MuJoCo use body `pos=` attributes for initial positions. - -### Velocity actuators: `armature/kv` time-constant must stay below the timestep - -A `` actuator on a joint with `armature="..."` behaves like a first-order servo with time-constant `τ = armature / kv`. If `τ` is larger than the scene `timestep`, the servo cannot inject enough velocity correction per step to overcome external load, and the joint effectively **stops responding to commands** — it stays pinned near zero even at full command. The per-step velocity correction scales as `kv · timestep / armature`, so halving the timestep halves the authority. - -This bit `hangar_sim`'s mecanum base: the wheels had `armature="1.0"`, `kv="50"` → `τ = 0.02 s`. It worked only because the timestep was `0.025 s` (above τ). Standardizing the timestep to `0.003 s` dropped it well below τ, the wheel servos lost authority, the wheels pinned at ~0 rad/s, and the base would not drive (the whole-body `ExecuteTrajectory` then hung forever waiting for the base to reach goal). Fix was `kv: 50 → 500` (τ → 0.002 s, below the new timestep), verified in standalone MuJoCo to be stable across `timestep` 0.025→0.002. Lowering `armature` instead also raises authority but went unstable at small timesteps — prefer raising `kv`. (hangar's scene ultimately runs `timestep="0.008"`, coarser than the 0.003 s the other configs use: at 0.003 the CI runner overran ~47% of sim steps and starved controller mode-switching. `kv=500` keeps the wheels valid there too — τ=0.002 s < 0.008 s.) - -The two coupled numbers live in different files: the actuator `kv` is in the `` blocks of `hangar_sim/description/ur5e_ridgeback.xml` (~line 1709), and the joint `armature` is in the per-wheel includes (`hangar_sim/description/{front,rear}_{left,right}_wheel_link.xml`, the wheel ``). - -Rule of thumb when changing a sim `timestep`: for every velocity actuator, check `armature/kv < timestep`. The symptom of violation is a joint that ignores commands (pinned), not one that oscillates. - -### MuJoCo documentation - -Refer to [docs.picknik.ai](https://docs.picknik.ai) for MuJoCo configuration guides: - -- [Physics Simulator Setup](https://docs.picknik.ai/how_to/configuration_tutorials/migrate_to_mujoco_config/) — creating scene.xml from URDF, camera/sensor setup, mesh conversion, MuJoCo Interactive Viewer -- [config.yaml Reference](https://docs.picknik.ai/how_to/configuration_tutorials/config_yaml_reference/) — `hardware` section for `picknik_mujoco_ros/MujocoSystem` plugin configuration -- [Simulator Keyframes Setup](https://docs.picknik.ai/how_to/configuration_tutorials/create_robot_sim_config/configure_keyframes/) — defining keyframes in scene.xml, `ResetMujocoKeyframe` Behavior -- [Optimize Model Meshes](https://docs.picknik.ai/how_to/configuration_tutorials/optimizing_robot_model_meshes/) — MuJoCo enforces 1-200,000 faces per STL -- [Simulation Troubleshooting](https://docs.picknik.ai/troubleshooting/Simulation%20Troubleshooting/) — physics parameters, grip stability, mass/inertia errors, rendering issues - -## Objective XML Files - -### MetadataFields required for CI - -Every objective XML file must include a `MetadataFields` block inside the `TreeNodesModel` section. The `validate_objectives` CI check will fail without it. - -```xml - - - - - - - - -``` - -- `runnable` — set to `"true"` for top-level objectives the user can run, `"false"` for subtrees only called by other objectives -- `subcategory` — groups the objective in the UI (e.g., `"AprilTag"`, `"Grasping"`, `"MuJoCo Simulation"`) - -### A gripper config needs `close_gripper.xml` / `open_gripper.xml`, or teleop gripper silently fails - -Teleoperation drives the gripper by looking up Objectives named exactly `"Close Gripper"` / `"Open Gripper"` (the `Request Teleoperation` SubTree in moveit_pro core). If a config package doesn't provide those overrides in its `objectives/` directory, the lookup falls back to moveit_pro's core placeholder, which logs `[ERROR] LogMessage Error: This robot configuration does not have a \`Close Gripper\` Objective configured to override this default.` on every BT tick for as long as the control is held, and the gripper never moves — even if some other Objective in the same config already drives the gripper directly via `MoveGripperAction` (that path bypasses the named-Objective lookup entirely). Any new config with a gripper needs both files; see `moveit_pro_kinova_configs/kinova_gen3_base_config/objectives/{close,open}_gripper.xml` for the reference pattern. + +@AGENTS.md From 4ea13a5cd08bb4b7750c89a1a861ce5acfef10ca Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 9 Sep 2026 20:06:33 -0400 Subject: [PATCH 3/8] no-mistakes(review): fix so101_sim review findings: guard docs, Home gripper, order_like --- README.md | 1 + src/so101_sim/README.md | 9 ++++++--- src/so101_sim/script/so101_arm_bridge.py | 9 ++++----- src/so101_sim/waypoints/so101_waypoints.yaml | 2 +- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 5ca5d0602..6b6a5592a 100644 --- a/README.md +++ b/README.md @@ -31,6 +31,7 @@ git submodule update --init src/moveit_pro_sam2 src/moveit_pro_sam3 - `lab_sim` - `lunar_sim` - `phoebe_sim` +- `so101_sim` - `vla_sim` - `moveit_pro_franka_configs/franka_base_config` - `moveit_pro_kinova_configs/kinova_gen3_base_config` diff --git a/src/so101_sim/README.md b/src/so101_sim/README.md index 3efb98647..bcb4551aa 100644 --- a/src/so101_sim/README.md +++ b/src/so101_sim/README.md @@ -22,9 +22,12 @@ messages restarts its trajectory on every tick, so a plan's goal would be accepted and then hang forever, or abort on a path tolerance the moving twin violated. The Objective keeps mirroring alive by ticking the bridge's `~/mirror` `Trigger` service in a loop; one second without a tick and the bridge -goes quiet. As a second guard, the bridge also stays quiet while a -`follow_joint_trajectory` goal is live, so an execution that starts anyway still -wins. +goes quiet. That heartbeat gate is the primary guard: **stop the Mirror +Objective before planning or executing a motion.** The bridge also skips a +publish while it believes a `follow_joint_trajectory` goal is live, but that is +best-effort only — the flag is set from `GoalStatusArray` messages, so a goal +started while the Mirror Objective is still publishing can lose the race with a +20 ms bridge tick. ## Execution to hardware is out of scope diff --git a/src/so101_sim/script/so101_arm_bridge.py b/src/so101_sim/script/so101_arm_bridge.py index 495c6a489..5d970ed73 100755 --- a/src/so101_sim/script/so101_arm_bridge.py +++ b/src/so101_sim/script/so101_arm_bridge.py @@ -95,21 +95,20 @@ def to_radians(degrees, signs, offsets): return [math.radians((d - o) * s) for d, o, s in zip(degrees, offsets, signs)] -def order_like(names, values, wanted=None): - """Reorder ``values`` from ``names`` into ``wanted`` (default JOINT_NAMES). +def order_like(names, values): + """Reorder ``values`` from ``names`` into JOINT_NAMES order. The bus and the controller both speak in joint names, but not necessarily in the same order, and a silently transposed pair is the kind of bug that only shows up as a robot bending the wrong way. """ - wanted = JOINT_NAMES if wanted is None else wanted if len(names) != len(values): raise ValueError(f"{len(names)} names but {len(values)} values") lookup = dict(zip(names, values)) - missing = [n for n in wanted if n not in lookup] + missing = [n for n in JOINT_NAMES if n not in lookup] if missing: raise KeyError(f"missing joints: {missing}") - return [lookup[n] for n in wanted] + return [lookup[n] for n in JOINT_NAMES] def fake_positions(elapsed_s, period_s): diff --git a/src/so101_sim/waypoints/so101_waypoints.yaml b/src/so101_sim/waypoints/so101_waypoints.yaml index 07fb316e0..37ae6df12 100644 --- a/src/so101_sim/waypoints/so101_waypoints.yaml +++ b/src/so101_sim/waypoints/so101_waypoints.yaml @@ -24,7 +24,7 @@ - -0.6 - 0.0 - 0.0 - - 0.5 + - 0.7 velocity: [] multi_dof_joint_state: header: From b0a836f1238c2197907da64bb670a71972babb43 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 9 Sep 2026 20:17:46 -0400 Subject: [PATCH 4/8] no-mistakes(review): reset sine phase on mirror start, restore LICENSE tail --- src/so101_sim/description/assets/LICENSE | 2 + src/so101_sim/script/so101_arm_bridge.py | 21 ++++++++--- src/so101_sim/test/test_so101_arm_bridge.py | 41 +++++++++++++++++++++ 3 files changed, 59 insertions(+), 5 deletions(-) diff --git a/src/so101_sim/description/assets/LICENSE b/src/so101_sim/description/assets/LICENSE index c39a97ad9..beb879c08 100644 --- a/src/so101_sim/description/assets/LICENSE +++ b/src/so101_sim/description/assets/LICENSE @@ -198,3 +198,5 @@ 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. diff --git a/src/so101_sim/script/so101_arm_bridge.py b/src/so101_sim/script/so101_arm_bridge.py index 5d970ed73..6100f4433 100755 --- a/src/so101_sim/script/so101_arm_bridge.py +++ b/src/so101_sim/script/so101_arm_bridge.py @@ -112,9 +112,17 @@ def order_like(names, values): def fake_positions(elapsed_s, period_s): - """A slow sine, phase-shifted per joint so the whole arm visibly moves.""" + """A slow sine that starts every joint at FAKE_CENTER and de-phases them. + + Each joint runs at a slightly different period so the whole arm visibly + moves rather than swinging as one rigid shape. Spreading the periods rather + than the starting phases keeps ``elapsed_s == 0`` equal to FAKE_CENTER, + which is where the mock hardware sits when mirroring begins. + """ return [ - center + amplitude * math.sin(2.0 * math.pi * elapsed_s / period_s + i * 0.7) + center + + amplitude + * math.sin(2.0 * math.pi * elapsed_s * (1.0 + 0.1 * i) / period_s) for i, (center, amplitude) in enumerate(zip(FAKE_CENTER, FAKE_AMPLITUDE)) ] @@ -122,8 +130,8 @@ def fake_positions(elapsed_s, period_s): class So101ArmBridge(Node): """Publish follower joint positions as single-point trajectories.""" - def __init__(self, source="fake", parameter_overrides=None): - super().__init__("so101_arm_bridge", parameter_overrides=parameter_overrides) + def __init__(self, source="fake"): + super().__init__("so101_arm_bridge") self.source = source publish_rate_hz = self.declare_parameter("publish_rate_hz", 50.0).value @@ -201,7 +209,10 @@ def build_message(self, positions): def on_mirror_tick(self, request, response): del request - self.last_mirror_tick = self.get_clock().now() + now = self.get_clock().now() + if not self.mirroring(): + self.start_time = now + self.last_mirror_tick = now response.success = True response.message = "mirroring" return response diff --git a/src/so101_sim/test/test_so101_arm_bridge.py b/src/so101_sim/test/test_so101_arm_bridge.py index 38d14c38e..7e4863e7c 100644 --- a/src/so101_sim/test/test_so101_arm_bridge.py +++ b/src/so101_sim/test/test_so101_arm_bridge.py @@ -43,6 +43,7 @@ sys.path.insert(0, str(Path(__file__).parents[1] / "script")) from so101_arm_bridge import ( # noqa: E402 + FAKE_CENTER, JOINT_NAMES, So101ArmBridge, fake_positions, @@ -239,6 +240,46 @@ def publish_status(status): bridge.destroy_node() +def test_mirroring_starts_the_sine_at_the_mock_start_state(ros_context): + """The twin must not snap when the Mirror Objective starts. + + The mock hardware sits at FAKE_CENTER until something drives it, so the + first point published after a not-mirroring -> mirroring transition has to + be that same pose however long the bridge has been up. + """ + received = [] + bridge = So101ArmBridge(source="fake") + listener = Node("test_mirror_start_listener") + listener.create_subscription( + JointTrajectory, + "/joint_trajectory_controller/joint_trajectory", + received.append, + 10, + ) + executor = SingleThreadedExecutor() + executor.add_node(bridge) + executor.add_node(listener) + + # Age the node well past the point where an unreset sine has swung away. + deadline = time.monotonic() + 0.5 + while time.monotonic() < deadline: + executor.spin_once(timeout_sec=0.02) + assert received == [] + + tick_mirroring(bridge) + deadline = time.monotonic() + 2.0 + while time.monotonic() < deadline and not received: + tick_mirroring(bridge) + executor.spin_once(timeout_sec=0.01) + + executor.shutdown() + listener.destroy_node() + bridge.destroy_node() + + assert received, "mirroring should start publishing once ticked" + assert received[0].points[0].positions == pytest.approx(FAKE_CENTER, abs=0.1) + + def test_real_source_is_still_a_stub(ros_context): bridge = So101ArmBridge(source="real") with pytest.raises(NotImplementedError): From 9591faec1b1659870132bf7516da25ed177ce1ff Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 9 Sep 2026 20:28:18 -0400 Subject: [PATCH 5/8] no-mistakes(review): restore closed-curve sine, pin phase once only --- src/so101_sim/script/so101_arm_bridge.py | 23 ++++++++++++++------- src/so101_sim/test/test_so101_arm_bridge.py | 11 ++++++---- 2 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/so101_sim/script/so101_arm_bridge.py b/src/so101_sim/script/so101_arm_bridge.py index 6100f4433..c5a6bd971 100755 --- a/src/so101_sim/script/so101_arm_bridge.py +++ b/src/so101_sim/script/so101_arm_bridge.py @@ -112,17 +112,21 @@ def order_like(names, values): def fake_positions(elapsed_s, period_s): - """A slow sine that starts every joint at FAKE_CENTER and de-phases them. + """A slow sine, phase-shifted per joint so the whole arm visibly moves. - Each joint runs at a slightly different period so the whole arm visibly - moves rather than swinging as one rigid shape. Spreading the periods rather - than the starting phases keeps ``elapsed_s == 0`` equal to FAKE_CENTER, - which is where the mock hardware sits when mirroring begins. + Every joint shares one period, so the motion is a single closed curve that + one period of sampling covers completely. Each joint's constant phase term + is subtracted, which puts ``elapsed_s == 0`` exactly at FAKE_CENTER, where + the mock hardware sits before anything drives it, and shifts that joint's + envelope by ``-amplitude * sin(phase)``. """ return [ center + amplitude - * math.sin(2.0 * math.pi * elapsed_s * (1.0 + 0.1 * i) / period_s) + * ( + math.sin(2.0 * math.pi * elapsed_s / period_s + i * 0.7) + - math.sin(i * 0.7) + ) for i, (center, amplitude) in enumerate(zip(FAKE_CENTER, FAKE_AMPLITUDE)) ] @@ -174,6 +178,10 @@ def __init__(self, source="fake"): self.last_mirror_tick = None self.create_service(Trigger, "~/mirror", self.on_mirror_tick) self.start_time = self.get_clock().now() + # The sine's phase is pinned to FAKE_CENTER once, for the first Mirror + # start. Later restarts continue from wall clock: the twin is then + # holding wherever the sine left it, and rewinding would snap it back. + self.sine_phase_pinned = False self.timer = self.create_timer(1.0 / publish_rate_hz, self.publish_once) self.get_logger().info( f"so101_arm_bridge ready to publish {self.source} joint states to " @@ -210,8 +218,9 @@ def build_message(self, positions): def on_mirror_tick(self, request, response): del request now = self.get_clock().now() - if not self.mirroring(): + if not self.sine_phase_pinned: self.start_time = now + self.sine_phase_pinned = True self.last_mirror_tick = now response.success = True response.message = "mirroring" diff --git a/src/so101_sim/test/test_so101_arm_bridge.py b/src/so101_sim/test/test_so101_arm_bridge.py index 7e4863e7c..b6180739e 100644 --- a/src/so101_sim/test/test_so101_arm_bridge.py +++ b/src/so101_sim/test/test_so101_arm_bridge.py @@ -90,6 +90,8 @@ def test_fake_positions_stay_inside_the_urdf_limits(): period = 12.0 moved = [False] * len(JOINT_NAMES) first = fake_positions(0.0, period) + # The mock hardware starts at FAKE_CENTER, so the sine has to as well. + assert first == pytest.approx(FAKE_CENTER) for step in range(241): positions = fake_positions(step * period / 240.0, period) for index, name in enumerate(JOINT_NAMES): @@ -244,8 +246,8 @@ def test_mirroring_starts_the_sine_at_the_mock_start_state(ros_context): """The twin must not snap when the Mirror Objective starts. The mock hardware sits at FAKE_CENTER until something drives it, so the - first point published after a not-mirroring -> mirroring transition has to - be that same pose however long the bridge has been up. + first point published after the first ever mirror start has to be that same + pose however long the bridge has been up before it. """ received = [] bridge = So101ArmBridge(source="fake") @@ -260,8 +262,9 @@ def test_mirroring_starts_the_sine_at_the_mock_start_state(ros_context): executor.add_node(bridge) executor.add_node(listener) - # Age the node well past the point where an unreset sine has swung away. - deadline = time.monotonic() + 0.5 + # Age the node up to where an unpinned sine sits near its peak, so a + # missing reset misses FAKE_CENTER by far more than the tolerance below. + deadline = time.monotonic() + 3.0 while time.monotonic() < deadline: executor.spin_once(timeout_sec=0.02) assert received == [] From 0993483d1d7b6d734ee30edbd20cf95e0493618d Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 9 Sep 2026 20:38:55 -0400 Subject: [PATCH 6/8] no-mistakes(review): restore untranslated sine, move start state to curve --- src/so101_sim/config/initial_positions.yaml | 15 +++++------ src/so101_sim/script/so101_arm_bridge.py | 22 +++++++---------- src/so101_sim/test/test_so101_arm_bridge.py | 26 ++++++++++++++------ src/so101_sim/waypoints/so101_waypoints.yaml | 10 ++++---- 4 files changed, 40 insertions(+), 33 deletions(-) diff --git a/src/so101_sim/config/initial_positions.yaml b/src/so101_sim/config/initial_positions.yaml index 4dadb0799..0abcee1e1 100644 --- a/src/so101_sim/config/initial_positions.yaml +++ b/src/so101_sim/config/initial_positions.yaml @@ -1,9 +1,10 @@ -# Joint positions the mock hardware starts at, in radians. These match the -# center of the bridge's fake sine, so the twin does not jump when it starts. +# Joint positions the mock hardware starts at, in radians. These are the +# bridge's fake sine at elapsed 0, so the twin does not jump when mirroring +# begins. They are not the sine's center: each joint carries a phase offset. initial_positions: shoulder_pan: 0.0 - shoulder_lift: 0.6 - elbow_flex: -0.6 - wrist_flex: 0.0 - wrist_roll: 0.0 - gripper: 0.7 + shoulder_lift: 0.728843537 + elbow_flex: -0.353637568 + wrist_flex: 0.302123278 + wrist_roll: 0.33498815 + gripper: 0.489530063 diff --git a/src/so101_sim/script/so101_arm_bridge.py b/src/so101_sim/script/so101_arm_bridge.py index c5a6bd971..be74fc1b8 100755 --- a/src/so101_sim/script/so101_arm_bridge.py +++ b/src/so101_sim/script/so101_arm_bridge.py @@ -115,18 +115,13 @@ def fake_positions(elapsed_s, period_s): """A slow sine, phase-shifted per joint so the whole arm visibly moves. Every joint shares one period, so the motion is a single closed curve that - one period of sampling covers completely. Each joint's constant phase term - is subtracted, which puts ``elapsed_s == 0`` exactly at FAKE_CENTER, where - the mock hardware sits before anything drives it, and shifts that joint's - envelope by ``-amplitude * sin(phase)``. + one period of sampling covers completely. FAKE_CENTER is that curve's + center, which the phase offsets mean is not where it starts: + ``elapsed_s == 0`` is the pose config/initial_positions.yaml puts the mock + hardware in, so the twin does not jump when mirroring begins. """ return [ - center - + amplitude - * ( - math.sin(2.0 * math.pi * elapsed_s / period_s + i * 0.7) - - math.sin(i * 0.7) - ) + center + amplitude * math.sin(2.0 * math.pi * elapsed_s / period_s + i * 0.7) for i, (center, amplitude) in enumerate(zip(FAKE_CENTER, FAKE_AMPLITUDE)) ] @@ -178,9 +173,10 @@ def __init__(self, source="fake"): self.last_mirror_tick = None self.create_service(Trigger, "~/mirror", self.on_mirror_tick) self.start_time = self.get_clock().now() - # The sine's phase is pinned to FAKE_CENTER once, for the first Mirror - # start. Later restarts continue from wall clock: the twin is then - # holding wherever the sine left it, and rewinding would snap it back. + # The sine's phase is pinned to the mock hardware's start pose once, + # for the first Mirror start. Later restarts continue from wall clock: + # the twin is then holding a pose the sine has already reached, and + # rewinding to the start pose would snap it back. self.sine_phase_pinned = False self.timer = self.create_timer(1.0 / publish_rate_hz, self.publish_once) self.get_logger().info( diff --git a/src/so101_sim/test/test_so101_arm_bridge.py b/src/so101_sim/test/test_so101_arm_bridge.py index b6180739e..2057cbe6b 100644 --- a/src/so101_sim/test/test_so101_arm_bridge.py +++ b/src/so101_sim/test/test_so101_arm_bridge.py @@ -36,6 +36,7 @@ import pytest import rclpy +import yaml from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from std_srvs.srv import Trigger @@ -43,7 +44,6 @@ sys.path.insert(0, str(Path(__file__).parents[1] / "script")) from so101_arm_bridge import ( # noqa: E402 - FAKE_CENTER, JOINT_NAMES, So101ArmBridge, fake_positions, @@ -52,6 +52,13 @@ ) +def mock_start_positions(): + """The pose config/initial_positions.yaml puts the mock hardware in.""" + path = Path(__file__).parents[1] / "config" / "initial_positions.yaml" + initial = yaml.safe_load(path.read_text())["initial_positions"] + return [initial[name] for name in JOINT_NAMES] + + def test_to_radians_applies_offset_then_sign(): # 90 deg with a 10 deg offset and a flipped sign is -80 deg. assert to_radians([90.0], [-1.0], [10.0]) == pytest.approx([math.radians(-80.0)]) @@ -90,8 +97,9 @@ def test_fake_positions_stay_inside_the_urdf_limits(): period = 12.0 moved = [False] * len(JOINT_NAMES) first = fake_positions(0.0, period) - # The mock hardware starts at FAKE_CENTER, so the sine has to as well. - assert first == pytest.approx(FAKE_CENTER) + # The mock hardware is configured to start where the sine does, or the + # twin jumps on the first published point. + assert first == pytest.approx(mock_start_positions()) for step in range(241): positions = fake_positions(step * period / 240.0, period) for index, name in enumerate(JOINT_NAMES): @@ -245,9 +253,9 @@ def publish_status(status): def test_mirroring_starts_the_sine_at_the_mock_start_state(ros_context): """The twin must not snap when the Mirror Objective starts. - The mock hardware sits at FAKE_CENTER until something drives it, so the - first point published after the first ever mirror start has to be that same - pose however long the bridge has been up before it. + The mock hardware sits at its configured start pose until something drives + it, so the first point published after the first ever mirror start has to be + that same pose however long the bridge has been up before it. """ received = [] bridge = So101ArmBridge(source="fake") @@ -263,7 +271,7 @@ def test_mirroring_starts_the_sine_at_the_mock_start_state(ros_context): executor.add_node(listener) # Age the node up to where an unpinned sine sits near its peak, so a - # missing reset misses FAKE_CENTER by far more than the tolerance below. + # missing reset misses the start pose by far more than the tolerance below. deadline = time.monotonic() + 3.0 while time.monotonic() < deadline: executor.spin_once(timeout_sec=0.02) @@ -280,7 +288,9 @@ def test_mirroring_starts_the_sine_at_the_mock_start_state(ros_context): bridge.destroy_node() assert received, "mirroring should start publishing once ticked" - assert received[0].points[0].positions == pytest.approx(FAKE_CENTER, abs=0.1) + assert received[0].points[0].positions == pytest.approx( + mock_start_positions(), abs=0.1 + ) def test_real_source_is_still_a_stub(ros_context): diff --git a/src/so101_sim/waypoints/so101_waypoints.yaml b/src/so101_sim/waypoints/so101_waypoints.yaml index 37ae6df12..09e83aeac 100644 --- a/src/so101_sim/waypoints/so101_waypoints.yaml +++ b/src/so101_sim/waypoints/so101_waypoints.yaml @@ -20,11 +20,11 @@ - gripper position: - 0.0 - - 0.6 - - -0.6 - - 0.0 - - 0.0 - - 0.7 + - 0.728843537 + - -0.353637568 + - 0.302123278 + - 0.33498815 + - 0.489530063 velocity: [] multi_dof_joint_state: header: From 91520000ff32d6142e079db0a64c15f4f4fc74df Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 9 Sep 2026 20:46:35 -0400 Subject: [PATCH 7/8] no-mistakes(review): align SRDF home and Home description, declare python3-yaml --- src/so101_sim/config/moveit/so101.srdf | 8 ++++---- src/so101_sim/package.xml | 1 + src/so101_sim/waypoints/so101_waypoints.yaml | 4 ++-- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/so101_sim/config/moveit/so101.srdf b/src/so101_sim/config/moveit/so101.srdf index 4adbb23b2..740bab375 100644 --- a/src/so101_sim/config/moveit/so101.srdf +++ b/src/so101_sim/config/moveit/so101.srdf @@ -18,10 +18,10 @@ /> - - - - + + + + diff --git a/src/so101_sim/package.xml b/src/so101_sim/package.xml index 59cd00dbf..5a98eaf87 100644 --- a/src/so101_sim/package.xml +++ b/src/so101_sim/package.xml @@ -34,6 +34,7 @@ ament_flake8 ament_lint_auto picknik_ament_copyright + python3-yaml rclpy diff --git a/src/so101_sim/waypoints/so101_waypoints.yaml b/src/so101_sim/waypoints/so101_waypoints.yaml index 09e83aeac..798b47791 100644 --- a/src/so101_sim/waypoints/so101_waypoints.yaml +++ b/src/so101_sim/waypoints/so101_waypoints.yaml @@ -1,5 +1,5 @@ -- description: Folded rest pose. Values are the mock hardware start state, not a taught - hardware pose. +- description: The pose the mock hardware starts in, which is the bridge's fake sine + at elapsed 0, not a taught hardware pose. favorite: true joint_group_names: - gripper From f48e802d0ff66c9750d650a8dc3c1fa6fcd4ee56 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 9 Sep 2026 22:48:52 -0400 Subject: [PATCH 8/8] fix(so101_sim): add the missing BSD-3 copyright notice to LICENSE The BSD-3 text requires redistributions to retain "the above copyright notice", but the file had none above it. Adds the header in the same form picknik_accessories/LICENSE uses, with the package's BSD-3-Clause tag. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01XM46CmXBMTVr3dRZQqmcb1 --- src/so101_sim/LICENSE | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/so101_sim/LICENSE b/src/so101_sim/LICENSE index 574ef0790..d379a5e63 100644 --- a/src/so101_sim/LICENSE +++ b/src/so101_sim/LICENSE @@ -1,3 +1,8 @@ +BSD 3-Clause License + +Copyright (c) 2026 PickNik Inc. +All rights reserved. + Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: