Approach-axis orientation planning via PoseCloud

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.

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

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