Skip to content

Optional 1-DoF jaw on the simulated gripper, to measure how the planner treats it - #39

Open
Nick Hehr (HipsterBrown) wants to merge 35 commits into
mainfrom
gripper-inputs
Open

Optional 1-DoF jaw on the simulated gripper, to measure how the planner treats it#39
Nick Hehr (HipsterBrown) wants to merge 35 commits into
mainfrom
gripper-inputs

Conversation

@HipsterBrown

Copy link
Copy Markdown
Contributor

Why

The gripper's kinematic model has always been 0-DoF, with a comment explaining that a jaw DoF would become a variable the motion planner is free to drive. That was a reasonable guess, but nobody had measured what the planner actually does with a joint that changes collision geometry while having no effect on the goal pose.

This adds an articulated_jaw config attribute to devrel:so101:simulated-gripper (default false, so nothing changes unless you opt in) and uses it to answer the question.

What the measurement found

TestPlannerJawTravel plans the same move across 10 random seeds, twice: once unobstructed, once with a wall forcing the RRT to search.

scenario jaw travel
direct (2-step plans) 0.0000 rad on every seed
obstructed (305-412 steps) 4.3%-19.9% of range, both directions, 8/10 seeds ending near their start

Reading armplanning predicts the opposite. computeJointSensitivities divides by the change a joint makes to goal distance, so a joint that changes nothing yields +Inf and gets clamped to its full range. That full search freedom is real, but it does not turn into full travel — IK ignores the jaw entirely, and the RRT uses it as slack and mostly puts it back.

The test asserts only structural facts (the trajectory has a gripper column, the jaw stays in limits) and logs the magnitudes, because the magnitudes were the unknown.

Also fixes a rendering bug that affects real users

While testing on hardware, the gripper rendered wrong in the 3D viewer. Root cause: spatialmath.Compose returns a quaternion, so the jaw mount link shipped "type":"quaternion". rdk parses that fine, but the viewer does not render it correctly. Every orientation in so101.json is euler_angles, which is why the arm always looked right. Converting is lossless (measured delta 0.000e+00 rad).

Two related documentation corrections, both verified against a live machine:

  • Geometries() does not feed the 3D viewer. The frame system takes the Kinematics() branch for grippers and never calls it. A prior comment claimed otherwise, and that claim was part of why the 0-DoF design looked acceptable.
  • defaultExecuteEpsilon is inert on the normal Move() path, which passes math.MaxFloat64. It only applies via the executeCheckStart plan/execute split, so the mid-travel-jaw hazard does not abort ordinary arm moves.

Where to focus review

  • internal/geometry/gripper.go — the branching model. Two leaves (tcp and gripper_moving) require output_frames, which keeps the TCP invariant in jaw angle while the mesh swings. SVA joint limits are degrees while the repo's constants are radians; the test asserts the parsed limits for that reason.
  • GoToInputs validation in components/simulated/simulated_gripper.go — validates every step before moving any, and tolerates a limit overshoot of a few ULP because rdk rejects even one.
  • set_position must stay non-blocking. Teleop issues one every tick. There is a regression test.
  • The hardware gripper is untouched and still always 0-DoF.

Every guard in this PR was mutation-tested: broken deliberately, confirmed to fail with the intended message, reverted. That turned up a real pre-existing hole — the shipping gripper's frozen jaw angle could have been wrong by the full range of the joint with every test still passing, because the only assertion compared pose origins, which are invariant under this joint's rotation.

Testing

go test ./... green, go vet clean, -race clean on components/simulated. The planner test adds ~37s and is skipped under -short.

Verified on a live machine: model renders correctly, GoToInputs drives the jaw to commanded angles, and the viewer animates on open/grab.

Follow-up

A design for the same flag on the hardware gripper is in docs/superpowers/specs/2026-08-24-hardware-articulated-jaw-design.md. It needs decisions this PR did not: caching (each CurrentInputs is a serial round trip and the viewer polls it), read-failure behavior (an error there breaks the whole machine's frame system), and refusing planner jaw commands while holding a part. That will be stacked on this branch.

🤖 Generated with Claude Code

Nick Hehr (HipsterBrown) and others added 30 commits August 23, 2026 22:00
Spec for making the simulated SO-101 gripper InputEnabled behind an
articulated_jaw config flag, with a branching kinematic model whose TCP
stays invariant while a revolute jaw joint drives the moving mesh.

The goal is measurement, not a feature: characterise what the motion
planner does with a DoF that changes collision geometry but has no
effect on the goal pose.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
The central prediction was inverted. computeJointSensitivities computes
startDistance / |myDistance - startDistance| -- inverse sensitivity, so
a joint that does not move the TCP divides by zero, yields +Inf, and
clamps to 1.0: its FULL range, not a minJog floor.

Also: keep set_position non-blocking (teleop calls it every tick), split
tests across packages to respect the internal/* dependency rule, source
the un-composed jaw mesh from gripperMeshPLY so the equivalence test is
not tautological, and add the missing config/wiring/instrumentation
tests.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Twelve TDD tasks: shared bijection, jawDoF threading, branching model
topology, equivalence + serialization guards, config flag, non-blocking
set_position split, input wiring, instrumentation, the epsilon hazard,
the planner measurement, and docs.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
- Task 3's jaw-movement assertion could never pass: gripperMovingVisualPose
  is a pure +Z translation and the joint rotates about +Z, so the mesh pose
  ORIGIN is invariant. Assert orientation delta and transformed vertices.
- Task 11's goal was physically unreachable (testGoal is never actually
  planned to anywhere in this repo), so the experiment would have passed
  green having measured nothing. Derive the goal from FK, add an obstructed
  scenario so the RRT actually searches, and require a successful plan.
- Task 11 needs go mod tidy before the new armplanning import compiles.
- gripper.Gripper does embed framesystem.InputEnabled; drop the wrong note.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Measured on the 0-DoF stand-in; with the jaw DoF and its geometry in the
frame system the search roughly doubles to ~300-420 steps.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
JawAngleFromPct -> JawRadiansFromPct, JawPctFromAngle -> JawPctFromRadians,
across the helpers, both call sites, and the plan/spec.

components/gripper/gripper.go:381 already carries a different percent->radian
mapping (servo shaft over +/-pi) sixty lines from the new call site, and this
repo names units everywhere else (MaxAccRads, JointTravelsDeg, StepsPerDegree).

Also documents that JawPctFromRadians saturates rather than validates, and
drops the doc comments' roster of callers that do not exist yet.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Adds require.Greater on GripperJointMax/Min (a zero-range joint would divide
to NaN and pass through the clamp untouched), an independent midpoint inverse
check, tighter clamp overshoots, and a components/simulated test asserting the
fully-open pose matches BuildGripperMeshes at GripperJointMax -- the prior
"not almost equal" check alone would not have caught the deleted /100.0.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Review found it a strict subset of two pre-existing tests that already cover
the jawDoF:false contract more strongly.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
TestZeroDoFModelUnchanged was a strict subset of the jawDoF: false
coverage already in TestBuildGripperModelPlacesFrameAtTCP and
TestBuildGripperModelCarriesMeshes (which assert stronger absolute
counts). Removed it and recorded where the jawDoF: false contract
lives instead. Also noted the new jawDoF parameter in
BuildGripperModel's doc comment, which still described a signature
without it.
BuildGripperModel's jawDoF now actually branches the SVA chain: a static
jaw_mount -> revolute jaw_joint -> gripper_moving branch carries the jaw
mesh, while a separate static branch reaches the tcp leaf, so the TCP stays
invariant across the jaw's full range. output_frames pins tcp as the
model's Transform since two leaves require it. Joint limits are written in
degrees (SVA convention) from the existing radian constants.

BuildGripperMeshes' body-mesh and moving-mesh construction is factored out
into buildGripperBodyMeshes/buildGripperMovingMesh so BuildGripperModel's
jawDoF and non-jawDoF paths share them. The jawDoF: false path is verified
byte-identical (OriginalFile.Bytes hashes unchanged for both gripper types
at both mesh detail levels).
Extract movingMeshPose so the frozen (jawDoF: false) branch and
BuildGripperMeshes share one source of truth for the moving mesh's pose,
instead of a second inlined copy of the same Compose chain. Mutation-tested:
flipping the frozen angle from GripperJointMin to GripperJointMax (shipping
a gripper frozen fully open) left the suite green until
TestBuildGripperModelCarriesMeshes gained an orientation comparison
alongside its existing position check -- a point-only check can't catch
this because the moving mesh's pose origin is invariant under the Yaw
rotation.

Delete gripperJawLinkFrame; appendJawBranch now names the jaw mesh link
from moving.Label(), so both topologies name it identically by
construction rather than by two "gripper_moving" literals staying in sync.

Rename BuildGripperModel's parent to bodyLeaf after the static-mesh loop,
documenting it as the fork point both the tcp link and appendJawBranch
hang off. Trim buildGripperMovingMesh's doc comment (drop a forward
reference to a Task 4 test that doesn't exist yet). Move a doc sentence
that's true in both jawDoF modes out of the false-only bullet, and note
that tcp's parent link differs by mode (poses are identical; the
frame-system node tree the 3D viewer draws is not).

Also add an orientation check to TestArticulatedGripperModelShape's
TCP-invariance loop -- a motion goal is a full pose, not just a point.
The jaw pose is now computed by two independent routes -- the frame system
composes it from the jaw joint (collision geometry) and BuildGripperMeshes
composes it by hand (the 3D viewer). Pin them to agree across the full
jaw range so the two never drift apart.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
A component ships its kinematics as ModelConfig().OriginalFile.Bytes, so a
jaw joint that only exists in the in-memory model would transmit as a 0-DoF
gripper -- the planner would never see the DoF, and every in-process test
would still pass. Round-trip through the real gRPC (de)serialization to
close that gap, mirroring TestURDFModelSurvivesSerialization for the arm.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
rdk rejects inputs exceeding a joint limit by a single ULP. Task 4's angle
sweep hit exactly that via left-associative (range*i)/steps.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Off by default: robot/framesystem's CurrentInputs hard-errors on any
InputEnabled==false component whose model has DoF, so wiring the flag on
without CurrentInputs/GoToInputs support would break the whole machine's
frame system. Task 8 wires the inputs; the "articulated" subtest here is
skipped until then.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
…ival

set_position must return before the jaw arrives (teleop.go issues one every
tick); Open/Grab/the upcoming GoToInputs need to block. TestSetPositionStaysNonBlocking
pins the non-blocking half; verified by mutation that routing set_position
through moveTo makes it fail (blocked ~500ms instead of <50ms).

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
CurrentInputs reports the jaw angle via geometry.JawRadiansFromPct.
GoToInputs validates every step of a batch before moving any of them, so a
batch that fails partway leaves the jaw where it started rather than
stranded mid-batch (verified by mutation: validating-and-moving inline
leaves the jaw at the first step's angle when the second step is rejected).

Validation tolerates a jawLimitEpsilon (1e-6 rad) beyond each joint limit:
rdk rejects an input 1 ULP outside a limit, and a planner trajectory riding
a limit can produce exactly that via left-associative float math (as seen
in Task 4's angle sweep). geometry.JawPctFromRadians already saturates to
0/100, so an epsilon-tolerated overshoot can never drive the jaw past its
physical stop.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Review follow-up on the Task 6-8 landing:

1. Stop() set targetPct = currentPct, which made an in-flight awaitArrival
   indistinguishable from arrival, so GoToInputs silently carried on to
   the next batch step (and could finish the whole batch) after a stop.
   Added a `stopped` bool mirroring the arm's simOperation.stopped: Stop()
   sets it only while moving, awaitArrival checks it before the arrival
   comparison and returns an error, and it is cleared once per commanded
   move (moveTo, or once per GoToInputs batch -- not per step, or a stop
   during one step would be forgotten by the next).

2. GoToInputs now awaits arrival only on a batch's final step. Intermediate
   targets are waypoints the interpolator passes through anyway; blocking
   on each cost a full 10ms tick per step for no benefit (measured: a
   50-step batch of negligible motion went from ~500ms to ~10ms).

3. jawLimitEpsilon's comment overstated its precision by nine orders of
   magnitude ("a few ULP" for a 1e-6 value that is ~4.5e9 ULP wide).
   Reworded to say what actually bounds the jaw: JawPctFromRadians's
   saturation to 0/100, not the epsilon's width.

4. Guarded against indexing limits[0] on an empty DoF slice: the arity
   check alone (len(step) != len(limits)) passes when both are 0, so a
   caller passing an empty step would have panicked. Added an explicit
   len(limits) != 1 check and hoisted jawLimits := limits[0].

Both correctness fixes verified by mutation: removing the stopped check
made TestGoToInputsAbortsOnStop fail with "error is expected but got nil"
(GoToInputs silently reported success after a mid-batch Stop); reverting
GoToInputs to await every step made TestGoToInputsAwaitsOnlyFinalStep fail
at 500.209ms against a 250ms bound (vs. ~10ms today).

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Records each GoToInputs batch (step count, total travel, Linf from
current) in a small ring buffer, logs it via the constructor's
previously-unused logger, and exposes it through a new
get_jaw_trajectory DoCommand. Also extends get_position with
jaw_angle_rad and dof.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
services/motion/builtin/builtin.go rejects a plan whose FIRST
trajectory step differs from CurrentInputs by more than
defaultExecuteEpsilon (0.01), aborting the whole move -- the arm's
move -- when it does. A jaw still travelling from an earlier Open()
is enough to trip it. This is a documentation test: it pins a
property of rdk's contract, not of this module's code.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
TestJawTrajectoryDoCommand took 11.6s because the batches issued just to
fill the ring buffer past jawTrajectoryHistory each swept the full jaw
range (~500ms at gripperTravelPctPerSec) x ~12 batches. Only the very
first batch needs real travel, to assert total_travel_rad > 0; the rest
now oscillate by a negligible 0.0005 rad near the current position so
GoToInputs's await-final-step-only behavior lets them complete almost
instantly. 11.60s -> 0.72s. No assertions changed.
Records the Task 12 experiment result for the simulated gripper's
articulated_jaw flag: the planner never moves an unconstrained jaw on
unobstructed plans, but moves it 4%-20% of its range under obstruction
(usually settling back near its start), per TestPlannerJawTravel.

docs/simulated.md: add the articulated_jaw attribute row and prose
covering the measured behavior, the execute-epsilon hazard a moving
jaw can trip on the arm's own move (services/motion/builtin/builtin.go:633),
and that jaw inputs are radians end to end.

CLAUDE.md: amend the gripper's "0-DoF on purpose" gotcha to note the
opt-in 1-DoF path is simulated-gripper-only; add gotchas for SVA's
degrees-vs-radians joint limits (referenceframe/frame_json.go:152),
computeJointSensitivities' inverse-sensitivity full-range clamp plus
the measured travel nuance, and rdk's 1-ULP bounds rejection. Corrected
the sensitivity gotcha's cited line to linearized_frame_system.go:80
(computed against go.mod's pinned rdk v1.0.0; the prompt's :78 pointed
at the wrong line).
spatialmath.Compose returns a *Quaternion, so NewOrientationConfig emitted
"quaternion" for jaw_mount -- the only link in either gripper model carrying
an orientation, and the link the whole jaw branch hangs off. rdk parses
quaternion and euler_angles identically (frame-system geometry poses verified
byte-equal either way), but the 3D viewer rendered the jaw branch splayed.
Every orientation in so101.json is euler_angles and the arm renders correctly.

The conversion is lossless: orientation delta measured at 0.000e+00 rad.

TestJawMountOrientationIsEuler pins the encoding rather than the value, and
fails with a quaternion.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Verified on a live machine: GetGeometries swings the jaw the full 110deg
between Grab and Open while the viewer does not move. local_robot.go:1362
takes the Kinematics branch for grippers and never calls Geometries(), so a
0-DoF model renders frozen regardless. Also records the euler_angles finding.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Converts a 0-100 percentage to radians using GripperJointMin/Max and sends
GoToInputs. The gripper API passes raw float64 radians with no degree
conversion, unlike the arm API.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Confirmed live after the euler_angles fix: the viewer polls GetCurrentInputs
and animates. My earlier note said it did not -- the jaw was moving, it was
just rendering splayed. Also records that gripper inputs are joint radians,
not a normalized 0-1.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Behind the same articulated_jaw flag, default false. Decides three things
the simulated version never had to: a 50ms position cache invalidated on
every write (the viewer polls CurrentInputs continuously, and each call is
a serial round trip), last-known-good on read failure (an error would abort
every arm move on the machine), and rejecting planner jaw commands while
holding (measured 4-20% jaw travel under obstruction would loosen a grip).

Requires implementing IsHoldingSomething, currently a stub that always
reports not-holding.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
1. Unconditional reject-while-holding would have broken pick-and-place.
   builtin.go skips a component in a trajectory step only when len(inputs)
   == 0, never when unchanged -- so a 1-DoF gripper is in EVERY step of
   EVERY plan carrying its current value (TestPlannerJawTravel's direct
   scenario shows exactly this, 0.0000 travel). Scope the rejection to
   batches that actually move the jaw, and skip no-op batches entirely.

2. Invalidating the cache under g.mu self-deadlocks: Open/Grab lock g.mu
   then call moveToPercent. Separate jawMu, never held across a DoCommand.

3. Invalidating in moveToPercent misses set_position's raw form, which
   calls servoDo directly. Invalidate in servoDo instead.

Also: only the final step's target is issued (an STS3215 tracks the latest
command), GoToInputs must set isMoving or Stop can never latch, and the
test harness needs two changes the spec claimed it did not.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Corrects a false claim (back-driving is bounded by the TTL like any other
read -- invalidation only shortens staleness), fixes a test row that still
described the superseded N-servo_move design, widens Scope to the two
packages the jawLimitEpsilon move touches, and adds the generation counter
needed because jawMu is released across the bus read (Stop clears isMoving
before issuing servo_stop, so the isMoving guard does not cover it).

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Move() passes math.MaxFloat64 as the execute epsilon (builtin.go:260); the
0.01 default applies only via the executeCheckStart plan/execute split. The
mid-travel-jaw hazard is real on that path but does NOT abort an ordinary
arm move, which is how docs/simulated.md and CLAUDE.md read after task 12.

TestExecuteEpsilonTripsOnMovingJaw's assertion is unaffected -- the Linf
distance does exceed 0.01 -- only the stated consequence was too broad.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
…ies to

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
…t shape

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016jMMQDWSEV2hC9ue1yPfYs
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant