Skip to navigation

Moves all six SO-101 joints with a sine target. Mock and real modes run at 100 Hz; SimpleMotionPlannerOp clamps each command to the measured position ± max_step_size.

ModeBehavior
mockSynthetic joint state and command validation
simIsaac Sim over UCX; separate graph without the motion planner
realFeetech joint-state reads and position commands
profile_realBounded servo speed/acceleration diagnostic

See application modes for clock and I/O differences.

Benchmark

Run

Run from the repository root:

./isaac_os build so101_sinus_motion
./isaac_os modes so101_sinus_motion

Mock

./isaac_os run so101_sinus_motion mock

The default run stops after 100 validated commands, about one second at 100 Hz. For a longer run:

./isaac_os run so101_sinus_motion mock \
--run-args='--message-count=11000'

--message-count is the number of completed sink commands, one per joint-state sample, so 11,000 commands take about 110 s at 100 Hz. The default timeout is 10 seconds plus three times the expected duration; override it with --completion-timeout-seconds.

Measure latency and cadence with Profile an Application and the app Benchmark.

Architecture

Mock and real share this graph:

Joint state Joint state Joint command Clamped command Joint-state source100 Hz SinusMotionOp SimpleMotionPlannerOp Command sink

Real mode creates one driver shared by source and sink. Its command edge uses a depth-one, drop-oldest queue. See the shared I/O contracts and real graph.

  • Joint-state source - Mock mode publishes a synthetic 100 Hz state. Real mode reads the Feetech bus at the same period (state_source.period: 0.01).

  • SinusMotionOp - For each of the six joints:

    target = default_pos + amplitude * sin(2 * pi * frequency * t)

    Each incoming joint state triggers command generation.

  • SimpleMotionPlannerOp - Receives the sine target and the current measured state. Each tick, every joint command is clamped to [measured - max_step_size, measured + max_step_size]. The bound is relative to measured position, not the previous command.

  • Command sink - Mock mode validates the clamped command. Real mode writes the command to the Feetech servos; see shutdown for hold behavior.

Configuration

Mock and real parameters: config/so101_sinus_motion_config.yaml.

KeyDefaultMeaning
state_source.period0.01Joint-state period in seconds (100 Hz).
sinus_motion_op.sin_frequency0.25Sine frequency in Hz (one cycle every 4 s).
sinus_motion_op.sin_amplitude0.20Amplitude in radians about default_pos.
sinus_motion_op.num_joints6Joint count (all six SO-101 joints, including gripper).
motion_planner_op.max_step_size0.20Maximum per-tick joint delta in radians, vs measured state.
real_robot.serial_port/dev/ttyACM0Feetech USB serial device.
real_robot.calibration_fileplaceholder JSON pathLeRobot follower calibration file.
real_robot.position_p_gain / _i_gain / _d_gain16 / 0 / 32Servo PID; I = 0 matches the LeRobot defaults.
real_robot.run_speed / run_acceleration1500 / 60Servo-side limits used for graph commands; see the units and special zero values below.
real_robot.home_position[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]Homing pose, equal to the configured sine mean.

real_robot keys match so101_reach.

Tip: For jerky motion, check max_step_size against amplitude and frequency. Very small steps may leave the arm nearly still: Feetech servos often ignore tiny offsets.

The gripper uses a different calibration convention from the other joints: coordinate 0 is its calibrated closed endpoint. Because the one scalar sine offset is applied to all six joints, the default gripper command opens during the positive half-cycle and its negative half-cycle clamps at the closed limit. The demo therefore does not sweep the gripper’s full calibrated range.

Servo gains and motion profile

❓ How do gains and motion limits affect the arm?

Sinus motion emits absolute, time-based targets, so it does not need the nonzero integral gain used by the reach application’s measured-plus-delta action law. Keeping position_i_gain at zero also avoids integral wind-up causing the servos to hunt around a fixed hold target after shutdown.

Real mode homes the arm to the all-zero sine mean before starting the graph. Homing uses its own fixed profile (400 encoder steps/s and acceleration 50), so changing real_robot.run_speed or real_robot.run_acceleration does not change the initial homing move. Gravity-loaded joints cannot meet the four-count homing gate with a proportional-only loop, so the driver writes I = 16 for that move and restores real_robot.position_i_gain (default 0) before the graph runs. A configured I gain of 16 or higher is left unchanged.

For graph commands, run_speed is measured in encoder steps/s (0 means maximum speed). A positive run_acceleration value is approximately 100 encoder steps/s² per unit; a higher value permits sharper acceleration, while 0 selects instant/unlimited acceleration. The default sine needs only about 205 encoder steps/s and 322 encoder steps/s² at its peaks. A continuously replaced sine target is not a reliable visual test of those limits. Use the bounded profile_real diagnostic to compare measured responses to isolated targets, keeping every other setting unchanged.

Simulation configuration

Sample YAML: sim/config/so101_sinus_motion_sim_config.yaml.

  • sim_so101.hostname / port - UCX peer (Sim). Defaults 127.0.0.1:50008.
  • sim_so101.joint_state_flatbuffer_buffer_size - receiver tagRecv buffer for the JointState FBS blob. Increase it if you extend the schema.
  • sim_so101.enable_camera - Enable the image_receiver + Holoviz chain. Defaults true; set to false for headless runs or scenes without UCXCameraHelper / UCXPublishImage OGN nodes.
  • sim_so101.camera_tensor_buffer_size - device-buffer size the image receiver preallocates for the second (tensor) tagSend on the image tag. Must be at least width * height * channels; default 640 * 480 * 3 = 921_600 (RGB).
  • sim_so101.image_flatbuffer_buffer_size - receiver tagRecv buffer for the Image metadata FBS blob.
  • sinus_motion_op.num_joints - number of joints on the arm (SO-101 = 6).
  • sinus_motion_op.sin_frequency, sin_amplitude - sine frequency in Hz and amplitude in radians.
  • sinus_motion_op.default_position, kp, kd - a scalar or single-element list for broadcast, or a length-num_joints list.

Wire format

Wire-compatible with Isaac Sim’s isaacsim.ucx.core publishers/subscribers. FBS schemas: isaac.JointState, isaac.JointCommand (and isaac.Image when enable_camera: true). FNV-1a tag hashes: see sim/main.cc. Joint envelopes reference the SDK’s holoscan.schema.Tensor. Boundary codecs translate their bytes to/from CPU vectors; wire timestamps become graph capture time. Camera messages use SDK ImageT with an explicit CUDA device binding.

Implementation notes

❓ How is the simulation graph scheduled?
  • The graph has connection, joint_state_rx and control_and_tx partitions, plus image_transport and holoviz when enable_camera: true.
  • UCX connects after graph startup so early receive notifications are accepted.
  • Commands go directly to the sink because the sine amplitude is bounded. Teleoperation or learned policies should add a motion planner.
  • Scheduling uses RealtimeClock. Sine phase and graph capture time use the received JointState timestamp, with no local-clock fallback.

Measure servo speed and acceleration

Complete real-hardware setup first. The app build includes this diagnostic for comparing isolated targets rather than a continuously replaced sine.

./isaac_os run so101_sinus_motion profile_real \
--run-args="--config-path=build/so101-sinus-real.yaml --output=build/profile.csv"

The output path must be new. The arm first performs the normal homing move, then shoulder pan moves +0.20 radians from its measured starting position and back, three times. The other five joints retain their measured starting targets. Each target is written once and held for three seconds while position and velocity are sampled about every 10 ms. This takes 18 seconds after homing. The diagnostic bypasses the sine generator and measured-position clamp so neither can continually replace the servo’s target. It uses the same driver and configured run speed, acceleration, and PID gains as real sinus mode. Sine amplitude, frequency, planner step size, and state-source period do not affect this fixed diagnostic.

Keep the shoulder-pan path clear, including its configured home position plus 0.20 radians. The driver’s calibrated joint limits still apply; a clamped target invalidates a profile comparison. Normal completion, SIGINT/SIGTERM after homing, and errors request a measured-position hold with torque enabled. If feedback or communication prevents the hold, the diagnostic retries up to three times. If all attempts fail, it exits with a warning that motion may continue. Keep clear and switch off follower power before handling the arm. The unpowered arm can fall; torque is not automatically disabled because that can drop the arm. An initialization failure removes the newly created empty CSV so the same output path can be retried. Initialization retains the standard driver’s homing and failure behavior described above.

Run separate CSV captures with only the indicated setting changed:

ComparisonFixed settingValues to compare
Accelerationrun_speed: 1500run_acceleration: 1 and 0
Speedrun_acceleration: 0run_speed: 50 and 1500

Each CSV row contains the zero-based phase, elapsed seconds since its target write, target radians, measured shoulder-pan radians, and measured velocity in radians/s. Compare the time to first reach within 0.02 radians of each target across the six phases, as well as the position/velocity traces. Confirm every phase reaches and holds its target; do not interpret an incomplete or interrupted capture as a pass. The diagnostic fails if a phase ends more than 0.02 radians from its target. Homing is excluded from the CSV. A low positive acceleration should take longer than unlimited acceleration, and a low positive speed should take longer than a high speed. Higher positive acceleration does not mean a smoother ramp.

Shutdown

❓ How does cancellation hold position?

In real mode, Ctrl+C requests a measured-position hold with torque enabled. The driver rejects later motion commands after the hold is requested.