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 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/.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..d379a5e63 --- /dev/null +++ b/src/so101_sim/LICENSE @@ -0,0 +1,30 @@ +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: + + * 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..bcb4551aa --- /dev/null +++ b/src/so101_sim/README.md @@ -0,0 +1,102 @@ +# 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. 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 + +**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..0abcee1e1 --- /dev/null +++ b/src/so101_sim/config/initial_positions.yaml @@ -0,0 +1,10 @@ +# 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.728843537 + elbow_flex: -0.353637568 + wrist_flex: 0.302123278 + wrist_roll: 0.33498815 + gripper: 0.489530063 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..740bab375 --- /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..beb879c08 --- /dev/null +++ b/src/so101_sim/description/assets/LICENSE @@ -0,0 +1,202 @@ + 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. + See the License for the specific language governing permissions and + limitations under the License. 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..5a98eaf87 --- /dev/null +++ b/src/so101_sim/package.xml @@ -0,0 +1,43 @@ + + + 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 + python3-yaml + 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..be74fc1b8 --- /dev/null +++ b/src/so101_sim/script/so101_arm_bridge.py @@ -0,0 +1,283 @@ +#!/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): + """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. + """ + 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 JOINT_NAMES if n not in lookup] + if missing: + raise KeyError(f"missing joints: {missing}") + return [lookup[n] for n in JOINT_NAMES] + + +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. 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) + 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"): + super().__init__("so101_arm_bridge") + + 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() + # 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( + 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 + now = self.get_clock().now() + 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" + 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..2057cbe6b --- /dev/null +++ b/src/so101_sim/test/test_so101_arm_bridge.py @@ -0,0 +1,300 @@ +#!/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 +import yaml +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 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)]) + 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) + # 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): + 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_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 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") + 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 up to where an unpinned sine sits near its peak, so a + # 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) + 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( + mock_start_positions(), abs=0.1 + ) + + +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..798b47791 --- /dev/null +++ b/src/so101_sim/waypoints/so101_waypoints.yaml @@ -0,0 +1,194 @@ +- 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 + - 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.728843537 + - -0.353637568 + - 0.302123278 + - 0.33498815 + - 0.489530063 + 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