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
Open
Optional 1-DoF jaw on the simulated gripper, to measure how the planner treats it#39Nick Hehr (HipsterBrown) wants to merge 35 commits into
Nick Hehr (HipsterBrown) wants to merge 35 commits into
Conversation
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
This was referenced Aug 25, 2026
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
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_jawconfig attribute todevrel:so101:simulated-gripper(defaultfalse, so nothing changes unless you opt in) and uses it to answer the question.What the measurement found
TestPlannerJawTravelplans the same move across 10 random seeds, twice: once unobstructed, once with a wall forcing the RRT to search.Reading
armplanningpredicts the opposite.computeJointSensitivitiesdivides by the change a joint makes to goal distance, so a joint that changes nothing yields+Infand 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.Composereturns a quaternion, so the jaw mount link shipped"type":"quaternion". rdk parses that fine, but the viewer does not render it correctly. Every orientation inso101.jsoniseuler_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 theKinematics()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.defaultExecuteEpsilonis inert on the normalMove()path, which passesmath.MaxFloat64. It only applies via theexecuteCheckStartplan/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 (tcpandgripper_moving) requireoutput_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.GoToInputsvalidation incomponents/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_positionmust stay non-blocking. Teleop issues one every tick. There is a regression test.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 vetclean,-raceclean oncomponents/simulated. The planner test adds ~37s and is skipped under-short.Verified on a live machine: model renders correctly,
GoToInputsdrives 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 (eachCurrentInputsis 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