Approach-axis orientation planning via PoseCloud#28
Open
Nick Hehr (HipsterBrown) wants to merge 23 commits into
Open
Approach-axis orientation planning via PoseCloud#28Nick Hehr (HipsterBrown) wants to merge 23 commits into
Nick Hehr (HipsterBrown) wants to merge 23 commits into
Conversation
Spec for re-adding orientation to MoveToPosition on the 5-DOF SO-101 by attaching a referenceframe.PoseCloud to the goal, replacing the blanket "position_only" goal metric with a configurable approach-axis cone. Planner semantics were established empirically against rdk v1.0.0 rather than from documentation, and three findings shaped the design: - PoseCloud.Theta encodes the AZIMUTH of a tilt, not roll, so Theta leeway must be 180. Below that the Theta check rejects tilts before the cone is consulted. Consequence: roll cannot be constrained alongside tilt, so the feature is approach-axis planning with free roll, not 6-DOF orientation. - defaultEpsilon (0.001) is added to every leeway, not just zeroed ones. Zero X/Y/Z therefore demands a 1-micron match, and the effective cone is acos(cos(tol) - 0.001) -- always slightly wider than requested. - OZ alone defines the cone, valid across [0, 180], saturating at 177.4374. Also bisected the minimum viam-server version to 0.127.0: v0.125-v0.126 read GoalCloud in armplanning but never decode it from proto, so the cloud is silently inert there. Bundles an RDK bump to v1.0.0, verified to build, test, and vet clean with zero source changes. Co-Authored-By: Claude Opus 4.8 (1M context) <[email protected]> Claude-Session: https://claude.ai/code/session_01PguhuvXbVmNHzUWzYyJFaD
Thirteen TDD tasks: RDK bump first as its own commit, then goal_cloud.go built up incrementally, then both arm models wired to it, then docs and a manual release step for the Registry min-server constraint. Also corrects the spec's ReferenceFrame section. PoseCloud.ReferenceFrame existed in rdk v0.123.0 but was REMOVED in v1.0.0, which the bump lands -- so referencing it would not compile. v1.0.0 keeps the struct's reference-frame doc comment as free-floating prose with no field attached, which reads as though the field survives. The complete v1.0.0 field set is X, Y, Z, OX, OY, OZ, Theta. Co-Authored-By: Claude Opus 4.8 (1M context) <[email protected]> Claude-Session: https://claude.ai/code/session_01PguhuvXbVmNHzUWzYyJFaD
Needed for referenceframe.PoseCloud support: v0.123.0 has the type but no PoseInCloud, and its ProtobufToPoseInFrame silently drops GoalCloud on decode. Builds, tests, and vets clean with no source changes.
Test the construction warnings (they were the untested production path), use utils.DegToRad instead of a local duplicate, and stop the saturation warning from citing a threshold that contradicts when it fires. Also amends Task 6's wrapMoveErr to name both tolerances: too small a position_tolerance_mm fails exactly like too tight a cone, so naming only orientation_tolerance_deg pointed at the wrong knob half the time.
poseCloudSaturationCos silently assumes coneToPoseCloud maps the cone to OZ = 1-cos(a). Task 3's other tests use literal degrees and would not notice if that formula changed, leaving resolveGoalCloudConfig warning at the wrong angle. Adds a test deriving the angle FROM the constant so the two cannot drift.
Both the spec and the plan asserted that any Theta leeway below 180 rejects tilts before the cone is consulted. Measurement disproves it: Theta=170 accepts a tilt about X, and even Theta=0 accepts a tilt about Y. A smaller Theta does not reject tilts -- it carves out a wedge of tilt DIRECTIONS. That mattered because the whole threat model is a future reader simplifying Theta: 180 to 0. A guard comment a skeptic can falsify in five minutes licenses the very edit it forbids. Replaced with the argument that survives testing: measured Theta = 90 - azimuth exactly, independent of tilt magnitude, so azimuth 270 reports -180 and only 180 admits every azimuth -- only 180 is isotropic.
Characterization tests, not guards: each overwrites the field it names, so a mutation to coneToPoseCloud is a no-op for them. TestConeBoundsTiltIsotropically and TestConePositionalBoxNotRadius are what guard the mapping. These earn their place by pinning the underlying rdk contracts -- ovX.Theta==90 / ovY.Theta==0 is asserted nowhere else -- so an rdk upgrade that changed Theta semantics fails here with a precise message instead of eight vague acceptance mismatches. Co-Authored-By: Claude Opus 4.8 (1M context) <[email protected]> Claude-Session: https://claude.ai/code/session_01PguhuvXbVmNHzUWzYyJFaD
Zero leeway does not make a cloud never match -- measured, it accepts an exact match and offsets up to 0.0009mm, rejecting at 0.0011mm. What it actually does is demand a match within 1 micron that no IK solution will realistically achieve. Same failure mode as the Theta comment: a claim a skeptic disproves in one line undermines the invariant it exists to protect. Also fixes attribution: azimuth sweeps [0, 360); it is the REPORTED Theta that sweeps [-180, 180].
…es claims The Task 5 test used x==y and ox==oy, so swapping those setters survived the entire suite (verified by mutation). Distinct values now, all seven asserted. Also removes the last 'never matches' overstatements -- including one in Task 11's README text, which would have shipped the claim to users right after it was purged from the code.
pathCone sat at iota's zero value, so buildMoveDestination's error return (nil, nil, 0, err) was indistinguishable from the cone path. A caller who typo'd OX for ox would have been told to widen tolerances they never set and audit their viam-server version -- the worst misdirection possible in a design with no fallback, where the error message is the entire UX. Also drops the both-keys error's false claim that goal_metric_type makes the planner ignore goal clouds. GetGoalMetric consults PoseInCloud regardless; it is specifically position_only's orientScale=0 that makes a cloud meaningless.
Rebasing onto main pulled in PR #27, which adds manual (hand-guided) teach mode and modifies arm.go's MoveToPosition to call exitManualLocked -- a motion command must take the arm out of manual mode. Task 9 said 'replace arm.go:482-504 entirely' with a body that has no such guard, which would have silently dropped that integration. The replacement body now keeps it, and all Tasks 8-10 line references are replaced with symbol names, since PR #27 shifted arm.go by ~180 lines. Also makes explicit that MoveToPosition must return buildMoveDestination's error unwrapped: the path is pathInvalid, and wrapping it would blame the cone.
…nfig Task 8: adds the user-facing config surface for the goal-cloud orientation planning added in Tasks 2-7. Both SO101ArmConfig and SO101SimulatedArmConfig gain OrientationToleranceDeg/PositionToleranceMM fields, validated via a new shared validateGoalCloudTolerances helper in goal_cloud.go (called from each model's Validate). MoveToPosition wiring is deferred to Tasks 9/10.
The comment claimed a literal degree check would miss [177.4374, 177.44). Wrong: the exact threshold is acos(-0.999) = 177.4374412669, so the band is [177.4374412669, 177.44). 177.4374 rounds DOWN, below the threshold -- at exactly 177.4374 the cone does not saturate and an antipodal orientation is rejected (verified against real rdk). That truncation is precisely why the constant is a cosine: any rounded degree literal is wrong in one direction or the other. The docs now say so.
wrapMoveErr was entirely unguarded: replacing its switch with `return err`, gutting either message, swapping which path emits the cone wording, or bypassing it from both models all left the suite green. captureMotion always returned success, so no test ever drove a failing Move. That matters because this design deliberately has no fallback -- a failed plan fails loudly, which makes the message the entire UX. It now gets the same pinning Theta: 180 got. Also softens an overstatement: GetGoalMetric consults PoseInCloud regardless of metric type, so position_only does not make a cloud meaningless -- it makes the cloud's orientation leeways stop mattering.
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
1 participant
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.
What this changes
Both arm models (
devrel:so101:arm,devrel:so101:simulated) defaultedMoveToPosition's planner goal metric toposition_only, discarding orientation entirely — a blunt workaround for the SO-101 being 5-DOF, where most six-DOF pose goals are unreachable.They now attach a
referenceframe.PoseCloudto the goal: an approach-axis cone constraining where the tool points, with roll free. Strictly better than the status quo —position_onlyfreed both the axis and the roll; this pins the axis.Also bumps
go.viam.com/rdkv0.123.0 → v1.0.0 (its own leading commit, zero source changes).What it does NOT do
This is not 6-DOF orientation planning, and that limitation is forced rather than chosen.
PoseCloud.Thetaencodes the azimuth of a tilt, not roll — measured exactly asTheta = 90 - azimuth, independent of tilt magnitude. Since reportedThetasweeps the full[-180, 180], only a leeway of 180 admits every azimuth, and at 180 roll is unconstrained. No value ofThetacan bound roll while permitting isotropic tilt.Behavior change (please read)
The cone is ON by default (30°), and it is stricter than
position_only. Goals that plan today may fail after this lands. That is deliberate — the point is to plan orientation again — and there is no fallback: a failed plan fails loudly.Per-call escape hatch to restore the old behavior:
{ "goal_metric_type": "position_only" }Requires viam-server >= 0.127.0. Older servers accept and silently ignore goal clouds, which makes planning revert to strict six-DOF scoring and fail. RDK v0.125.0–v0.126.1 read
GoalCloudinarmplanningbut never decode it from proto; v0.127.0 is the first with both halves.New config (both arm models)
orientation_tolerance_deg0means the default, not exact.position_tolerance_mmsqrt(3)× this (~1.73mm).New
extrakeys:goal_metric_type(caller's metric wins, no cloud) andpose_cloud(raw cloud, lowercase keysx,y,z,ox,oy,oz,theta). Passing both is an error.Verification approach
Every load-bearing value was established by running code against real rdk v1.0.0, not read from documentation — which proved actively misleading. Three findings drove the design, all now recorded in
CLAUDE.md:Thetamust be exactly 180. Anything less silently carves out a wedge of tilt directions (not, as one might assume, rejecting tilts outright —Theta: 0still accepts a 5° tilt about Y).defaultEpsilon = 0.001is added to every leeway, not just zeroed ones. ZeroX/Y/Ztherefore demands a match within 1 micron, so positional leeway is mandatory; and the enforced cone is always slightly wider than requested:acos(cos(tol) - 0.001).OZalone defines the cone, valid across [0, 180], saturating atacos(-0.999).A cloud the solver can't realistically land inside is worse than no cloud — planning reverts to strict six-DOF scoring and fails. The first mapping I wrote would have failed 100% of moves for exactly this reason.
Tests
39 tests. The suite asserts real behavior against rdk rather than freezing constants, and the load-bearing guards were mutation-verified lethal:
Theta: 180 → 0fails 8 tests; dropping the constructor wiring, bypassingwrapMoveErr, or mis-routing its branches each fail their guard.TestGoalCloudSurvivesSerializationguards the module-boundary gotcha — the cloud is only useful if it crosses gRPC, and an in-process check would pass even if it never shipped.Follow-ups (not in this PR)
meta.json(no such field) or the deploy workflow — it's a manual post-publish step. If it's skipped, users hit precisely the silent-ignore failure this design exists to make loud.position_tolerance_mmon hardware. The 1.0mm default is a starting point, not a derived value: its ~1.73mm worst-case corner is comparable to the arm's own repeatability rather than safely below it.NewSO101'sgoalCloudwiring is untested (needs serial hardware); the simulated equivalent is covered and guards the shared contract. Documented inCLAUDE.md.Design and plan:
docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md,docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md.🤖 Generated with Claude Code
https://claude.ai/code/session_01PguhuvXbVmNHzUWzYyJFaD