From 002a2f7fb06e360a9e0eae541c649f8ff2623e5c Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 12:20:05 -0400 Subject: [PATCH 01/23] docs: design for approach-axis orientation planning via PoseCloud 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) Claude-Session: https://claude.ai/code/session_01PguhuvXbVmNHzUWzYyJFaD --- ...026-07-15-pose-cloud-orientation-design.md | 489 ++++++++++++++++++ 1 file changed, 489 insertions(+) create mode 100644 docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md diff --git a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md new file mode 100644 index 0000000..3394d3f --- /dev/null +++ b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md @@ -0,0 +1,489 @@ +# Approach-axis orientation planning via PoseCloud + +**Date:** 2026-07-15 +**Status:** Approved design, pending implementation plan +**Affects:** `devrel:so101:arm`, `devrel:so101:simulated` + +## Problem + +The SO-101 is a 5-DOF arm. Six-DOF pose targets are reachable only on a 5-dimensional +submanifold of SE(3), so most combinations of target position and target orientation are +unreachable and produce `zero IK solutions produced`. Both arm models work around this by +defaulting the planner's goal metric to `position_only` (`arm.go:491`, `simulated.go:307`), +which matches the target point and accepts whatever orientation falls out. + +That is a blunt instrument: it discards orientation entirely, including the orientation +the arm *could* have honored. We want the planner to respect the requested approach +direction where the geometry allows it. + +## What this delivers (and does not) + +This is **approach-axis planning with free roll**, not 6-DOF orientation planning. +The caller constrains where the tool *points* — a cone around the goal's orientation +axis — and the planner picks the roll about that axis. + +Roll cannot be constrained simultaneously. This is forced by the orientation-vector +parameterization, not chosen; see "Theta encodes azimuth" below. + +It remains a strict improvement over the status quo: `position_only` frees *both* the +axis and the roll; this pins the axis. + +### Why an approach-axis cone suits this arm + +Joints 2-4 pitch about parallel axes, so the tool axis is confined to the vertical plane +set by joint 1's pan, and joint 5 rolls about that axis. For a given target position, the +reachable orientations form a 2-parameter family (in-plane pitch + roll) out of 3. The +missing DOF is out-of-plane tilt. Since the direction of that shortfall is not knowable +in advance, the cone is isotropic. + +## Verified planner semantics + +Established empirically by running Go programs against `rdk v1.0.0`'s +`referenceframe.PoseCloud`, not from documentation. Each item below contradicts a +reasonable reading of the docs, and each is load-bearing. + +### PoseCloud is a scoring shortcut, not a constraint + +`armplanning.PlannerOptions.GetGoalMetric` (v1.0.0, `planner_options.go:177`) consults +`goal.GoalCloud.PoseInCloud(goal.Pose(), &dq)`. Inside the cloud the candidate scores +zero; outside, the full weighted metric applies with `orientScale = 100`. For a goal the +arm cannot reach exactly, landing in the cloud is the *only* path to success. A cloud that +never matches is therefore worse than no cloud at all: planning silently reverts to strict +6-DOF scoring and fails. + +### Theta encodes tilt azimuth, not roll + +`PoseInCloud` compares the *relative* pose, `PoseBetween(goal, candidate)`, where zero +deviation is the orientation vector `(0, 0, 1)`. For a pure tilt with no roll, the +relative `Theta` reports the **azimuth of the tilt**: + +| tilt axis | tilt 5° | tilt 20° | +|---|---|---| +| X | `Theta = 90` | `Theta = 90` | +| Y | `Theta = 0` | `Theta = 0` | +| XY diagonal | `Theta = 45` | `Theta = 45` | + +`Theta` measures roll only when tilt is zero. There is also a parameterization singularity +at identity: a 0.5° tilt about X reports `Theta = 0`, while 5° reports `Theta = 90`. + +Consequence: **`Theta` leeway must be 180.** At anything less, the `Theta` check rejects +tilts by azimuth before the `OX`/`OY`/`OZ` cone is ever consulted — a `Theta: 0` cloud +rejects even a 5° tilt, making the cone useless. And because `Theta` mixes azimuth with +roll, no value of it can bound roll while permitting isotropic tilt. Hence: roll is free. + +### OZ alone defines the cone, across the full 0-180 range + +With `Theta: 180`, the tilt bound is enforced entirely by `OZ`, verified isotropic across +azimuths 0/45/90/135/180/270 (29° accepted, 31° rejected at a 30° cone). `OX`/`OY` set to +`sin(tolerance)` are redundant, so they are set to `1` (unconstrained). This is why the +Viam docs example reads `OX: 1, OY: 1, OZ: 0.1`. + +The mapping stays correct **above 90°**, contrary to the intuition that `OZ = 1 - cos(a)` +reaching 1 makes it degenerate. Verified: + +| cone | `OZ` | behavior | +|---|---|---| +| 90° | 1.000 | 89° accepted, 91° rejected | +| 120° | 1.500 | 119° accepted, 121° rejected | +| 177.40° | 1.998971 | 180° tilt still **rejected** | +| 177.44° | 1.999002 | saturated — every orientation accepted | + +So `orientation_tolerance_deg` is meaningful across `[0, 180]`. + +**Saturation begins at 177.4374°**, not 179. Acceptance is +`|1 - OZ_rel| <= OZ + epsilon` with a maximum possible deviation of 2.0, so everything is +accepted once `1 - cos(a) + 0.001 >= 2`, i.e. `cos(a) <= -0.999`, i.e. +`a >= acos(-0.999) = 177.4374°`. Verified: 177.40 still rejects a 180° tilt; 177.44 +accepts it. At or above that, the cone legitimately means "orientation free" (comparable +to `position_only`). + +### Epsilon is added to every leeway + +`PoseInCloud` tests `math.Abs(between.Point().X) > pc.X + defaultEpsilon` with +`defaultEpsilon = 0.001`. It is **additive to every leeway**, not merely a floor on zeros. +Units are **millimeters** for `X`/`Y`/`Z` and **degrees** for `Theta`. So +`position_tolerance_mm: 1.0` yields a true bound of **1.001mm** — verified: 1.0009mm +accepted, 1.0015mm rejected. + +At zero the same rule demands a match within 1 micron — verified: a 0.01mm offset is +rejected. Positional leeway is therefore mandatory, not optional. + +The same additivity means the **effective cone is always slightly wider than requested**: +the orientation test reduces to `cos(tilt) >= cos(tol) - 0.001`, so + +``` +effective_cone = acos(cos(tol) - 0.001) +``` + +Verified against measured acceptance to four decimals: `0° → 2.5626°`, `1° → 2.7508°`, +`2.6° → 3.6509°`, `10° → 10.3247°`, `30° → 30.1144°`, `90° → 90.0573°`. The widening is +not confined to small values, and the effective cone is never narrower than ~2.5626°. + +### The positional cloud is a per-axis box, not a radius + +`X`/`Y`/`Z` are checked independently against `PoseBetween(goal, candidate)`, i.e. along +the goal frame's axes. `position_tolerance_mm: 1.0` therefore permits a worst-case corner +deviation of `sqrt(3) x 1.001 ≈ 1.73mm`, not 1mm — verified: a per-axis offset of 1.0005mm +(norm 1.7329mm) is accepted. + +### ReferenceFrame is inert and must be left unset + +`PoseCloud.ReferenceFrame` carries the struct's longest doc comment and invites use, but: +`PoseInCloud` never reads it, and `PoseCloud.ToProto()` does not serialize it — +`commonpb.PoseCloud` has no such field (only `x`, `y`, `z`, `o_x`, `o_y`, `o_z`, `theta`). +Any value set is silently dropped at the gRPC boundary and would break the round-trip +test. Leave it zero. + +## The mapping + +```go +&referenceframe.PoseCloud{ + X: posTolMM, Y: posTolMM, Z: posTolMM, // MUST be > 0; 0 means match-within-1-micron + OX: 1, OY: 1, // deliberately unconstrained; OZ alone defines the cone + OZ: 1 - math.Cos(rad(toleranceDeg)), + Theta: 180, // MUST be 180: Theta encodes tilt AZIMUTH, not roll + // ReferenceFrame deliberately unset: inert, and not wire-serialized. +} +``` + +## Config surface + +Both `SO101ArmConfig` (`arm.go:70`) and `SO101SimulatedArmConfig` (`simulated.go:42`) +gain the same two fields, so the simulated model remains a faithful stand-in for +motion-plan testing. + +```go +// OrientationToleranceDeg is the half-angle, in degrees, of the cone of acceptable +// end-effector approach directions around the goal orientation. Roll about that axis +// is NOT constrained. Valid range [0, 180]. Zero or unset means the default, 30. +// +// The planner adds a 0.001 epsilon to the OZ leeway, so the cone actually enforced is +// acos(cos(tol) - 0.001) -- always slightly wider than requested (30 gives ~30.11) and +// never narrower than ~2.56. At or above 177.44 every orientation is accepted. +OrientationToleranceDeg float64 `json:"orientation_tolerance_deg,omitempty"` + +// PositionToleranceMM is the per-axis positional leeway of the goal cloud, applied as a +// box along the goal frame's axes (worst-case corner deviation is sqrt(3) times this). +// It must be large enough for the IK solver to land inside, or the cloud never matches +// and every move fails. Zero or unset means the default, 1.0. +PositionToleranceMM float64 `json:"position_tolerance_mm,omitempty"` +``` + +Defaults: `defaultOrientationToleranceDeg = 30.0`, `defaultPositionToleranceMM = 1.0`. + +`Validate` rejects: +- `orientation_tolerance_deg < 0` or `> 180` +- `position_tolerance_mm < 0` + +Zero and unset are indistinguishable in Go for a `float64` with `omitempty`, and both mean +"use the default". Consequence: **the friendly knob cannot express a near-exact cone** — +`0` is captured by defaulting. Near-exact orientation requires the raw `pose_cloud` +escape hatch. This is accepted; a near-exact cone on a 5-DOF arm fails almost everywhere +anyway (and the epsilon floors it at ~2.56° regardless). + +No upper bound is enforced on `position_tolerance_mm`: large values are legitimate (the +Viam docs' own example uses 75mm). But because a large cloud makes the planner report +success far from the goal — a silent-wrong-answer, the failure mode this design otherwise +avoids — `resolveGoalCloudConfig` logs a warning above 10mm. It also warns when the cone is +saturated and accepts every orientation. + +The saturation check must be implemented as `math.Cos(rad(tol)) <= -0.999`, **not** as a +literal `tol >= 177.44`: saturation actually begins at 177.4374°, so a literal 177.44 +threshold would stay silent across `[177.4374, 177.44)`, which is saturated. The rounded +177.44 figure is for user-facing docs only, where "≥ 177.44 accepts any orientation" is +true as written. + +`Validate` only ever returns errors; it has no logger, which is why these warnings live in +`resolveGoalCloudConfig` at construction instead. + +### On the default of 1.0mm + +`position_only` today drives the solver to converge as tightly as IK allows. A cloud lets +it stop as soon as it is inside, so positional accuracy loosens to the box above: +`sqrt(3) x 1.001 ≈ 1.73mm` worst case. That is *on the order of* the arm's repeatability +(estimated ±2mm; this figure is an unverified estimate, not a datasheet value), not +comfortably below it. + +The default is therefore a **starting point to be tuned empirically during the +experiment**, not a derived value. The geometry is documented so the trade-off is +legible: smaller tolerance means better accuracy but the cloud matches less often, and a +cloud that never matches fails every move. + +## Behavior + +The cone is **on by default** (30°). This is a deliberate behavior change: a cone is +*stricter* than `position_only`, which accepts any orientation, so goals that plan today +may fail after this lands. That is accepted — the point is to plan orientation again. + +There is **no fallback**. A failed cone plan fails the move. One code path, predictable, +and it forces explicit tuning rather than silently degrading. + +### Precedence + +| `extra` contains | Destination | Extras forwarded to planner | +|---|---|---| +| neither key | cone from config | all caller keys verbatim | +| `pose_cloud` only | raw cloud, replacing the cone | all caller keys **except** `pose_cloud` | +| `goal_metric_type` only | **no cloud** | all caller keys verbatim (incl. `goal_metric_type`) | +| **both keys** | — | **error** | + +The `goal_metric_type` row avoids the trap where `position_only` sets `orientScale = 0`, +making any cloud meaningless. It also keeps today's behavior reachable per-call. + +Both keys together is incoherent — a raw cloud plus a metric that ignores clouds — so it +errors rather than silently dropping one. Silent-drop is the exact failure mode this +design exists to eliminate. + +`pose_cloud` is stripped from the forwarded extras because it is not a planner key. +`goal_metric_type` is forwarded because it is one. All other caller keys pass through, +preserving `arm.go`'s existing promise that callers "may override any key". + +### The `pose_cloud` wire format + +`extra` arrives via structpb, so every JSON number is a `float64`. + +Accepted keys, matching `referenceframe.PoseCloud`'s own JSON tags exactly: +`x`, `y`, `z`, `ox`, `oy`, `oz`, `theta` — **lowercase**. All are optional; omitted fields +stay `0`, which per the epsilon rule means ~zero leeway. That is faithful passthrough and +the point of an escape hatch, but it is sharp, and is documented as expert-mode. + +Note the casing is a genuine hazard: the Viam docs example renders these as `OX`/`OY`, and +the protobuf JSON spells them `o_x`/`oX`. Neither is accepted. Unknown keys are therefore +**rejected with an error** rather than ignored, so a typo or a copy-paste from the docs +fails loudly instead of silently producing a 1-micron cloud. `reference_frame` is +rejected for the same reason: it is inert and not wire-serialized. + +## Components + +New file `goal_cloud.go`: + +```go +// goalCloudConfig is the resolved tolerance pair, letting both arm models share one +// code path despite separate config structs. Defaults are already applied. +type goalCloudConfig struct { + OrientationToleranceDeg float64 + PositionToleranceMM float64 +} + +// resolveGoalCloudConfig applies defaultOrientationToleranceDeg / defaultPositionToleranceMM +// to zero or unset values, and emits the two construction warnings (see "Config surface"). +// It is the single owner of both defaulting and those warnings, so each constructor is one +// call and neither model re-implements the thresholds. Validate has no logger, which is why +// the warnings live here rather than there. +func resolveGoalCloudConfig(tolDeg, posTolMM float64, logger logging.Logger) goalCloudConfig + +func coneToPoseCloud(cfg goalCloudConfig) *referenceframe.PoseCloud + +// goalPath reports which precedence row buildMoveDestination took, so the caller can +// wrap failures appropriately without re-inspecting extra (which would duplicate the +// precedence logic in both models and let it drift). +type goalPath int + +const ( + pathCone goalPath = iota // cone from config + pathRawCloud // caller-supplied pose_cloud + pathMetricType // caller-supplied goal_metric_type; no cloud sent +) + +// buildMoveDestination applies the precedence table. +// +// originFrame is the FULLY-FORMED frame name (e.g. "myarm_origin"); this function does +// not append "_origin". It is passed through to NewPoseInFrame unmodified. +// +// On success returns a destination, a NEW extras map (the caller's map is never mutated), +// and the path taken. On error returns (nil, nil, 0, err). +func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalCloudConfig, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, map[string]interface{}, goalPath, error) +``` + +`buildMoveDestination` errors when: +- `extra` contains both `pose_cloud` and `goal_metric_type` +- `pose_cloud` is not a `map[string]interface{}` +- `pose_cloud` contains an unknown key (including `reference_frame`, `OX`, `o_x`, …) +- a `pose_cloud` value is not a `float64`, or is NaN or Inf +- a `pose_cloud` value is negative + +Both models embed `resource.AlwaysRebuild` and have **no `Reconfigure`** — they are +rebuilt on config change. `goalCloudConfig` is therefore resolved once at construction and +never mutated, so it needs no mutex despite `so101` holding an `mu sync.RWMutex` for other +state. + +`MoveToPosition` shrinks to a `buildMoveDestination` call plus the existing `motion.Move`, +collapsing the near-identical bodies currently duplicated across `arm.go` and +`simulated.go` rather than doubling them. The call site keeps today's frame expression: + +```go +dest, planExtra, path, err := buildMoveDestination( + fmt.Sprintf("%v_origin", s.Name().Name), pose, s.goalCloud, extra) +``` + +## Data flow + +``` +arm.MoveToPosition(pose, extra) + └─ buildMoveDestination → (PoseInFrame{pose, GoalCloud}, planExtra) + └─ motion.Move(MoveReq{Destination, Extra}) + └─ PoseInFrameToProtobuf serializes GoalCloud + ═══════════════ module gRPC boundary ═══════════════ + └─ ProtobufToPoseInFrame decodes GoalCloud ← needs viam-server >= 0.127.0 + └─ armplanning GetGoalMetric → PoseInCloud → score 0 inside the cloud +``` + +The module only ever *sends* the cloud; viam-server plans. The module's own RDK version +therefore has no bearing on whether the cloud is honored — that depends solely on the +server. + +### Minimum viam-server version: 0.127.0 + +Established by bisecting RDK releases. Two independent pieces must both be present: + +| RDK version | `armplanning` reads `GoalCloud` | `ProtobufToPoseInFrame` decodes `GoalCloud` | +|---|---|---| +| v0.123.0 – v0.124.0 | no | no | +| v0.125.0 – v0.126.1 | **yes** | no | +| **v0.127.0** and later | yes | **yes** | + +The v0.125–v0.126 band is the trap: the planner consults a field that proto never +populates, so the cloud is silently inert. **v0.127.0** is the first release where the +full path works — verified by diffing `ProtobufToPoseInFrame`, which gains +`result.GoalCloud = PoseCloudFromProto(proto.GetGoalCloud())` in v0.127.0 and lacks it in +v0.126.1. + +viam-server releases track RDK version numbers, so this maps to viam-server **0.127.0**. +The implementer should confirm that release exists before publishing the constraint. + +Enforced by declaring the minimum Registry-side. Nothing lands in `meta.json`: its schema +(`cli/module.schema.json`) defines no version-constraint property, so this is **not** a +code or workflow change. + +That makes it a **manual post-publish step**, not something `.github/workflows/deploy.yml` +or `viamrobotics/build-action` can carry: after the release uploads the version, set the +minimum viam-server version on the module's Registry page. The plan must include it as an +explicit release-checklist item, or the constraint silently never gets applied and users +land in exactly the silent-ignore failure this design is trying to make loud. + +## RDK bump to v1.0.0 + +Bundled into this work, as its own commit ahead of the feature so a bisect can separate +"the upgrade broke it" from "the cone broke it". + +Verified in a throwaway worktree: `go build ./cmd/module/ .`, `go test ./cmd/module/ .` +(`ok so_arm 2.721s`), and `go vet` all pass with **zero source changes**. + +`go.mod` moves: + +| dep | from | to | +|---|---|---| +| `go.viam.com/rdk` | v0.123.0 | v1.0.0 | +| `go.viam.com/api` | v0.1.539 | v0.1.566 | +| `go.viam.com/utils` | v0.4.19 | v0.6.6 | +| `go` directive | 1.25.1 | 1.25.9 | + +Nothing in CI pins a Go version, and 1.25.1→1.25.9 is a patch-level move within 1.25.x +that modern toolchains auto-resolve. Low risk, but the plan must verify CI rather than +assume. + +The bump's payoff is testing: v0.123.0 has the `PoseCloud` type but no `PoseInCloud`, and +its `ProtobufToPoseInFrame` silently drops `GoalCloud` on decode. v1.0.0 has both, so cone +semantics and the proto round-trip become directly assertable. + +`CLAUDE.md`'s header pins `go.viam.com/rdk v0.123.0` and must be updated. + +## Error handling + +Fail-loudly makes the error message the entire UX. The confusing failure mode: on an +outdated viam-server the cloud is dropped, the planner reverts to strict 6-DOF scoring, +and essentially every move fails with no hint why. + +The cone-specific wrapping applies **only on the cone path**, because on the other two +paths it would misdirect — telling a caller who just passed `position_only` to pass +`position_only`, or blaming a config tolerance that had no bearing on a raw-cloud failure. +The `goalPath` value returned by `buildMoveDestination` selects the wrapping, so the +precedence logic is never re-derived at the call site: + +| `goalPath` | error | +|---|---| +| `pathCone` | wrapped with the tolerance and all three remedies (below) | +| `pathRawCloud` | wrapped noting the caller-supplied cloud; no tolerance cited | +| `pathMetricType` | returned unwrapped — today's behavior | + +```go +// pathCone only +return fmt.Errorf( + "move to position failed with orientation_tolerance_deg=%.1f (approach-axis cone): %w; "+ + "widen the tolerance, pass extra {\"goal_metric_type\": \"position_only\"} to ignore "+ + "orientation, or check that viam-server is >= 0.127.0 (older servers ignore goal clouds)", + tolDeg, err) +``` + +The effective cloud is logged at debug on each move. + +## Testing + +`goal_cloud_test.go` — pure, no hardware, no motion service: + +- **Cone semantics via `PoseInCloud`** (available post-bump), asserting real behavior + rather than freezing `OZ == 0.13397`: + - 30° cone: 29° accepted, 31° rejected, across azimuths 0/45/90/135/180/270. + - Above 90°: cone 90 → 89 accepted / 91 rejected; cone 120 → 119 accepted / 121 + rejected. + - Saturation boundary: cone 177.40 → 180° tilt **rejected**; cone 177.44 → 180° + accepted. Guards the exact `acos(-0.999)` threshold against drift. + - Effective cone widening: `acos(cos(tol) - 0.001)`, asserted against measured + acceptance for 0 → 2.5626, 2.6 → 3.6509, 30 → 30.1144, 90 → 90.0573. + - Roll free: rolls 0/45/90/179 accepted at zero tilt. + - Tilt+roll combined (verified values): at a 30° cone, `tilt 20` and `tilt 29` accepted + at rolls 0/70/170; `tilt 31` rejected at rolls 0/70/170. Roll does not shift the tilt + bound. + - Positional box at `posTol = 1.0`: 1.0009mm accepted, 1.0015mm rejected (the additive + epsilon); per-axis 1.0005mm on all three axes accepted at norm 1.7329mm (box, not + radius). +- **Regression guards** encoding *why* the mapping looks as it does, so a future + "simplification" fails loudly: + - `Theta: 0` makes a 30° cone reject a 5° tilt. + - Zero positional leeway rejects a 0.01mm offset. +- `resolveGoalCloudConfig` defaulting when zero/unset, and leaving non-zero values alone. +- The full four-row precedence table, including that both keys together errors, that + `pose_cloud` is stripped while `goal_metric_type` is forwarded, that the caller's + `extra` map is not mutated, and that each row returns the expected `goalPath`. +- `pose_cloud` parsing: lowercase keys accepted; `OX`/`o_x`/`reference_frame`/unknown keys + rejected; non-`float64`, NaN, Inf, and negative values rejected. +- `Validate` rejecting `orientation_tolerance_deg` `< 0` and `> 180`, and + `position_tolerance_mm < 0`. + +**Proto round-trip**: `PoseInFrameToProtobuf` → `ProtobufToPoseInFrame` preserves +`GoalCloud` field-for-field. Directly guards the CLAUDE.md module-boundary gotcha — that a +component ships over gRPC and in-memory state does not survive. In-process assertions +would miss it. + +**Both models**: `MoveToPosition` sends a destination carrying the expected `GoalCloud`, +using `testutils/inject/motion.MotionService`'s +`MoveFunc func(ctx, req motion.MoveReq) (bool, error)` to capture the `MoveReq`. The repo +has no existing `MoveToPosition` tests or motion mocks, but the inject helper exists on +the pinned version, so this needs no new infrastructure. + +## Docs + +- **README**: `orientation_tolerance_deg` and `position_tolerance_mm` in both arm model + sections, each stating that roll is unconstrained, that the positional tolerance is a + per-axis box (corner ≈ `sqrt(3)` times the value), that `0` means the 30° default rather + than exact, that the enforced cone is `acos(cos(tol) - 0.001)` and so always slightly + wider than requested, that `>= 177.44` means orientation-free, and that goal clouds + require **viam-server >= 0.127.0**. Document the `pose_cloud` escape hatch with its exact + lowercase keys and the warning that docs-style `OX` casing is rejected. +- **CLAUDE.md**: update the RDK version, and add a gotcha covering `Theta`-encodes-azimuth + (forcing `Theta: 180`), `OZ`-alone-defines-the-cone (valid across 0-180, saturating at + 177.44), the additive `0.001` epsilon and the ~2.56° effective-cone floor it implies, + that `ReferenceFrame` is inert and unserialized, and that goal clouds need viam-server + >= 0.127.0 (v0.125-v0.126 read the field but never decode it). Without it, the next + reader will "simplify" `Theta: 180` to `0` and break every move. + +## Out of scope + +Deliberately excluded; all available later, none needed to try this: + +- Version detection or a fallback ladder (cone → wider cone → `position_only`). +- Separate `x`/`y`/`z` leeway knobs on the friendly surface. +- A shared-config refactor unifying the two arm models beyond `goalCloudConfig`. +- Constraining roll via a separate `OrientationConstraint` in `Constraints`. From 3e438ad621177bda2e1fcc17d504b3585f208d6a Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 12:42:25 -0400 Subject: [PATCH 02/23] docs: implementation plan for approach-axis orientation planning 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) Claude-Session: https://claude.ai/code/session_01PguhuvXbVmNHzUWzYyJFaD --- .../2026-07-15-pose-cloud-orientation.md | 1338 +++++++++++++++++ ...026-07-15-pose-cloud-orientation-design.md | 32 +- 2 files changed, 1358 insertions(+), 12 deletions(-) create mode 100644 docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md new file mode 100644 index 0000000..35c0670 --- /dev/null +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -0,0 +1,1338 @@ +# Approach-Axis Orientation Planning via PoseCloud — Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Replace the blanket `position_only` goal metric in both SO-101 arm models' `MoveToPosition` with a configurable approach-axis cone, expressed as a `referenceframe.PoseCloud` attached to the goal. + +**Architecture:** A new pure helper (`goal_cloud.go`) converts a degrees-based cone into a `PoseCloud` and applies a precedence table over the caller's `extra` map. Both arm models resolve their config into a shared `goalCloudConfig` at construction and delegate to that helper. The module only *sends* the cloud; viam-server plans, so honoring it requires viam-server >= 0.127.0. + +**Tech Stack:** Go 1.25, `go.viam.com/rdk` (bumped v0.123.0 → v1.0.0), `testify` for assertions. + +**Spec:** `docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md` — read it first. The planner semantics are counterintuitive and were established empirically; do not "simplify" the mapping. + +--- + +## Critical context (read before touching code) + +Three facts drive every design choice. All were verified empirically against rdk v1.0.0: + +1. **`PoseCloud.Theta` encodes the AZIMUTH of a tilt, not roll.** Tilt about X → `Theta=90`; about Y → `Theta=0`; diagonal → `Theta=45`. So `Theta` **must be 180** or the check rejects tilts by azimuth before the cone is consulted (a `Theta: 0` cloud rejects even a 5° tilt). Consequence: roll cannot be constrained alongside tilt. This is approach-axis planning with **free roll**. +2. **`defaultEpsilon = 0.001` is ADDED to every leeway**, not just zeroed ones. Zero `X`/`Y`/`Z` therefore demands a 1-micron match, so positional leeway is **mandatory**. The effective cone is `acos(cos(tol) - 0.001)` — always slightly wider than requested. +3. **`OZ` alone defines the cone.** `OX`/`OY` are set to `1` (unconstrained). Valid across `[0, 180]`, saturating at `acos(-0.999) = 177.4374°`. + +A cloud that never matches is **worse than no cloud**: the planner silently reverts to strict 6-DOF scoring and every move fails. + +--- + +## File Structure + +| File | Responsibility | +|---|---| +| `goal_cloud.go` (create) | The whole concern: defaults, cone→cloud math, `pose_cloud` parsing, precedence, error wrapping. Pure — no serial port, no motion service, no hardware. | +| `goal_cloud_test.go` (create) | Cone semantics via `PoseInCloud`, regression guards, precedence, parsing, proto round-trip. | +| `arm.go` (modify) | Config fields + `Validate` rules; `goalCloud` field; `MoveToPosition` delegates to the helper. | +| `simulated.go` (modify) | Same three changes as `arm.go`. | +| `arm_move_to_position_test.go` (create) | Both models send a destination carrying the expected `GoalCloud`. | +| `go.mod` / `go.sum` (modify) | RDK bump. | +| `README.md`, `CLAUDE.md` (modify) | Docs. | + +--- + +## Task 1: Bump RDK to v1.0.0 + +Its own commit, ahead of the feature, so a bisect can separate "the upgrade broke it" from "the cone broke it". Verified to need **zero source changes**. + +**Files:** +- Modify: `go.mod`, `go.sum` +- Modify: `CLAUDE.md` (header pins `v0.123.0`) + +- [ ] **Step 1: Record the baseline** + +Run: `go build ./cmd/module/ . && go test ./cmd/module/ . && go vet ./cmd/module/ .` +Expected: all pass. (`cmd/cli/` is excluded deliberately — it has multiple `func main()` and will not compile as a package. Never use `./...`.) + +- [ ] **Step 2: Bump** + +```bash +GOFLAGS=-mod=mod go get go.viam.com/rdk@v1.0.0 +GOFLAGS=-mod=mod go mod tidy +``` + +Expected `go.mod` result: `rdk v0.123.0→v1.0.0`, `api v0.1.539→v0.1.566`, `utils v0.4.19→v0.6.6`, `go` directive `1.25.1→1.25.9`. + +- [ ] **Step 3: Verify nothing broke** + +Run: `go build ./cmd/module/ . && go test ./cmd/module/ . && go vet ./cmd/module/ .` +Expected: all pass, **no source edits required**. If anything fails, STOP and report — the spec's premise was that this bump is source-clean. + +Note: `TestEnumerateSerialPorts` in `discovery_test.go` fails on machines with no serial hardware. That is environment-dependent and **not** a regression. + +- [ ] **Step 4: Update the CLAUDE.md version pin** + +In `CLAUDE.md`, change `go.viam.com/rdk v0.123.0` to `go.viam.com/rdk v1.0.0`. + +- [ ] **Step 5: Commit** + +```bash +git add go.mod go.sum CLAUDE.md +git commit -m "chore: bump rdk to v1.0.0 + +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." +``` + +- [ ] **Step 6: Verify CI** + +Push the branch and confirm `.github/workflows` passes. The `go` directive moved 1.25.1→1.25.9 and nothing pins a Go version, so confirm the build-action's toolchain resolves it rather than assuming. + +--- + +## Task 2: `goalCloudConfig` + `resolveGoalCloudConfig` + +Single owner of defaulting **and** the two construction warnings. `Validate` has no logger, which is why the warnings live here. + +**Files:** +- Create: `goal_cloud.go` +- Test: `goal_cloud_test.go` + +- [ ] **Step 1: Write the failing test** + +Import only what this task uses — Go fails the build on unused imports, and `gofmt` does +not strip them. Later tasks add more (`math`, `r3`, `referenceframe`, `spatialmath` in +Task 3; `require` in Task 5). + +```go +package so_arm + +import ( + "testing" + + "github.com/stretchr/testify/assert" +) + +func TestResolveGoalCloudConfigDefaults(t *testing.T) { + // Zero and unset are indistinguishable for a float64 with omitempty; both default. + got := resolveGoalCloudConfig(0, 0, nil) + assert.Equal(t, defaultOrientationToleranceDeg, got.OrientationToleranceDeg) + assert.Equal(t, defaultPositionToleranceMM, got.PositionToleranceMM) +} + +func TestResolveGoalCloudConfigKeepsExplicitValues(t *testing.T) { + got := resolveGoalCloudConfig(12.5, 0.25, nil) + assert.Equal(t, 12.5, got.OrientationToleranceDeg) + assert.Equal(t, 0.25, got.PositionToleranceMM) +} +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `go test . -run TestResolveGoalCloudConfig -v` +Expected: FAIL — `undefined: resolveGoalCloudConfig` + +- [ ] **Step 3: Write the implementation** + +Create `goal_cloud.go`: + +```go +package so_arm + +import ( + "math" + + "go.viam.com/rdk/logging" +) + +const ( + // defaultOrientationToleranceDeg is the approach-axis cone half-angle used when + // orientation_tolerance_deg is zero or unset. + defaultOrientationToleranceDeg = 30.0 + + // defaultPositionToleranceMM is the per-axis positional leeway used when + // position_tolerance_mm is zero or unset. It must stay comfortably above the IK + // solver's convergence: a cloud the solver can never land inside is worse than no + // cloud at all, because planning reverts to strict 6-DOF scoring and always fails. + defaultPositionToleranceMM = 1.0 + + // warnPositionToleranceMM is the point above which a cloud is loose enough that the + // planner may report success visibly far from the goal. + warnPositionToleranceMM = 10.0 + + // poseCloudSaturationCos is the cosine at which the cone accepts every orientation. + // PoseInCloud adds a 0.001 epsilon to the OZ leeway and the maximum possible + // deviation is 2.0, so saturation begins where 1-cos(a)+0.001 >= 2. Expressed as a + // cosine rather than the rounded 177.44 degrees: the true threshold is 177.4374, and + // a literal degree comparison would miss the [177.4374, 177.44) band. + poseCloudSaturationCos = -0.999 +) + +// goalCloudConfig is the resolved tolerance pair, letting both arm models share one code +// path despite their separate config structs. Defaults are already applied. +type goalCloudConfig struct { + OrientationToleranceDeg float64 + PositionToleranceMM float64 +} + +// resolveGoalCloudConfig applies defaults to zero or unset values and emits the two +// construction warnings. It is the single owner of both, so each constructor is one call +// and neither arm model re-implements the thresholds. logger may be nil (unit tests). +func resolveGoalCloudConfig(tolDeg, posTolMM float64, logger logging.Logger) goalCloudConfig { + cfg := goalCloudConfig{OrientationToleranceDeg: tolDeg, PositionToleranceMM: posTolMM} + if cfg.OrientationToleranceDeg == 0 { + cfg.OrientationToleranceDeg = defaultOrientationToleranceDeg + } + if cfg.PositionToleranceMM == 0 { + cfg.PositionToleranceMM = defaultPositionToleranceMM + } + + if logger == nil { + return cfg + } + if cfg.PositionToleranceMM > warnPositionToleranceMM { + logger.Warnf("position_tolerance_mm=%.1f is large; the planner may report success up to %.1fmm "+ + "from the goal (the cloud is a per-axis box, so the worst-case corner is sqrt(3) times this)", + cfg.PositionToleranceMM, math.Sqrt(3)*cfg.PositionToleranceMM) + } + if math.Cos(degToRad(cfg.OrientationToleranceDeg)) <= poseCloudSaturationCos { + logger.Warnf("orientation_tolerance_deg=%.1f saturates the approach-axis cone (>= 177.44); "+ + "every orientation will be accepted, which is equivalent to ignoring orientation", + cfg.OrientationToleranceDeg) + } + return cfg +} + +func degToRad(deg float64) float64 { return deg * math.Pi / 180 } +``` + +- [ ] **Step 4: Run test to verify it passes** + +Run: `go test . -run TestResolveGoalCloudConfig -v` +Expected: PASS + +- [ ] **Step 5: Commit** + +```bash +gofmt -s -w . && go vet ./cmd/module/ . +git add goal_cloud.go goal_cloud_test.go +git commit -m "feat(arm): add goalCloudConfig with defaulting and construction warnings" +``` + +--- + +## Task 3: `coneToPoseCloud` + verified cone semantics + +The heart of the change. Every assertion below is a **measured** value, not a derivation. If one fails, the mapping is wrong — do not adjust the expectation to match. + +**Files:** +- Modify: `goal_cloud.go` +- Test: `goal_cloud_test.go` + +- [ ] **Step 1: Write the failing tests** + +Add to `goal_cloud_test.go`. Extend the import block to exactly: + +```go +import ( + "math" + "testing" + + "github.com/golang/geo/r3" + "github.com/stretchr/testify/assert" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" +) +``` + +```go +// testGoal is a tool pointing straight down, a typical SO-101 grasp pose. +func testGoal() spatialmath.Pose { + return spatialmath.NewPose( + r3.Vector{X: 300, Y: 0, Z: 200}, + &spatialmath.OrientationVectorDegrees{OX: 0, OY: 0, OZ: -1, Theta: 0}, + ) +} + +// tiltedBy returns testGoal() tilted by deg about the axis (ax, ay, 0). +func tiltedBy(ax, ay, deg float64) spatialmath.Pose { + return spatialmath.Compose(testGoal(), spatialmath.NewPoseFromOrientation( + &spatialmath.R4AA{RX: ax, RY: ay, RZ: 0, Theta: degToRad(deg)})) +} + +func coneCloud(tolDeg float64) *referenceframe.PoseCloud { + return coneToPoseCloud(goalCloudConfig{OrientationToleranceDeg: tolDeg, PositionToleranceMM: 1.0}) +} + +func TestConeToPoseCloudFields(t *testing.T) { + c := coneCloud(30) + assert.Equal(t, 1.0, c.X) + assert.Equal(t, 1.0, c.Y) + assert.Equal(t, 1.0, c.Z) + // OX/OY are deliberately unconstrained: OZ alone defines the cone. + assert.Equal(t, 1.0, c.OX) + assert.Equal(t, 1.0, c.OY) + assert.InDelta(t, 1-math.Cos(degToRad(30)), c.OZ, 1e-12) + // MUST be 180: Theta encodes tilt AZIMUTH, not roll. See the spec. + assert.Equal(t, 180.0, c.Theta) + // X, Y, Z, OX, OY, OZ, Theta is the COMPLETE field set in rdk v1.0.0. Do not look for + // a ReferenceFrame field: v0.123.0 had one, v1.0.0 removed it, and the struct's + // free-floating doc comment about reference frames misleadingly survives. +} + +func TestConeBoundsTiltIsotropically(t *testing.T) { + c := coneCloud(30) + for _, azDeg := range []float64{0, 45, 90, 135, 180, 270} { + ax, ay := math.Cos(degToRad(azDeg)), math.Sin(degToRad(azDeg)) + assert.True(t, c.PoseInCloud(testGoal(), tiltedBy(ax, ay, 29)), + "29deg tilt at azimuth %.0f should be accepted", azDeg) + assert.False(t, c.PoseInCloud(testGoal(), tiltedBy(ax, ay, 31)), + "31deg tilt at azimuth %.0f should be rejected", azDeg) + } +} + +func TestConeBoundsAbove90Degrees(t *testing.T) { + // The cone does NOT go degenerate above 90, despite OZ = 1-cos(a) exceeding 1. + assert.True(t, coneCloud(90).PoseInCloud(testGoal(), tiltedBy(1, 0, 89))) + assert.False(t, coneCloud(90).PoseInCloud(testGoal(), tiltedBy(1, 0, 91))) + assert.True(t, coneCloud(120).PoseInCloud(testGoal(), tiltedBy(1, 0, 119))) + assert.False(t, coneCloud(120).PoseInCloud(testGoal(), tiltedBy(1, 0, 121))) +} + +func TestConeSaturationBoundary(t *testing.T) { + // Saturation begins at acos(-0.999) = 177.4374, NOT 179. + assert.False(t, coneCloud(177.40).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), + "177.40 must still reject a 180deg tilt") + assert.True(t, coneCloud(177.44).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), + "177.44 must be saturated") +} + +func TestConeRollIsFree(t *testing.T) { + c := coneCloud(30) + for _, roll := range []float64{0, 45, 90, 179} { + p := spatialmath.Compose(testGoal(), spatialmath.NewPoseFromOrientation( + &spatialmath.R4AA{RX: 0, RY: 0, RZ: 1, Theta: degToRad(roll)})) + assert.True(t, c.PoseInCloud(testGoal(), p), "roll %.0f should be accepted", roll) + } +} + +func TestConeTiltPlusRollStaysBounded(t *testing.T) { + c := coneCloud(30) + for _, roll := range []float64{0, 70, 170} { + for _, tc := range []struct { + tilt float64 + want bool + }{{20, true}, {29, true}, {31, false}} { + p := spatialmath.Compose(testGoal(), spatialmath.Compose( + spatialmath.NewPoseFromOrientation(&spatialmath.R4AA{RX: 1, Theta: degToRad(tc.tilt)}), + spatialmath.NewPoseFromOrientation(&spatialmath.R4AA{RZ: 1, Theta: degToRad(roll)}))) + assert.Equal(t, tc.want, c.PoseInCloud(testGoal(), p), + "tilt %.0f roll %.0f", tc.tilt, roll) + } + } +} + +func TestConeEffectiveWideningFormula(t *testing.T) { + // The epsilon is ADDED to the leeway, so the enforced cone is always slightly wider + // than requested: acos(cos(tol) - 0.001). Measured by binary search on acceptance. + for _, tc := range []struct{ requested, want float64 }{ + {0, 2.5626}, {1, 2.7508}, {2.6, 3.6509}, {10, 10.3247}, {30, 30.1144}, {90, 90.0573}, + } { + c := coneCloud(tc.requested) + lo, hi := 0.0, 180.0 + for i := 0; i < 60; i++ { + mid := (lo + hi) / 2 + if c.PoseInCloud(testGoal(), tiltedBy(1, 0, mid)) { + lo = mid + } else { + hi = mid + } + } + assert.InDelta(t, tc.want, lo, 0.01, "effective cone for requested %.1f", tc.requested) + } +} + +func TestConePositionalBoxNotRadius(t *testing.T) { + c := coneCloud(30) + offset := func(dx, dy, dz float64) spatialmath.Pose { + g := testGoal() + return spatialmath.NewPose( + r3.Vector{X: g.Point().X + dx, Y: g.Point().Y + dy, Z: g.Point().Z + dz}, + g.Orientation()) + } + // The epsilon is additive: at X=1.0 the true bound is 1.001mm. + assert.True(t, c.PoseInCloud(testGoal(), offset(1.0009, 0, 0))) + assert.False(t, c.PoseInCloud(testGoal(), offset(1.0015, 0, 0))) + // Per-axis box, not a radius: all three axes at the bound is accepted at norm ~1.73mm. + assert.True(t, c.PoseInCloud(testGoal(), offset(1.0005, 1.0005, 1.0005))) +} +``` + +- [ ] **Step 2: Run tests to verify they fail** + +Run: `go test . -run TestCone -v` +Expected: FAIL — `undefined: coneToPoseCloud` + +- [ ] **Step 3: Write the implementation** + +Add to `goal_cloud.go` (add `"go.viam.com/rdk/referenceframe"` to imports): + +```go +// coneToPoseCloud converts an approach-axis cone half-angle into a PoseCloud. +// +// Every field here is load-bearing and was established empirically; see +// docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md before changing any +// of them: +// +// - X/Y/Z must be > 0. PoseInCloud adds a 0.001 epsilon to each leeway, so a zero +// leeway demands a 1-micron match and the cloud would never match. +// - OX/OY are deliberately 1 (unconstrained). OZ alone defines the cone. +// - Theta must be 180. For a pure tilt, PoseCloud's Theta reports the AZIMUTH of that +// tilt (about X -> 90, about Y -> 0), not the roll. At any smaller leeway the Theta +// check rejects tilts before the cone is consulted. The cost is that roll cannot be +// constrained at all, which is why this is approach-axis planning with free roll. +func coneToPoseCloud(cfg goalCloudConfig) *referenceframe.PoseCloud { + return &referenceframe.PoseCloud{ + X: cfg.PositionToleranceMM, + Y: cfg.PositionToleranceMM, + Z: cfg.PositionToleranceMM, + OX: 1, + OY: 1, + OZ: 1 - math.Cos(degToRad(cfg.OrientationToleranceDeg)), + Theta: 180, + } +} +``` + +These seven are the complete field set in rdk v1.0.0. (v0.123.0 also had a +`ReferenceFrame`; v1.0.0 removed it, but left the struct's doc comment about reference +frames in place, which reads as though the field still exists. It does not.) + +- [ ] **Step 4: Run tests to verify they pass** + +Run: `go test . -run TestCone -v` +Expected: all PASS + +- [ ] **Step 5: Commit** + +```bash +gofmt -s -w . && go vet ./cmd/module/ . +git add goal_cloud.go goal_cloud_test.go +git commit -m "feat(arm): add coneToPoseCloud approach-axis cone mapping" +``` + +--- + +## Task 4: Regression guards for the two counterintuitive findings + +These encode *why* the mapping looks as it does. Without them, a future "simplification" of `Theta: 180` → `0` silently breaks every move while the other tests still pass. + +**Files:** +- Test: `goal_cloud_test.go` + +- [ ] **Step 1: Write the guards** + +```go +// TestRegressionThetaMustBe180 documents why coneToPoseCloud sets Theta: 180. +// PoseCloud's Theta encodes the AZIMUTH of a tilt, not roll, so any smaller leeway +// rejects tilts before the OX/OY/OZ cone is consulted. +func TestRegressionThetaMustBe180(t *testing.T) { + broken := coneCloud(30) + broken.Theta = 0 // the "obvious simplification" + assert.False(t, broken.PoseInCloud(testGoal(), tiltedBy(1, 0, 5)), + "with Theta=0 even a 5deg tilt is rejected, defeating the cone entirely") + + // Proof of the cause: a pure tilt about X reports Theta=90, about Y reports Theta=0. + ovX := spatialmath.PoseBetween(testGoal(), tiltedBy(1, 0, 5)).Orientation().OrientationVectorDegrees() + assert.InDelta(t, 90.0, ovX.Theta, 1e-6, "tilt about X reports Theta=90, not roll") + ovY := spatialmath.PoseBetween(testGoal(), tiltedBy(0, 1, 5)).Orientation().OrientationVectorDegrees() + assert.InDelta(t, 0.0, ovY.Theta, 1e-6, "tilt about Y reports Theta=0") +} + +// TestRegressionPositionalLeewayIsMandatory documents why X/Y/Z must be > 0: the 0.001 +// epsilon means a zero leeway demands a 1-micron match, so the cloud never matches and +// planning silently reverts to strict 6-DOF scoring. +func TestRegressionPositionalLeewayIsMandatory(t *testing.T) { + broken := coneCloud(30) + broken.X, broken.Y, broken.Z = 0, 0, 0 + g := testGoal() + nudged := spatialmath.NewPose( + r3.Vector{X: g.Point().X + 0.01, Y: g.Point().Y, Z: g.Point().Z}, g.Orientation()) + assert.False(t, broken.PoseInCloud(g, nudged), + "with zero positional leeway even a 0.01mm offset is rejected") +} +``` + +- [ ] **Step 2: Run and verify they pass** + +Run: `go test . -run TestRegression -v` +Expected: PASS (these assert current behavior; they should pass immediately) + +- [ ] **Step 3: Commit** + +```bash +git add goal_cloud_test.go +git commit -m "test(arm): guard the Theta=180 and positional-leeway invariants" +``` + +--- + +## Task 5: `pose_cloud` parsing + +The expert-mode escape hatch. Keys are **lowercase**, matching `referenceframe.PoseCloud`'s JSON tags. Unknown keys are rejected because the Viam docs render them `OX`/`OY` and protobuf JSON spells them `o_x` — a silently-ignored typo would produce a 1-micron cloud that fails every move. `reference_frame` is rejected under the same rule: it is not a `PoseCloud` field in v1.0.0. + +Note this task's tests are the first to use `require`; add `"github.com/stretchr/testify/require"` to the import block. + +**Files:** +- Modify: `goal_cloud.go` +- Test: `goal_cloud_test.go` + +- [ ] **Step 1: Write the failing tests** + +```go +func TestParsePoseCloudValid(t *testing.T) { + // extra arrives via structpb, so every JSON number is a float64. + got, err := parsePoseCloud(map[string]interface{}{ + "x": 2.0, "y": 2.0, "z": 0.5, "ox": 1.0, "oy": 1.0, "oz": 0.1, "theta": 180.0, + }) + require.NoError(t, err) + assert.Equal(t, 2.0, got.X) + assert.Equal(t, 0.5, got.Z) + assert.Equal(t, 0.1, got.OZ) + assert.Equal(t, 180.0, got.Theta) +} + +func TestParsePoseCloudOmittedFieldsStayZero(t *testing.T) { + got, err := parsePoseCloud(map[string]interface{}{"oz": 0.1}) + require.NoError(t, err) + assert.Equal(t, 0.0, got.X, "omitted fields stay zero -- sharp, but faithful passthrough") +} + +func TestParsePoseCloudRejects(t *testing.T) { + for name, input := range map[string]interface{}{ + "not an object": "nope", + "docs-style casing": map[string]interface{}{"OX": 1.0}, + "protobuf casing": map[string]interface{}{"o_x": 1.0}, + "not a field in v1.0": map[string]interface{}{"reference_frame": 1.0}, + "unknown key": map[string]interface{}{"wobble": 1.0}, + "non-numeric": map[string]interface{}{"oz": "0.1"}, + "negative": map[string]interface{}{"oz": -0.1}, + "NaN": map[string]interface{}{"oz": math.NaN()}, + "Inf": map[string]interface{}{"oz": math.Inf(1)}, + } { + t.Run(name, func(t *testing.T) { + _, err := parsePoseCloud(input) + assert.Error(t, err) + }) + } +} +``` + +- [ ] **Step 2: Run tests to verify they fail** + +Run: `go test . -run TestParsePoseCloud -v` +Expected: FAIL — `undefined: parsePoseCloud` + +- [ ] **Step 3: Write the implementation** + +Add to `goal_cloud.go` (add `"fmt"` and `"sort"` to imports): + +```go +// poseCloudSetters maps the accepted pose_cloud keys to their fields. The names match +// referenceframe.PoseCloud's own JSON tags exactly, and are all lowercase. Note the Viam +// docs render these as OX/OY and protobuf JSON spells them o_x/oX; neither is accepted, +// which is why unknown keys are an error rather than ignored. This is also the complete +// field set: "reference_frame" is not a PoseCloud field in rdk v1.0.0. +var poseCloudSetters = map[string]func(*referenceframe.PoseCloud, float64){ + "x": func(p *referenceframe.PoseCloud, v float64) { p.X = v }, + "y": func(p *referenceframe.PoseCloud, v float64) { p.Y = v }, + "z": func(p *referenceframe.PoseCloud, v float64) { p.Z = v }, + "ox": func(p *referenceframe.PoseCloud, v float64) { p.OX = v }, + "oy": func(p *referenceframe.PoseCloud, v float64) { p.OY = v }, + "oz": func(p *referenceframe.PoseCloud, v float64) { p.OZ = v }, + "theta": func(p *referenceframe.PoseCloud, v float64) { p.Theta = v }, +} + +func poseCloudKeyList() string { + keys := make([]string, 0, len(poseCloudSetters)) + for k := range poseCloudSetters { + keys = append(keys, k) + } + sort.Strings(keys) + return strings.Join(keys, ", ") +} + +// parsePoseCloud converts a caller-supplied extra["pose_cloud"] into a PoseCloud. +// Omitted fields stay zero, which -- given the additive 0.001 epsilon -- means +// near-zero leeway. That is faithful passthrough and the point of an escape hatch, but it +// is sharp: a cloud whose leeways are all zero will never match. +func parsePoseCloud(raw interface{}) (*referenceframe.PoseCloud, error) { + m, ok := raw.(map[string]interface{}) + if !ok { + return nil, fmt.Errorf("pose_cloud must be an object, got %T", raw) + } + pc := &referenceframe.PoseCloud{} + for k, rv := range m { + set, known := poseCloudSetters[k] + if !known { + return nil, fmt.Errorf("pose_cloud: unknown key %q; valid keys are %s (all lowercase; "+ + "the Viam docs' OX/OY casing and protobuf's o_x are not accepted)", k, poseCloudKeyList()) + } + v, ok := rv.(float64) + if !ok { + return nil, fmt.Errorf("pose_cloud: %q must be a number, got %T", k, rv) + } + if math.IsNaN(v) || math.IsInf(v, 0) { + return nil, fmt.Errorf("pose_cloud: %q must be finite, got %v", k, v) + } + if v < 0 { + return nil, fmt.Errorf("pose_cloud: %q must not be negative, got %v", k, v) + } + set(pc, v) + } + return pc, nil +} +``` + +Also add `"strings"` to the import block. + +- [ ] **Step 4: Run tests to verify they pass** + +Run: `go test . -run TestParsePoseCloud -v` +Expected: all PASS + +- [ ] **Step 5: Commit** + +```bash +gofmt -s -w . && go vet ./cmd/module/ . +git add goal_cloud.go goal_cloud_test.go +git commit -m "feat(arm): parse the raw pose_cloud escape hatch from extra" +``` + +--- + +## Task 6: `buildMoveDestination` + precedence + `goalPath` + +**Files:** +- Modify: `goal_cloud.go` +- Test: `goal_cloud_test.go` + +The precedence table: + +| `extra` contains | Destination | Extras forwarded | `goalPath` | +|---|---|---|---| +| neither key | cone from config | all caller keys verbatim | `pathCone` | +| `pose_cloud` only | raw cloud | all keys **except** `pose_cloud` | `pathRawCloud` | +| `goal_metric_type` only | **no cloud** | all keys verbatim | `pathMetricType` | +| **both** | — | — | **error** | + +- [ ] **Step 1: Write the failing tests** + +```go +func TestBuildMoveDestinationConePath(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + dest, planExtra, path, err := buildMoveDestination("myarm_origin", testGoal(), cfg, nil) + require.NoError(t, err) + assert.Equal(t, pathCone, path) + assert.Equal(t, "myarm_origin", dest.Parent(), "the frame name is passed through unmodified") + require.NotNil(t, dest.GoalCloud) + assert.Equal(t, 180.0, dest.GoalCloud.Theta) + assert.NotContains(t, planExtra, "goal_metric_type", "the cone replaces position_only") +} + +func TestBuildMoveDestinationForwardsCallerExtras(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + _, planExtra, _, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"timeout": 5.0}) + require.NoError(t, err) + assert.Equal(t, 5.0, planExtra["timeout"], "unrelated caller keys still pass through") +} + +func TestBuildMoveDestinationRawCloudPath(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + dest, planExtra, path, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"pose_cloud": map[string]interface{}{"oz": 0.5}, "timeout": 5.0}) + require.NoError(t, err) + assert.Equal(t, pathRawCloud, path) + require.NotNil(t, dest.GoalCloud) + assert.Equal(t, 0.5, dest.GoalCloud.OZ, "the raw cloud replaces the cone entirely") + assert.NotContains(t, planExtra, "pose_cloud", "pose_cloud is not a planner key") + assert.Equal(t, 5.0, planExtra["timeout"]) +} + +func TestBuildMoveDestinationMetricTypePath(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + dest, planExtra, path, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"goal_metric_type": "position_only"}) + require.NoError(t, err) + assert.Equal(t, pathMetricType, path) + assert.Nil(t, dest.GoalCloud, "no cloud: position_only sets orientScale=0, making a cloud meaningless") + assert.Equal(t, "position_only", planExtra["goal_metric_type"], "the caller's metric is forwarded") +} + +func TestBuildMoveDestinationRejectsBothKeys(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + _, _, _, err := buildMoveDestination("a_origin", testGoal(), cfg, map[string]interface{}{ + "pose_cloud": map[string]interface{}{"oz": 0.5}, + "goal_metric_type": "position_only", + }) + require.Error(t, err, "incoherent: a raw cloud plus a metric that ignores clouds") +} + +func TestBuildMoveDestinationDoesNotMutateCallerExtra(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + extra := map[string]interface{}{"pose_cloud": map[string]interface{}{"oz": 0.5}} + _, _, _, err := buildMoveDestination("a_origin", testGoal(), cfg, extra) + require.NoError(t, err) + assert.Contains(t, extra, "pose_cloud", "the caller's map must not be mutated") +} + +func TestBuildMoveDestinationPropagatesParseError(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + dest, planExtra, _, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"pose_cloud": map[string]interface{}{"OX": 1.0}}) + require.Error(t, err) + assert.Nil(t, dest) + assert.Nil(t, planExtra) +} +``` + +- [ ] **Step 2: Run tests to verify they fail** + +Run: `go test . -run TestBuildMoveDestination -v` +Expected: FAIL — `undefined: buildMoveDestination` + +- [ ] **Step 3: Write the implementation** + +Add to `goal_cloud.go` (add `"go.viam.com/rdk/spatialmath"` to imports): + +```go +const ( + extraKeyPoseCloud = "pose_cloud" + extraKeyGoalMetricType = "goal_metric_type" +) + +// goalPath reports which precedence row buildMoveDestination took, so callers can wrap +// failures appropriately without re-inspecting extra (which would duplicate the +// precedence logic across both arm models and let it drift). +type goalPath int + +const ( + pathCone goalPath = iota // cone from config + pathRawCloud // caller-supplied pose_cloud + pathMetricType // caller-supplied goal_metric_type; no cloud sent +) + +// buildMoveDestination applies the precedence table (see the plan/spec). +// +// originFrame is the FULLY-FORMED frame name (e.g. "myarm_origin"); this function does +// not append "_origin". It is passed to NewPoseInFrame unmodified. +// +// On success it returns a destination, a NEW extras map (the caller's map is never +// mutated), and the path taken. On error it returns (nil, nil, 0, err). +func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalCloudConfig, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, map[string]interface{}, goalPath, error) { + rawCloud, hasCloud := extra[extraKeyPoseCloud] + _, hasMetric := extra[extraKeyGoalMetricType] + + if hasCloud && hasMetric { + return nil, nil, 0, fmt.Errorf( + "extra cannot contain both %q and %q: %q makes the planner ignore goal clouds, "+ + "so the combination is incoherent", extraKeyPoseCloud, extraKeyGoalMetricType, + extraKeyGoalMetricType) + } + + planExtra := make(map[string]interface{}, len(extra)) + for k, v := range extra { + if k == extraKeyPoseCloud { + continue // consumed here; not a planner key + } + planExtra[k] = v + } + + switch { + case hasMetric: + // The caller's metric wins and no cloud is sent: position_only sets orientScale=0, + // which would make any cloud meaningless. + return referenceframe.NewPoseInFrame(originFrame, pose), planExtra, pathMetricType, nil + case hasCloud: + pc, err := parsePoseCloud(rawCloud) + if err != nil { + return nil, nil, 0, err + } + return referenceframe.NewPoseInFrameWithGoalCloud(originFrame, pose, pc), planExtra, pathRawCloud, nil + default: + return referenceframe.NewPoseInFrameWithGoalCloud(originFrame, pose, coneToPoseCloud(cfg)), + planExtra, pathCone, nil + } +} + +// wrapMoveErr adds actionable guidance to a planning failure. The cone-specific wording +// applies only on the cone path: on the other paths it would misdirect, telling a caller +// who just passed position_only to pass position_only, or blaming a config tolerance that +// had no bearing on a raw-cloud failure. +func wrapMoveErr(err error, path goalPath, cfg goalCloudConfig) error { + if err == nil { + return nil + } + switch path { + case pathCone: + return fmt.Errorf("move to position failed with orientation_tolerance_deg=%.1f "+ + "(approach-axis cone): %w; widen the tolerance, pass extra "+ + "{\"goal_metric_type\": \"position_only\"} to ignore orientation, or check that "+ + "viam-server is >= 0.127.0 (older servers silently ignore goal clouds)", + cfg.OrientationToleranceDeg, err) + case pathRawCloud: + return fmt.Errorf("move to position failed with the caller-supplied pose_cloud: %w; "+ + "widen the cloud, or check that viam-server is >= 0.127.0 (older servers silently "+ + "ignore goal clouds)", err) + default: + return err + } +} +``` + +- [ ] **Step 4: Run tests to verify they pass** + +Run: `go test . -run TestBuildMoveDestination -v` +Expected: all PASS + +- [ ] **Step 5: Commit** + +```bash +gofmt -s -w . && go vet ./cmd/module/ . +git add goal_cloud.go goal_cloud_test.go +git commit -m "feat(arm): add buildMoveDestination precedence and error wrapping" +``` + +--- + +## Task 7: Proto round-trip guard + +CLAUDE.md's module-boundary gotcha: a component ships over gRPC, and in-memory state does not survive. An in-process assertion would miss a serialization break entirely. This is the test that proves the cloud actually reaches viam-server. + +**Files:** +- Test: `goal_cloud_test.go` + +- [ ] **Step 1: Write the test** + +```go +// TestGoalCloudSurvivesSerialization guards the module-boundary gotcha: the cloud is only +// useful if it crosses gRPC. An in-process check would pass even if it never shipped. +func TestGoalCloudSurvivesSerialization(t *testing.T) { + cfg := resolveGoalCloudConfig(30, 1.0, nil) + dest, _, _, err := buildMoveDestination("myarm_origin", testGoal(), cfg, nil) + require.NoError(t, err) + + proto := referenceframe.PoseInFrameToProtobuf(dest) + require.NotNil(t, proto.GoalCloud, "the cloud must be serialized onto the wire") + + got := referenceframe.ProtobufToPoseInFrame(proto) + require.NotNil(t, got.GoalCloud, "the cloud must survive the round trip") + + want := dest.GoalCloud + assert.InDelta(t, want.X, got.GoalCloud.X, 1e-9) + assert.InDelta(t, want.Y, got.GoalCloud.Y, 1e-9) + assert.InDelta(t, want.Z, got.GoalCloud.Z, 1e-9) + assert.InDelta(t, want.OX, got.GoalCloud.OX, 1e-9) + assert.InDelta(t, want.OY, got.GoalCloud.OY, 1e-9) + assert.InDelta(t, want.OZ, got.GoalCloud.OZ, 1e-9) + assert.InDelta(t, want.Theta, got.GoalCloud.Theta, 1e-9) +} +``` + +- [ ] **Step 2: Run and verify** + +Run: `go test . -run TestGoalCloudSurvives -v` +Expected: PASS. If the round-trip fails, the RDK bump did not land — recheck Task 1. + +- [ ] **Step 3: Commit** + +```bash +git add goal_cloud_test.go +git commit -m "test(arm): guard that the goal cloud survives gRPC serialization" +``` + +--- + +## Task 8: Config fields and `Validate` on both models + +**Files:** +- Modify: `arm.go:70-100` (`SO101ArmConfig`), `arm.go:103+` (`Validate`) +- Modify: `simulated.go:42-71` (`SO101SimulatedArmConfig`), `simulated.go:75-91` (`Validate`) +- Test: `goal_cloud_test.go` + +- [ ] **Step 1: Write the failing tests** + +```go +func TestArmConfigValidateToleranceRanges(t *testing.T) { + base := func() *SO101ArmConfig { return &SO101ArmConfig{Port: "/dev/null"} } + + for name, mutate := range map[string]func(*SO101ArmConfig){ + "negative orientation": func(c *SO101ArmConfig) { c.OrientationToleranceDeg = -1 }, + "orientation over 180": func(c *SO101ArmConfig) { c.OrientationToleranceDeg = 181 }, + "NaN orientation": func(c *SO101ArmConfig) { c.OrientationToleranceDeg = math.NaN() }, + "negative position": func(c *SO101ArmConfig) { c.PositionToleranceMM = -0.1 }, + "NaN position": func(c *SO101ArmConfig) { c.PositionToleranceMM = math.NaN() }, + } { + t.Run(name, func(t *testing.T) { + c := base() + mutate(c) + _, _, err := c.Validate("") + assert.Error(t, err) + }) + } + + t.Run("valid bounds accepted", func(t *testing.T) { + c := base() + c.OrientationToleranceDeg, c.PositionToleranceMM = 180, 75 + _, _, err := c.Validate("") + assert.NoError(t, err) + }) +} + +func TestSimulatedConfigValidateToleranceRanges(t *testing.T) { + c := &SO101SimulatedArmConfig{OrientationToleranceDeg: 181} + _, _, err := c.Validate("") + assert.Error(t, err) + + c = &SO101SimulatedArmConfig{PositionToleranceMM: -1} + _, _, err = c.Validate("") + assert.Error(t, err) +} +``` + +- [ ] **Step 2: Run tests to verify they fail** + +Run: `go test . -run "ConfigValidateToleranceRanges" -v` +Expected: FAIL — unknown fields `OrientationToleranceDeg` / `PositionToleranceMM` + +- [ ] **Step 3: Add the config fields** + +Add to **both** `SO101ArmConfig` (arm.go) and `SO101SimulatedArmConfig` (simulated.go): + +```go + // OrientationToleranceDeg is the half-angle, in degrees, of the cone of acceptable + // end-effector approach directions around the goal orientation. Roll about that axis + // is NOT constrained. Valid range [0, 180]. Zero or unset means + // defaultOrientationToleranceDeg (30). + // + // The planner adds a 0.001 epsilon to the OZ leeway, so the cone actually enforced is + // acos(cos(tol) - 0.001): always slightly wider than requested (30 gives ~30.11) and + // never narrower than ~2.56. At or above 177.44 every orientation is accepted. + // + // Requires viam-server >= 0.127.0; older servers silently ignore goal clouds. + OrientationToleranceDeg float64 `json:"orientation_tolerance_deg,omitempty"` + + // PositionToleranceMM is the per-axis positional leeway of the goal cloud, applied as + // a box along the goal frame's axes (worst-case corner deviation is sqrt(3) times this + // value). It must be large enough for the IK solver to land inside, or the cloud never + // matches and every move fails. Zero or unset means defaultPositionToleranceMM (1.0). + PositionToleranceMM float64 `json:"position_tolerance_mm,omitempty"` +``` + +- [ ] **Step 4: Add the shared validation helper** + +Add to `goal_cloud.go`: + +```go +// validateGoalCloudTolerances is shared by both arm models' Validate methods. Note NaN +// must be rejected explicitly: NaN comparisons are always false, so a NaN would slip +// through the range checks. +func validateGoalCloudTolerances(tolDeg, posTolMM float64) error { + if math.IsNaN(tolDeg) || tolDeg < 0 || tolDeg > 180 { + return fmt.Errorf("orientation_tolerance_deg must be in [0, 180], got %v", tolDeg) + } + if math.IsNaN(posTolMM) || posTolMM < 0 { + return fmt.Errorf("position_tolerance_mm must not be negative, got %v", posTolMM) + } + return nil +} +``` + +Call it from **both** `Validate` methods, before the `deps` construction and after the existing checks: + +```go + if err := validateGoalCloudTolerances(cfg.OrientationToleranceDeg, cfg.PositionToleranceMM); err != nil { + return nil, nil, err + } +``` + +- [ ] **Step 5: Run tests to verify they pass** + +Run: `go test . -run "ConfigValidateToleranceRanges" -v` +Expected: PASS + +- [ ] **Step 6: Commit** + +```bash +gofmt -s -w . && go vet ./cmd/module/ . +git add goal_cloud.go arm.go simulated.go goal_cloud_test.go +git commit -m "feat(arm): add orientation_tolerance_deg and position_tolerance_mm config" +``` + +--- + +## Task 9: Wire the hardware arm's `MoveToPosition` + +**Files:** +- Modify: `arm.go:138-160` (struct), `arm.go:353+` (`NewSO101`), `arm.go:482-504` (`MoveToPosition`) +- Test: `arm_move_to_position_test.go` (create) + +- [ ] **Step 1: Write the failing test** + +Create `arm_move_to_position_test.go`. Note the package at `go.viam.com/rdk/testutils/inject/motion` is named `inject`, so the alias is required. Tests live in package `so_arm`, so the struct can be built directly — no serial port needed. + +```go +package so_arm + +import ( + "context" + "testing" + + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" + "go.viam.com/rdk/components/arm" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/services/motion" + injectmotion "go.viam.com/rdk/testutils/inject/motion" +) + +// captureMotion returns an injected motion service that records the last MoveReq. +func captureMotion(got *motion.MoveReq) *injectmotion.MotionService { + ms := injectmotion.NewMotionService("builtin") + ms.MoveFunc = func(ctx context.Context, req motion.MoveReq) (bool, error) { + *got = req + return true, nil + } + return ms +} + +func TestHardwareArmMoveToPositionSendsGoalCloud(t *testing.T) { + var got motion.MoveReq + a := &so101{ + name: arm.Named("myarm"), + logger: logging.NewTestLogger(t), + motion: captureMotion(&got), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + + require.NoError(t, a.MoveToPosition(context.Background(), testGoal(), nil)) + + assert.Equal(t, "myarm", got.ComponentName) + assert.Equal(t, "myarm_origin", got.Destination.Parent()) + require.NotNil(t, got.Destination.GoalCloud, "the cone must ship to the planner") + assert.Equal(t, 180.0, got.Destination.GoalCloud.Theta) + assert.NotContains(t, got.Extra, "goal_metric_type", "the cone replaces position_only") +} + +func TestHardwareArmMoveToPositionHonorsMetricTypeOverride(t *testing.T) { + var got motion.MoveReq + a := &so101{ + name: arm.Named("myarm"), + logger: logging.NewTestLogger(t), + motion: captureMotion(&got), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + + require.NoError(t, a.MoveToPosition(context.Background(), testGoal(), + map[string]interface{}{"goal_metric_type": "position_only"})) + + assert.Nil(t, got.Destination.GoalCloud, "no cloud when the caller picks the metric") + assert.Equal(t, "position_only", got.Extra["goal_metric_type"]) +} +``` + +`testGoal()` comes from `goal_cloud_test.go` — same package `so_arm`, so it resolves without import. + +- [ ] **Step 2: Run test to verify it fails** + +Run: `go test . -run TestHardwareArmMoveToPosition -v` +Expected: FAIL — unknown field `goalCloud` + +- [ ] **Step 3: Add the struct field** + +In `arm.go`, add to the `so101` struct **above the `mu sync.RWMutex` line** (i.e. near +`cfg`/`opMgr`, not inside the mutex-guarded block) — the field is immutable after +construction, and placing it under `mu` would imply otherwise: + +```go + // goalCloud is the resolved approach-axis tolerance pair. Set once in NewSO101 and + // never mutated (the model is resource.AlwaysRebuild), so it needs no mutex. + goalCloud goalCloudConfig +``` + +- [ ] **Step 4: Resolve it in the constructor** + +In `NewSO101`, add to the `&so101{...}` literal: + +```go + goalCloud: resolveGoalCloudConfig(conf.OrientationToleranceDeg, conf.PositionToleranceMM, logger), +``` + +- [ ] **Step 5: Rewrite `MoveToPosition`** + +Replace `arm.go:482-504` entirely: + +```go +// MoveToPosition moves the arm's end-effector to the target pose. +// +// The SO-101 is a 5-DOF arm, so most six-DOF pose targets are unreachable. Rather than +// discard orientation wholesale, the planner is given an approach-axis cone (a +// referenceframe.PoseCloud) around the goal orientation: the tool's pointing direction is +// constrained to within orientation_tolerance_deg, while roll about that axis is free. +// +// Callers may bypass the cone via extra: "goal_metric_type" restores the old +// orientation-agnostic behavior, and "pose_cloud" supplies a raw cloud. See goal_cloud.go. +// +// Requires viam-server >= 0.127.0; older servers silently ignore goal clouds, which makes +// planning revert to strict six-DOF scoring and fail. +func (s *so101) MoveToPosition(ctx context.Context, pose spatialmath.Pose, extra map[string]interface{}) error { + dest, planExtra, path, err := buildMoveDestination( + fmt.Sprintf("%v_origin", s.Name().Name), pose, s.goalCloud, extra) + if err != nil { + return err + } + s.logger.Debugf("MoveToPosition goal cloud: %+v", dest.GoalCloud) + + _, err = s.motion.Move(ctx, motion.MoveReq{ + ComponentName: s.Name().Name, + Destination: dest, + Extra: planExtra, + }) + return wrapMoveErr(err, path, s.goalCloud) +} +``` + +Remove the now-unused `referenceframe` import from `arm.go` only if nothing else uses it — it almost certainly still does. + +- [ ] **Step 6: Run tests to verify they pass** + +Run: `go test . -run TestHardwareArmMoveToPosition -v` +Expected: PASS + +- [ ] **Step 7: Commit** + +```bash +gofmt -s -w . && go vet ./cmd/module/ . +git add arm.go arm_move_to_position_test.go +git commit -m "feat(arm): plan MoveToPosition with an approach-axis cone" +``` + +--- + +## Task 10: Wire the simulated arm's `MoveToPosition` + +Identical shape to Task 9. The simulated model must stay a faithful stand-in for motion-plan testing. + +**Files:** +- Modify: `simulated.go:114-140` (struct), `simulated.go:145+` (`newSimulatedSO101`), `simulated.go:296-318` (`MoveToPosition`) +- Test: `arm_move_to_position_test.go` + +- [ ] **Step 1: Write the failing test** + +```go +func TestSimulatedArmMoveToPositionSendsGoalCloud(t *testing.T) { + var got motion.MoveReq + s := &simulatedSO101{ + name: arm.Named("simarm"), + logger: logging.NewTestLogger(t), + motion: captureMotion(&got), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + + require.NoError(t, s.MoveToPosition(context.Background(), testGoal(), nil)) + + assert.Equal(t, "simarm_origin", got.Destination.Parent()) + require.NotNil(t, got.Destination.GoalCloud) + assert.Equal(t, 180.0, got.Destination.GoalCloud.Theta) +} + +func TestSimulatedArmMoveToPositionRequiresMotionService(t *testing.T) { + s := &simulatedSO101{name: arm.Named("simarm"), logger: logging.NewTestLogger(t)} + err := s.MoveToPosition(context.Background(), testGoal(), nil) + require.Error(t, err, "the nil-motion guard must still fire") +} +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `go test . -run TestSimulatedArmMoveToPosition -v` +Expected: FAIL — unknown field `goalCloud` + +- [ ] **Step 3: Add the struct field** + +In `simulated.go`, add to `simulatedSO101` **above the `// mu guards the fields below.` +comment** (near `motion`/`speed`), for the same reason as Task 9: + +```go + // goalCloud is the resolved approach-axis tolerance pair. Set once in + // newSimulatedSO101 and never mutated (resource.AlwaysRebuild), so it needs no mutex. + goalCloud goalCloudConfig +``` + +- [ ] **Step 4: Resolve it in the constructor** + +In `newSimulatedSO101`, add to the `&simulatedSO101{...}` literal: + +```go + goalCloud: resolveGoalCloudConfig(conf.OrientationToleranceDeg, conf.PositionToleranceMM, logger), +``` + +- [ ] **Step 5: Rewrite `MoveToPosition`** + +Replace `simulated.go:296-318`, keeping the nil-motion guard: + +```go +// MoveToPosition moves the arm's end effector to the target pose using the motion service. +// +// As with the devrel:so101:arm model, the planner is given an approach-axis cone around +// the goal orientation rather than discarding orientation via "position_only": the tool's +// pointing direction is constrained to within orientation_tolerance_deg, while roll about +// that axis is free. Callers may bypass the cone via extra; see goal_cloud.go. +// +// Requires viam-server >= 0.127.0; older servers silently ignore goal clouds. +func (s *simulatedSO101) MoveToPosition(ctx context.Context, pose spatialmath.Pose, extra map[string]interface{}) error { + if s.motion == nil { + return errors.New("MoveToPosition requires a motion service, which was not available at construction") + } + + dest, planExtra, path, err := buildMoveDestination( + fmt.Sprintf("%v_origin", s.name.Name), pose, s.goalCloud, extra) + if err != nil { + return err + } + s.logger.Debugf("MoveToPosition goal cloud: %+v", dest.GoalCloud) + + _, err = s.motion.Move(ctx, motion.MoveReq{ + ComponentName: s.name.Name, + Destination: dest, + Extra: planExtra, + }) + return wrapMoveErr(err, path, s.goalCloud) +} +``` + +- [ ] **Step 6: Run the full suite** + +Run: `go test ./cmd/module/ .` +Expected: PASS (except the environment-dependent `TestEnumerateSerialPorts`) + +- [ ] **Step 7: Commit** + +```bash +gofmt -s -w . && go vet ./cmd/module/ . +git add simulated.go arm_move_to_position_test.go +git commit -m "feat(simulated): plan MoveToPosition with an approach-axis cone" +``` + +--- + +## Task 11: README + +**Files:** +- Modify: `README.md` — the `## Model devrel:so101:arm` and `## Model devrel:so101:simulated` sections + +- [ ] **Step 1: Document both attributes in both model sections** + +Follow the existing attribute-table style. Cover, for each model: + +- `orientation_tolerance_deg` — approach-axis cone half-angle. **Roll is not constrained.** Range `[0, 180]`, default `30`. `0` means the default, **not** exact. The enforced cone is `acos(cos(tol) - 0.001)`, so always slightly wider than requested (30 → ~30.11) and never narrower than ~2.56. At `>= 177.44` every orientation is accepted. +- `position_tolerance_mm` — per-axis box leeway along the goal frame's axes, default `1.0`. Worst-case corner deviation is `sqrt(3)` times the value (~1.73mm at the default). Too small and the cloud never matches, so every move fails. + +- [ ] **Step 2: Add a short subsection on the escape hatch and server requirement** + +State plainly: +- Goal clouds require **viam-server >= 0.127.0**. Older servers accept and silently ignore them, which makes planning revert to strict six-DOF scoring and fail. +- `extra: {"goal_metric_type": "position_only"}` restores the previous orientation-agnostic behavior per-call. +- `extra: {"pose_cloud": {...}}` supplies a raw cloud. Keys are **lowercase**: `x`, `y`, `z`, `ox`, `oy`, `oz`, `theta`. The Viam docs' `OX` casing and protobuf's `o_x` are **rejected**. Omitted fields mean near-zero leeway — expert mode. + +- [ ] **Step 3: Commit** + +```bash +git add README.md +git commit -m "docs: document orientation_tolerance_deg and position_tolerance_mm" +``` + +--- + +## Task 12: CLAUDE.md gotcha + +This is the hard-won knowledge that makes the code look wrong-but-correct. Without it, the next reader "simplifies" `Theta: 180` → `0` and breaks every move while most tests still pass. + +**Files:** +- Modify: `CLAUDE.md` — the `## Gotchas` section + +- [ ] **Step 1: Add the gotcha** + +```markdown +- **`referenceframe.PoseCloud` semantics are counterintuitive and were established + empirically** (see `docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md`; + `goal_cloud.go` depends on all of it): + - `PoseInCloud` compares the *relative* pose `PoseBetween(goal, candidate)`, where zero + deviation is the orientation vector `(0,0,1)`. + - **`Theta` encodes the AZIMUTH of a tilt, not roll** (tilt about X → `Theta=90`, about + Y → `Theta=0`). So `coneToPoseCloud` must set `Theta: 180`; anything less rejects tilts + by azimuth before the cone is consulted. The cost is that roll cannot be constrained + alongside tilt — hence "approach-axis planning with free roll". + - **`OZ` alone defines the cone**; `OX`/`OY` are set to `1` (unconstrained). Valid across + `[0, 180]`, saturating at `acos(-0.999) = 177.4374°` — check saturation with + `cos(tol) <= -0.999`, not a literal degree comparison. + - **`defaultEpsilon = 0.001` is ADDED to every leeway**, not just zeroed ones. Zero + `X`/`Y`/`Z` therefore demands a 1-micron match, so positional leeway is mandatory; and + the effective cone is `acos(cos(tol) - 0.001)`, always slightly wider than requested + (floor ~2.56°). + - The cloud only *zeroes the score* when a candidate is inside it. A cloud that never + matches is **worse than no cloud**: planning reverts to strict six-DOF scoring and + fails. `position_only` sets `orientScale=0`, which makes any cloud meaningless — the + two are mutually exclusive. + - **There is no `ReferenceFrame` field.** v0.123.0 had one; v1.0.0 removed it while + leaving the struct's doc comment about reference frames in place as free-floating + prose, so it reads as though the field still exists. The complete field set is + `X, Y, Z, OX, OY, OZ, Theta`. +- **Goal clouds require viam-server >= 0.127.0.** RDK v0.125.0–v0.126.1 read `GoalCloud` in + `armplanning` but never decode it in `ProtobufToPoseInFrame`, so the cloud is silently + inert there; v0.127.0 is the first release with both halves. The module only *sends* the + cloud, so its own RDK version has no bearing on whether the server honors it. The minimum + is declared Registry-side — `meta.json` has no field for it. +``` + +- [ ] **Step 2: Commit** + +```bash +git add CLAUDE.md +git commit -m "docs: record PoseCloud semantics and the viam-server floor in CLAUDE.md" +``` + +--- + +## Task 13: Release checklist — declare the minimum viam-server version + +**This is a manual post-publish step.** It cannot be carried by `meta.json`, `.github/workflows/deploy.yml`, or `viamrobotics/build-action`. If it is skipped, users on older servers hit exactly the silent-ignore failure this design exists to make loud. + +- [ ] **Step 1: Verify the viam-server release exists** + +Confirm a viam-server **0.127.0** release exists before publishing the constraint. viam-server releases track RDK version numbers, but confirm rather than assume. + +- [ ] **Step 2: Publish, then set the constraint** + +After the GitHub release runs `.github/workflows/deploy.yml` and the version uploads to the registry, set the minimum viam-server version to **0.127.0** on the module's Registry page. + +- [ ] **Step 3: Verify end-to-end on real hardware** + +The unit tests prove the cloud is *built and shipped* correctly; they cannot prove the arm plans well. On a real SO-101 against viam-server >= 0.127.0: + +- A reachable pose with a plausible approach direction plans and executes. +- The tool's approach axis visibly respects the cone. +- Tighten `orientation_tolerance_deg` until planning fails, confirming the failure is loud and the error names the tolerance. +- **Tune `defaultPositionToleranceMM`.** The 1.0 default is a starting point, not a derived value: its worst-case corner deviation (~1.73mm) is comparable to the arm's repeatability rather than safely below it. Record what value actually works and update the default if warranted. + +--- + +## Verification checklist + +- [ ] `go build ./cmd/module/ .` passes +- [ ] `go test ./cmd/module/ .` passes (`TestEnumerateSerialPorts` may fail without serial hardware — not a regression) +- [ ] `go vet ./cmd/module/ .` passes +- [ ] `gofmt -s -w .` leaves no diff +- [ ] CI green (watch the `go` directive 1.25.1→1.25.9) +- [ ] Hardware verification from Task 13 done, and the positional default revisited diff --git a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md index 3394d3f..34cff77 100644 --- a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md +++ b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md @@ -126,13 +126,19 @@ the goal frame's axes. `position_tolerance_mm: 1.0` therefore permits a worst-ca deviation of `sqrt(3) x 1.001 ≈ 1.73mm`, not 1mm — verified: a per-axis offset of 1.0005mm (norm 1.7329mm) is accepted. -### ReferenceFrame is inert and must be left unset +### ReferenceFrame does not exist post-bump -`PoseCloud.ReferenceFrame` carries the struct's longest doc comment and invites use, but: -`PoseInCloud` never reads it, and `PoseCloud.ToProto()` does not serialize it — -`commonpb.PoseCloud` has no such field (only `x`, `y`, `z`, `o_x`, `o_y`, `o_z`, `theta`). -Any value set is silently dropped at the gRPC boundary and would break the round-trip -test. Leave it zero. +v0.123.0's `PoseCloud` had a `ReferenceFrame string` field (`pose_cloud.go:45`). **v1.0.0 +removes it.** Post-bump the fields are exactly `X`, `Y`, `Z`, `OX`, `OY`, `OZ`, `Theta`. + +The long doc comment about "evaluating in the reference frame of the object being asked to +move" survives in v1.0.0 as free-floating prose inside the struct, with no field attached — +which reliably reads as though the field still exists. It does not. Referencing it will not +compile, and it was never wire-serialized anyway: `commonpb.PoseCloud` carries only `x`, +`y`, `z`, `o_x`, `o_y`, `o_z`, `theta`. + +Consequence for the `pose_cloud` escape hatch: `reference_frame` is rejected as an unknown +key because it is **not a `PoseCloud` field at all**, not because it is inert. ## The mapping @@ -142,10 +148,11 @@ test. Leave it zero. OX: 1, OY: 1, // deliberately unconstrained; OZ alone defines the cone OZ: 1 - math.Cos(rad(toleranceDeg)), Theta: 180, // MUST be 180: Theta encodes tilt AZIMUTH, not roll - // ReferenceFrame deliberately unset: inert, and not wire-serialized. } ``` +These seven are the *complete* field set in v1.0.0; there is nothing else to set. + ## Config surface Both `SO101ArmConfig` (`arm.go:70`) and `SO101SimulatedArmConfig` (`simulated.go:42`) @@ -250,8 +257,8 @@ the point of an escape hatch, but it is sharp, and is documented as expert-mode. Note the casing is a genuine hazard: the Viam docs example renders these as `OX`/`OY`, and the protobuf JSON spells them `o_x`/`oX`. Neither is accepted. Unknown keys are therefore **rejected with an error** rather than ignored, so a typo or a copy-paste from the docs -fails loudly instead of silently producing a 1-micron cloud. `reference_frame` is -rejected for the same reason: it is inert and not wire-serialized. +fails loudly instead of silently producing a 1-micron cloud. `reference_frame` is rejected +under the same rule — it is not a `PoseCloud` field in v1.0.0. ## Components @@ -475,9 +482,10 @@ the pinned version, so this needs no new infrastructure. - **CLAUDE.md**: update the RDK version, and add a gotcha covering `Theta`-encodes-azimuth (forcing `Theta: 180`), `OZ`-alone-defines-the-cone (valid across 0-180, saturating at 177.44), the additive `0.001` epsilon and the ~2.56° effective-cone floor it implies, - that `ReferenceFrame` is inert and unserialized, and that goal clouds need viam-server - >= 0.127.0 (v0.125-v0.126 read the field but never decode it). Without it, the next - reader will "simplify" `Theta: 180` to `0` and break every move. + that `ReferenceFrame` was removed in v1.0.0 (v0.123.0 had it) leaving + `X, Y, Z, OX, OY, OZ, Theta` as the complete field set, and that goal clouds need + viam-server >= 0.127.0 (v0.125-v0.126 read the field but never decode it). Without it, + the next reader will "simplify" `Theta: 180` to `0` and break every move. ## Out of scope From d4ec67af922cc59a4afa48bb9fa7db8979d3830e Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 12:57:35 -0400 Subject: [PATCH 03/23] chore: bump rdk to v1.0.0 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. --- CLAUDE.md | 2 +- go.mod | 25 +++++++++++---------- go.sum | 66 +++++++++++++++++++++++++++++++++---------------------- 3 files changed, 54 insertions(+), 39 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 163a343..4c1f9d7 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -2,7 +2,7 @@ `viam-devrel/so-101` — a Viam module for the LeRobot **SO-101 / SO-ARM101** robot arm (TheRobotStudio open hardware: 6× Feetech STS3215 servos on one serial/USB bus @ 1 Mbaud). -Go module `so_arm`, Go 1.25, `go.viam.com/rdk v0.123.0`. It can drive either the leader or +Go module `so_arm`, Go 1.25, `go.viam.com/rdk v1.0.0`. It can drive either the leader or the follower arm, or both as separate components for mirrored teleoperation. ## Models diff --git a/go.mod b/go.mod index 0fce31e..cc039ff 100644 --- a/go.mod +++ b/go.mod @@ -1,6 +1,6 @@ module so_arm -go 1.25.1 +go 1.25.9 require ( github.com/golang/geo v0.0.0-20230421003525-6adc56603217 @@ -8,10 +8,10 @@ require ( github.com/pkg/errors v0.9.1 github.com/stretchr/testify v1.11.1 go.bug.st/serial v1.6.4 - go.viam.com/api v0.1.539 - go.viam.com/rdk v0.123.0 + go.viam.com/api v0.1.566 + go.viam.com/rdk v1.0.0 go.viam.com/test v1.2.4 - go.viam.com/utils v0.4.19 + go.viam.com/utils v0.6.6 google.golang.org/protobuf v1.36.11 ) @@ -26,6 +26,7 @@ require ( codeberg.org/go-latex/latex v0.1.0 // indirect codeberg.org/go-pdf/fpdf v0.10.0 // indirect git.sr.ht/~sbinet/gg v0.6.0 // indirect + github.com/Microsoft/go-winio v0.6.2 // indirect github.com/a8m/envsubst v1.4.2 // indirect github.com/ajstarks/svgo v0.0.0-20211024235047-1546f124cd8b // indirect github.com/apache/arrow/go/arrow v0.0.0-20201229220542-30ce2eb5d4dc // indirect @@ -78,7 +79,7 @@ require ( github.com/fsnotify/fsnotify v1.9.0 // indirect github.com/fullstorydev/grpcurl v1.8.6 // indirect github.com/go-gl/mathgl v1.0.0 // indirect - github.com/go-jose/go-jose/v4 v4.1.3 // indirect + github.com/go-jose/go-jose/v4 v4.1.4 // indirect github.com/go-logr/logr v1.4.3 // indirect github.com/go-logr/stdr v1.2.2 // indirect github.com/go-viper/mapstructure/v2 v2.4.0 // indirect @@ -170,11 +171,11 @@ require ( go.opentelemetry.io/auto/sdk v1.2.1 // indirect go.opentelemetry.io/contrib/instrumentation/google.golang.org/grpc/otelgrpc v0.54.0 // indirect go.opentelemetry.io/contrib/instrumentation/net/http/otelhttp v0.54.0 // indirect - go.opentelemetry.io/otel v1.40.0 // indirect + go.opentelemetry.io/otel v1.43.0 // indirect go.opentelemetry.io/otel/exporters/otlp/otlptrace v1.40.0 // indirect - go.opentelemetry.io/otel/metric v1.40.0 // indirect - go.opentelemetry.io/otel/sdk v1.40.0 // indirect - go.opentelemetry.io/otel/trace v1.40.0 // indirect + go.opentelemetry.io/otel/metric v1.43.0 // indirect + go.opentelemetry.io/otel/sdk v1.43.0 // indirect + go.opentelemetry.io/otel/trace v1.43.0 // indirect go.opentelemetry.io/proto/otlp v1.9.0 // indirect go.uber.org/goleak v1.3.0 // indirect go.uber.org/multierr v1.11.0 // indirect @@ -187,18 +188,18 @@ require ( golang.org/x/net v0.49.0 // indirect golang.org/x/oauth2 v0.34.0 // indirect golang.org/x/sync v0.19.0 // indirect - golang.org/x/sys v0.40.0 // indirect + golang.org/x/sys v0.42.0 // indirect golang.org/x/text v0.33.0 // indirect golang.org/x/time v0.10.0 // indirect golang.org/x/tools v0.40.0 // indirect golang.org/x/xerrors v0.0.0-20220907171357-04be3eba64a2 // indirect - gonum.org/v1/gonum v0.16.0 // indirect + gonum.org/v1/gonum v0.17.0 // indirect gonum.org/v1/plot v0.15.2 // indirect google.golang.org/api v0.196.0 // indirect google.golang.org/genproto v0.0.0-20240903143218-8af14fe29dc1 // indirect google.golang.org/genproto/googleapis/api v0.0.0-20260128011058-8636f8732409 // indirect google.golang.org/genproto/googleapis/rpc v0.0.0-20260128011058-8636f8732409 // indirect - google.golang.org/grpc v1.79.3 // indirect + google.golang.org/grpc v1.80.0 // indirect gopkg.in/natefinch/lumberjack.v2 v2.2.1 // indirect gopkg.in/yaml.v3 v3.0.1 // indirect gorgonia.org/tensor v0.9.24 // indirect diff --git a/go.sum b/go.sum index 1659f9a..cd75209 100644 --- a/go.sum +++ b/go.sum @@ -65,6 +65,8 @@ github.com/Knetic/govaluate v3.0.1-0.20171022003610-9aa49832a739+incompatible/go github.com/Masterminds/semver v1.5.0 h1:H65muMkzWKEuNDnfl9d70GUjFniHKHRbFPGBuZ3QEww= github.com/Masterminds/semver/v3 v3.3.1 h1:QtNSWtVZ3nBfk8mAOu/B6v7FMJ+NHTIgUPi7rj+4nv4= github.com/Masterminds/semver/v3 v3.3.1/go.mod h1:4V+yj/TJE1HU9XfppCwVMZq3I84lprf4nC11bSS5beM= +github.com/Microsoft/go-winio v0.6.2 h1:F2VQgta7ecxGYO8k3ZZz3RS8fVIXVxONVUPlNERoyfY= +github.com/Microsoft/go-winio v0.6.2/go.mod h1:yd8OoFMLzJbo9gZq8j5qaps8bJ9aShtEA8Ipt1oGCvU= github.com/Shopify/sarama v1.19.0/go.mod h1:FVkBWblsNy7DGZRfXLU0O9RCGt5g3g3yEuWXgklEdEo= github.com/Shopify/toxiproxy v2.1.4+incompatible/go.mod h1:OXgGpZ6Cli1/URJOF1DMxUHB2q5Ap20/P/eIdh4G0pI= github.com/VividCortex/gohistogram v1.0.0/go.mod h1:Pf5mBqqDxYaXu3hDrrU+w6nw50o/4+TcAqDqk/vUH7g= @@ -174,6 +176,16 @@ github.com/census-instrumentation/opencensus-proto v0.4.1/go.mod h1:4T9NM4+4Vw91 github.com/cespare/xxhash/v2 v2.1.1/go.mod h1:VGX0DQ3Q6kWi7AoAeZDth3/j3BFtOZR5XLFGgcrjCOs= github.com/cespare/xxhash/v2 v2.3.0 h1:UL815xU9SqsFlibzuggzjXhog7bL6oX9BbNZnL2UFvs= github.com/cespare/xxhash/v2 v2.3.0/go.mod h1:VGX0DQ3Q6kWi7AoAeZDth3/j3BFtOZR5XLFGgcrjCOs= +github.com/charmbracelet/x/conpty v0.1.0 h1:4zc8KaIcbiL4mghEON8D72agYtSeIgq8FSThSPQIb+U= +github.com/charmbracelet/x/conpty v0.1.0/go.mod h1:rMFsDJoDwVmiYM10aD4bH2XiRgwI7NYJtQgl5yskjEQ= +github.com/charmbracelet/x/errors v0.0.0-20240508181413-e8d8b6e2de86 h1:JSt3B+U9iqk37QUU2Rvb6DSBYRLtWqFqfxf8l5hOZUA= +github.com/charmbracelet/x/errors v0.0.0-20240508181413-e8d8b6e2de86/go.mod h1:2P0UgXMEa6TsToMSuFqKFQR+fZTO9CNGUNokkPatT/0= +github.com/charmbracelet/x/term v0.2.1 h1:AQeHeLZ1OqSXhrAWpYUtZyX1T3zVxfpZuEQMIQaGIAQ= +github.com/charmbracelet/x/term v0.2.1/go.mod h1:oQ4enTYFV7QN4m0i9mzHrViD7TQKvNEEkHUMCmsxdUg= +github.com/charmbracelet/x/termios v0.1.1 h1:o3Q2bT8eqzGnGPOYheoYS8eEleT5ZVNYNy8JawjaNZY= +github.com/charmbracelet/x/termios v0.1.1/go.mod h1:rB7fnv1TgOPOyyKRJ9o+AsTU/vK5WHJ2ivHeut/Pcwo= +github.com/charmbracelet/x/xpty v0.1.2 h1:Pqmu4TEJ8KeA9uSkISKMU3f+C1F6OGBn8ABuGlqCbtI= +github.com/charmbracelet/x/xpty v0.1.2/go.mod h1:XK2Z0id5rtLWcpeNiMYBccNNBrP2IJnzHI0Lq13Xzq4= github.com/chenzhekl/goply v0.0.0-20190930133256-258c2381defd h1:S0onsSZ3RawTrm4KrxPs7KoPT8R8aBZmbXvOUGkV5O8= github.com/chenzhekl/goply v0.0.0-20190930133256-258c2381defd/go.mod h1:P2dOeu3SNXtjA5VOH7tF0AnGm/eYrst9YA89b36c35I= github.com/chewxy/hm v1.0.0 h1:zy/TSv3LV2nD3dwUEQL2VhXeoXbb9QkpmdRAVUFiA6k= @@ -280,8 +292,8 @@ github.com/go-gl/glfw/v3.3/glfw v0.0.0-20191125211704-12ad95a8df72/go.mod h1:tQ2 github.com/go-gl/glfw/v3.3/glfw v0.0.0-20200222043503-6f7a984d4dc4/go.mod h1:tQ2UAYgL5IevRw8kRxooKSPJfGvJ9fJQFa0TUsXzTg8= github.com/go-gl/mathgl v1.0.0 h1:t9DznWJlXxxjeeKLIdovCOVJQk/GzDEL7h/h+Ro2B68= github.com/go-gl/mathgl v1.0.0/go.mod h1:yhpkQzEiH9yPyxDUGzkmgScbaBVlhC06qodikEM0ZwQ= -github.com/go-jose/go-jose/v4 v4.1.3 h1:CVLmWDhDVRa6Mi/IgCgaopNosCaHz7zrMeF9MlZRkrs= -github.com/go-jose/go-jose/v4 v4.1.3/go.mod h1:x4oUasVrzR7071A4TnHLGSPpNOm2a21K9Kf04k1rs08= +github.com/go-jose/go-jose/v4 v4.1.4 h1:moDMcTHmvE6Groj34emNPLs/qtYXRVcd6S7NHbHz3kA= +github.com/go-jose/go-jose/v4 v4.1.4/go.mod h1:x4oUasVrzR7071A4TnHLGSPpNOm2a21K9Kf04k1rs08= github.com/go-kit/kit v0.8.0/go.mod h1:xBxKIO96dXMWWy0MnWVtmwkA9/13aqxPnvrjFYMA2as= github.com/go-kit/kit v0.9.0/go.mod h1:xBxKIO96dXMWWy0MnWVtmwkA9/13aqxPnvrjFYMA2as= github.com/go-kit/kit v0.10.0/go.mod h1:xUsJbQ/Fp4kEt7AFgCuvyX4a71u8h9jB8tj/ORgOZ7o= @@ -294,6 +306,8 @@ github.com/go-logr/logr v1.4.3 h1:CjnDlHq8ikf6E492q6eKboGOC0T8CDaOvkHCIg8idEI= github.com/go-logr/logr v1.4.3/go.mod h1:9T104GzyrTigFIr8wt5mBrctHMim0Nb2HLGrmQ40KvY= github.com/go-logr/stdr v1.2.2 h1:hSWxHoqTgW2S2qGc0LTAI563KZ5YKYRhT3MFKZMbjag= github.com/go-logr/stdr v1.2.2/go.mod h1:mMo/vtBO5dYbehREoey6XUKy/eSumjCCveDpRre4VKE= +github.com/go-nlopt/nlopt v0.0.0-20230219125344-443d3362dcb5 h1:JlR5qQ/dy4NPpeKld/CJR6cIcL0ll4OQ7ieylY5kJ20= +github.com/go-nlopt/nlopt v0.0.0-20230219125344-443d3362dcb5/go.mod h1:crLzNxWuUkZODn9zme0coCcBvPQrM3hnbQWR3uolF8o= github.com/go-ole/go-ole v1.2.6 h1:/Fpf6oFPoeFik9ty7siob0G6Ke8QvQEuVcuChpwXzpY= github.com/go-ole/go-ole v1.2.6/go.mod h1:pprOEPIfldk/42T2oK7lQ4v4JSDwmV0As9GaiUsvbm0= github.com/go-playground/assert/v2 v2.0.1/go.mod h1:VDjEfimB/XKnb+ZQfWdccd7VUvScMdVu0Titje2rxJ4= @@ -640,8 +654,8 @@ github.com/pion/mdns v0.0.12 h1:CiMYlY+O0azojWDmxdNr7ADGrnZ+V6Ilfner+6mSVK8= github.com/pion/mdns v0.0.12/go.mod h1:VExJjv8to/6Wqm1FXK+Ii/Z9tsVk/F5sD/N70cnYFbk= github.com/pion/mdns/v2 v2.1.0 h1:3IJ9+Xio6tWYjhN6WwuY142P/1jA0D5ERaIqawg/fOY= github.com/pion/mdns/v2 v2.1.0/go.mod h1:pcez23GdynwcfRU1977qKU0mDxSeucttSHbCSfFOd9A= -github.com/pion/mediadevices v0.9.0 h1:3ozqxVjN0PTbb1IfTbZ4af4R/fQ5cSlbYL3euCZag7w= -github.com/pion/mediadevices v0.9.0/go.mod h1:0dGJQq8VCPo7AXWmhqRITIFyw66uylwDecq7oN+G3gM= +github.com/pion/mediadevices v0.10.0 h1:xsOwvucz5ZLBABae11bx4Nzofca8NbMcOHFrwgQPgiI= +github.com/pion/mediadevices v0.10.0/go.mod h1:0dGJQq8VCPo7AXWmhqRITIFyw66uylwDecq7oN+G3gM= github.com/pion/randutil v0.1.0 h1:CFG1UdESneORglEsnimhUjf33Rwjubwj6xfiOXBa3mA= github.com/pion/randutil v0.1.0/go.mod h1:XcJrSMMbbMRhASFVOlj/5hQial/Y8oH/HVo7TBZq+j8= github.com/pion/rtcp v1.2.12/go.mod h1:sn6qjxvnwyAkkPzPULIbVqSKI5Dv54Rv7VG0kNxh9L4= @@ -865,20 +879,20 @@ go.opentelemetry.io/contrib/instrumentation/google.golang.org/grpc/otelgrpc v0.5 go.opentelemetry.io/contrib/instrumentation/google.golang.org/grpc/otelgrpc v0.54.0/go.mod h1:B9yO6b04uB80CzjedvewuqDhxJxi11s7/GtiGa8bAjI= go.opentelemetry.io/contrib/instrumentation/net/http/otelhttp v0.54.0 h1:TT4fX+nBOA/+LUkobKGW1ydGcn+G3vRw9+g5HwCphpk= go.opentelemetry.io/contrib/instrumentation/net/http/otelhttp v0.54.0/go.mod h1:L7UH0GbB0p47T4Rri3uHjbpCFYrVrwc1I25QhNPiGK8= -go.opentelemetry.io/otel v1.40.0 h1:oA5YeOcpRTXq6NN7frwmwFR0Cn3RhTVZvXsP4duvCms= -go.opentelemetry.io/otel v1.40.0/go.mod h1:IMb+uXZUKkMXdPddhwAHm6UfOwJyh4ct1ybIlV14J0g= +go.opentelemetry.io/otel v1.43.0 h1:mYIM03dnh5zfN7HautFE4ieIig9amkNANT+xcVxAj9I= +go.opentelemetry.io/otel v1.43.0/go.mod h1:JuG+u74mvjvcm8vj8pI5XiHy1zDeoCS2LB1spIq7Ay0= go.opentelemetry.io/otel/exporters/otlp/otlptrace v1.40.0 h1:QKdN8ly8zEMrByybbQgv8cWBcdAarwmIPZ6FThrWXJs= go.opentelemetry.io/otel/exporters/otlp/otlptrace v1.40.0/go.mod h1:bTdK1nhqF76qiPoCCdyFIV+N/sRHYXYCTQc+3VCi3MI= go.opentelemetry.io/otel/exporters/otlp/otlptrace/otlptracegrpc v1.40.0 h1:DvJDOPmSWQHWywQS6lKL+pb8s3gBLOZUtw4N+mavW1I= go.opentelemetry.io/otel/exporters/otlp/otlptrace/otlptracegrpc v1.40.0/go.mod h1:EtekO9DEJb4/jRyN4v4Qjc2yA7AtfCBuz2FynRUWTXs= -go.opentelemetry.io/otel/metric v1.40.0 h1:rcZe317KPftE2rstWIBitCdVp89A2HqjkxR3c11+p9g= -go.opentelemetry.io/otel/metric v1.40.0/go.mod h1:ib/crwQH7N3r5kfiBZQbwrTge743UDc7DTFVZrrXnqc= -go.opentelemetry.io/otel/sdk v1.40.0 h1:KHW/jUzgo6wsPh9At46+h4upjtccTmuZCFAc9OJ71f8= -go.opentelemetry.io/otel/sdk v1.40.0/go.mod h1:Ph7EFdYvxq72Y8Li9q8KebuYUr2KoeyHx0DRMKrYBUE= -go.opentelemetry.io/otel/sdk/metric v1.40.0 h1:mtmdVqgQkeRxHgRv4qhyJduP3fYJRMX4AtAlbuWdCYw= -go.opentelemetry.io/otel/sdk/metric v1.40.0/go.mod h1:4Z2bGMf0KSK3uRjlczMOeMhKU2rhUqdWNoKcYrtcBPg= -go.opentelemetry.io/otel/trace v1.40.0 h1:WA4etStDttCSYuhwvEa8OP8I5EWu24lkOzp+ZYblVjw= -go.opentelemetry.io/otel/trace v1.40.0/go.mod h1:zeAhriXecNGP/s2SEG3+Y8X9ujcJOTqQ5RgdEJcawiA= +go.opentelemetry.io/otel/metric v1.43.0 h1:d7638QeInOnuwOONPp4JAOGfbCEpYb+K6DVWvdxGzgM= +go.opentelemetry.io/otel/metric v1.43.0/go.mod h1:RDnPtIxvqlgO8GRW18W6Z/4P462ldprJtfxHxyKd2PY= +go.opentelemetry.io/otel/sdk v1.43.0 h1:pi5mE86i5rTeLXqoF/hhiBtUNcrAGHLKQdhg4h4V9Dg= +go.opentelemetry.io/otel/sdk v1.43.0/go.mod h1:P+IkVU3iWukmiit/Yf9AWvpyRDlUeBaRg6Y+C58QHzg= +go.opentelemetry.io/otel/sdk/metric v1.43.0 h1:S88dyqXjJkuBNLeMcVPRFXpRw2fuwdvfCGLEo89fDkw= +go.opentelemetry.io/otel/sdk/metric v1.43.0/go.mod h1:C/RJtwSEJ5hzTiUz5pXF1kILHStzb9zFlIEe85bhj6A= +go.opentelemetry.io/otel/trace v1.43.0 h1:BkNrHpup+4k4w+ZZ86CZoHHEkohws8AY+WTX09nk+3A= +go.opentelemetry.io/otel/trace v1.43.0/go.mod h1:/QJhyVBUUswCphDVxq+8mld+AvhXZLhe+8WVFxiFff0= go.opentelemetry.io/proto/otlp v0.7.0/go.mod h1:PqfVotwruBrMGOCsRd/89rSnXhoiJIqeYNgFYFoEGnI= go.opentelemetry.io/proto/otlp v1.9.0 h1:l706jCMITVouPOqEnii2fIAuO3IVGBRPV5ICjceRb/A= go.opentelemetry.io/proto/otlp v1.9.0/go.mod h1:xE+Cx5E/eEHw+ISFkwPLwCZefwVjY+pqKg1qcK03+/4= @@ -902,14 +916,14 @@ go.uber.org/zap v1.13.0/go.mod h1:zwrFLgMcdUuIBviXEYEH1YKNaOBnKXsx2IPda5bBwHM= go.uber.org/zap v1.18.1/go.mod h1:xg/QME4nWcxGxrpdeYfq7UvYrLh66cuVKdrbD1XF/NI= go.uber.org/zap v1.27.0 h1:aJMhYGrd5QSmlpLMr2MftRKl7t8J8PTZPA732ud/XR8= go.uber.org/zap v1.27.0/go.mod h1:GB2qFLM7cTU87MWRP2mPIjqfIDnGu+VIO4V/SdhGo2E= -go.viam.com/api v0.1.539 h1:kXUPtScO2Fqdsd1Ifp47G9LpUOrzZEWs06Gf+Bkw17A= -go.viam.com/api v0.1.539/go.mod h1:qSrz3j4+QlXvw7ANs1G2fsX532vUQzLBbgaTxMu3lAw= -go.viam.com/rdk v0.123.0 h1:b4mfTMGrGgRMjEcvQypBc6BiTzjGtl1//YdZeJI70nM= -go.viam.com/rdk v0.123.0/go.mod h1:T89ZACp+vof67M7YbLgVFq5nzc7xVKTsK2a3GIAXLaw= +go.viam.com/api v0.1.566 h1:o5nWDj6at04Y8nHGC7mZQDA6NIuGdPpwqhNLY+LQtfs= +go.viam.com/api v0.1.566/go.mod h1:nVe4WXrtc8aupJ8OWXSYx6KhCiOkr3VCbkwxD4D41xQ= +go.viam.com/rdk v1.0.0 h1:i7nTVaccN7N045xwNyYm3vBspuY+NzwT4PX9vLw7GxU= +go.viam.com/rdk v1.0.0/go.mod h1:1dl6guxr5j+Th2CsQNcIF5uNgVaFuIiKHZm1Cu5G7Bo= go.viam.com/test v1.2.4 h1:JYgZhsuGAQ8sL9jWkziAXN9VJJiKbjoi9BsO33TW3ug= go.viam.com/test v1.2.4/go.mod h1:zI2xzosHdqXAJ/kFqcN+OIF78kQuTV2nIhGZ8EzvaJI= -go.viam.com/utils v0.4.19 h1:RK17IbyqwE/Zt2Y+RRoLxlooI74w4kihDSyEeBQ4Zus= -go.viam.com/utils v0.4.19/go.mod h1:dElxHWXNzmITjAm4M9vpCWFXWdtGnAKYmciUm8poHLw= +go.viam.com/utils v0.6.6 h1:1R+SpKz1McCAIC0JshgkfHy+uDu4okNmDq/AYl9m8iE= +go.viam.com/utils v0.6.6/go.mod h1:sAqzMj1M4weq/0HIqdfzrjCZ6zIzDhUemH8Tml1Hyag= go4.org/unsafe/assume-no-moving-gc v0.0.0-20230525183740-e7c30c78aeb2 h1:WJhcL4p+YeDxmZWg141nRm7XC8IDmhz7lk5GpadO1Sg= go4.org/unsafe/assume-no-moving-gc v0.0.0-20230525183740-e7c30c78aeb2/go.mod h1:FftLjUGFEDu5k8lt0ddY+HcrH/qU/0qk+H8j9/nTl3E= goji.io v2.0.2+incompatible h1:uIssv/elbKRLznFUy3Xj4+2Mz/qKhek/9aZQDUMae7c= @@ -1091,8 +1105,8 @@ golang.org/x/sys v0.11.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= golang.org/x/sys v0.16.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= golang.org/x/sys v0.17.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= golang.org/x/sys v0.18.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= -golang.org/x/sys v0.40.0 h1:DBZZqJ2Rkml6QMQsZywtnjnnGvHza6BTfYFWY9kjEWQ= -golang.org/x/sys v0.40.0/go.mod h1:OgkHotnGiDImocRcuBABYBEXf8A9a87e/uXjp9XT3ks= +golang.org/x/sys v0.42.0 h1:omrd2nAlyT5ESRdCLYdm3+fMfNFE/+Rf4bDIQImRJeo= +golang.org/x/sys v0.42.0/go.mod h1:4GL1E5IUh+htKOUEOaiffhrAeqysfVGipDYzABqnCmw= golang.org/x/term v0.0.0-20201126162022-7de9c90e9dd1/go.mod h1:bj7SfCRtBDWHUb9snDiAeCFNEtKQo2Wmx5Cou7ajbmo= golang.org/x/term v0.0.0-20210927222741-03fcf44c2211/go.mod h1:jbD1KX2456YbFQfuXm/mYQcufACuNUgVhRMnK/tPxf8= golang.org/x/term v0.5.0/go.mod h1:jMB1sMXY+tzblOD4FWmEbocvup2/aLOaQEp7JmGp78k= @@ -1175,8 +1189,8 @@ golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8T golang.org/x/xerrors v0.0.0-20200804184101-5ec99f83aff1/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= golang.org/x/xerrors v0.0.0-20220907171357-04be3eba64a2 h1:H2TDz8ibqkAF6YGhCdN3jS9O0/s90v0rJh3X/OLHEUk= golang.org/x/xerrors v0.0.0-20220907171357-04be3eba64a2/go.mod h1:K8+ghG5WaK9qNqU5K3HdILfMLy1f3aNYFI/wnl100a8= -gonum.org/v1/gonum v0.16.0 h1:5+ul4Swaf3ESvrOnidPp4GZbzf0mxVQpDCYUQE7OJfk= -gonum.org/v1/gonum v0.16.0/go.mod h1:fef3am4MQ93R2HHpKnLk4/Tbh/s0+wqD5nfa6Pnwy4E= +gonum.org/v1/gonum v0.17.0 h1:VbpOemQlsSMrYmn7T2OUvQ4dqxQXU+ouZFQsZOx50z4= +gonum.org/v1/gonum v0.17.0/go.mod h1:El3tOrEuMpv2UdMrbNlKEh9vd86bmQ6vqIcDwxEOc1E= gonum.org/v1/plot v0.15.2 h1:Tlfh/jBk2tqjLZ4/P8ZIwGrLEWQSPDLRm/SNWKNXiGI= gonum.org/v1/plot v0.15.2/go.mod h1:DX+x+DWso3LTha+AdkJEv5Txvi+Tql3KAGkehP0/Ubg= google.golang.org/api v0.3.1/go.mod h1:6wY9I6uQWHQ8EM57III9mq/AjF+i8G65rmVagqKMtkk= @@ -1248,8 +1262,8 @@ google.golang.org/grpc v1.33.1/go.mod h1:fr5YgcSWrqhRRxogOsw7RzIpsmvOZ6IcH4kBYTp google.golang.org/grpc v1.33.2/go.mod h1:JMHMWHQWaTccqQQlmk3MJZS+GWXOdAesneDmEnv2fbc= google.golang.org/grpc v1.36.0/go.mod h1:qjiiYl8FncCW8feJPdyg3v6XW24KsRHe+dy9BAGRRjU= google.golang.org/grpc v1.44.0/go.mod h1:k+4IHHFw41K8+bbowsex27ge2rCb65oeWqe4jJ590SU= -google.golang.org/grpc v1.79.3 h1:sybAEdRIEtvcD68Gx7dmnwjZKlyfuc61Dyo9pGXXkKE= -google.golang.org/grpc v1.79.3/go.mod h1:KmT0Kjez+0dde/v2j9vzwoAScgEPx/Bw1CYChhHLrHQ= +google.golang.org/grpc v1.80.0 h1:Xr6m2WmWZLETvUNvIUmeD5OAagMw3FiKmMlTdViWsHM= +google.golang.org/grpc v1.80.0/go.mod h1:ho/dLnxwi3EDJA4Zghp7k2Ec1+c2jqup0bFkw07bwF4= google.golang.org/grpc/cmd/protoc-gen-go-grpc v0.0.0-20200910201057-6591123024b3/go.mod h1:6Kw0yEErY5E/yWrBtf03jp27GLLJujG4z/JK95pnjjw= google.golang.org/protobuf v0.0.0-20200109180630-ec00e32a8dfd/go.mod h1:DFci5gLYBciE7Vtevhsrf46CRTquxDuWsQurQQe4oz8= google.golang.org/protobuf v0.0.0-20200221191635-4d8936d0db64/go.mod h1:kwYJMbMJ01Woi6D6+Kah6886xMZcty6N08ah7+eCXa0= From 0693f0fb619dfb62118097c4f1d7dabf35645966 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 13:15:18 -0400 Subject: [PATCH 04/23] feat(arm): add goalCloudConfig with defaulting and construction warnings --- goal_cloud.go | 69 ++++++++++++++++++++++++++++++++++++++++++++++ goal_cloud_test.go | 53 +++++++++++++++++++++++++++++++++++ 2 files changed, 122 insertions(+) create mode 100644 goal_cloud.go create mode 100644 goal_cloud_test.go diff --git a/goal_cloud.go b/goal_cloud.go new file mode 100644 index 0000000..a906850 --- /dev/null +++ b/goal_cloud.go @@ -0,0 +1,69 @@ +package so_arm + +import ( + "math" + + "go.viam.com/rdk/logging" + "go.viam.com/rdk/utils" +) + +const ( + // defaultOrientationToleranceDeg is the approach-axis cone half-angle used when + // orientation_tolerance_deg is zero or unset. + defaultOrientationToleranceDeg = 30.0 + + // defaultPositionToleranceMM is the per-axis positional leeway used when + // position_tolerance_mm is zero or unset. It must stay comfortably above the IK + // solver's convergence: a cloud the solver can never land inside is worse than no + // cloud at all, because planning reverts to strict 6-DOF scoring and always fails. + defaultPositionToleranceMM = 1.0 + + // warnPositionToleranceMM is the point above which a cloud is loose enough that the + // planner may report success visibly far from the goal. + warnPositionToleranceMM = 10.0 + + // poseCloudSaturationCos is the cosine at which the cone accepts every orientation. + // PoseInCloud adds a 0.001 epsilon to the OZ leeway and the maximum possible + // deviation is 2.0, so saturation begins where 1-cos(a)+0.001 >= 2. Expressed as a + // cosine rather than the rounded 177.44 degrees: the true threshold is 177.4374, and + // a literal degree comparison would miss the [177.4374, 177.44) band. + poseCloudSaturationCos = -0.999 +) + +// goalCloudConfig is the resolved tolerance pair, letting both arm models share one code +// path despite their separate config structs. Defaults are already applied. +type goalCloudConfig struct { + OrientationToleranceDeg float64 + PositionToleranceMM float64 +} + +// resolveGoalCloudConfig applies defaults to zero or unset values and emits the two +// construction warnings. It is the single owner of both, so each constructor is one call +// and neither arm model re-implements the thresholds. logger may be nil (unit tests). +func resolveGoalCloudConfig(tolDeg, posTolMM float64, logger logging.Logger) goalCloudConfig { + cfg := goalCloudConfig{OrientationToleranceDeg: tolDeg, PositionToleranceMM: posTolMM} + if cfg.OrientationToleranceDeg == 0 { + cfg.OrientationToleranceDeg = defaultOrientationToleranceDeg + } + if cfg.PositionToleranceMM == 0 { + cfg.PositionToleranceMM = defaultPositionToleranceMM + } + + if logger == nil { + return cfg + } + if cfg.PositionToleranceMM > warnPositionToleranceMM { + logger.Warnf("position_tolerance_mm=%g is large; the planner may report success up to %.2fmm "+ + "from the goal (the cloud is a per-axis box, so the worst-case corner is sqrt(3) times this)", + cfg.PositionToleranceMM, math.Sqrt(3)*cfg.PositionToleranceMM) + } + // Cite 177.4374 (the true acos(-0.999) threshold), NOT the rounded 177.44 used in + // user-facing docs: this guard fires across [177.4374, 177.44), so quoting 177.44 here + // would contradict the action taken. %g likewise avoids printing 177.4374 as "177.4". + if math.Cos(utils.DegToRad(cfg.OrientationToleranceDeg)) <= poseCloudSaturationCos { + logger.Warnf("orientation_tolerance_deg=%g saturates the approach-axis cone (>= 177.4374); "+ + "every orientation will be accepted, which is equivalent to ignoring orientation", + cfg.OrientationToleranceDeg) + } + return cfg +} diff --git a/goal_cloud_test.go b/goal_cloud_test.go new file mode 100644 index 0000000..df8ada7 --- /dev/null +++ b/goal_cloud_test.go @@ -0,0 +1,53 @@ +package so_arm + +import ( + "testing" + + "github.com/stretchr/testify/assert" + "go.viam.com/rdk/logging" +) + +func TestResolveGoalCloudConfigDefaults(t *testing.T) { + // Zero and unset are indistinguishable for a float64 with omitempty; both default. + got := resolveGoalCloudConfig(0, 0, nil) + assert.Equal(t, defaultOrientationToleranceDeg, got.OrientationToleranceDeg) + assert.Equal(t, defaultPositionToleranceMM, got.PositionToleranceMM) +} + +func TestResolveGoalCloudConfigKeepsExplicitValues(t *testing.T) { + got := resolveGoalCloudConfig(12.5, 0.25, nil) + assert.Equal(t, 12.5, got.OrientationToleranceDeg) + assert.Equal(t, 0.25, got.PositionToleranceMM) +} + +// The warnings are the production path -- both tests above pass a nil logger and so +// exercise only the early return. resolveGoalCloudConfig is the SINGLE owner of these +// thresholds; nothing downstream re-checks them, so nothing downstream would catch a +// regression either. +func TestResolveGoalCloudConfigWarnsAtSaturationBoundary(t *testing.T) { + // The boundary is acos(-0.999) = 177.4374 -- exactly the band a literal ">= 177.44" + // check would miss, which is why poseCloudSaturationCos is a cosine. + logger, logs := logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(177.438, 0, logger) + assert.Equal(t, 1, logs.FilterMessageSnippet("saturates").Len(), "just inside the boundary must warn") + + logger, logs = logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(177.40, 0, logger) + assert.Equal(t, 0, logs.FilterMessageSnippet("saturates").Len(), "just outside must not warn") +} + +func TestResolveGoalCloudConfigWarnsOnLargePositionTolerance(t *testing.T) { + logger, logs := logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(0, 25, logger) + assert.Equal(t, 1, logs.FilterMessageSnippet("is large").Len()) + + logger, logs = logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(0, 10, logger) + assert.Equal(t, 0, logs.FilterMessageSnippet("is large").Len(), "at the threshold must not warn") +} + +func TestResolveGoalCloudConfigDefaultsDoNotWarn(t *testing.T) { + logger, logs := logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(0, 0, logger) + assert.Equal(t, 0, logs.Len(), "the defaults must be quiet") +} From 887252d1c7efa1024adb1cdb39878f7b7707c284 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 13:15:18 -0400 Subject: [PATCH 05/23] docs: amend Task 2 for code review findings 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. --- .../2026-07-15-pose-cloud-orientation.md | 92 +++++++++++++++---- 1 file changed, 76 insertions(+), 16 deletions(-) diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md index 35c0670..195938d 100644 --- a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -123,8 +123,42 @@ func TestResolveGoalCloudConfigKeepsExplicitValues(t *testing.T) { assert.Equal(t, 12.5, got.OrientationToleranceDeg) assert.Equal(t, 0.25, got.PositionToleranceMM) } + +// The warnings are the production path -- both tests above pass a nil logger and so +// exercise only the early return. resolveGoalCloudConfig is the SINGLE owner of these +// thresholds; nothing downstream re-checks them, so nothing downstream would catch a +// regression either. Uses logging.NewObservedTestLogger (rdk logging/logging.go:145). +func TestResolveGoalCloudConfigWarnsAtSaturationBoundary(t *testing.T) { + // The boundary is acos(-0.999) = 177.4374 -- exactly the band a literal ">= 177.44" + // check would miss, which is why poseCloudSaturationCos is a cosine. + logger, logs := logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(177.438, 0, logger) + assert.Equal(t, 1, logs.FilterMessageSnippet("saturates").Len(), "just inside the boundary must warn") + + logger, logs = logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(177.40, 0, logger) + assert.Equal(t, 0, logs.FilterMessageSnippet("saturates").Len(), "just outside must not warn") +} + +func TestResolveGoalCloudConfigWarnsOnLargePositionTolerance(t *testing.T) { + logger, logs := logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(0, 25, logger) + assert.Equal(t, 1, logs.FilterMessageSnippet("is large").Len()) + + logger, logs = logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(0, 10, logger) + assert.Equal(t, 0, logs.FilterMessageSnippet("is large").Len(), "at the threshold must not warn") +} + +func TestResolveGoalCloudConfigDefaultsDoNotWarn(t *testing.T) { + logger, logs := logging.NewObservedTestLogger(t) + resolveGoalCloudConfig(0, 0, logger) + assert.Equal(t, 0, logs.Len(), "the defaults must be quiet") +} ``` +The warning tests need `"go.viam.com/rdk/logging"` in the test import block as well. + - [ ] **Step 2: Run test to verify it fails** Run: `go test . -run TestResolveGoalCloudConfig -v` @@ -189,19 +223,33 @@ func resolveGoalCloudConfig(tolDeg, posTolMM float64, logger logging.Logger) goa return cfg } if cfg.PositionToleranceMM > warnPositionToleranceMM { - logger.Warnf("position_tolerance_mm=%.1f is large; the planner may report success up to %.1fmm "+ + logger.Warnf("position_tolerance_mm=%g is large; the planner may report success up to %.2fmm "+ "from the goal (the cloud is a per-axis box, so the worst-case corner is sqrt(3) times this)", cfg.PositionToleranceMM, math.Sqrt(3)*cfg.PositionToleranceMM) } - if math.Cos(degToRad(cfg.OrientationToleranceDeg)) <= poseCloudSaturationCos { - logger.Warnf("orientation_tolerance_deg=%.1f saturates the approach-axis cone (>= 177.44); "+ + // Cite 177.4374 (the true acos(-0.999) threshold), NOT the rounded 177.44 used in + // user-facing docs: this guard fires across [177.4374, 177.44), so quoting 177.44 here + // would contradict the action taken. %g likewise avoids printing 177.4374 as "177.4". + if math.Cos(utils.DegToRad(cfg.OrientationToleranceDeg)) <= poseCloudSaturationCos { + logger.Warnf("orientation_tolerance_deg=%g saturates the approach-axis cone (>= 177.4374); "+ "every orientation will be accepted, which is equivalent to ignoring orientation", cfg.OrientationToleranceDeg) } return cfg } +``` + +**Use `utils.DegToRad`, do not declare a local `degToRad`.** `go.viam.com/rdk/utils.DegToRad` +is identical and the package already uses it (`manager.go:130`, `speed.go`); a second +spelling of deg→rad in one package invites a third. The import block is therefore: -func degToRad(deg float64) float64 { return deg * math.Pi / 180 } +```go +import ( + "math" + + "go.viam.com/rdk/logging" + "go.viam.com/rdk/utils" +) ``` - [ ] **Step 4: Run test to verify it passes** @@ -238,11 +286,16 @@ import ( "github.com/golang/geo/r3" "github.com/stretchr/testify/assert" + "go.viam.com/rdk/logging" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" ) ``` +(`logging` and `utils` arrive in Task 2 via the warning tests and `utils.DegToRad`; listed +here so the block is complete.) + ```go // testGoal is a tool pointing straight down, a typical SO-101 grasp pose. func testGoal() spatialmath.Pose { @@ -255,7 +308,7 @@ func testGoal() spatialmath.Pose { // tiltedBy returns testGoal() tilted by deg about the axis (ax, ay, 0). func tiltedBy(ax, ay, deg float64) spatialmath.Pose { return spatialmath.Compose(testGoal(), spatialmath.NewPoseFromOrientation( - &spatialmath.R4AA{RX: ax, RY: ay, RZ: 0, Theta: degToRad(deg)})) + &spatialmath.R4AA{RX: ax, RY: ay, RZ: 0, Theta: utils.DegToRad(deg)})) } func coneCloud(tolDeg float64) *referenceframe.PoseCloud { @@ -270,7 +323,7 @@ func TestConeToPoseCloudFields(t *testing.T) { // OX/OY are deliberately unconstrained: OZ alone defines the cone. assert.Equal(t, 1.0, c.OX) assert.Equal(t, 1.0, c.OY) - assert.InDelta(t, 1-math.Cos(degToRad(30)), c.OZ, 1e-12) + assert.InDelta(t, 1-math.Cos(utils.DegToRad(30)), c.OZ, 1e-12) // MUST be 180: Theta encodes tilt AZIMUTH, not roll. See the spec. assert.Equal(t, 180.0, c.Theta) // X, Y, Z, OX, OY, OZ, Theta is the COMPLETE field set in rdk v1.0.0. Do not look for @@ -281,7 +334,7 @@ func TestConeToPoseCloudFields(t *testing.T) { func TestConeBoundsTiltIsotropically(t *testing.T) { c := coneCloud(30) for _, azDeg := range []float64{0, 45, 90, 135, 180, 270} { - ax, ay := math.Cos(degToRad(azDeg)), math.Sin(degToRad(azDeg)) + ax, ay := math.Cos(utils.DegToRad(azDeg)), math.Sin(utils.DegToRad(azDeg)) assert.True(t, c.PoseInCloud(testGoal(), tiltedBy(ax, ay, 29)), "29deg tilt at azimuth %.0f should be accepted", azDeg) assert.False(t, c.PoseInCloud(testGoal(), tiltedBy(ax, ay, 31)), @@ -309,7 +362,7 @@ func TestConeRollIsFree(t *testing.T) { c := coneCloud(30) for _, roll := range []float64{0, 45, 90, 179} { p := spatialmath.Compose(testGoal(), spatialmath.NewPoseFromOrientation( - &spatialmath.R4AA{RX: 0, RY: 0, RZ: 1, Theta: degToRad(roll)})) + &spatialmath.R4AA{RX: 0, RY: 0, RZ: 1, Theta: utils.DegToRad(roll)})) assert.True(t, c.PoseInCloud(testGoal(), p), "roll %.0f should be accepted", roll) } } @@ -322,8 +375,8 @@ func TestConeTiltPlusRollStaysBounded(t *testing.T) { want bool }{{20, true}, {29, true}, {31, false}} { p := spatialmath.Compose(testGoal(), spatialmath.Compose( - spatialmath.NewPoseFromOrientation(&spatialmath.R4AA{RX: 1, Theta: degToRad(tc.tilt)}), - spatialmath.NewPoseFromOrientation(&spatialmath.R4AA{RZ: 1, Theta: degToRad(roll)}))) + spatialmath.NewPoseFromOrientation(&spatialmath.R4AA{RX: 1, Theta: utils.DegToRad(tc.tilt)}), + spatialmath.NewPoseFromOrientation(&spatialmath.R4AA{RZ: 1, Theta: utils.DegToRad(roll)}))) assert.Equal(t, tc.want, c.PoseInCloud(testGoal(), p), "tilt %.0f roll %.0f", tc.tilt, roll) } @@ -396,7 +449,7 @@ func coneToPoseCloud(cfg goalCloudConfig) *referenceframe.PoseCloud { Z: cfg.PositionToleranceMM, OX: 1, OY: 1, - OZ: 1 - math.Cos(degToRad(cfg.OrientationToleranceDeg)), + OZ: 1 - math.Cos(utils.DegToRad(cfg.OrientationToleranceDeg)), Theta: 180, } } @@ -770,17 +823,24 @@ func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalClo // applies only on the cone path: on the other paths it would misdirect, telling a caller // who just passed position_only to pass position_only, or blaming a config tolerance that // had no bearing on a raw-cloud failure. +// +// The cone message names BOTH tolerances. Either can cause the failure: too tight a cone +// leaves no reachable orientation, and too small a position tolerance means the solver can +// never land inside the cloud (which reverts planning to strict 6-DOF scoring and fails +// every move). Naming only the orientation knob would point at the wrong one half the time. func wrapMoveErr(err error, path goalPath, cfg goalCloudConfig) error { if err == nil { return nil } switch path { case pathCone: - return fmt.Errorf("move to position failed with orientation_tolerance_deg=%.1f "+ - "(approach-axis cone): %w; widen the tolerance, pass extra "+ - "{\"goal_metric_type\": \"position_only\"} to ignore orientation, or check that "+ - "viam-server is >= 0.127.0 (older servers silently ignore goal clouds)", - cfg.OrientationToleranceDeg, err) + return fmt.Errorf("move to position failed with orientation_tolerance_deg=%g, "+ + "position_tolerance_mm=%g (approach-axis cone): %w; widen either tolerance "+ + "(too small a position_tolerance_mm means the solver can never land inside the "+ + "goal cloud), pass extra {\"goal_metric_type\": \"position_only\"} to ignore "+ + "orientation, or check that viam-server is >= 0.127.0 (older servers silently "+ + "ignore goal clouds)", + cfg.OrientationToleranceDeg, cfg.PositionToleranceMM, err) case pathRawCloud: return fmt.Errorf("move to position failed with the caller-supplied pose_cloud: %w; "+ "widen the cloud, or check that viam-server is >= 0.127.0 (older servers silently "+ From 000b4c68345ef85cc0df914193090b19d057b260 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 13:23:05 -0400 Subject: [PATCH 06/23] feat(arm): add coneToPoseCloud approach-axis cone mapping --- goal_cloud.go | 31 +++++++++ goal_cloud_test.go | 168 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 199 insertions(+) diff --git a/goal_cloud.go b/goal_cloud.go index a906850..3cffd1e 100644 --- a/goal_cloud.go +++ b/goal_cloud.go @@ -4,6 +4,7 @@ import ( "math" "go.viam.com/rdk/logging" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/utils" ) @@ -67,3 +68,33 @@ func resolveGoalCloudConfig(tolDeg, posTolMM float64, logger logging.Logger) goa } return cfg } + +// coneToPoseCloud converts an approach-axis cone half-angle into a PoseCloud. +// +// Every field here is load-bearing and was established empirically; see +// docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md before changing any +// of them: +// +// - X/Y/Z must be > 0. PoseInCloud adds a 0.001 epsilon to each leeway, so a zero +// leeway demands a 1-micron match and the cloud would never match. +// - OX/OY are deliberately 1 (unconstrained). OZ alone defines the cone. +// - Theta must be exactly 180. For a pure tilt, the relative Theta reports the tilt's +// AZIMUTH (about X -> 90, about Y -> 0), not the roll -- and it does so independent of +// the tilt's magnitude. Azimuth sweeps the full [-180, 180], so ONLY 180 admits every +// azimuth; that is, only 180 yields an isotropic cone. Smaller values do not reject +// tilts outright -- they silently carve out a wedge of tilt DIRECTIONS (Theta=90 still +// accepts a tilt about X, but rejects azimuths past 180). TestConeBoundsTiltIsotropically +// pins this: at azimuth 270 the reported Theta is exactly -180. +// The cost is that roll cannot be constrained at all, which is why this is +// approach-axis planning with free roll. +func coneToPoseCloud(cfg goalCloudConfig) *referenceframe.PoseCloud { + return &referenceframe.PoseCloud{ + X: cfg.PositionToleranceMM, + Y: cfg.PositionToleranceMM, + Z: cfg.PositionToleranceMM, + OX: 1, + OY: 1, + OZ: 1 - math.Cos(utils.DegToRad(cfg.OrientationToleranceDeg)), + Theta: 180, + } +} diff --git a/goal_cloud_test.go b/goal_cloud_test.go index df8ada7..e59db52 100644 --- a/goal_cloud_test.go +++ b/goal_cloud_test.go @@ -1,10 +1,15 @@ package so_arm import ( + "math" "testing" + "github.com/golang/geo/r3" "github.com/stretchr/testify/assert" "go.viam.com/rdk/logging" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" ) func TestResolveGoalCloudConfigDefaults(t *testing.T) { @@ -51,3 +56,166 @@ func TestResolveGoalCloudConfigDefaultsDoNotWarn(t *testing.T) { resolveGoalCloudConfig(0, 0, logger) assert.Equal(t, 0, logs.Len(), "the defaults must be quiet") } + +// testGoal is a tool pointing straight down, a typical SO-101 grasp pose. +func testGoal() spatialmath.Pose { + return spatialmath.NewPose( + r3.Vector{X: 300, Y: 0, Z: 200}, + &spatialmath.OrientationVectorDegrees{OX: 0, OY: 0, OZ: -1, Theta: 0}, + ) +} + +// tiltedBy returns testGoal() tilted by deg about the goal's local (ax, ay, 0) axis. The +// rotation composes onto the goal, so it applies in the goal's own frame -- and since the +// goal points straight down, that local frame is not the world frame. +func tiltedBy(ax, ay, deg float64) spatialmath.Pose { + return spatialmath.Compose(testGoal(), spatialmath.NewPoseFromOrientation( + &spatialmath.R4AA{RX: ax, RY: ay, RZ: 0, Theta: utils.DegToRad(deg)})) +} + +// coneCloud builds the cloud under test for a cone half-angle. Its position tolerance is +// deliberately 2.5 rather than 1: that keeps it distinct from the unconstrained OX/OY +// sentinel of 1, so the position -> X/Y/Z mapping stays legible and a transposition of the +// two would be caught. +func coneCloud(tolDeg float64) *referenceframe.PoseCloud { + return coneToPoseCloud(goalCloudConfig{OrientationToleranceDeg: tolDeg, PositionToleranceMM: 2.5}) +} + +// TestConeToPoseCloudFields pins the complete field set: X, Y, Z, OX, OY, OZ, Theta is the +// WHOLE struct in rdk v1.0.0. Do not look for a ReferenceFrame field -- v0.123.0 had one, +// v1.0.0 removed it, and the struct's free-floating doc comment about reference frames +// misleadingly survives. +func TestConeToPoseCloudFields(t *testing.T) { + c := coneCloud(30) + // The position tolerance maps to all three positional axes... + assert.Equal(t, 2.5, c.X) + assert.Equal(t, 2.5, c.Y) + assert.Equal(t, 2.5, c.Z) + // ...while OX/OY are deliberately unconstrained: OZ alone defines the cone. + assert.Equal(t, 1.0, c.OX) + assert.Equal(t, 1.0, c.OY) + assert.InDelta(t, 1-math.Cos(utils.DegToRad(30)), c.OZ, 1e-12) + // MUST be 180: Theta encodes tilt AZIMUTH, not roll. See the spec. + assert.Equal(t, 180.0, c.Theta) +} + +func TestConeBoundsTiltIsotropically(t *testing.T) { + c := coneCloud(30) + for _, azDeg := range []float64{0, 45, 90, 135, 180, 270} { + ax, ay := math.Cos(utils.DegToRad(azDeg)), math.Sin(utils.DegToRad(azDeg)) + assert.True(t, c.PoseInCloud(testGoal(), tiltedBy(ax, ay, 29)), + "29deg tilt at azimuth %.0f should be accepted", azDeg) + assert.False(t, c.PoseInCloud(testGoal(), tiltedBy(ax, ay, 31)), + "31deg tilt at azimuth %.0f should be rejected", azDeg) + } +} + +func TestConeBoundsAbove90Degrees(t *testing.T) { + // The cone does NOT go degenerate above 90, despite OZ = 1-cos(a) exceeding 1. + assert.True(t, coneCloud(90).PoseInCloud(testGoal(), tiltedBy(1, 0, 89))) + assert.False(t, coneCloud(90).PoseInCloud(testGoal(), tiltedBy(1, 0, 91))) + assert.True(t, coneCloud(120).PoseInCloud(testGoal(), tiltedBy(1, 0, 119))) + assert.False(t, coneCloud(120).PoseInCloud(testGoal(), tiltedBy(1, 0, 121))) +} + +// TestConeSaturationBoundary is the empirical anchor for saturation: it brackets the +// boundary with literal MEASURED degrees, owing nothing to poseCloudSaturationCos. Its +// sibling TestSaturationConstantMatchesConeMapping is the drift guard, deriving the same +// boundary FROM that constant. Both are needed: this one would still pass if the constant +// were wrong, and the sibling would still pass if both the constant and the mapping drifted +// together. +func TestConeSaturationBoundary(t *testing.T) { + // Saturation begins at acos(-0.999) = 177.4374, NOT 179. + assert.False(t, coneCloud(177.40).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), + "177.40 must still reject a 180deg tilt") + assert.True(t, coneCloud(177.44).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), + "177.44 must be saturated") +} + +// TestSaturationConstantMatchesConeMapping ties Task 2's poseCloudSaturationCos to this +// task's cone mapping. The constant is only correct while coneToPoseCloud maps the cone to +// OZ = 1-cos(a); if that formula ever changes, resolveGoalCloudConfig would keep warning at +// the wrong angle with nothing to catch it. The tests above use literal degrees and would +// not notice. This one derives the angle FROM the constant, so the two cannot drift apart. +func TestSaturationConstantMatchesConeMapping(t *testing.T) { + satDeg := math.Acos(poseCloudSaturationCos) * 180 / math.Pi // 177.4374... + assert.True(t, coneCloud(satDeg+0.001).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), + "at the angle where resolveGoalCloudConfig warns, the cone must actually be saturated") + assert.False(t, coneCloud(satDeg-0.01).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), + "just below that angle the cone must still bound tilt") +} + +func TestConeRollIsFree(t *testing.T) { + c := coneCloud(30) + // 180 is the actual boundary case: |Theta| = 180 against a leeway of 180 + 0.001. + for _, roll := range []float64{0, 45, 90, 179, 180} { + p := spatialmath.Compose(testGoal(), spatialmath.NewPoseFromOrientation( + &spatialmath.R4AA{RX: 0, RY: 0, RZ: 1, Theta: utils.DegToRad(roll)})) + assert.True(t, c.PoseInCloud(testGoal(), p), "roll %.0f should be accepted", roll) + } +} + +func TestConeTiltPlusRollStaysBounded(t *testing.T) { + c := coneCloud(30) + for _, roll := range []float64{0, 70, 170} { + for _, tc := range []struct { + tilt float64 + want bool + }{{20, true}, {29, true}, {31, false}} { + p := spatialmath.Compose(testGoal(), spatialmath.Compose( + spatialmath.NewPoseFromOrientation(&spatialmath.R4AA{RX: 1, Theta: utils.DegToRad(tc.tilt)}), + spatialmath.NewPoseFromOrientation(&spatialmath.R4AA{RZ: 1, Theta: utils.DegToRad(roll)}))) + assert.Equal(t, tc.want, c.PoseInCloud(testGoal(), p), + "tilt %.0f roll %.0f", tc.tilt, roll) + } + } +} + +// effectiveConeDeg measures the cone rdk's PoseInCloud actually enforces, by binary +// search on acceptance. +func effectiveConeDeg(c *referenceframe.PoseCloud) float64 { + lo, hi := 0.0, 180.0 + for i := 0; i < 60; i++ { + mid := (lo + hi) / 2 + if c.PoseInCloud(testGoal(), tiltedBy(1, 0, mid)) { + lo = mid + } else { + hi = mid + } + } + return lo +} + +func TestConeEffectiveWideningFormula(t *testing.T) { + // rdk ADDS its 0.001 epsilon to the leeway, so the cone actually enforced is always + // slightly wider than requested. Comparing measurement (LHS, from rdk's real + // PoseInCloud) against our model (RHS) is not tautological: it pins that our + // understanding of rdk's behavior is right, for any angle rather than six points. + // The sample spans all three regimes: 0 is epsilon-dominated, 2.6 is the crossover + // where the requested leeway equals the epsilon, 90+ is epsilon-negligible. + for _, requested := range []float64{0, 1, 2.6, 10, 30, 90, 150} { + want := math.Acos(math.Cos(utils.DegToRad(requested))-0.001) * 180 / math.Pi + assert.InDelta(t, want, effectiveConeDeg(coneCloud(requested)), 0.01, + "effective cone for requested %.1f", requested) + } + // One literal anchor, independent of the formula: at tolerance 0 the epsilon alone + // still admits ~2.5626deg. That is the floor a caller cannot get below. + assert.InDelta(t, 2.5626, effectiveConeDeg(coneCloud(0)), 0.01) +} + +func TestConePositionalBoxNotRadius(t *testing.T) { + // Deliberately NOT coneCloud (which uses 2.5): the bounds below are measured values + // tied to a 1.0mm tolerance, so this test pins its own. + c := coneToPoseCloud(goalCloudConfig{OrientationToleranceDeg: 30, PositionToleranceMM: 1.0}) + offset := func(dx, dy, dz float64) spatialmath.Pose { + g := testGoal() + return spatialmath.NewPose( + r3.Vector{X: g.Point().X + dx, Y: g.Point().Y + dy, Z: g.Point().Z + dz}, + g.Orientation()) + } + // The epsilon is additive: at X=1.0 the true bound is 1.001mm. + assert.True(t, c.PoseInCloud(testGoal(), offset(1.0009, 0, 0))) + assert.False(t, c.PoseInCloud(testGoal(), offset(1.0015, 0, 0))) + // Per-axis box, not a radius: all three axes at the bound is accepted at norm ~1.73mm. + assert.True(t, c.PoseInCloud(testGoal(), offset(1.0005, 1.0005, 1.0005))) +} From 8a66004a3067f67610c2cf897cfb5b2a613e0424 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 13:23:52 -0400 Subject: [PATCH 07/23] docs: tie the saturation constant to the cone mapping in Task 3 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. --- .../plans/2026-07-15-pose-cloud-orientation.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md index 195938d..badd5ba 100644 --- a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -358,6 +358,19 @@ func TestConeSaturationBoundary(t *testing.T) { "177.44 must be saturated") } +// TestSaturationConstantMatchesConeMapping ties Task 2's poseCloudSaturationCos to this +// task's cone mapping. The constant is only correct while coneToPoseCloud maps the cone to +// OZ = 1-cos(a); if that formula ever changes, resolveGoalCloudConfig would keep warning at +// the wrong angle with nothing to catch it. The tests above use literal degrees and would +// not notice. This one derives the angle FROM the constant, so the two cannot drift apart. +func TestSaturationConstantMatchesConeMapping(t *testing.T) { + satDeg := math.Acos(poseCloudSaturationCos) * 180 / math.Pi // 177.4374... + assert.True(t, coneCloud(satDeg+0.001).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), + "at the angle where resolveGoalCloudConfig warns, the cone must actually be saturated") + assert.False(t, coneCloud(satDeg-0.01).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), + "just below that angle the cone must still bound tilt") +} + func TestConeRollIsFree(t *testing.T) { c := coneCloud(30) for _, roll := range []float64{0, 45, 90, 179} { From 82303101868459c704024a85da46d4e86cd9f812 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 13:41:19 -0400 Subject: [PATCH 08/23] docs: replace the false Theta claim with the isotropy argument 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. --- .../2026-07-15-pose-cloud-orientation.md | 15 ++++++---- ...026-07-15-pose-cloud-orientation-design.md | 30 ++++++++++++++++--- 2 files changed, 36 insertions(+), 9 deletions(-) diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md index badd5ba..565706b 100644 --- a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -16,7 +16,7 @@ Three facts drive every design choice. All were verified empirically against rdk v1.0.0: -1. **`PoseCloud.Theta` encodes the AZIMUTH of a tilt, not roll.** Tilt about X → `Theta=90`; about Y → `Theta=0`; diagonal → `Theta=45`. So `Theta` **must be 180** or the check rejects tilts by azimuth before the cone is consulted (a `Theta: 0` cloud rejects even a 5° tilt). Consequence: roll cannot be constrained alongside tilt. This is approach-axis planning with **free roll**. +1. **`PoseCloud.Theta` encodes the AZIMUTH of a tilt, not roll.** Measured exactly: `Theta = 90 - azimuth`, independent of the tilt's magnitude (about X → `90`; about Y → `0`; diagonal → `45`; azimuth 270 → `-180`). Since azimuth sweeps the full `[-180, 180]`, **only `Theta: 180` admits every azimuth** — only 180 gives an *isotropic* cone. Beware the tempting shorthand "smaller Theta rejects tilts": it is **false** (a `Theta: 0` cloud still accepts a 5° tilt about Y). Smaller values silently carve out a wedge of tilt *directions*. Consequence: roll cannot be constrained alongside tilt. This is approach-axis planning with **free roll**. 2. **`defaultEpsilon = 0.001` is ADDED to every leeway**, not just zeroed ones. Zero `X`/`Y`/`Z` therefore demands a 1-micron match, so positional leeway is **mandatory**. The effective cone is `acos(cos(tol) - 0.001)` — always slightly wider than requested. 3. **`OZ` alone defines the cone.** `OX`/`OY` are set to `1` (unconstrained). Valid across `[0, 180]`, saturating at `acos(-0.999) = 177.4374°`. @@ -451,10 +451,15 @@ Add to `goal_cloud.go` (add `"go.viam.com/rdk/referenceframe"` to imports): // - X/Y/Z must be > 0. PoseInCloud adds a 0.001 epsilon to each leeway, so a zero // leeway demands a 1-micron match and the cloud would never match. // - OX/OY are deliberately 1 (unconstrained). OZ alone defines the cone. -// - Theta must be 180. For a pure tilt, PoseCloud's Theta reports the AZIMUTH of that -// tilt (about X -> 90, about Y -> 0), not the roll. At any smaller leeway the Theta -// check rejects tilts before the cone is consulted. The cost is that roll cannot be -// constrained at all, which is why this is approach-axis planning with free roll. +// - Theta must be exactly 180. For a pure tilt, the relative Theta reports the tilt's +// AZIMUTH (about X -> 90, about Y -> 0), not the roll -- and it does so independent of +// the tilt's magnitude. Azimuth sweeps the full [-180, 180], so ONLY 180 admits every +// azimuth; that is, only 180 yields an isotropic cone. Smaller values do not reject +// tilts outright -- they silently carve out a wedge of tilt DIRECTIONS (Theta=90 still +// accepts a tilt about X, but rejects azimuths past 180). TestConeBoundsTiltIsotropically +// pins this: at azimuth 270 the reported Theta is exactly -180. +// The cost is that roll cannot be constrained at all, which is why this is +// approach-axis planning with free roll. func coneToPoseCloud(cfg goalCloudConfig) *referenceframe.PoseCloud { return &referenceframe.PoseCloud{ X: cfg.PositionToleranceMM, diff --git a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md index 34cff77..3db5084 100644 --- a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md +++ b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md @@ -66,10 +66,32 @@ relative `Theta` reports the **azimuth of the tilt**: `Theta` measures roll only when tilt is zero. There is also a parameterization singularity at identity: a 0.5° tilt about X reports `Theta = 0`, while 5° reports `Theta = 90`. -Consequence: **`Theta` leeway must be 180.** At anything less, the `Theta` check rejects -tilts by azimuth before the `OX`/`OY`/`OZ` cone is ever consulted — a `Theta: 0` cloud -rejects even a 5° tilt, making the cone useless. And because `Theta` mixes azimuth with -roll, no value of it can bound roll while permitting isotropic tilt. Hence: roll is free. +The relationship is exact: measured **`Theta = 90 - azimuth`, independent of the tilt's +magnitude**. Verified across azimuths 0/45/90/135/180/225/270 at tilts of 5°, 20° and 29° — +every reading matched the prediction to three decimals, and azimuth 270 reports exactly +`-180`. + +Consequence: **`Theta` leeway must be exactly 180.** Azimuth sweeps the full `[-180, 180]`, +so only a leeway of 180 admits every azimuth — only 180 yields an *isotropic* cone. + +The precise failure mode matters, because the tempting shorthand ("smaller Theta rejects +tilts") is **false** and a reader who tests it will falsify it. A smaller `Theta` does not +reject tilts outright; it silently carves out a wedge of tilt *directions*. Measured with a +30° cone against a 5° tilt: + +| `Theta` leeway | azimuth 0 (about X) | azimuth 90 (about Y) | azimuth 270 | +|---|---|---|---| +| 0 | rejected | **accepted** | rejected | +| 90 | **accepted** | accepted | rejected | +| 170 | **accepted** | accepted | rejected | +| 180 | accepted | accepted | **accepted** | + +So even `Theta: 0` accepts a 5° tilt about Y. Only the isotropy argument holds under +testing, which is why it — not the shorthand — is what `coneToPoseCloud`'s doc comment +carries, alongside a pointer to the azimuth-270 test that enforces it. + +And because `Theta` mixes azimuth with roll, no value of it can bound roll while permitting +isotropic tilt. Hence: roll is free. ### OZ alone defines the cone, across the full 0-180 range From af889200772df70b98d57365bae9f28afb57210c Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 13:48:00 -0400 Subject: [PATCH 09/23] test(arm): pin rdk's Theta-is-azimuth and epsilon contracts 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) Claude-Session: https://claude.ai/code/session_01PguhuvXbVmNHzUWzYyJFaD --- goal_cloud.go | 19 +++++++++++-------- goal_cloud_test.go | 47 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 58 insertions(+), 8 deletions(-) diff --git a/goal_cloud.go b/goal_cloud.go index 3cffd1e..6f5d6af 100644 --- a/goal_cloud.go +++ b/goal_cloud.go @@ -15,8 +15,8 @@ const ( // defaultPositionToleranceMM is the per-axis positional leeway used when // position_tolerance_mm is zero or unset. It must stay comfortably above the IK - // solver's convergence: a cloud the solver can never land inside is worse than no - // cloud at all, because planning reverts to strict 6-DOF scoring and always fails. + // solver's convergence: a cloud the solver realistically cannot land inside is worse + // than no cloud at all, because planning reverts to strict 6-DOF scoring. defaultPositionToleranceMM = 1.0 // warnPositionToleranceMM is the point above which a cloud is loose enough that the @@ -76,15 +76,18 @@ func resolveGoalCloudConfig(tolDeg, posTolMM float64, logger logging.Logger) goa // of them: // // - X/Y/Z must be > 0. PoseInCloud adds a 0.001 epsilon to each leeway, so a zero -// leeway demands a 1-micron match and the cloud would never match. +// leeway demands a match within 1 micron -- which no IK solution will realistically +// achieve, making the cloud useless in practice even though it does technically +// accept an exact match. // - OX/OY are deliberately 1 (unconstrained). OZ alone defines the cone. // - Theta must be exactly 180. For a pure tilt, the relative Theta reports the tilt's // AZIMUTH (about X -> 90, about Y -> 0), not the roll -- and it does so independent of -// the tilt's magnitude. Azimuth sweeps the full [-180, 180], so ONLY 180 admits every -// azimuth; that is, only 180 yields an isotropic cone. Smaller values do not reject -// tilts outright -- they silently carve out a wedge of tilt DIRECTIONS (Theta=90 still -// accepts a tilt about X, but rejects azimuths past 180). TestConeBoundsTiltIsotropically -// pins this: at azimuth 270 the reported Theta is exactly -180. +// the tilt's magnitude. The reported Theta sweeps the full [-180, 180] as azimuth goes +// around, so ONLY a leeway of 180 admits every azimuth; that is, only 180 yields an +// isotropic cone. Smaller values do not reject tilts outright -- they silently carve out +// a wedge of tilt DIRECTIONS (Theta=90 still accepts a tilt about X, but rejects azimuths +// past 180). TestConeBoundsTiltIsotropically pins this: at azimuth 270 the reported Theta +// is exactly -180. // The cost is that roll cannot be constrained at all, which is why this is // approach-axis planning with free roll. func coneToPoseCloud(cfg goalCloudConfig) *referenceframe.PoseCloud { diff --git a/goal_cloud_test.go b/goal_cloud_test.go index e59db52..f0be119 100644 --- a/goal_cloud_test.go +++ b/goal_cloud_test.go @@ -219,3 +219,50 @@ func TestConePositionalBoxNotRadius(t *testing.T) { // Per-axis box, not a radius: all three axes at the bound is accepted at norm ~1.73mm. assert.True(t, c.PoseInCloud(testGoal(), offset(1.0005, 1.0005, 1.0005))) } + +// TestRDKContractThetaReportsTiltAzimuth documents why coneToPoseCloud sets Theta: 180, by +// pinning the mechanism directly: the relative Theta reports a tilt's AZIMUTH, not its roll. +// +// This is a characterization test for rdk's Theta semantics, not a guard on coneToPoseCloud +// (it overwrites Theta, so a mutation there is a no-op here -- TestConeBoundsTiltIsotropically +// is what guards the mapping end-to-end). It earns its place by pinning the underlying rdk +// contract that nothing else asserts: ovX.Theta == 90 and ovY.Theta == 0. Were an rdk upgrade +// to change those semantics, the acceptance tests would fail with vague mismatches while this +// one names the broken contract outright. +// +// Note the tempting shorthand -- "a smaller Theta rejects tilts" -- is FALSE: Theta=0 below +// rejects a tilt about X but would ACCEPT one about Y (azimuth 90 reports Theta=0). What a +// smaller Theta actually does is carve out a wedge of tilt DIRECTIONS. Since reported Theta +// sweeps the full [-180, 180] as azimuth goes around, only a leeway of 180 admits every +// azimuth. +func TestRDKContractThetaReportsTiltAzimuth(t *testing.T) { + broken := coneCloud(30) + broken.Theta = 0 // the "obvious simplification" + assert.False(t, broken.PoseInCloud(testGoal(), tiltedBy(1, 0, 5)), + "with Theta=0 even a 5deg tilt is rejected, defeating the cone entirely") + + // Proof of the cause: a pure tilt about X reports Theta=90, about Y reports Theta=0. + ovX := spatialmath.PoseBetween(testGoal(), tiltedBy(1, 0, 5)).Orientation().OrientationVectorDegrees() + assert.InDelta(t, 90.0, ovX.Theta, 1e-6, "tilt about X reports Theta=90, not roll") + ovY := spatialmath.PoseBetween(testGoal(), tiltedBy(0, 1, 5)).Orientation().OrientationVectorDegrees() + assert.InDelta(t, 0.0, ovY.Theta, 1e-6, "tilt about Y reports Theta=0") +} + +// TestRDKContractZeroLeewayDemandsMicronMatch documents why X/Y/Z must be > 0: the 0.001 +// epsilon means a zero leeway demands a match within 1 micron -- which no IK solution will +// realistically achieve, making the cloud useless in practice even though it does technically +// accept an exact match (measured: it still accepts 0.0009mm, and rejects 0.0011mm). +// +// This is a characterization test for rdk's epsilon rule, not a guard on coneToPoseCloud +// (it overwrites X/Y/Z, so a mutation there is a no-op here -- TestConePositionalBoxNotRadius +// is what guards the mapping). It earns its place by demonstrating the failure mode that +// motivates defaultPositionToleranceMM: the cloud a caller gets with zero leeway. +func TestRDKContractZeroLeewayDemandsMicronMatch(t *testing.T) { + broken := coneCloud(30) + broken.X, broken.Y, broken.Z = 0, 0, 0 + g := testGoal() + nudged := spatialmath.NewPose( + r3.Vector{X: g.Point().X + 0.01, Y: g.Point().Y, Z: g.Point().Z}, g.Orientation()) + assert.False(t, broken.PoseInCloud(g, nudged), + "with zero positional leeway even a 0.01mm offset is rejected") +} From 3bd7d6fb041c4b15672b40fe6157a20eb619a5b2 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 13:57:22 -0400 Subject: [PATCH 10/23] docs: drop the falsifiable "never matches" and azimuth-range claims 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]. --- .../plans/2026-07-15-pose-cloud-orientation.md | 11 +++++++---- .../specs/2026-07-15-pose-cloud-orientation-design.md | 9 +++++---- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md index 565706b..c77a3f3 100644 --- a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -449,7 +449,9 @@ Add to `goal_cloud.go` (add `"go.viam.com/rdk/referenceframe"` to imports): // of them: // // - X/Y/Z must be > 0. PoseInCloud adds a 0.001 epsilon to each leeway, so a zero -// leeway demands a 1-micron match and the cloud would never match. +// leeway demands a match within 1 micron -- which no IK solution will realistically +// achieve, making the cloud useless in practice even though it does technically +// accept an exact match. // - OX/OY are deliberately 1 (unconstrained). OZ alone defines the cone. // - Theta must be exactly 180. For a pure tilt, the relative Theta reports the tilt's // AZIMUTH (about X -> 90, about Y -> 0), not the roll -- and it does so independent of @@ -519,8 +521,8 @@ func TestRegressionThetaMustBe180(t *testing.T) { } // TestRegressionPositionalLeewayIsMandatory documents why X/Y/Z must be > 0: the 0.001 -// epsilon means a zero leeway demands a 1-micron match, so the cloud never matches and -// planning silently reverts to strict 6-DOF scoring. +// epsilon means a zero leeway demands a match within 1 micron, which no IK solution will +// realistically achieve -- so planning silently reverts to strict 6-DOF scoring. func TestRegressionPositionalLeewayIsMandatory(t *testing.T) { broken := coneCloud(30) broken.X, broken.Y, broken.Z = 0, 0, 0 @@ -634,7 +636,8 @@ func poseCloudKeyList() string { // parsePoseCloud converts a caller-supplied extra["pose_cloud"] into a PoseCloud. // Omitted fields stay zero, which -- given the additive 0.001 epsilon -- means // near-zero leeway. That is faithful passthrough and the point of an escape hatch, but it -// is sharp: a cloud whose leeways are all zero will never match. +// is sharp: a cloud whose leeways are all zero demands a match within 1 micron / 0.001 +// degrees, which no IK solution will realistically achieve. func parsePoseCloud(raw interface{}) (*referenceframe.PoseCloud, error) { m, ok := raw.(map[string]interface{}) if !ok { diff --git a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md index 3db5084..92e6cda 100644 --- a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md +++ b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md @@ -71,8 +71,9 @@ magnitude**. Verified across azimuths 0/45/90/135/180/225/270 at tilts of 5°, 2 every reading matched the prediction to three decimals, and azimuth 270 reports exactly `-180`. -Consequence: **`Theta` leeway must be exactly 180.** Azimuth sweeps the full `[-180, 180]`, -so only a leeway of 180 admits every azimuth — only 180 yields an *isotropic* cone. +Consequence: **`Theta` leeway must be exactly 180.** Azimuth sweeps `[0, 360)`, so the +*reported* `Theta` sweeps the full `[-180, 180]` — and only a leeway of 180 admits every +azimuth. Only 180 yields an *isotropic* cone. The precise failure mode matters, because the tempting shorthand ("smaller Theta rejects tilts") is **false** and a reader who tests it will falsify it. A smaller `Theta` does not @@ -193,8 +194,8 @@ OrientationToleranceDeg float64 `json:"orientation_tolerance_deg,omitempty"` // PositionToleranceMM is the per-axis positional leeway of the goal cloud, applied as a // box along the goal frame's axes (worst-case corner deviation is sqrt(3) times this). -// It must be large enough for the IK solver to land inside, or the cloud never matches -// and every move fails. Zero or unset means the default, 1.0. +// It must be large enough for the IK solver to realistically land inside; a cloud only an +// exact match could satisfy means every move fails. Zero or unset means the default, 1.0. PositionToleranceMM float64 `json:"position_tolerance_mm,omitempty"` ``` From d7a820e6021ec366ae18622916e1271f76573b60 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 13:59:38 -0400 Subject: [PATCH 11/23] feat(arm): parse the raw pose_cloud escape hatch from extra --- goal_cloud.go | 59 ++++++++++++++++++++++++++++++++++++++++++++++ goal_cloud_test.go | 45 +++++++++++++++++++++++++++++++++++ 2 files changed, 104 insertions(+) diff --git a/goal_cloud.go b/goal_cloud.go index 6f5d6af..d5c6819 100644 --- a/goal_cloud.go +++ b/goal_cloud.go @@ -1,7 +1,10 @@ package so_arm import ( + "fmt" "math" + "sort" + "strings" "go.viam.com/rdk/logging" "go.viam.com/rdk/referenceframe" @@ -101,3 +104,59 @@ func coneToPoseCloud(cfg goalCloudConfig) *referenceframe.PoseCloud { Theta: 180, } } + +// poseCloudSetters maps the accepted pose_cloud keys to their fields. The names match +// referenceframe.PoseCloud's own JSON tags exactly, and are all lowercase. Note the Viam +// docs render these as OX/OY and protobuf JSON spells them o_x/oX; neither is accepted, +// which is why unknown keys are an error rather than ignored. This is also the complete +// field set: "reference_frame" is not a PoseCloud field in rdk v1.0.0. +var poseCloudSetters = map[string]func(*referenceframe.PoseCloud, float64){ + "x": func(p *referenceframe.PoseCloud, v float64) { p.X = v }, + "y": func(p *referenceframe.PoseCloud, v float64) { p.Y = v }, + "z": func(p *referenceframe.PoseCloud, v float64) { p.Z = v }, + "ox": func(p *referenceframe.PoseCloud, v float64) { p.OX = v }, + "oy": func(p *referenceframe.PoseCloud, v float64) { p.OY = v }, + "oz": func(p *referenceframe.PoseCloud, v float64) { p.OZ = v }, + "theta": func(p *referenceframe.PoseCloud, v float64) { p.Theta = v }, +} + +func poseCloudKeyList() string { + keys := make([]string, 0, len(poseCloudSetters)) + for k := range poseCloudSetters { + keys = append(keys, k) + } + sort.Strings(keys) + return strings.Join(keys, ", ") +} + +// parsePoseCloud converts a caller-supplied extra["pose_cloud"] into a PoseCloud. +// Omitted fields stay zero, which -- given the additive 0.001 epsilon -- means +// near-zero leeway. That is faithful passthrough and the point of an escape hatch, but it +// is sharp: a cloud whose leeways are all zero demands a match within 1 micron / 0.001 +// degrees, which no IK solution will realistically achieve. +func parsePoseCloud(raw interface{}) (*referenceframe.PoseCloud, error) { + m, ok := raw.(map[string]interface{}) + if !ok { + return nil, fmt.Errorf("pose_cloud must be an object, got %T", raw) + } + pc := &referenceframe.PoseCloud{} + for k, rv := range m { + set, known := poseCloudSetters[k] + if !known { + return nil, fmt.Errorf("pose_cloud: unknown key %q; valid keys are %s (all lowercase; "+ + "the Viam docs' OX/OY casing and protobuf's o_x are not accepted)", k, poseCloudKeyList()) + } + v, ok := rv.(float64) + if !ok { + return nil, fmt.Errorf("pose_cloud: %q must be a number, got %T", k, rv) + } + if math.IsNaN(v) || math.IsInf(v, 0) { + return nil, fmt.Errorf("pose_cloud: %q must be finite, got %v", k, v) + } + if v < 0 { + return nil, fmt.Errorf("pose_cloud: %q must not be negative, got %v", k, v) + } + set(pc, v) + } + return pc, nil +} diff --git a/goal_cloud_test.go b/goal_cloud_test.go index f0be119..bbfeddf 100644 --- a/goal_cloud_test.go +++ b/goal_cloud_test.go @@ -6,6 +6,7 @@ import ( "github.com/golang/geo/r3" "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" "go.viam.com/rdk/logging" "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" @@ -266,3 +267,47 @@ func TestRDKContractZeroLeewayDemandsMicronMatch(t *testing.T) { assert.False(t, broken.PoseInCloud(g, nudged), "with zero positional leeway even a 0.01mm offset is rejected") } + +func TestParsePoseCloudValid(t *testing.T) { + // extra arrives via structpb, so every JSON number is a float64. + // + // Every value is DISTINCT and all seven are asserted: identical values (e.g. x==y) + // would let a setter transposition survive undetected, and coneToPoseCloud's OX:1/OY:1 + // means no other test would catch it either. + got, err := parsePoseCloud(map[string]interface{}{ + "x": 2.0, "y": 3.0, "z": 0.5, "ox": 0.25, "oy": 0.5, "oz": 0.1, "theta": 180.0, + }) + require.NoError(t, err) + assert.Equal(t, 2.0, got.X) + assert.Equal(t, 3.0, got.Y) + assert.Equal(t, 0.5, got.Z) + assert.Equal(t, 0.25, got.OX) + assert.Equal(t, 0.5, got.OY) + assert.Equal(t, 0.1, got.OZ) + assert.Equal(t, 180.0, got.Theta) +} + +func TestParsePoseCloudOmittedFieldsStayZero(t *testing.T) { + got, err := parsePoseCloud(map[string]interface{}{"oz": 0.1}) + require.NoError(t, err) + assert.Equal(t, 0.0, got.X, "omitted fields stay zero -- sharp, but faithful passthrough") +} + +func TestParsePoseCloudRejects(t *testing.T) { + for name, input := range map[string]interface{}{ + "not an object": "nope", + "docs-style casing": map[string]interface{}{"OX": 1.0}, + "protobuf casing": map[string]interface{}{"o_x": 1.0}, + "not a field in v1.0": map[string]interface{}{"reference_frame": 1.0}, + "unknown key": map[string]interface{}{"wobble": 1.0}, + "non-numeric": map[string]interface{}{"oz": "0.1"}, + "negative": map[string]interface{}{"oz": -0.1}, + "NaN": map[string]interface{}{"oz": math.NaN()}, + "Inf": map[string]interface{}{"oz": math.Inf(1)}, + } { + t.Run(name, func(t *testing.T) { + _, err := parsePoseCloud(input) + assert.Error(t, err) + }) + } +} From 876feec003c737a33a58c325693316b016075a41 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 14:08:07 -0400 Subject: [PATCH 12/23] docs: pin all seven pose_cloud setters and purge the last never-matches 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. --- .../plans/2026-07-15-pose-cloud-orientation.md | 12 +++++++++--- .../2026-07-15-pose-cloud-orientation-design.md | 8 ++++---- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md index c77a3f3..5921dd9 100644 --- a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -20,7 +20,7 @@ Three facts drive every design choice. All were verified empirically against rdk 2. **`defaultEpsilon = 0.001` is ADDED to every leeway**, not just zeroed ones. Zero `X`/`Y`/`Z` therefore demands a 1-micron match, so positional leeway is **mandatory**. The effective cone is `acos(cos(tol) - 0.001)` — always slightly wider than requested. 3. **`OZ` alone defines the cone.** `OX`/`OY` are set to `1` (unconstrained). Valid across `[0, 180]`, saturating at `acos(-0.999) = 177.4374°`. -A cloud that never matches is **worse than no cloud**: the planner silently reverts to strict 6-DOF scoring and every move fails. +A cloud the solver cannot realistically land inside is **worse than no cloud**: the planner silently reverts to strict 6-DOF scoring and moves fail. --- @@ -563,12 +563,18 @@ Note this task's tests are the first to use `require`; add `"github.com/stretchr ```go func TestParsePoseCloudValid(t *testing.T) { // extra arrives via structpb, so every JSON number is a float64. + // Every value is DISTINCT and all seven are asserted: identical values (e.g. x==y) + // would let a setter transposition survive undetected, and coneToPoseCloud's OX:1/OY:1 + // means no other test would catch it either. got, err := parsePoseCloud(map[string]interface{}{ - "x": 2.0, "y": 2.0, "z": 0.5, "ox": 1.0, "oy": 1.0, "oz": 0.1, "theta": 180.0, + "x": 2.0, "y": 3.0, "z": 0.5, "ox": 0.25, "oy": 0.5, "oz": 0.1, "theta": 180.0, }) require.NoError(t, err) assert.Equal(t, 2.0, got.X) + assert.Equal(t, 3.0, got.Y) assert.Equal(t, 0.5, got.Z) + assert.Equal(t, 0.25, got.OX) + assert.Equal(t, 0.5, got.OY) assert.Equal(t, 0.1, got.OZ) assert.Equal(t, 180.0, got.Theta) } @@ -1318,7 +1324,7 @@ git commit -m "feat(simulated): plan MoveToPosition with an approach-axis cone" Follow the existing attribute-table style. Cover, for each model: - `orientation_tolerance_deg` — approach-axis cone half-angle. **Roll is not constrained.** Range `[0, 180]`, default `30`. `0` means the default, **not** exact. The enforced cone is `acos(cos(tol) - 0.001)`, so always slightly wider than requested (30 → ~30.11) and never narrower than ~2.56. At `>= 177.44` every orientation is accepted. -- `position_tolerance_mm` — per-axis box leeway along the goal frame's axes, default `1.0`. Worst-case corner deviation is `sqrt(3)` times the value (~1.73mm at the default). Too small and the cloud never matches, so every move fails. +- `position_tolerance_mm` — per-axis box leeway along the goal frame's axes, default `1.0`. Worst-case corner deviation is `sqrt(3)` times the value (~1.73mm at the default). Too small and the IK solver cannot realistically land inside the cloud, so planning reverts to strict six-DOF scoring and moves fail. - [ ] **Step 2: Add a short subsection on the escape hatch and server requirement** diff --git a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md index 92e6cda..ca5eb9c 100644 --- a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md +++ b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md @@ -47,9 +47,9 @@ reasonable reading of the docs, and each is load-bearing. `armplanning.PlannerOptions.GetGoalMetric` (v1.0.0, `planner_options.go:177`) consults `goal.GoalCloud.PoseInCloud(goal.Pose(), &dq)`. Inside the cloud the candidate scores zero; outside, the full weighted metric applies with `orientScale = 100`. For a goal the -arm cannot reach exactly, landing in the cloud is the *only* path to success. A cloud that -never matches is therefore worse than no cloud at all: planning silently reverts to strict -6-DOF scoring and fails. +arm cannot reach exactly, landing in the cloud is the *only* path to success. A cloud the solver +cannot realistically land inside is therefore worse than no cloud at all: planning silently +reverts to strict 6-DOF scoring and fails. ### Theta encodes tilt azimuth, not roll @@ -237,7 +237,7 @@ comfortably below it. The default is therefore a **starting point to be tuned empirically during the experiment**, not a derived value. The geometry is documented so the trade-off is legible: smaller tolerance means better accuracy but the cloud matches less often, and a -cloud that never matches fails every move. +cloud the solver cannot realistically land inside fails every move. ## Behavior From 8ae717a88666f3f9c5c60b46df0f3a68c8095e7e Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 14:11:16 -0400 Subject: [PATCH 13/23] feat(arm): add buildMoveDestination precedence and error wrapping --- goal_cloud.go | 100 +++++++++++++++++++++++++++++++++++++++++++++ goal_cloud_test.go | 83 +++++++++++++++++++++++++++++++++++++ 2 files changed, 183 insertions(+) diff --git a/goal_cloud.go b/goal_cloud.go index d5c6819..66db0ca 100644 --- a/goal_cloud.go +++ b/goal_cloud.go @@ -8,6 +8,7 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" ) @@ -160,3 +161,102 @@ func parsePoseCloud(raw interface{}) (*referenceframe.PoseCloud, error) { } return pc, nil } + +const ( + extraKeyPoseCloud = "pose_cloud" + extraKeyGoalMetricType = "goal_metric_type" +) + +// goalPath reports which precedence row buildMoveDestination took, so callers can wrap +// failures appropriately without re-inspecting extra (which would duplicate the +// precedence logic across both arm models and let it drift). +type goalPath int + +const ( + // pathInvalid is the zero value, returned alongside any error. It exists so that an + // error return is never mistakable for pathCone: wrapMoveErr would otherwise dress a + // pose_cloud parse error up as a cone-planning failure and tell the caller to widen + // tolerances they never set. + pathInvalid goalPath = iota + pathCone // cone from config + pathRawCloud // caller-supplied pose_cloud + pathMetricType // caller-supplied goal_metric_type; no cloud sent +) + +// buildMoveDestination applies the precedence table (see the plan/spec). +// +// originFrame is the FULLY-FORMED frame name (e.g. "myarm_origin"); this function does +// not append "_origin". It is passed to NewPoseInFrame unmodified. +// +// On success it returns a destination, a NEW extras map (the caller's map is never +// mutated), and the path taken. On error it returns (nil, nil, pathInvalid, err). +func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalCloudConfig, + extra map[string]interface{}, +) (*referenceframe.PoseInFrame, map[string]interface{}, goalPath, error) { + rawCloud, hasCloud := extra[extraKeyPoseCloud] + _, hasMetric := extra[extraKeyGoalMetricType] + + if hasCloud && hasMetric { + return nil, nil, pathInvalid, fmt.Errorf( + "extra cannot contain both %q and %q (got %s=%v): a goal cloud only matters when the "+ + "planner scores orientation, and %s=\"position_only\" sets orientScale=0, which "+ + "would make the cloud meaningless. Pass one or the other", + extraKeyPoseCloud, extraKeyGoalMetricType, extraKeyGoalMetricType, + extra[extraKeyGoalMetricType], extraKeyGoalMetricType) + } + + planExtra := make(map[string]interface{}, len(extra)) + for k, v := range extra { + if k == extraKeyPoseCloud { + continue // consumed here; not a planner key + } + planExtra[k] = v + } + + switch { + case hasMetric: + // The caller's metric wins and no cloud is sent: position_only sets orientScale=0, + // which would make any cloud meaningless. + return referenceframe.NewPoseInFrame(originFrame, pose), planExtra, pathMetricType, nil + case hasCloud: + pc, err := parsePoseCloud(rawCloud) + if err != nil { + return nil, nil, pathInvalid, err + } + return referenceframe.NewPoseInFrameWithGoalCloud(originFrame, pose, pc), planExtra, pathRawCloud, nil + default: + return referenceframe.NewPoseInFrameWithGoalCloud(originFrame, pose, coneToPoseCloud(cfg)), + planExtra, pathCone, nil + } +} + +// wrapMoveErr adds actionable guidance to a planning failure. The cone-specific wording +// applies only on the cone path: on the other paths it would misdirect, telling a caller +// who just passed position_only to pass position_only, or blaming a config tolerance that +// had no bearing on a raw-cloud failure. +// +// The cone message names BOTH tolerances. Either can cause the failure: too tight a cone +// leaves no reachable orientation, and too small a position tolerance means the solver can +// never land inside the cloud (which reverts planning to strict 6-DOF scoring and fails +// every move). Naming only the orientation knob would point at the wrong one half the time. +func wrapMoveErr(err error, path goalPath, cfg goalCloudConfig) error { + if err == nil { + return nil + } + switch path { + case pathCone: + return fmt.Errorf("move to position failed with orientation_tolerance_deg=%g, "+ + "position_tolerance_mm=%g (approach-axis cone): %w; widen either tolerance "+ + "(too small a position_tolerance_mm means the solver can never land inside the "+ + "goal cloud), pass extra {\"goal_metric_type\": \"position_only\"} to ignore "+ + "orientation, or check that viam-server is >= 0.127.0 (older servers silently "+ + "ignore goal clouds)", + cfg.OrientationToleranceDeg, cfg.PositionToleranceMM, err) + case pathRawCloud: + return fmt.Errorf("move to position failed with the caller-supplied pose_cloud: %w; "+ + "widen the cloud, or check that viam-server is >= 0.127.0 (older servers silently "+ + "ignore goal clouds)", err) + default: + return err + } +} diff --git a/goal_cloud_test.go b/goal_cloud_test.go index bbfeddf..ae4c593 100644 --- a/goal_cloud_test.go +++ b/goal_cloud_test.go @@ -304,6 +304,7 @@ func TestParsePoseCloudRejects(t *testing.T) { "negative": map[string]interface{}{"oz": -0.1}, "NaN": map[string]interface{}{"oz": math.NaN()}, "Inf": map[string]interface{}{"oz": math.Inf(1)}, + "nil value": nil, } { t.Run(name, func(t *testing.T) { _, err := parsePoseCloud(input) @@ -311,3 +312,85 @@ func TestParsePoseCloudRejects(t *testing.T) { }) } } + +func TestBuildMoveDestinationConePath(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + dest, planExtra, path, err := buildMoveDestination("myarm_origin", testGoal(), cfg, nil) + require.NoError(t, err) + assert.Equal(t, pathCone, path) + assert.Equal(t, "myarm_origin", dest.Parent(), "the frame name is passed through unmodified") + require.NotNil(t, dest.GoalCloud) + assert.Equal(t, coneToPoseCloud(cfg), dest.GoalCloud, "the destination carries the config's cone") + assert.NotContains(t, planExtra, "goal_metric_type", "the cone replaces position_only") +} + +func TestBuildMoveDestinationForwardsCallerExtras(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + _, planExtra, _, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"timeout": 5.0}) + require.NoError(t, err) + assert.Equal(t, 5.0, planExtra["timeout"], "unrelated caller keys still pass through") +} + +func TestBuildMoveDestinationRawCloudPath(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + dest, planExtra, path, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"pose_cloud": map[string]interface{}{"oz": 0.5}, "timeout": 5.0}) + require.NoError(t, err) + assert.Equal(t, pathRawCloud, path) + require.NotNil(t, dest.GoalCloud) + assert.Equal(t, 0.5, dest.GoalCloud.OZ, "the raw cloud replaces the cone entirely") + assert.NotContains(t, planExtra, "pose_cloud", "pose_cloud is not a planner key") + assert.Equal(t, 5.0, planExtra["timeout"]) +} + +func TestBuildMoveDestinationMetricTypePath(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + dest, planExtra, path, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"goal_metric_type": "position_only"}) + require.NoError(t, err) + assert.Equal(t, pathMetricType, path) + assert.Nil(t, dest.GoalCloud, "no cloud: position_only sets orientScale=0, making a cloud meaningless") + assert.Equal(t, "position_only", planExtra["goal_metric_type"], "the caller's metric is forwarded") +} + +func TestBuildMoveDestinationRejectsBothKeys(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + _, _, _, err := buildMoveDestination("a_origin", testGoal(), cfg, map[string]interface{}{ + "pose_cloud": map[string]interface{}{"oz": 0.5}, + "goal_metric_type": "position_only", + }) + require.Error(t, err, "incoherent: a raw cloud plus a metric that ignores clouds") + // The message is the entire UX here -- there is no fallback -- so pin that it names + // both offending keys rather than leaving the caller to guess which one to drop. + assert.ErrorContains(t, err, "pose_cloud") + assert.ErrorContains(t, err, "goal_metric_type") +} + +func TestBuildMoveDestinationDoesNotMutateCallerExtra(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + extra := map[string]interface{}{"pose_cloud": map[string]interface{}{"oz": 0.5}} + _, _, _, err := buildMoveDestination("a_origin", testGoal(), cfg, extra) + require.NoError(t, err) + assert.Contains(t, extra, "pose_cloud", "the caller's map must not be mutated") +} + +func TestBuildMoveDestinationPropagatesParseError(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + dest, planExtra, _, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"pose_cloud": map[string]interface{}{"OX": 1.0}}) + require.Error(t, err) + assert.Nil(t, dest) + assert.Nil(t, planExtra) +} + +func TestBuildMoveDestinationReturnsPathInvalidOnError(t *testing.T) { + cfg := resolveGoalCloudConfig(0, 0, nil) + _, _, path, err := buildMoveDestination("a_origin", testGoal(), cfg, + map[string]interface{}{"pose_cloud": map[string]interface{}{"OX": 1.0}}) + require.Error(t, err) + assert.Equal(t, pathInvalid, path, + "an error must not return pathCone, or wrapMoveErr would blame the cone for a parse error") + // The guarantee that makes this matter: wrapMoveErr must not dress it up as a cone failure. + assert.Equal(t, err, wrapMoveErr(err, path, cfg), "pathInvalid must pass the error through unwrapped") +} From 1bfc612575c8b97aa52e2e5c046730484c23b2ea Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 14:20:31 -0400 Subject: [PATCH 14/23] docs: give goalPath an explicit pathInvalid zero value 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. --- .../plans/2026-07-15-pose-cloud-orientation.md | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md index 5921dd9..1d9c0db 100644 --- a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -796,7 +796,12 @@ const ( type goalPath int const ( - pathCone goalPath = iota // cone from config + // pathInvalid is the zero value, returned alongside any error. It exists so an error + // return is never mistakable for pathCone: otherwise wrapMoveErr would dress a + // pose_cloud parse error up as a cone-planning failure and tell the caller to widen + // tolerances they never set. + pathInvalid goalPath = iota + pathCone // cone from config pathRawCloud // caller-supplied pose_cloud pathMetricType // caller-supplied goal_metric_type; no cloud sent ) @@ -807,7 +812,7 @@ const ( // not append "_origin". It is passed to NewPoseInFrame unmodified. // // On success it returns a destination, a NEW extras map (the caller's map is never -// mutated), and the path taken. On error it returns (nil, nil, 0, err). +// mutated), and the path taken. On error it returns (nil, nil, pathInvalid, err). func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalCloudConfig, extra map[string]interface{}, ) (*referenceframe.PoseInFrame, map[string]interface{}, goalPath, error) { @@ -815,7 +820,7 @@ func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalClo _, hasMetric := extra[extraKeyGoalMetricType] if hasCloud && hasMetric { - return nil, nil, 0, fmt.Errorf( + return nil, nil, pathInvalid, fmt.Errorf( "extra cannot contain both %q and %q: %q makes the planner ignore goal clouds, "+ "so the combination is incoherent", extraKeyPoseCloud, extraKeyGoalMetricType, extraKeyGoalMetricType) @@ -837,7 +842,7 @@ func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalClo case hasCloud: pc, err := parsePoseCloud(rawCloud) if err != nil { - return nil, nil, 0, err + return nil, nil, pathInvalid, err } return referenceframe.NewPoseInFrameWithGoalCloud(originFrame, pose, pc), planExtra, pathRawCloud, nil default: From 4d695a24950c95f2267616231bb067daed1684eb Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 14:34:20 -0400 Subject: [PATCH 15/23] docs: adapt Tasks 8-10 to main's manual/teach mode (PR #27) 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. --- .../2026-07-15-pose-cloud-orientation.md | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md index 1d9c0db..378b07b 100644 --- a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -949,8 +949,8 @@ git commit -m "test(arm): guard that the goal cloud survives gRPC serialization" ## Task 8: Config fields and `Validate` on both models **Files:** -- Modify: `arm.go:70-100` (`SO101ArmConfig`), `arm.go:103+` (`Validate`) -- Modify: `simulated.go:42-71` (`SO101SimulatedArmConfig`), `simulated.go:75-91` (`Validate`) +- Modify: `arm.go` — `SO101ArmConfig` and its `Validate`. **Line numbers are stale** (PR #27 added manual-mode config fields); locate by symbol name. +- Modify: `simulated.go` — `SO101SimulatedArmConfig` and its `Validate`. Locate by symbol name. - Test: `goal_cloud_test.go` - [ ] **Step 1: Write the failing tests** @@ -1067,7 +1067,7 @@ git commit -m "feat(arm): add orientation_tolerance_deg and position_tolerance_m ## Task 9: Wire the hardware arm's `MoveToPosition` **Files:** -- Modify: `arm.go:138-160` (struct), `arm.go:353+` (`NewSO101`), `arm.go:482-504` (`MoveToPosition`) +- Modify: `arm.go` — the `so101` struct, `NewSO101`, and `MoveToPosition` (~line 530). **Line numbers below are stale**: main gained PR #27 (manual/teach mode, +180 lines in arm.go) after this plan was written. Locate by symbol name, not by line. - Test: `arm_move_to_position_test.go` (create) - [ ] **Step 1: Write the failing test** @@ -1163,7 +1163,7 @@ In `NewSO101`, add to the `&so101{...}` literal: - [ ] **Step 5: Rewrite `MoveToPosition`** -Replace `arm.go:482-504` entirely: +Replace the body of `MoveToPosition` (locate by name; it is around line 530 after PR #27). **It now begins with a manual-mode guard that you MUST keep** — see the code below: ```go // MoveToPosition moves the arm's end-effector to the target pose. @@ -1179,9 +1179,18 @@ Replace `arm.go:482-504` entirely: // Requires viam-server >= 0.127.0; older servers silently ignore goal clouds, which makes // planning revert to strict six-DOF scoring and fail. func (s *so101) MoveToPosition(ctx context.Context, pose spatialmath.Pose, extra map[string]interface{}) error { + // PRESERVE THIS: added by the manual (hand-guided) teach mode feature. A motion + // command must take the arm out of manual mode. Do not drop it when rewriting below. + s.mu.Lock() + s.exitManualLocked("motion command received") + s.mu.Unlock() + dest, planExtra, path, err := buildMoveDestination( fmt.Sprintf("%v_origin", s.Name().Name), pose, s.goalCloud, extra) if err != nil { + // Return the build error UNWRAPPED. Do not pass it through wrapMoveErr: the path + // is pathInvalid, and wrapping a pose_cloud parse error as a cone-planning failure + // would tell the caller to widen tolerances they never set. return err } s.logger.Debugf("MoveToPosition goal cloud: %+v", dest.GoalCloud) @@ -1217,7 +1226,7 @@ git commit -m "feat(arm): plan MoveToPosition with an approach-axis cone" Identical shape to Task 9. The simulated model must stay a faithful stand-in for motion-plan testing. **Files:** -- Modify: `simulated.go:114-140` (struct), `simulated.go:145+` (`newSimulatedSO101`), `simulated.go:296-318` (`MoveToPosition`) +- Modify: `simulated.go` — the `simulatedSO101` struct, `newSimulatedSO101`, and `MoveToPosition`. Locate by symbol name; line numbers may have drifted. - Test: `arm_move_to_position_test.go` - [ ] **Step 1: Write the failing test** @@ -1272,7 +1281,7 @@ In `newSimulatedSO101`, add to the `&simulatedSO101{...}` literal: - [ ] **Step 5: Rewrite `MoveToPosition`** -Replace `simulated.go:296-318`, keeping the nil-motion guard: +Replace the body of `simulated.go`'s `MoveToPosition` (locate by name), keeping the nil-motion guard. Note the simulated model has NO manual mode, so it needs no exitManual guard: ```go // MoveToPosition moves the arm's end effector to the target pose using the motion service. From 3378b97d7b9cb66dd5398f1544e2d688460e7d60 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 14:35:56 -0400 Subject: [PATCH 16/23] test(arm): guard that the goal cloud survives gRPC serialization --- goal_cloud_test.go | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/goal_cloud_test.go b/goal_cloud_test.go index ae4c593..ab6a1c5 100644 --- a/goal_cloud_test.go +++ b/goal_cloud_test.go @@ -394,3 +394,26 @@ func TestBuildMoveDestinationReturnsPathInvalidOnError(t *testing.T) { // The guarantee that makes this matter: wrapMoveErr must not dress it up as a cone failure. assert.Equal(t, err, wrapMoveErr(err, path, cfg), "pathInvalid must pass the error through unwrapped") } + +// TestGoalCloudSurvivesSerialization guards the module-boundary gotcha: the cloud is only +// useful if it crosses gRPC. An in-process check would pass even if it never shipped. +func TestGoalCloudSurvivesSerialization(t *testing.T) { + cfg := resolveGoalCloudConfig(30, 1.0, nil) + dest, _, _, err := buildMoveDestination("myarm_origin", testGoal(), cfg, nil) + require.NoError(t, err) + + proto := referenceframe.PoseInFrameToProtobuf(dest) + require.NotNil(t, proto.GoalCloud, "the cloud must be serialized onto the wire") + + got := referenceframe.ProtobufToPoseInFrame(proto) + require.NotNil(t, got.GoalCloud, "the cloud must survive the round trip") + + want := dest.GoalCloud + assert.InDelta(t, want.X, got.GoalCloud.X, 1e-9) + assert.InDelta(t, want.Y, got.GoalCloud.Y, 1e-9) + assert.InDelta(t, want.Z, got.GoalCloud.Z, 1e-9) + assert.InDelta(t, want.OX, got.GoalCloud.OX, 1e-9) + assert.InDelta(t, want.OY, got.GoalCloud.OY, 1e-9) + assert.InDelta(t, want.OZ, got.GoalCloud.OZ, 1e-9) + assert.InDelta(t, want.Theta, got.GoalCloud.Theta, 1e-9) +} From 75dedad7f4d625ec2166273dc6ec8cb8969586ed Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 15:38:19 -0400 Subject: [PATCH 17/23] feat(arm): add orientation_tolerance_deg and position_tolerance_mm config 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. --- arm.go | 23 +++++++++++++++++++++ goal_cloud.go | 29 ++++++++++++++++++++------ goal_cloud_test.go | 51 +++++++++++++++++++++++++++++++++++++++++++--- simulated.go | 23 +++++++++++++++++++++ 4 files changed, 117 insertions(+), 9 deletions(-) diff --git a/arm.go b/arm.go index 72564db..a6066b9 100644 --- a/arm.go +++ b/arm.go @@ -101,6 +101,25 @@ type SO101ArmConfig struct { // ManualMode tunes hand-guided ("manual") mode. Optional; nil disables // the feature / uses built-in defaults throughout. ManualMode *ManualModeConfig `json:"manual_mode,omitempty"` + + // OrientationToleranceDeg is the half-angle, in degrees, of the cone of acceptable + // end-effector approach directions around the goal orientation. Roll about that axis + // is NOT constrained. Valid range [0, 180]. Zero or unset means + // defaultOrientationToleranceDeg (30). + // + // The planner adds a 0.001 epsilon to the OZ leeway, so the cone actually enforced is + // acos(cos(tol) - 0.001): always slightly wider than requested (30 gives ~30.11) and + // never narrower than ~2.56. At or above 177.44 every orientation is accepted. + // + // Requires viam-server >= 0.127.0; older servers silently ignore goal clouds. + OrientationToleranceDeg float64 `json:"orientation_tolerance_deg,omitempty"` + + // PositionToleranceMM is the per-axis positional leeway of the goal cloud, applied as + // a box along the goal frame's axes (worst-case corner deviation is sqrt(3) times this + // value). It must be large enough for the IK solver to realistically land inside; a + // cloud only an exact match could satisfy means every move fails. Zero or unset means + // defaultPositionToleranceMM (1.0). + PositionToleranceMM float64 `json:"position_tolerance_mm,omitempty"` } // ManualModeConfig tunes hand-guided ("manual") mode. All fields optional; @@ -162,6 +181,10 @@ func (cfg *SO101ArmConfig) Validate(path string) ([]string, []string, error) { return nil, nil, err } + if err := validateGoalCloudTolerances(cfg.OrientationToleranceDeg, cfg.PositionToleranceMM); err != nil { + return nil, nil, err + } + deps := []string{} if cfg.Motion != "" { diff --git a/goal_cloud.go b/goal_cloud.go index 66db0ca..a85e9c2 100644 --- a/goal_cloud.go +++ b/goal_cloud.go @@ -30,8 +30,10 @@ const ( // poseCloudSaturationCos is the cosine at which the cone accepts every orientation. // PoseInCloud adds a 0.001 epsilon to the OZ leeway and the maximum possible // deviation is 2.0, so saturation begins where 1-cos(a)+0.001 >= 2. Expressed as a - // cosine rather than the rounded 177.44 degrees: the true threshold is 177.4374, and - // a literal degree comparison would miss the [177.4374, 177.44) band. + // cosine rather than a degree literal: the exact threshold is acos(-0.999) = + // 177.4374412669, so any rounded degree constant is wrong in one direction or the + // other -- 177.4374 sits below it (misses [177.4374412669, 177.44)), and 177.44 sits + // above it. Comparing cosines avoids the choice entirely. poseCloudSaturationCos = -0.999 ) @@ -62,17 +64,32 @@ func resolveGoalCloudConfig(tolDeg, posTolMM float64, logger logging.Logger) goa "from the goal (the cloud is a per-axis box, so the worst-case corner is sqrt(3) times this)", cfg.PositionToleranceMM, math.Sqrt(3)*cfg.PositionToleranceMM) } - // Cite 177.4374 (the true acos(-0.999) threshold), NOT the rounded 177.44 used in - // user-facing docs: this guard fires across [177.4374, 177.44), so quoting 177.44 here - // would contradict the action taken. %g likewise avoids printing 177.4374 as "177.4". + // Cite 177.4374412669 (the exact acos(-0.999) threshold), NOT the rounded 177.44 used + // in user-facing docs: this guard fires across [177.4374412669, 177.44), so quoting + // 177.44 here would contradict the action taken. Do not round the cited value down + // either -- 177.4374 sits below the threshold, where this guard does not fire. %g + // likewise avoids printing it as "177.4". if math.Cos(utils.DegToRad(cfg.OrientationToleranceDeg)) <= poseCloudSaturationCos { - logger.Warnf("orientation_tolerance_deg=%g saturates the approach-axis cone (>= 177.4374); "+ + logger.Warnf("orientation_tolerance_deg=%g saturates the approach-axis cone (>= 177.4374412669); "+ "every orientation will be accepted, which is equivalent to ignoring orientation", cfg.OrientationToleranceDeg) } return cfg } +// validateGoalCloudTolerances is shared by both arm models' Validate methods. Note NaN +// must be rejected explicitly: NaN comparisons are always false, so a NaN would slip +// through the range checks. +func validateGoalCloudTolerances(tolDeg, posTolMM float64) error { + if math.IsNaN(tolDeg) || tolDeg < 0 || tolDeg > 180 { + return fmt.Errorf("orientation_tolerance_deg must be in [0, 180], got %v", tolDeg) + } + if math.IsNaN(posTolMM) || posTolMM < 0 { + return fmt.Errorf("position_tolerance_mm must not be negative, got %v", posTolMM) + } + return nil +} + // coneToPoseCloud converts an approach-axis cone half-angle into a PoseCloud. // // Every field here is load-bearing and was established empirically; see diff --git a/goal_cloud_test.go b/goal_cloud_test.go index ab6a1c5..5aa60f9 100644 --- a/goal_cloud_test.go +++ b/goal_cloud_test.go @@ -31,8 +31,8 @@ func TestResolveGoalCloudConfigKeepsExplicitValues(t *testing.T) { // thresholds; nothing downstream re-checks them, so nothing downstream would catch a // regression either. func TestResolveGoalCloudConfigWarnsAtSaturationBoundary(t *testing.T) { - // The boundary is acos(-0.999) = 177.4374 -- exactly the band a literal ">= 177.44" - // check would miss, which is why poseCloudSaturationCos is a cosine. + // The boundary is acos(-0.999) = 177.4374412669 -- exactly the band a literal + // ">= 177.44" check would miss, which is why poseCloudSaturationCos is a cosine. logger, logs := logging.NewObservedTestLogger(t) resolveGoalCloudConfig(177.438, 0, logger) assert.Equal(t, 1, logs.FilterMessageSnippet("saturates").Len(), "just inside the boundary must warn") @@ -126,7 +126,7 @@ func TestConeBoundsAbove90Degrees(t *testing.T) { // were wrong, and the sibling would still pass if both the constant and the mapping drifted // together. func TestConeSaturationBoundary(t *testing.T) { - // Saturation begins at acos(-0.999) = 177.4374, NOT 179. + // Saturation begins at acos(-0.999) = 177.4374412669, NOT 179. assert.False(t, coneCloud(177.40).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), "177.40 must still reject a 180deg tilt") assert.True(t, coneCloud(177.44).PoseInCloud(testGoal(), tiltedBy(1, 0, 180)), @@ -417,3 +417,48 @@ func TestGoalCloudSurvivesSerialization(t *testing.T) { assert.InDelta(t, want.OZ, got.GoalCloud.OZ, 1e-9) assert.InDelta(t, want.Theta, got.GoalCloud.Theta, 1e-9) } + +func TestArmConfigValidateToleranceRanges(t *testing.T) { + base := func() *SO101ArmConfig { return &SO101ArmConfig{Port: "/dev/null"} } + + for name, mutate := range map[string]func(*SO101ArmConfig){ + "negative orientation": func(c *SO101ArmConfig) { c.OrientationToleranceDeg = -1 }, + "orientation over 180": func(c *SO101ArmConfig) { c.OrientationToleranceDeg = 181 }, + "NaN orientation": func(c *SO101ArmConfig) { c.OrientationToleranceDeg = math.NaN() }, + "negative position": func(c *SO101ArmConfig) { c.PositionToleranceMM = -0.1 }, + "NaN position": func(c *SO101ArmConfig) { c.PositionToleranceMM = math.NaN() }, + } { + t.Run(name, func(t *testing.T) { + c := base() + mutate(c) + _, _, err := c.Validate("") + assert.Error(t, err) + }) + } + + t.Run("valid bounds accepted", func(t *testing.T) { + c := base() + c.OrientationToleranceDeg, c.PositionToleranceMM = 180, 75 + _, _, err := c.Validate("") + assert.NoError(t, err) + }) +} + +func TestSimulatedConfigValidateToleranceRanges(t *testing.T) { + c := &SO101SimulatedArmConfig{OrientationToleranceDeg: 181} + _, _, err := c.Validate("") + assert.Error(t, err) + + c = &SO101SimulatedArmConfig{PositionToleranceMM: -1} + _, _, err = c.Validate("") + assert.Error(t, err) + + // Negative control: without this, the two assertions above would pass even if Validate + // errored unconditionally, i.e. they could not tell "tolerances validated" from + // "Validate always fails". + t.Run("valid bounds accepted", func(t *testing.T) { + c := &SO101SimulatedArmConfig{OrientationToleranceDeg: 30, PositionToleranceMM: 5} + _, _, err := c.Validate("") + assert.NoError(t, err) + }) +} diff --git a/simulated.go b/simulated.go index c1eb059..fb963f9 100644 --- a/simulated.go +++ b/simulated.go @@ -68,6 +68,25 @@ type SO101SimulatedArmConfig struct { // aggressive. Defaults to 0.9 for each of the 5 meshes when empty. Ignored unless // UseURDF is set. MeshDecimationRatios []float64 `json:"mesh_decimation_ratios,omitempty"` + + // OrientationToleranceDeg is the half-angle, in degrees, of the cone of acceptable + // end-effector approach directions around the goal orientation. Roll about that axis + // is NOT constrained. Valid range [0, 180]. Zero or unset means + // defaultOrientationToleranceDeg (30). + // + // The planner adds a 0.001 epsilon to the OZ leeway, so the cone actually enforced is + // acos(cos(tol) - 0.001): always slightly wider than requested (30 gives ~30.11) and + // never narrower than ~2.56. At or above 177.44 every orientation is accepted. + // + // Requires viam-server >= 0.127.0; older servers silently ignore goal clouds. + OrientationToleranceDeg float64 `json:"orientation_tolerance_deg,omitempty"` + + // PositionToleranceMM is the per-axis positional leeway of the goal cloud, applied as + // a box along the goal frame's axes (worst-case corner deviation is sqrt(3) times this + // value). It must be large enough for the IK solver to realistically land inside; a + // cloud only an exact match could satisfy means every move fails. Zero or unset means + // defaultPositionToleranceMM (1.0). + PositionToleranceMM float64 `json:"position_tolerance_mm,omitempty"` } // Validate ensures all parts of the config are valid and declares the motion service @@ -83,6 +102,10 @@ func (cfg *SO101SimulatedArmConfig) Validate(path string) ([]string, []string, e } } + if err := validateGoalCloudTolerances(cfg.OrientationToleranceDeg, cfg.PositionToleranceMM); err != nil { + return nil, nil, err + } + motionName := cfg.Motion if motionName == "" { motionName = "builtin" From 0aa9c5649d4f179a7deb895eb6b65ddb388598b7 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 15:45:47 -0400 Subject: [PATCH 18/23] docs: state the saturation threshold as a cosine, not a rounded degree 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. --- .../plans/2026-07-15-pose-cloud-orientation.md | 15 +++++++++------ .../2026-07-15-pose-cloud-orientation-design.md | 4 +++- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md index 378b07b..f7751bf 100644 --- a/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -195,8 +195,10 @@ const ( // poseCloudSaturationCos is the cosine at which the cone accepts every orientation. // PoseInCloud adds a 0.001 epsilon to the OZ leeway and the maximum possible // deviation is 2.0, so saturation begins where 1-cos(a)+0.001 >= 2. Expressed as a - // cosine rather than the rounded 177.44 degrees: the true threshold is 177.4374, and - // a literal degree comparison would miss the [177.4374, 177.44) band. + // cosine rather than a degree literal: the exact threshold is acos(-0.999) = + // 177.4374412669, so any rounded degree constant is wrong in one direction or the + // other -- 177.4374 sits BELOW it (and so would miss [177.4374412669, 177.44)), + // while 177.44 sits above it. Comparing cosines avoids the choice entirely. poseCloudSaturationCos = -0.999 ) @@ -227,11 +229,12 @@ func resolveGoalCloudConfig(tolDeg, posTolMM float64, logger logging.Logger) goa "from the goal (the cloud is a per-axis box, so the worst-case corner is sqrt(3) times this)", cfg.PositionToleranceMM, math.Sqrt(3)*cfg.PositionToleranceMM) } - // Cite 177.4374 (the true acos(-0.999) threshold), NOT the rounded 177.44 used in - // user-facing docs: this guard fires across [177.4374, 177.44), so quoting 177.44 here - // would contradict the action taken. %g likewise avoids printing 177.4374 as "177.4". + // Cite acos(-0.999) ~= 177.4375, NOT the rounded 177.44 used in user-facing docs: + // this guard fires across [177.4374412669, 177.44), so quoting 177.44 here would + // contradict the action taken. Note 177.4374 would be wrong too -- it rounds DOWN, + // below the true threshold. %g likewise avoids printing the value as "177.4". if math.Cos(utils.DegToRad(cfg.OrientationToleranceDeg)) <= poseCloudSaturationCos { - logger.Warnf("orientation_tolerance_deg=%g saturates the approach-axis cone (>= 177.4374); "+ + logger.Warnf("orientation_tolerance_deg=%g saturates the approach-axis cone (acos(-0.999) ~= 177.4375); "+ "every orientation will be accepted, which is equivalent to ignoring orientation", cfg.OrientationToleranceDeg) } diff --git a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md index ca5eb9c..2f1f7de 100644 --- a/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md +++ b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md @@ -219,7 +219,9 @@ saturated and accepts every orientation. The saturation check must be implemented as `math.Cos(rad(tol)) <= -0.999`, **not** as a literal `tol >= 177.44`: saturation actually begins at 177.4374°, so a literal 177.44 -threshold would stay silent across `[177.4374, 177.44)`, which is saturated. The rounded +threshold would stay silent across `[177.4374412669, 177.44)`, which is saturated. Note +`177.4374` is wrong in the other direction -- it rounds DOWN, below the true threshold, so +`177.4374` itself does NOT saturate (verified: antipodal rejected there). The rounded 177.44 figure is for user-facing docs only, where "≥ 177.44 accepts any orientation" is true as written. From ffecacc45ae5caf523a1635193607d5f6f0bbaa3 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 15:55:15 -0400 Subject: [PATCH 19/23] feat(arm): plan MoveToPosition with an approach-axis cone --- arm.go | 49 +++++++++++++++++++------------ arm_move_to_position_test.go | 57 ++++++++++++++++++++++++++++++++++++ 2 files changed, 88 insertions(+), 18 deletions(-) create mode 100644 arm_move_to_position_test.go diff --git a/arm.go b/arm.go index a6066b9..5a54e8b 100644 --- a/arm.go +++ b/arm.go @@ -207,6 +207,10 @@ type so101 struct { controller *SafeSoArmController manual *manualSession + // goalCloud is the resolved approach-axis tolerance pair. Set once in NewSO101 and + // never mutated (the model is resource.AlwaysRebuild), so it needs no mutex. + goalCloud goalCloudConfig + mu sync.RWMutex moveLock sync.Mutex isMoving atomic.Bool @@ -499,6 +503,7 @@ func NewSO101(ctx context.Context, deps resource.Dependencies, name resource.Nam defaultSpeed: speedDegsPerSec, defaultAcc: accelerationDegsPerSec, motion: ms, + goalCloud: resolveGoalCloudConfig(conf.OrientationToleranceDeg, conf.PositionToleranceMM, logger), cancelCtx: cancelCtx, cancelFunc: cancelFunc, initCtx: ctx, // Store initialization context @@ -544,30 +549,38 @@ func (s *so101) EndPosition(ctx context.Context, extra map[string]interface{}) ( // MoveToPosition moves the arm's end-effector to the target pose. // -// The SO-101 is a 5-DOF arm. Six-DOF pose targets (position + orientation) are only reachable -// on a 5-dimensional submanifold of SE(3); most combinations of target position and target -// orientation are unreachable, producing "zero IK solutions produced" errors. We therefore -// default the planner's goal metric to "position_only" so the solver matches the target point -// and accepts whatever orientation falls out. Callers who want different planner behavior may -// override any key by passing their own value via extra. +// The SO-101 is a 5-DOF arm, so most six-DOF pose targets are unreachable. Rather than +// discard orientation wholesale, the planner is given an approach-axis cone (a +// referenceframe.PoseCloud) around the goal orientation: the tool's pointing direction is +// constrained to within orientation_tolerance_deg, while roll about that axis is free. +// +// Callers may bypass the cone via extra: "goal_metric_type" restores the old +// orientation-agnostic behavior, and "pose_cloud" supplies a raw cloud. See goal_cloud.go. +// +// Requires viam-server >= 0.127.0; older servers silently ignore goal clouds, which makes +// planning revert to strict six-DOF scoring and fail. func (s *so101) MoveToPosition(ctx context.Context, pose spatialmath.Pose, extra map[string]interface{}) error { + // A motion command takes the arm out of hand-guided ("manual") mode. s.mu.Lock() s.exitManualLocked("motion command received") s.mu.Unlock() - planExtra := map[string]interface{}{"goal_metric_type": "position_only"} - for k, v := range extra { - planExtra[k] = v + dest, planExtra, path, err := buildMoveDestination( + fmt.Sprintf("%v_origin", s.Name().Name), pose, s.goalCloud, extra) + if err != nil { + // Return the build error UNWRAPPED: path is pathInvalid, and wrapping a pose_cloud + // parse error as a cone-planning failure would tell the caller to widen tolerances + // they never set. + return err } - _, err := s.motion.Move( - ctx, - motion.MoveReq{ - ComponentName: s.Name().Name, - Destination: referenceframe.NewPoseInFrame(fmt.Sprintf("%v_origin", s.Name().Name), pose), - Extra: planExtra, - }, - ) - return err + s.logger.Debugf("MoveToPosition goal cloud: %+v", dest.GoalCloud) + + _, err = s.motion.Move(ctx, motion.MoveReq{ + ComponentName: s.Name().Name, + Destination: dest, + Extra: planExtra, + }) + return wrapMoveErr(err, path, s.goalCloud) } // clampPositions clamps joint inputs (already in radians) to each joint's calibrated diff --git a/arm_move_to_position_test.go b/arm_move_to_position_test.go new file mode 100644 index 0000000..d6419d7 --- /dev/null +++ b/arm_move_to_position_test.go @@ -0,0 +1,57 @@ +package so_arm + +import ( + "context" + "testing" + + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" + "go.viam.com/rdk/components/arm" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/services/motion" + injectmotion "go.viam.com/rdk/testutils/inject/motion" +) + +// captureMotion returns an injected motion service that records the last MoveReq. +func captureMotion(got *motion.MoveReq) *injectmotion.MotionService { + ms := injectmotion.NewMotionService("builtin") + ms.MoveFunc = func(ctx context.Context, req motion.MoveReq) (bool, error) { + *got = req + return true, nil + } + return ms +} + +func TestHardwareArmMoveToPositionSendsGoalCloud(t *testing.T) { + var got motion.MoveReq + a := &so101{ + name: arm.Named("myarm"), + logger: logging.NewTestLogger(t), + motion: captureMotion(&got), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + + require.NoError(t, a.MoveToPosition(context.Background(), testGoal(), nil)) + + assert.Equal(t, "myarm", got.ComponentName) + assert.Equal(t, "myarm_origin", got.Destination.Parent()) + require.NotNil(t, got.Destination.GoalCloud, "the cone must ship to the planner") + assert.Equal(t, 180.0, got.Destination.GoalCloud.Theta) + assert.NotContains(t, got.Extra, "goal_metric_type", "the cone replaces position_only") +} + +func TestHardwareArmMoveToPositionHonorsMetricTypeOverride(t *testing.T) { + var got motion.MoveReq + a := &so101{ + name: arm.Named("myarm"), + logger: logging.NewTestLogger(t), + motion: captureMotion(&got), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + + require.NoError(t, a.MoveToPosition(context.Background(), testGoal(), + map[string]interface{}{"goal_metric_type": "position_only"})) + + assert.Nil(t, got.Destination.GoalCloud, "no cloud when the caller picks the metric") + assert.Equal(t, "position_only", got.Extra["goal_metric_type"]) +} From d18da69251ac8e9df1a846acc68c55d7796e0d32 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 15:58:41 -0400 Subject: [PATCH 20/23] feat(simulated): plan MoveToPosition with an approach-axis cone --- arm_move_to_position_test.go | 95 ++++++++++++++++++++++++++++++++++++ simulated.go | 32 ++++++++---- 2 files changed, 117 insertions(+), 10 deletions(-) diff --git a/arm_move_to_position_test.go b/arm_move_to_position_test.go index d6419d7..8a4d33d 100644 --- a/arm_move_to_position_test.go +++ b/arm_move_to_position_test.go @@ -8,6 +8,7 @@ import ( "github.com/stretchr/testify/require" "go.viam.com/rdk/components/arm" "go.viam.com/rdk/logging" + "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" injectmotion "go.viam.com/rdk/testutils/inject/motion" ) @@ -55,3 +56,97 @@ func TestHardwareArmMoveToPositionHonorsMetricTypeOverride(t *testing.T) { assert.Nil(t, got.Destination.GoalCloud, "no cloud when the caller picks the metric") assert.Equal(t, "position_only", got.Extra["goal_metric_type"]) } + +func TestSimulatedArmMoveToPositionSendsGoalCloud(t *testing.T) { + var got motion.MoveReq + s := &simulatedSO101{ + name: arm.Named("simarm"), + logger: logging.NewTestLogger(t), + motion: captureMotion(&got), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + + require.NoError(t, s.MoveToPosition(context.Background(), testGoal(), nil)) + + assert.Equal(t, "simarm_origin", got.Destination.Parent()) + require.NotNil(t, got.Destination.GoalCloud) + assert.Equal(t, 180.0, got.Destination.GoalCloud.Theta) +} + +func TestSimulatedArmMoveToPositionRequiresMotionService(t *testing.T) { + s := &simulatedSO101{name: arm.Named("simarm"), logger: logging.NewTestLogger(t)} + err := s.MoveToPosition(context.Background(), testGoal(), nil) + // Pin the guard's identity, not merely "an error": without asserting the message, + // removing the guard is still caught, but only as a SIGSEGV that panics the whole test + // binary -- a failure that names this test while explaining nothing. + require.Error(t, err, "the nil-motion guard must still fire") + assert.ErrorContains(t, err, "requires a motion service") +} + +// simArmWithTolerances builds a simulated arm through the REAL constructor (nil deps, as +// simulated_test.go's newTestSimArm does; simulated clock off so no goroutine starts), so +// a test can observe what newSimulatedSO101 itself resolves into goalCloud. +func simArmWithTolerances(t *testing.T, tolDeg, posTolMM float64) *simulatedSO101 { + t.Helper() + simulateTime := false + conf := resource.Config{ + Name: "simarm", + API: arm.API, + Model: SO101SimulatedModel, + ConvertedAttributes: &SO101SimulatedArmConfig{ + SimulateTime: &simulateTime, + OrientationToleranceDeg: tolDeg, + PositionToleranceMM: posTolMM, + }, + } + // deps is nil: resolving goalCloud does not need the motion service. + a, err := newSimulatedSO101(context.Background(), nil, conf, logging.NewTestLogger(t)) + require.NoError(t, err) + return a.(*simulatedSO101) +} + +// TestSimulatedConstructorWiresGoalCloud guards the one thing the struct-literal tests +// above cannot see: that the CONSTRUCTOR resolves the config into goalCloud. Dropping that +// line leaves goalCloud zero-valued -- X/Y/Z/OZ all 0 -- which per goal_cloud.go is a cloud +// no IK solution realistically satisfies, so planning reverts to strict 6-DOF scoring and +// every move fails silently, with the construction-time warnings dead. The struct-literal +// tests set the field themselves and would stay green through all of that. +func TestSimulatedConstructorWiresGoalCloud(t *testing.T) { + // Defaults: catches the line being dropped entirely. + sim := simArmWithTolerances(t, 0, 0) + assert.Equal(t, goalCloudConfig{ + OrientationToleranceDeg: defaultOrientationToleranceDeg, + PositionToleranceMM: defaultPositionToleranceMM, + }, sim.goalCloud, "the constructor must resolve defaults into goalCloud") + + // Explicit values: catches a line that resolves but ignores the config. + sim = simArmWithTolerances(t, 12.5, 0.25) + assert.Equal(t, goalCloudConfig{OrientationToleranceDeg: 12.5, PositionToleranceMM: 0.25}, + sim.goalCloud, "explicit config must reach goalCloud unchanged") +} + +// TestHardwareArmMoveToPositionExitsManualMode guards PR #27's integration: a motion +// command must take the arm out of hand-guided mode. Nothing else asserts this -- the +// manual-mode tests cover exitManualLocked in isolation, not that MoveToPosition calls it. +func TestHardwareArmMoveToPositionExitsManualMode(t *testing.T) { + var got motion.MoveReq + a := &so101{ + name: arm.Named("myarm"), + logger: logging.NewTestLogger(t), + motion: captureMotion(&got), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + // exitManualLocked no-ops unless the session is non-nil AND running, so the test only + // has teeth with a started session. newFakeIO stands in for the servo bus. + io := newFakeIO() + a.manual = newManualSession(context.Background(), io, manualDefaults(), 0, 0, a.logger) + a.manual.start() + require.True(t, a.manual.running(), "precondition: the arm must be in manual mode") + + require.NoError(t, a.MoveToPosition(context.Background(), testGoal(), nil)) + + assert.Nil(t, a.manual, "a motion command must tear the manual session down") + io.mu.Lock() + defer io.mu.Unlock() + assert.True(t, io.complianceRestored, "exiting manual mode must restore servo stiffness") +} diff --git a/simulated.go b/simulated.go index fb963f9..a19a17e 100644 --- a/simulated.go +++ b/simulated.go @@ -152,6 +152,10 @@ type simulatedSO101 struct { // visualizeEEFrame adds the colored EE coordinate-frame marker to Get3DModels. visualizeEEFrame bool + // goalCloud is the resolved approach-axis tolerance pair. Set once in + // newSimulatedSO101 and never mutated (resource.AlwaysRebuild), so it needs no mutex. + goalCloud goalCloudConfig + // lifetime management closed atomic.Bool cancelCtx context.Context @@ -205,6 +209,7 @@ func newSimulatedSO101( motion: ms, speed: speedDegsPerSec * math.Pi / 180.0, visualizeEEFrame: conf.VisualizeEEFrame, + goalCloud: resolveGoalCloudConfig(conf.OrientationToleranceDeg, conf.PositionToleranceMM, logger), cancelCtx: cancelCtx, cancelFunc: cancelFunc, currInputs: make([]float64, len(model.DoF())), @@ -318,26 +323,33 @@ func (s *simulatedSO101) EndPosition(ctx context.Context, extra map[string]inter // MoveToPosition moves the arm's end effector to the target pose using the motion service. // -// The SO-101 is a 5-DOF arm, so most six-DOF pose targets are unreachable. As with the -// devrel:so101:arm model, the planner's goal metric defaults to "position_only" so the -// solver matches the target point and accepts whatever orientation results. Callers may -// override any planner key via extra. +// As with the devrel:so101:arm model, the planner is given an approach-axis cone around +// the goal orientation rather than discarding orientation via "position_only": the tool's +// pointing direction is constrained to within orientation_tolerance_deg, while roll about +// that axis is free. Callers may bypass the cone via extra; see goal_cloud.go. +// +// Requires viam-server >= 0.127.0; older servers silently ignore goal clouds. func (s *simulatedSO101) MoveToPosition(ctx context.Context, pose spatialmath.Pose, extra map[string]interface{}) error { if s.motion == nil { return errors.New("MoveToPosition requires a motion service, which was not available at construction") } - planExtra := map[string]interface{}{"goal_metric_type": "position_only"} - for k, v := range extra { - planExtra[k] = v + dest, planExtra, path, err := buildMoveDestination( + fmt.Sprintf("%v_origin", s.name.Name), pose, s.goalCloud, extra) + if err != nil { + // Return the build error UNWRAPPED: path is pathInvalid, and wrapping a pose_cloud + // parse error as a cone-planning failure would tell the caller to widen tolerances + // they never set. + return err } + s.logger.Debugf("MoveToPosition goal cloud: %+v", dest.GoalCloud) - _, err := s.motion.Move(ctx, motion.MoveReq{ + _, err = s.motion.Move(ctx, motion.MoveReq{ ComponentName: s.name.Name, - Destination: referenceframe.NewPoseInFrame(fmt.Sprintf("%v_origin", s.name.Name), pose), + Destination: dest, Extra: planExtra, }) - return err + return wrapMoveErr(err, path, s.goalCloud) } // MoveToJointPositions starts a move to the given joint configuration and blocks until it From 2381cdaa67eae106d2f8eec4e7abf9cc805a0804 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 16:12:09 -0400 Subject: [PATCH 21/23] docs: document orientation_tolerance_deg and position_tolerance_mm --- README.md | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index d3be253..9c22b42 100644 --- a/README.md +++ b/README.md @@ -70,6 +70,8 @@ The following attributes are available for the arm component: | `use_urdf` | bool | Optional | When `true`, sources kinematics and collision geometry from the bundled `arm/so101.urdf` instead of the embedded `so101.json`. Requires the `VIAM_MODULE_ROOT` environment variable to be set (viam-server sets this automatically). This is a drop-in swap: the bundled URDF uses `so101.json`'s frame names and TCP exactly, so the frame names, TCP pose, and kinematics are identical — only the collision geometry upgrades from primitive shapes to per-link meshes. Default `false`. | | `mesh_decimation_ratios` | []float | Optional | Per-collision-mesh simplification ratios, one per arm link in document order (base, shoulder, upper_arm, lower_arm, wrist), each in `[0, 1]`. Only used when `use_urdf` is `true`. Only values strictly in `(0, 1)` decimate (lower = more aggressive). Defaults to `0.9` for each of the 5 meshes when empty. | | `manual_mode` | object | Optional | Tuning for hand-guided [manual mode](#manual-mode) (see below). Omit for the built-in defaults. | +| `orientation_tolerance_deg` | float | Optional | Half-angle, in degrees, of the cone of acceptable end-effector approach directions around the goal orientation used by `MoveToPosition` (see [Approach-axis orientation planning](#approach-axis-orientation-planning) below). **Roll about that axis is NOT constrained.** Valid range `[0, 180]`. `0`/unset means the default (`30`), **not** an exact match. The planner adds a small epsilon to the cone, so the angle actually enforced is `acos(cos(tol) - 0.001)` — always slightly wider than requested (`30` gives ~`30.11`) and never narrower than ~`2.56`, regardless of how small a value you configure. At or above `177.44`, every orientation is accepted. | +| `position_tolerance_mm` | float | Optional | Per-axis positional leeway, in millimeters, of the `MoveToPosition` goal cloud, applied as a box along the goal frame's axes (not a radius) — default `1.0`. Worst-case corner deviation is `sqrt(3)` times this value (~`1.73mm` at the default). Too small a value and the IK solver cannot realistically land inside the cloud, so planning reverts to strict six-DOF scoring and moves fail. | **If you're building and setting up an arm for the first time, please see the [calibration sensor component](#model-devrelso101calibration) for setup instructions.** @@ -101,6 +103,21 @@ The hardware arm enforces a real per-joint speed cap on the Feetech servos: each `acceleration_degs_per_sec_per_sec` is validated and stored when set via config or `set_acceleration` DoCommand, but is not yet written to the servo hardware. Acceleration enforcement is a planned follow-up. +### Approach-axis orientation planning + +The SO-101 is a 5-DOF arm, so most six-DOF pose targets are unreachable exactly. Rather than discard orientation wholesale, `MoveToPosition` attaches a `referenceframe.PoseCloud` to the goal: a cone that constrains where the tool points (the approach axis), while **roll about that axis is free**. `orientation_tolerance_deg` and `position_tolerance_mm` (documented in the Attributes table above) control the cone's size. + +**Behavior change for existing users:** this cone is on by default and is **stricter** than the previous behavior, which passed `goal_metric_type: position_only` and accepted any orientation at all. A goal that planned successfully before may now fail to plan. There is deliberately no fallback — a failed plan fails loudly rather than silently accepting an orientation you didn't intend. If you relied on the old orientation-agnostic behavior, pass `extra: {"goal_metric_type": "position_only"}` on the call (see below) to restore it. + +**Requires viam-server >= 0.127.0.** Older servers accept a goal cloud but silently ignore it, which means planning falls back to strict six-degree-of-freedom scoring against the exact goal pose and fails. + +`MoveToPosition` accepts two mutually exclusive `extra` keys to bypass the default cone: + +- `extra: {"goal_metric_type": "position_only"}` — restores the previous orientation-agnostic behavior for that call; no cloud is sent. +- `extra: {"pose_cloud": {...}}` — supplies a raw `referenceframe.PoseCloud`, replacing the cone entirely (expert mode). Keys are **lowercase**: `x`, `y`, `z`, `ox`, `oy`, `oz`, `theta`. Note the Viam docs render these fields as `OX`/`OY` and the protobuf wire format spells them `o_x` — **neither of those is accepted here**; an unknown key is rejected with an error rather than silently ignored, so a typo fails loudly instead of quietly producing a near-zero-leeway cloud that fails every move. Any field you omit means ~zero leeway for that field. + +Passing both `goal_metric_type` and `pose_cloud` in the same `extra` map is an error. + ### Manual Mode Manual mode is a live jog-by-hand mode using position-deviation admittance. While active, a background control loop keeps each arm servo in position mode holding a goal — so the arm holds against gravity and never goes limp. When you backdrive a joint past a small angular deadband, the goal follows your hand, trailing by the deadband (which leaves a small restoring force so the joint still holds against gravity); when released, the joint holds its new pose. Torque is never fully disabled. The servo's `present_load` register is not used for control — it saturates on the gentlest push and doesn't encode push direction — and is reported only as telemetry. @@ -391,6 +408,8 @@ The following attributes are available for the simulated arm component: | `visualize_ee_frame` | bool | Optional | When `true`, serves a colored XYZ coordinate-frame marker at the end-effector for the 3D viewer. Default is `false`. | | `use_urdf` | bool | Optional | When `true`, sources kinematics and collision geometry from the bundled `arm/so101.urdf` instead of the embedded `so101.json`. Requires the `VIAM_MODULE_ROOT` environment variable to be set (viam-server sets this automatically). This is a drop-in swap: the bundled URDF uses `so101.json`'s frame names and TCP exactly, so the frame names, TCP pose, and kinematics are identical — only the collision geometry upgrades from primitive shapes to per-link meshes. Default `false`. | | `mesh_decimation_ratios` | []float | Optional | Per-collision-mesh simplification ratios, one per arm link in document order (base, shoulder, upper_arm, lower_arm, wrist), each in `[0, 1]`. Only used when `use_urdf` is `true`. Only values strictly in `(0, 1)` decimate (lower = more aggressive). Defaults to `0.9` for each of the 5 meshes when empty. | +| `orientation_tolerance_deg` | float | Optional | Half-angle, in degrees, of the cone of acceptable end-effector approach directions around the goal orientation used by `MoveToPosition` (see [Approach-axis orientation planning](#approach-axis-orientation-planning) below). **Roll about that axis is NOT constrained.** Valid range `[0, 180]`. `0`/unset means the default (`30`), **not** an exact match. The planner adds a small epsilon to the cone, so the angle actually enforced is `acos(cos(tol) - 0.001)` — always slightly wider than requested (`30` gives ~`30.11`) and never narrower than ~`2.56`, regardless of how small a value you configure. At or above `177.44`, every orientation is accepted. | +| `position_tolerance_mm` | float | Optional | Per-axis positional leeway, in millimeters, of the `MoveToPosition` goal cloud, applied as a box along the goal frame's axes (not a radius) — default `1.0`. Worst-case corner deviation is `sqrt(3)` times this value (~`1.73mm` at the default). Too small a value and the IK solver cannot realistically land inside the cloud, so planning reverts to strict six-DOF scoring and moves fail. | Example configuration with attributes: @@ -403,7 +422,7 @@ Example configuration with attributes: ### Behavior - `MoveToJointPositions`, `MoveThroughJointPositions`, and `GoToInputs` interpolate the joints toward each target over time; the calls block until the arm arrives. -- `MoveToPosition` plans through the motion service. Because the SO-101 is a 5-DOF arm, it defaults the planner goal metric to `position_only`; pass your own `goal_metric_type` via `extra` to override. +- `MoveToPosition` plans through the motion service. As with the `devrel:so101:arm` model, it attaches an approach-axis cone (`orientation_tolerance_deg`/`position_tolerance_mm`) to the goal by default instead of ignoring orientation — see [Approach-axis orientation planning](#approach-axis-orientation-planning) above for the config attributes, the `extra` escape hatches, and the viam-server version requirement. - `JointPositions`, `EndPosition`, `Geometries`, and `IsMoving` report the simulated state. ### DoCommand From 50fb26502147847b4f9cf6d06f0ad4e9e245fa8f Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 16:13:20 -0400 Subject: [PATCH 22/23] docs: record PoseCloud semantics and the viam-server floor in CLAUDE.md --- CLAUDE.md | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/CLAUDE.md b/CLAUDE.md index 4c1f9d7..54a42fb 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -147,3 +147,44 @@ calibration wizard. It is bundled into `module.tar.gz` and needs **Node ≥ 20** the RDK motion planner feeds dense waypoints that keep per-waypoint displacements small, so independent-joint motion stays smooth on planned paths. Also note: `acceleration_degs_per_sec_per_sec` is validated and stored on the hardware arm but not yet written to servo hardware (planned follow-up). +- **`referenceframe.PoseCloud` semantics are counterintuitive and were established + empirically** (see `docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md`; + `goal_cloud.go` depends on all of it): + - `PoseInCloud` compares the *relative* pose `PoseBetween(goal, candidate)`, where zero + deviation is the orientation vector `(0,0,1)`. + - **`Theta` encodes the AZIMUTH of a tilt, not roll.** Measured exactly: + `Theta = 90 - azimuth`, independent of tilt magnitude (about X → 90, about Y → 0, + azimuth 270 → -180). Since reported Theta sweeps the full `[-180, 180]`, **only a + leeway of 180 admits every azimuth** — only 180 gives an isotropic cone. Beware the + tempting shorthand "a smaller Theta rejects tilts": it is **false** (`Theta: 0` still + accepts a 5° tilt about Y). Smaller values silently carve out a wedge of tilt + *directions*. `TestConeBoundsTiltIsotropically`'s azimuth-270 case is what pins this. + The cost: roll cannot be constrained alongside tilt, which is why this is + approach-axis planning with **free roll**. + - **`OZ` alone defines the cone**; `OX`/`OY` are set to `1` (unconstrained). Valid across + `[0, 180]`, saturating at `acos(-0.999) = 177.4374412669`. Check saturation by + comparing cosines (`cos(tol) <= -0.999`), never a rounded degree literal — every + rounding is wrong in one direction or the other. + - **`defaultEpsilon = 0.001` is ADDED to every leeway**, not just zeroed ones. So zero + `X`/`Y`/`Z` demands a match within 1 micron (which no IK solution realistically + achieves), and the effective cone is `acos(cos(tol) - 0.001)` — always slightly wider + than requested, floored at ~2.56°. + - The cloud only *zeroes the score* when a candidate is inside it. A cloud the solver + cannot realistically land inside is **worse than no cloud**: planning reverts to strict + six-DOF scoring and fails. `position_only` sets `orientScale=0`, making any cloud + meaningless — the two are mutually exclusive. + - **There is no `ReferenceFrame` field.** v0.123.0 had one; v1.0.0 removed it while + leaving the struct's reference-frame doc comment in place as free-floating prose, so it + reads as though the field still exists. The complete field set is + `X, Y, Z, OX, OY, OZ, Theta`. +- **Goal clouds require viam-server >= 0.127.0.** RDK v0.125.0–v0.126.1 read `GoalCloud` in + `armplanning` but never decode it in `ProtobufToPoseInFrame`, so the cloud is silently + inert there; v0.127.0 is the first release with both halves. The module only *sends* the + cloud — its own RDK version has no bearing on whether the server honors it. The minimum is + declared Registry-side; `meta.json` has no field for it. +- **`NewSO101`'s `goalCloud` wiring is untested** — it needs serial hardware, so no test + constructs the hardware arm. Deleting `goalCloud: resolveGoalCloudConfig(...)` from + `NewSO101` leaves the whole suite green, and the result is a zero-valued cloud that fails + every move silently. `TestSimulatedConstructorWiresGoalCloud` covers the equivalent line + in `newSimulatedSO101` and the shared `resolveGoalCloudConfig` contract, but the hardware + constructor's line is guarded only by review. From a54c13fd917363c025f2800df30d6a0912fcd12e Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 15 Jul 2026 16:29:27 -0400 Subject: [PATCH 23/23] test(arm): pin wrapMoveErr's messages, the no-fallback design's whole UX 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. --- CLAUDE.md | 6 ++- arm_move_to_position_test.go | 80 ++++++++++++++++++++++++++++++++++++ goal_cloud.go | 7 ++-- goal_cloud_test.go | 2 +- 4 files changed, 88 insertions(+), 7 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 54a42fb..2835ff5 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -171,8 +171,10 @@ calibration wizard. It is bundled into `module.tar.gz` and needs **Node ≥ 20** than requested, floored at ~2.56°. - The cloud only *zeroes the score* when a candidate is inside it. A cloud the solver cannot realistically land inside is **worse than no cloud**: planning reverts to strict - six-DOF scoring and fails. `position_only` sets `orientScale=0`, making any cloud - meaningless — the two are mutually exclusive. + six-DOF scoring and fails. `position_only` sets `orientScale=0`, so a cloud's + orientation leeways stop mattering (its positional leeways still zero the positional + residual, since `GetGoalMetric` consults `PoseInCloud` regardless of metric type). + Passing both is rejected as incoherent rather than silently honoring one. - **There is no `ReferenceFrame` field.** v0.123.0 had one; v1.0.0 removed it while leaving the struct's reference-frame doc comment in place as free-floating prose, so it reads as though the field still exists. The complete field set is diff --git a/arm_move_to_position_test.go b/arm_move_to_position_test.go index 8a4d33d..53220ee 100644 --- a/arm_move_to_position_test.go +++ b/arm_move_to_position_test.go @@ -2,6 +2,8 @@ package so_arm import ( "context" + "errors" + "fmt" "testing" "github.com/stretchr/testify/assert" @@ -23,6 +25,17 @@ func captureMotion(got *motion.MoveReq) *injectmotion.MotionService { return ms } +// failingMotion returns an injected motion service whose Move always fails, so tests can +// exercise wrapMoveErr -- the error message is this design's entire UX, since a failed +// plan deliberately has no fallback. +func failingMotion(err error) *injectmotion.MotionService { + ms := injectmotion.NewMotionService("builtin") + ms.MoveFunc = func(ctx context.Context, req motion.MoveReq) (bool, error) { + return false, err + } + return ms +} + func TestHardwareArmMoveToPositionSendsGoalCloud(t *testing.T) { var got motion.MoveReq a := &so101{ @@ -57,6 +70,73 @@ func TestHardwareArmMoveToPositionHonorsMetricTypeOverride(t *testing.T) { assert.Equal(t, "position_only", got.Extra["goal_metric_type"]) } +// TestHardwareArmMoveToPositionWrapsConeFailure drives a REAL failing Move down the cone +// path (no extra) and pins wrapMoveErr's cone-branch message. This is the design's whole +// UX for a failed plan -- there is deliberately no fallback -- so the message must name +// BOTH tolerances (either can cause the failure), the position_only remedy, and the +// silent-ignore version hint. +func TestHardwareArmMoveToPositionWrapsConeFailure(t *testing.T) { + sentinel := errors.New("boom") + cfg := resolveGoalCloudConfig(0, 0, nil) + a := &so101{ + name: arm.Named("myarm"), + logger: logging.NewTestLogger(t), + motion: failingMotion(sentinel), + goalCloud: cfg, + } + + err := a.MoveToPosition(context.Background(), testGoal(), nil) + require.Error(t, err) + assert.ErrorIs(t, err, sentinel, "the underlying error must still be reachable via errors.Is") + assert.ErrorContains(t, err, fmt.Sprintf("orientation_tolerance_deg=%g", cfg.OrientationToleranceDeg)) + assert.ErrorContains(t, err, fmt.Sprintf("position_tolerance_mm=%g", cfg.PositionToleranceMM), + "both tolerances must be named -- either can cause the failure") + assert.ErrorContains(t, err, "position_only", "the remedy must be named") + assert.ErrorContains(t, err, "0.127.0", "the silent-ignore version hint must be named") +} + +// TestHardwareArmMoveToPositionWrapsRawCloudFailure drives a REAL failing Move down the +// raw-cloud path (caller-supplied pose_cloud) and pins wrapMoveErr's raw-cloud branch. The +// important assertion is the NEGATIVE one: this path must NOT name +// orientation_tolerance_deg, since the config tolerance had no bearing on a caller-supplied +// cloud's failure and naming it would misdirect. This is also what catches a `case` +// swapped to emit the cone wording on the wrong path. +func TestHardwareArmMoveToPositionWrapsRawCloudFailure(t *testing.T) { + sentinel := errors.New("boom") + a := &so101{ + name: arm.Named("myarm"), + logger: logging.NewTestLogger(t), + motion: failingMotion(sentinel), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + + err := a.MoveToPosition(context.Background(), testGoal(), + map[string]interface{}{"pose_cloud": map[string]interface{}{"oz": 0.5}}) + require.Error(t, err) + assert.ErrorIs(t, err, sentinel) + assert.ErrorContains(t, err, "pose_cloud") + assert.NotContains(t, err.Error(), "orientation_tolerance_deg", + "the config tolerance had no bearing on a caller-supplied cloud's failure") +} + +// TestHardwareArmMoveToPositionMetricTypeFailureUnwrapped drives a REAL failing Move down +// the metric-type path (caller-supplied goal_metric_type; no cloud sent) and pins that +// wrapMoveErr passes the error through EXACTLY unwrapped: the caller chose the metric, so +// cone wording would just tell them to do what they already did. +func TestHardwareArmMoveToPositionMetricTypeFailureUnwrapped(t *testing.T) { + sentinel := errors.New("boom") + a := &so101{ + name: arm.Named("myarm"), + logger: logging.NewTestLogger(t), + motion: failingMotion(sentinel), + goalCloud: resolveGoalCloudConfig(0, 0, nil), + } + + err := a.MoveToPosition(context.Background(), testGoal(), + map[string]interface{}{"goal_metric_type": "position_only"}) + assert.Equal(t, sentinel, err, "the metric-type path must return the error exactly unwrapped") +} + func TestSimulatedArmMoveToPositionSendsGoalCloud(t *testing.T) { var got motion.MoveReq s := &simulatedSO101{ diff --git a/goal_cloud.go b/goal_cloud.go index a85e9c2..9b0df28 100644 --- a/goal_cloud.go +++ b/goal_cloud.go @@ -215,9 +215,8 @@ func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalClo if hasCloud && hasMetric { return nil, nil, pathInvalid, fmt.Errorf( - "extra cannot contain both %q and %q (got %s=%v): a goal cloud only matters when the "+ - "planner scores orientation, and %s=\"position_only\" sets orientScale=0, which "+ - "would make the cloud meaningless. Pass one or the other", + "extra cannot contain both %q and %q (got %s=%v): %s=\"position_only\" sets "+ + "orientScale=0, so the cone's orientation leeways stop mattering. Pass one or the other", extraKeyPoseCloud, extraKeyGoalMetricType, extraKeyGoalMetricType, extra[extraKeyGoalMetricType], extraKeyGoalMetricType) } @@ -233,7 +232,7 @@ func buildMoveDestination(originFrame string, pose spatialmath.Pose, cfg goalClo switch { case hasMetric: // The caller's metric wins and no cloud is sent: position_only sets orientScale=0, - // which would make any cloud meaningless. + // so a cloud's orientation leeways would stop mattering. return referenceframe.NewPoseInFrame(originFrame, pose), planExtra, pathMetricType, nil case hasCloud: pc, err := parsePoseCloud(rawCloud) diff --git a/goal_cloud_test.go b/goal_cloud_test.go index 5aa60f9..86aea1d 100644 --- a/goal_cloud_test.go +++ b/goal_cloud_test.go @@ -350,7 +350,7 @@ func TestBuildMoveDestinationMetricTypePath(t *testing.T) { map[string]interface{}{"goal_metric_type": "position_only"}) require.NoError(t, err) assert.Equal(t, pathMetricType, path) - assert.Nil(t, dest.GoalCloud, "no cloud: position_only sets orientScale=0, making a cloud meaningless") + assert.Nil(t, dest.GoalCloud, "no cloud: position_only sets orientScale=0, so a cloud's orientation leeways would stop mattering") assert.Equal(t, "position_only", planExtra["goal_metric_type"], "the caller's metric is forwarded") }