Skip to navigation

SO-101 Reach Task

View as Markdown

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.
ModeExecutableBehavior
mockmock_so101_reachConstant state; validates inference and commands without moving a robot
realso101_reach_realReads and commands the arm through the Feetech bus
simso101_reach_simReceives simulated state and sends bounded position commands over UCX

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.

./isaac_os build so101_reach
./isaac_os modes so101_reach

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_os run so101_reach 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:

./isaac_os run so101_reach mock --run-args='--message-count=6000 --completion-timeout-seconds=120'

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.

StateSampleCpuT StateSampleCpuT PolicyDeltaCpuT JointCommandCpuT State sourceOnClock · 100 Hz So101ReachFusedOpFK + goals + TensorRT · OnClock 50 Hz So101DeltaMotionControlOpdelta → bounded target · OnEach state Command sinkOnEach command
  • State source: So101StateSourceOp reads positions and velocities every state_source.period, with a monotonic source_sequence and a fault_epoch incremented on bus-read failure. MockStateSourceOp emits a constant pose at the same rate.
  • Policy: So101ReachFusedOp combines the goal grid, fingertip FK, 20-D observation and TensorRT inference. It computes FK locally without publishing link poses. OnClock triggers it every policy.inference_period; LatestWithin{inference_period} with Skip skips inference on late or missing state.
  • Motion controller: So101DeltaMotionControlOp runs OnEach state sample: 100 Hz in mock and real modes, the simulator’s step rate in sim. It reads deltas with LatestWithin{motion_control.target_expiry} and SafeDefault on missing, holding and re-clamping the accepted target against fresh state on every tick.
  • Command sink: So101CommandSinkOp writes servo positions and latches a present-position hold on cancellation, rejecting later commands. stop() requests the same idempotent hold. MockCommandSinkOp checks 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_epoch changes;
  • source_sequence regresses; 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)

IndexContent
0-5Joint position relative to policy.default_joint_position. Slots 4-5 (wrist_roll, gripper) are forced to zero: both joints were held at their defaults during training
6-8Fingertip position (xyz) from closed-form FK
9-15Goal pose: position (xyz) plus orientation quaternion (wxyz), fixed at identity
16-19Last action (the 4-D arm deltas, after clipping)

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:

clipped = clip(action, -action_clip, +action_clip) # fused op, 50 Hz
target = clamp(measured + clipped * action_scale,
joint_pos_limits_lower, joint_pos_limits_upper) # controller
target = clamp(target, measured ± action_max_step_size)
command = clamp(target, measured ± motion_max_step_size) # every 100 Hz 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.

KeyDefaultMeaning
policy.onnx_pathdata/grip_reach_delta_policy_onnx/policy_fingertip_delta.onnxPolicy ONNX; TensorRT builds the engine on first run.
policy.inference_period0.02Policy tick period in seconds (50 Hz).
policy.action_clip1.0Symmetric clip applied to the raw policy output.
policy.default_joint_position[0, 0, 0, 1.57, 0, 0]Training defaults subtracted for joint_pos_rel.
policy.goal_pos_x/y/z_range[0.20, 0.20], [-0.112, 0.112], [0.136, 0.264]Goal grid extent in meters; keep it within the training command ranges.
policy.goal_grid_nx/ny/nz1 / 3 / 3Goals per axis; at most 100 per axis.
policy.goal_hold_time10.0Seconds before an unreached goal times out.
policy.goal_settle_time1.0Seconds within tolerance before a goal counts as reached.
policy.reach_threshold0.05Fingertip-to-goal tolerance in meters.
state_source.period0.01Joint-state period in seconds (100 Hz).
motion_control.action_scale0.2Radians per unit of clipped policy output.
motion_control.action_max_step_size0.1Bound on a new target vs. measured position, in radians; set it to the max_step_size the policy trained with.
motion_control.motion_max_step_size0.05Per-tick bound on the emitted command vs. measured position.
motion_control.target_expiry0.04Seconds before the held target is discarded (two policy periods).
motion_control.fixed_joint_positions[0.0, 0.0]Held wrist_roll and gripper positions.
motion_control.joint_pos_limits_lower / _upper±1.92, ±1.75, ±1.69, ±1.66Per-joint limits for the four actuated joints.
real_robot.serial_port/dev/ttyACM0Feetech USB serial device.
real_robot.calibration_fileplaceholder JSON pathLeRobot follower calibration; a placeholder is rejected at startup.
real_robot.position_p_gain / _i_gain / _d_gain16 / 16 / 32Servo PID registers; see the hardware notes for why I = 16.
real_robot.run_speed / run_acceleration1500 / 60Servo motion profile.
real_robot.home_position[0.0, 0.0, 0.0, 1.5708, 0.0, 0.0]Homing pose written before the graph runs; gripper zero is calibrated closed.
sim_so101.hostname127.0.0.1Isaac Sim UCX listener host for sim.
sim_so101.port50008Isaac Sim UCX listener port; must match the simulator launcher’s --port.

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

OptionModesDefault
--config-pathmock, real, simapplications/so101_reach/config/so101_reach_config.yaml
--message-countmock, real100 in mock; unlimited in real
--completion-timeout-secondsmock30
--message-countsimUnlimited; counts received joint states
--compile-onlysimOff; compile the graph without connecting or running inference

Tests

./isaac_os test so101_reach

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.