arm-lab

A serial-chain manipulator stack whose production algorithms are written from scratch in Rust — no ROS, MoveIt, kinematics library, or off-the-shelf production planner. Independently maintained OxMPL and k are pinned only in separate evaluation targets for cross-implementation baselines.

Evidence index (simulation; sampled where collision claims are involved; no hardware validation): independent IK protocol / report / targets / results / validation · randomized evaluation protocol / report / queries / planning / tracking · independent planner protocol / report / paired CSV / validation · multi-scene report · planning CSV · tracking CSV · attached payload protocol / results · corner-stop protocol / results · robustness report · main CI · citation

The kinematic chain (offsets, joint axes, limits, end-effector site) is extracted from the compiled MuJoCo model — never hand-entered as DH parameters — so the geometry used by the algorithms is guaranteed identical to the one the physics simulates. Forward kinematics, geometric Jacobians, damped-least-squares inverse kinematics, RRT-Connect with shortcutting, and rest-to-rest 7-phase S-curves on every shortcut edge are implemented in this repo. Robot collision checks call mj_collision on interpolated joint-space states; Demo 3 additionally uses explicit, pair-scoped mj_geomDistance queries for an EE-attached cube proxy. The independent k + urdf-rs stack is used only in test/evaluation targets to cross-check FK and IK; no production library path calls it.

Demo 3 (above): IK solves pick and place poses; the joint-space straight line between them violates both the robot predicate and the attached cube's 5 mm pillar margin. Fixed-seed RRT-Connect finds a 6-waypoint sampled-clear carry in 5.4–5.6 ms across two repeated headless runs; per-edge S-curves execute the carry in 5.13 s with 0.0022 rad worst tracking error. Every shortcut corner is a full stop, making position, velocity, and acceleration continuous while keeping joint jerk bounded almost everywhere. The motion is not geometrically blended or time-optimal. The cube is a mocap weld (scripted attach, not contact-rich grasping). Demo 2 (pillar dodge, no object) is in docs/demo2.gif. Demo 1 (IK target sequence) is in docs/demo1.gif.

Numbers (measured, reproducible via cargo test)

Metric	Result
IK success, 1000 random reachable targets (full 6-DOF pose task, home then ≤8 seeded restarts)	97.8%
Same arm-lab cohort, home only / no restart	71.3%
Independent k Jacobian IK, same targets and ≤8 restart starts	71.8%
Restart-enabled union solved by arm-lab or k	98.2%
Mean iterations reported for arm-lab's returned/final attempt	20.7 (excludes preceding failed attempts)
FK vs independent k/urdf-rs chain (random configurations)	agreement to < 1e-9
FK vs MuJoCo body/site poses (random configurations)	agreement to < 1e-9
Geometric Jacobian vs numerical differentiation	agreement to < 1e-5
Demo 1 final Cartesian error per target (after gravity compensation)	≤ 1e-4 m
RRT-Connect, cluttered UR5e (pillar on the pan sweep), 11 seeds	median 6.7 ms (min 3.2, max 13.4)
Same query: tree size → shortcut	64 nodes → 3 waypoints
Planner determinism	identical path given identical seed
Demo 2 corner-stop S-curve time law (seed 20260816, v≤0.55 a≤1.8 j≤8)	1919 samples, 3.836 s, peak joint |qd| 0.550 rad/s
Demo 2 tracking (PD + vel FF + gravity compensation)	worst 0.0020 rad · final goal 0.0002 rad
Timed-trajectory determinism	identical q(t) given identical seed + limits
Demo 3 attached-cube carry (pick→place, seed 20260816)	6 waypoints, 47 sampled states, 0/47 payload-margin violations
Demo 3 sampled minimum cube/environment distance (5 mm threshold)	11.849 mm vs pillar
Demo 3 tracking (full pick-and-place, two repeated headless runs)	worst 0.0022 rad

The 1 s median planning-time exit criterion is met by two orders of magnitude. The IK stress test samples targets from valid random joint configurations; the 2.2% non-converged slice is reported without assigning an untested cause. The solver honors limits by construction (clamped every iteration, asserted in tests).

Independent IK cross-check (simulation)

The pre-result protocol recreates the existing 1,000-target full-pose cohort instead of selecting a new sample. The 97.8% restart-enabled arm-lab control was already known and is labeled non-blind. Removing restarts reduces arm-lab to 713/1,000; the eight deterministic restarts recover 265 additional targets for 978/1,000. Independently maintained k 0.32.0 solves 328/1,000 from home and 718/1,000 with the same eight restart joint vectors.

Among restart-enabled outcomes, both implementations solve 714 targets, arm-lab alone solves 264, k alone solves four, and neither solves 18; the union is 982/1,000. The exact two-sided sign/McNemar calculation on 268 discordant targets is 8.998e-73, labeled descriptive for this single deterministic cohort. Maximum starts and iterations per start are matched, but update rules, damping, early stopping, and per-iteration work differ, so this is not a compute or runtime ranking. Every success is rechecked through arm-lab FK at <1e-4 m and <1e-3 rad within compiled limits. The complete targets, 4,000 result rows, generated report, and validation record retain all failures and the four independent-only recoveries. Targets are reachable by construction but collision-unaware and not Cartesian-uniform; the evidence supports no hardware, workspace-wide, or universal solver claim.

Robustness envelope (simulation)

robustness_bench replays one deterministic, collision-free trajectory across seven plant conditions and four controller ablations: nominal, a rigid tool payload, reduced actuator gains, added damping, command latency, an external torque pulse, and a combined shift. Model-based controllers receive gravity/Coriolis bias only from the unchanged nominal model, not the perturbed plant.

The generated full results and raw CSV state their pass thresholds and limitations. This is a simulation stress test, not hardware validation or a sim-to-real guarantee.

Attached-payload carry check (simulation)

Demo 3 now transforms the compiled 50 mm cube geom from end-effector FK at every carry query and calls MuJoCo's explicit pair-distance API only for cube↔floor, cube↔table, and cube↔pillar. A state is rejected below a declared 5 mm payload margin, while the original robot rule remains a zero-penetration check. Both predicates independently block the 23-sample straight edge. The fixed-seed plan succeeds with 6 shortcut waypoints and 47 densified states; a fresh checker finds zero robot collisions, zero payload-margin violations, and an 11.849 mm minimum sampled cube distance.

The pre-result protocol and result/limitations report preserve the exact pair scope, thresholds, negative outcomes, transform regression, and sampled-only claim boundary. This does not model object slip, grasp uncertainty, fingers, continuous swept volume, or payload-versus-robot self-contact.

Multi-scene, multi-query extension (simulation)

multi_query_bench adds nine fixed scene-query fixtures (three per shipped MJCF scene, six unique joint-pair definitions) while retaining the original declared numeric tracking thresholds. Selected joint pairs are deliberately repeated across scenes to isolate geometry effects. Across five fixed planner seeds per fixture, all 30/30 direct-free and 15/15 obstructed trials succeeded. Executed states are checked after every settling, path, and hold step with a threshold of exactly 0.0 m; a case passes only with zero sampled robot contacts whose signed distance is negative. This is a sampled penetration gate, not a positive-clearance certificate.

Position PD meets the numeric gates in 0/18 tracking cases. Desired-velocity feedforward meets the numeric gates in 14/18 cases and passes the full zero-penetration gate in 13/18. The four sampled-penetration cases are all the retained reverse_cross_workspace negative: 18--54 path steps depending on plant/controller, with 0.030--0.079 mm maximum actual penetration; settling and hold remain penetration-free. The generated report, 45-row planning CSV, and 36-row tracking CSV retain every outcome and include exact joint vectors, seeds, trajectory metrics, per-phase penetration counts/depths and contact identities, pass criteria, and claim boundaries. The fixtures are hand-designed and deterministic, not a sampled task distribution; the results do not estimate hardware or workspace-wide success probability.

Predeclared randomized extension (simulation)

randomized_eval follows a protocol committed before the evaluator and result-bearing runs. It retains the first 100 accepted joint-space query pairs from a frozen generator in each of the three scenes (300 total), conditional only on collision-free endpoints and at least 0.75 rad separation. It does not select on direct-path or planner outcome. Only 83/300 queries have a sampled-clear straight interpolant. The unchanged canonical RRT-Connect configuration solves 138/300 (46.0%, 95% Wilson 40.4--51.7%) and recovers 55 of 217 blocked-direct pairs; all 162 unconnected outcomes remain in the raw artifacts. Five paired seeds on every blocked query produce 271/1,085 successes with the default 0.05 goal bias and 270/1,085 with goal bias removed, showing no material aggregate gain from that field in this frozen sample.

Both nominal controllers replay every successful canonical trajectory. Position PD meets the numeric gates in 0/138 cases. Velocity feedforward meets them in 104/138 (75.4%, 95% Wilson 67.6--81.8%) and passes the added zero-sampled-penetration gate in 90/138 (65.2%, 95% Wilson 57.0--72.7%). Thirty-seven of 276 controller replays contain sampled penetration, with 3.2141 mm maximum depth; these failures are retained rather than hidden. See the pre-result protocol, generated report, and complete query, planning, and tracking CSVs. The generator is uniform in compiled joint limits under the stated conditions, not uniform in Cartesian workspace or representative of real tasks; collision checks remain discrete and all evidence is simulation-only.

Independent planner cross-check (simulation)

An outcome-blind protocol pins independently maintained OxMPL 0.6.0 before comparing its RRT-Connect implementation with the committed in-repository rows on all 217 blocked-direct queries and the same five seeds. The in-repository planner succeeds on 271/1,085 paired trials; OxMPL succeeds on 290/1,085. The paired cells are 271 both, 0 in-repository only, 19 OxMPL only, and 795 neither. The predeclared exact two-sided McNemar/binomial calculation is 0.00000381, but it is descriptive only: five seeds share each query, violating independent-pair assumptions. At the query level, at least one seed succeeds for 56/217 blocked queries in-repository and 58/217 with OxMPL.

Every nominal external path is rechecked with a fresh arm-lab collision checker at 0.05-rad spacing: 290/290 pass, with zero invalid-path outcomes. Two consecutive full runs on the recorded Fedora/Ryzen host agree in every status and path digest after normalizing wall time. This corroborates every in-repository success in the frozen sample and shows limited complementary recovery, but it is not a compute-matched or universal planner ranking: the in-repository artifact uses 2,000 iterations, OxMPL uses a predeclared 250-ms timeout, and the external evaluator consumes the eight-decimal serialized query vectors. The integer seeds and replicate indices are paired, but the two planners use different PRNGs and therefore do not receive identical random samples. See the generated report, complete 1,085-row paired CSV, and validation record.

Layout

crates/arm-lab        the library: chain extraction, FK, Jacobians, DLS IK, RRT-Connect, S-curve, MuJoCo collision
crates/arm-lab-demo   demo1 (IK), demo2 (pillar dodge), demo3 (pick-and-place), Rerun + offscreen GIF
assets/ur5e           vendored MuJoCo-Menagerie UR5e + cluttered / pick-place scenes (see license)

Run it

# library tests (cross-checks, 1000-target IK table, planner correctness + timing)
cargo test -p arm-lab

# demo 1, headless: writes demo_output/demo1.rrd
cargo run --release -p arm-lab-demo --bin demo1

# demo 1 + offscreen GIF
cargo run --release -p arm-lab-demo --bin demo1 -- --render

# demo 2: plan around the pillar, execute, write demo_output/demo2.rrd
cargo run --release -p arm-lab-demo --bin demo2

# demo 2 + offscreen GIF
cargo run --release -p arm-lab-demo --bin demo2 -- --render

# demo 3: pick-and-place around a pillar, write demo_output/demo3.rrd
cargo run --release -p arm-lab-demo --bin demo3

# demo 3 + offscreen GIF
cargo run --release -p arm-lab-demo --bin demo3 -- --render

# stream live into a Rerun viewer started with `rerun`
cargo run --release -p arm-lab-demo --bin demo3 -- --connect

# deterministic controller-ablation × plant-shift matrix; write Markdown + CSV
cargo run --release -p arm-lab-demo --bin robustness_bench -- --write

# three scenes × three fixed queries; write planning/tracking CSVs + report
cargo run --release -p arm-lab-demo --bin multi_query_bench -- --write

# rerun the bounded matrix and verify deterministic committed fields/outcomes
cargo run --release -p arm-lab-demo --bin multi_query_bench -- --check

# predeclared 300-query randomized evaluation; write or verify all raw outcomes
cargo run --release -p arm-lab-demo --bin randomized_eval -- --write
cargo run --release -p arm-lab-demo --bin randomized_eval -- --check

# independent external RRT-Connect baseline; full pinned-host repeat or fast artifact audit
cargo run --release -p arm-lab-demo --bin oxmpl_baseline -- --check
cargo run --release -p arm-lab-demo --bin oxmpl_baseline -- --verify-artifacts

# independent IK baseline + restart ablation; write or byte-check all 4,000 rows
cargo run --release -p arm-lab --example ik_baseline -- --write
cargo run --release -p arm-lab --example ik_baseline -- --check

Requirements: Rust stable, a C++ toolchain, and (for --render) ffmpeg on PATH. On Linux without system MuJoCo, mujoco-rs auto-downloads MuJoCo 3.9 at build time. Set MUJOCO_DOWNLOAD_DIR to an absolute directory before building and add its downloaded lib/ directory to LD_LIBRARY_PATH before running; see the mujoco-rs docs.

Design notes

• Chain extraction, not re-modeling. Chain::from_mujoco walks body_parentid from the tip body to the world, collecting static transforms, hinge axes, anchors, and limits from the compiled model, plus the EE site as the tool frame. One source of truth for geometry.
• DLS IK with adaptive damping. Each step solves Δq = Jᵀ(JJᵀ + λ²I)⁻¹e with a diagonal nullspace bias toward a rest pose; λ scales down with the error so the endgame converges Newton-like while near-singular regions stay damped. Joint limits are clamped every iteration; seeded random restarts (TRAC-IK style) recover from bad basins.
• RRT-Connect, from scratch. Two trees grow toward each other (Kuffner & LaValle 2000). EXTEND takes one joint-space step; CONNECT greedily repeats it. Edges are collision-checked discretely by interpolating at resolution (0.05 rad L2 by default); the robot predicate calls MuJoCo mj_collision, and Demo 3's carry also calls explicit distance queries for three payload/environment pairs. This is sampled collision checking, not continuous certification. Greedy then random shortcutting removes redundant waypoints; the path is densified to the same resolution for execution. Sampling uses the in-repo SplitMix64 RNG — same seed, same path.
• Collision filter. Only contacts that involve a robot collision geom (contype ≠ 0, attached to a chain body, not the world) count. Floor-vs-pillar contacts are ignored; parent–child pairs are already excluded by MuJoCo. Visual meshes never participate.
• Pair-scoped attached load. AttachedBoxCollisionChecker solves the mocap-body pose so the compiled proxy geom equals T_world_EE * T_EE_proxy, including nonzero geom-local offsets. It queries only constructor-validated environment names and rejects signed distances < 0.005 m in Demo 3. Robot and place-pad geoms are excluded from this scope; no global positive robot threshold is applied.
• Deterministic. Restart sampling, RRT sampling, and random shortcutting all use the in-repo RNG with a fixed seed. The timed trajectory is bit-stable given the same seed and limits (CI golden test).
• Per-edge corner-stop S-curve time law. Every nonzero shortcut edge gets an independent rest-to-rest 7-phase bang-bang-jerk profile. Per-joint (v, a, j) limits are converted using that edge's constant tangent; rounding duration up to the 2 ms simulator grid only slows the curve. Interior waypoints are sampled once at zero velocity and acceleration, so position, velocity, and acceleration are continuous and joint jerk remains bounded almost everywhere. Jerk can jump between finite values; the motion is deliberately full-stop rather than geometrically blended or time-optimal.
• Physics-side servo. The demos apply the exact MuJoCo bias force as gravity/Coriolis feedforward. Position actuators are commanded as a PD tracker with velocity feedforward: ctrl = q_des + (kv/kp)·qd_des yields τ = kp(q_des − q) + kv(qd_des − qd). Worst joint-space tracking is 0.0020 rad (Demo 2) and 0.0022 rad across two repeated attached-load Demo 3 runs.
• Scripted grasp. Demo 3 welds a mocap cube to the EE after the pick descend and parks it on the place pad after the place descend. During carry, that same compiled box is a 5 mm pair-scoped planning proxy against floor, table, and pillar. This remains a kinematics/planning demo, not contact-rich grasping: it does not model slip, fingers, compliance, grasp uncertainty, continuous swept volume, calibration error, or cube-versus-robot self-contact.

Roadmap

• Demo 1: FK + DLS IK, Rerun telemetry, offscreen-rendered GIF
• RRT-Connect with MuJoCo collision checks and shortcutting (Demo 2)
• Position/velocity/acceleration-continuous corner-stop S-curve time law with almost-everywhere jerk bounds; joint-space PD + velocity feedforward
• Pick-and-place with obstacle dodging; benchmark tables
• Reproducible controller robustness matrix with raw CSV and explicit sim-only limits
• Multi-scene, multi-query planning and tracking extension with raw CSVs
• Pair-scoped attached-cube collision proxy for Demo 3 carry
• Predeclared 300-query randomized planning and nominal-tracking evaluation
• Predeclared independent OxMPL comparison on all 217 blocked randomized queries
• Predeclared independent k IK comparison and restart ablation on 1,000 targets

License & assets

Project code: Apache-2.0. The separate planner comparison executable depends on non-vendored OxMPL 0.6.0 under BSD-3-Clause. The test/evaluation-only IK comparison uses non-vendored k 0.32.0 and urdf-rs 0.9.0, both Apache-2.0. The vendored assets/ur5e model and meshes are from MuJoCo Menagerie (UR5e description © 2018 ROS Industrial Consortium, BSD-3; see assets/ur5e/LICENSE), with a local modification adding offscreen buffer size to scene.xml. scene_cluttered.xml and scene_pickplace.xml are original to this repo.

Research users can cite the software using CITATION.cff.