diff --git a/CLAUDE.md b/CLAUDE.md index 163a343..2835ff5 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 @@ -147,3 +147,46 @@ 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`, 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 + `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. 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 diff --git a/arm.go b/arm.go index 72564db..5a54e8b 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 != "" { @@ -184,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 @@ -476,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 @@ -521,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..53220ee --- /dev/null +++ b/arm_move_to_position_test.go @@ -0,0 +1,232 @@ +package so_arm + +import ( + "context" + "errors" + "fmt" + "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/resource" + "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 +} + +// 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{ + 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"]) +} + +// 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{ + 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/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..f7751bf --- /dev/null +++ b/docs/superpowers/plans/2026-07-15-pose-cloud-orientation.md @@ -0,0 +1,1442 @@ +# 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.** 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°`. + +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. + +--- + +## 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) +} + +// 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` +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 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 +) + +// 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 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 (acos(-0.999) ~= 177.4375); "+ + "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: + +```go +import ( + "math" + + "go.viam.com/rdk/logging" + "go.viam.com/rdk/utils" +) +``` + +- [ ] **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/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 { + 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: utils.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(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 + // 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(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))) +} + +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) + for _, roll := range []float64{0, 45, 90, 179} { + 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) + } + } +} + +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 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 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, + } +} +``` + +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 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 + 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. + // 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) + }) + } +} +``` + +- [ ] **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 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 +} +``` + +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 ( + // 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 +) + +// 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: %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, 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 + } +} +``` + +- [ ] **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` — `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** + +```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` — 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** + +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 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. +// +// 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 { + // 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) + + _, 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` — 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** + +```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 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. +// +// 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 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** + +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 new file mode 100644 index 0000000..2f1f7de --- /dev/null +++ b/docs/superpowers/specs/2026-07-15-pose-cloud-orientation-design.md @@ -0,0 +1,522 @@ +# 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 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 + +`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`. + +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 `[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 +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 + +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 does not exist post-bump + +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 + +```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 +} +``` + +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`) +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 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"` +``` + +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.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. + +`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 the solver cannot realistically land inside 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 +under the same rule — it is not a `PoseCloud` field in v1.0.0. + +## 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` 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 + +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`. 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= diff --git a/goal_cloud.go b/goal_cloud.go new file mode 100644 index 0000000..9b0df28 --- /dev/null +++ b/goal_cloud.go @@ -0,0 +1,278 @@ +package so_arm + +import ( + "fmt" + "math" + "sort" + "strings" + + "go.viam.com/rdk/logging" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" + "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 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 + // 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 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 +) + +// 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.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.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 +// 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 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. 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 { + 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, + } +} + +// 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 +} + +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): %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) + } + + 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, + // so a cloud's orientation leeways would stop mattering. + 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 new file mode 100644 index 0000000..86aea1d --- /dev/null +++ b/goal_cloud_test.go @@ -0,0 +1,464 @@ +package so_arm + +import ( + "math" + "testing" + + "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" + "go.viam.com/rdk/utils" +) + +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.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") + + 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") +} + +// 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.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)), + "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))) +} + +// 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") +} + +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)}, + "nil value": nil, + } { + t.Run(name, func(t *testing.T) { + _, err := parsePoseCloud(input) + assert.Error(t, err) + }) + } +} + +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, so a cloud's orientation leeways would stop mattering") + 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") +} + +// 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) +} + +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..a19a17e 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" @@ -129,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 @@ -182,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())), @@ -295,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