SO-101 Reach Task
Moves the SO-101 fingertip through a grid of 3-D goals using a policy trained in Isaac Lab. The policy produces four joint-position deltas at 50 Hz; the motion controller bounds commands against measured joint positions at 100 Hz in mock and real modes.
- Positions are in radians; the Feetech driver converts encoder values.
- Wrist-roll and gripper stay at fixed positions. Gripper zero is calibrated closed.
See application modes for clock and I/O differences.
Run
Run from the repository root. All modes need a CUDA-capable GPU; the build downloads the policy model.
TensorRT builds and caches the engine beside the ONNX on first use, keyed by GPU and TensorRT version. No graph commands are issued during that build; real mode homes the arm first.
Mock
Isaac Sim
Real robot
Mock
The default run stops after 100 commands (about one second at 100 Hz), with a 30 s
timeout. Without CUDA it exits 77, which test_mock_so101_reach treats as skipped.
For a longer run:
Mock validates temporal contracts, inference and command bounds. Its constant
pose does not respond to commands, so it cannot validate reaching. Runs longer
than policy.goal_hold_time log expected goal timeouts.
Architecture
All modes share the policy and motion controller. Mock and real modes use four operators. Sim replaces the state source with a UCX state receiver and a state adapter, replaces the command sink with a UCX command sender, and adds an operator that opens the simulator connection after startup.
- State source:
So101StateSourceOpreads positions and velocities everystate_source.period, with a monotonicsource_sequenceand afault_epochincremented on bus-read failure.MockStateSourceOpemits a constant pose at the same rate. - Policy:
So101ReachFusedOpcombines the goal grid, fingertip FK, 20-D observation and TensorRT inference. It computes FK locally without publishing link poses.OnClocktriggers it everypolicy.inference_period;LatestWithin{inference_period}withSkipskips inference on late or missing state. - Motion controller:
So101DeltaMotionControlOprunsOnEachstate sample: 100 Hz in mock and real modes, the simulator’s step rate in sim. It reads deltas withLatestWithin{motion_control.target_expiry}andSafeDefaulton missing, holding and re-clamping the accepted target against fresh state on every tick. - Command sink:
So101CommandSinkOpwrites servo positions and latches a present-position hold on cancellation, rejecting later commands.stop()requests the same idempotent hold.MockCommandSinkOpchecks shape and finiteness, logs latency every tenth command and counts completed commands for--message-count.
Each operator has its own partition on RealtimeClock. In real mode, all four
edges use depth-one drop-oldest connections, avoiding state-source backpressure
and queued commands during shutdown.
Staleness and fault handling
The controller drops its target and emits nothing if:
fault_epochchanges;source_sequenceregresses; or- the target exceeds
motion_control.target_expiry.
The servos then retain their last goal. An accepted delta must match the measured fault epoch, reference a sequence no newer than the measured state, and be strictly newer than the last accepted delta. There is no separate planner or watchdog.
Observation vector (20-dim)
The fused operator fixes this layout in code. At startup it requires float32
obs of shape [1, 20] and float32 actions of shape [1, 4]. Different
observation terms require a code change.
The policy omits joint velocity because the Feetech speed readings are too noisy for training.
Action vector (4-dim)
Indices 0-3 are shoulder_pan, shoulder_lift, elbow_flex, and wrist_flex. Actions are
deltas against the measured joint position. The policy operator clips them, the controller bounds
the target it derives from them, and the controller bounds the emitted command again every tick:
Re-anchoring on measured means the per-tick bound holds even for a joint measured outside the
training articulation’s range. Joints 4-5 are written from motion_control.fixed_joint_positions.
JointCommandCpuT also carries velocity, effort, kp, and kd arrays; the controller fills them
with zeros to preserve the message invariant, and the Feetech sink ignores them because the servos
are position-controlled.
Goal sequence
policy.goal_grid_n{x,y,z} and the three policy.goal_pos_*_range pairs build a uniformly spaced
grid, iterated in Z/Y/X order - by default a 1×3×3 grid of nine goals in the y–z plane at
x = 0.20 m. A goal counts as reached once the fingertip stays within policy.reach_threshold for
policy.goal_settle_time; leaving the threshold before then cancels the settle. Outside the
threshold, policy.goal_hold_time is the per-goal budget, after which the operator logs a timeout
and advances. After the last goal the sequence wraps and logs how many goals were achieved.
Configuration
Parameters live in config/so101_reach_config.yaml.
The real_robot block matches so101_sinus_motion
and is read straight from the YAML before the graph compiles.
Policy model
The build downloads the delta policy from
nvidia/so101-reach on Hugging Face. Training,
I/O contract, and license terms can be found on HF.
To run your own policy, set policy.onnx_path to a file outside
data/grip_reach_delta_policy_onnx/, which the build overwrites.
Policy contract: Use a policy trained for joint-position deltas. Tensor-shape validation does not check the action law; a shape-compatible absolute-action policy would produce incorrect commands under this delta controller.
Command-line options
Tests
This covers config decoding, the bounded mock graph, simulation boundary checks and simulation graph compilation. The operators test FK, the fixed observation, goal grid and delta control law. Graph compilation does not establish reach convergence.
Benchmark capture
See Profile an Application for source-to-sink and path-level statistics from an Nsight Systems capture of these binaries.
Hardware notes
Servo gains
❓ Why does reach use integral gain?
The servo position loop uses real_robot.position_p_gain, _i_gain and _d_gain.
With measured + delta commands, the scaled, clamped delta is the servo’s position
error. At LeRobot’s default I = 0, gravity-loaded joints can stall: re-anchoring
each command to measured position prevents the error from accumulating.
shoulder_pan may track while shoulder_lift and elbow_flex barely move.
Reach therefore uses position_i_gain: 16. Raise it gradually; integral wind-up
causes overshoot around a held target.
Servo speed
real_robot.run_speed is in encoder steps/s, at π/2048 rad per step. The default
1500 limits speed to about 2.3 rad/s. Both command bounds
(action_max_step_size / inference_period and motion_max_step_size / state_source.period)
allow 5 rad/s at their defaults, so servo speed limits motion first. Keep the two command
bounds consistent: a safety limiter tighter than the policy’s trained step reshapes normal
motion instead of only catching faults.
Shutdown
❓ How does the hold prevent further motion?
Closing the serial port alone would leave the servos driving toward their last
measured + delta target. Ctrl+C and runtime exceptions request a measured-position
hold; the driver then rejects later motion commands.
The hold sets integral gain to zero while retaining P/D gains and torque, preventing delayed integral corrections.