From 5087649ddc47a349e09db38a6f9bebf21487dd1f Mon Sep 17 00:00:00 2001 From: Devin C Date: Thu, 6 Aug 2026 14:28:55 -0400 Subject: [PATCH 1/5] add do command wiring for planning and execution --- .changeset/rotten-poems-clap.md | 5 + .../MoveFrame/__tests__/planDoCommand.spec.ts | 149 ++++++++++++++ src/lib/plugins/MoveFrame/planDoCommand.ts | 192 ++++++++++++++++++ 3 files changed, 346 insertions(+) create mode 100644 .changeset/rotten-poems-clap.md create mode 100644 src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts create mode 100644 src/lib/plugins/MoveFrame/planDoCommand.ts diff --git a/.changeset/rotten-poems-clap.md b/.changeset/rotten-poems-clap.md new file mode 100644 index 000000000..aa6484483 --- /dev/null +++ b/.changeset/rotten-poems-clap.md @@ -0,0 +1,5 @@ +--- +'@viamrobotics/motion-tools': patch +--- + +Ask RDK to check the start state before executing a previewed plan diff --git a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts new file mode 100644 index 000000000..c5bcd7288 --- /dev/null +++ b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts @@ -0,0 +1,149 @@ +import type { JsonValue } from '@bufbuild/protobuf' + +import { describe, expect, it } from 'vitest' + +import { Pose } from '$lib/math' + +import { parseMoveOptions } from '../parseMoveOptions' +import { + executeCommand, + isAlreadyAtGoal, + parsePlanResult, + planCommand, + PlanCommandError, + type TrajectoryStep, +} from '../planDoCommand' + +const goal = new Pose(100, -200, 350).merge({ oX: 0, oY: 0, oZ: 1, theta: 45 }) + +const request = (overrides: Partial[0]> = {}) => + planCommand({ + service: 'builtin', + componentName: 'left-arm', + destination: { referenceFrame: 'world', pose: goal }, + ...overrides, + }) + +/** RDK `protojson.Unmarshal`s the command's value, so the payload is a JSON *string*. */ +const moveRequestOf = (command: Record) => + JSON.parse(command.plan as string) as Record + +describe('planCommand', () => { + it('addresses the motion service and the frame being moved', () => { + const moveRequest = moveRequestOf(request()) + expect(moveRequest.name).toBe('builtin') + expect(moveRequest.componentName).toBe('left-arm') + }) + + it('serializes the destination with protojson field names and units', () => { + expect(moveRequestOf(request()).destination).toEqual({ + referenceFrame: 'world', + // Millimetres, with `theta` in degrees — what `toDestinationPose` produces. + pose: { x: 100, y: -200, z: 350, oX: 0, oY: 0, oZ: 1, theta: 45 }, + }) + }) + + it('omits world state and constraints when the panel`s fields are empty', () => { + const moveRequest = moveRequestOf(request(parseMoveOptions('', ''))) + expect(moveRequest.worldState).toBeUndefined() + expect(moveRequest.constraints).toBeUndefined() + }) + + // The preview has to plan the same problem the subsequent move would, or it previews a + // different one. + it('passes through the same world state and constraints `move` would receive', () => { + const options = parseMoveOptions( + '{"obstacles":[{"referenceFrame":"world","geometries":[{"sphere":{"radiusMm":50}}]}]}', + '{"linearConstraint":[{"lineToleranceMm":5}]}' + ) + + const moveRequest = moveRequestOf(request(options)) + expect(moveRequest.worldState).toEqual({ + obstacles: [{ referenceFrame: 'world', geometries: [{ sphere: { radiusMm: 50 } }] }], + }) + expect(moveRequest.constraints).toEqual({ linearConstraint: [{ lineToleranceMm: 5 }] }) + }) +}) + +describe('executeCommand', () => { + const trajectory: TrajectoryStep[] = [{ 'left-arm': [0, 0.5] }, { 'left-arm': [0.1, 0.4] }] + + it('sends the trajectory back verbatim', () => { + expect(executeCommand(trajectory).execute).toEqual(trajectory) + }) + + /** + * The key's presence is the switch (`builtin.go:376`). Without it epsilon is `math.MaxFloat64`, + * so RDK compares the trajectory's first step against where the components actually are and can + * never find them too far away — a plan validated from one configuration runs from any other. + */ + it('arms the start-state check RDK will not run unasked', () => { + expect(executeCommand(trajectory)).toHaveProperty('executeCheckStart') + }) + + // Anything ≤ 0 selects `defaultExecuteEpsilon`, so the tolerance stays RDK's to choose. + it('defers the tolerance to RDK rather than naming one', () => { + expect(executeCommand(trajectory).executeCheckStart).toBeLessThanOrEqual(0) + }) +}) + +describe('parsePlanResult', () => { + it('reads the trajectory RDK returns under the command`s own key', () => { + const { trajectory } = parsePlanResult({ + plan: [{ 'left-arm': [0, 0.5], 'left-gripper': [] }], + }) + + expect(trajectory).toEqual([{ 'left-arm': [0, 0.5], 'left-gripper': [] }]) + }) + + // Throwing beats an empty result, which would read as "planned fine, nothing to show". + it.each([ + ['a non-object reply', 'nope' as JsonValue], + ['a reply with no plan key', { execute: true } as JsonValue], + ['a plan that is not an array', { plan: { arm: [0] } } as JsonValue], + ['joint values that are not numbers', { plan: [{ arm: ['0'] }] } as JsonValue], + ['an empty trajectory', { plan: [] } as JsonValue], + ])('rejects %s', (_label, value) => { + expect(() => parsePlanResult(value)).toThrow(PlanCommandError) + }) + + /** + * There is no capability or version RPC to probe a machine with, so the only evidence available is + * what came back. An RDK older than ~v0.101 serialises `Input` as `{Value: number}`, which reaches + * us as a successful plan we cannot read — worth saying, rather than reporting it as no plan. + */ + it('tells an unreadable trajectory apart from an absent one', () => { + const old = { plan: [{ arm: [{ Value: 0.1 }] }] } as JsonValue + + expect(() => parsePlanResult(old)).toThrow(/older than/) + expect(() => parsePlanResult({ execute: true })).not.toThrow(/older than/) + }) +}) + +/** + * RDK seeds the trajectory with the start configuration before planning towards the goal, so a move + * that is already satisfied comes back as two identical steps. It never comes back empty, and never + * as an error — so a check keyed on emptiness never fires and the user gets a two-frame scrubber + * that appears to do nothing. + */ +describe('isAlreadyAtGoal', () => { + it('recognises the start configuration returned twice', () => { + expect(isAlreadyAtGoal([{ arm: [0, 1.5] }, { arm: [0, 1.5] }])).toBe(true) + }) + + it.each<[string, TrajectoryStep[]]>([ + ['a real move', [{ arm: [0] }, { arm: [1] }]], + ['a single step', [{ arm: [0] }]], + ['nothing at all', []], + ['different components', [{ arm: [0] }, { gantry: [0] }]], + ['a component only one step has', [{ arm: [0] }, { arm: [0], gripper: [1] }]], + ['different joint counts', [{ arm: [0] }, { arm: [0, 0] }]], + ])('is false for %s', (_label, trajectory) => { + expect(isAlreadyAtGoal(trajectory)).toBe(false) + }) + + // The arm goes somewhere and comes back. Hiding that would be worse than showing it. + it('is false for a longer plan that ends where it started', () => { + expect(isAlreadyAtGoal([{ arm: [0] }, { arm: [1] }, { arm: [0] }])).toBe(false) + }) +}) diff --git a/src/lib/plugins/MoveFrame/planDoCommand.ts b/src/lib/plugins/MoveFrame/planDoCommand.ts new file mode 100644 index 000000000..b7af57250 --- /dev/null +++ b/src/lib/plugins/MoveFrame/planDoCommand.ts @@ -0,0 +1,192 @@ +/** + * The builtin motion service's two `DoCommand` verbs, which have no place in the motion proto and + * so no generated client: + * + * - `plan` — a protojson `motion.v1.MoveRequest` **as a string**. Plans without executing and + * answers with `motionplan.Trajectory`. + * - `execute` — a `motionplan.Trajectory`. Runs it verbatim, with no replanning. + * + * Only `services/motion/builtin` implements them; any other motion service errors, which is the + * behaviour the panel surfaces. + * + * `motionplan.Trajectory` is `[]referenceframe.FrameSystemInputs`, and `FrameSystemInputs` is + * `map[string][]Input` where `Input` is a `float64` alias — so on the wire it is exactly the + * `trajectory` array a plan dump carries, and the replayer's kinematics read it unchanged. + */ + +import type { JsonValue } from '@bufbuild/protobuf' + +import { Constraints, WorldState } from '@viamrobotics/sdk' + +import type { Pose } from '$lib/math' +import type { TrajectoryStep } from '$lib/motion/jointPose' + +export interface PlanResult { + trajectory: TrajectoryStep[] +} + +export interface PlanRequest { + /** The motion service's own resource name, `MoveRequest.name`. */ + service: string + /** The frame being moved, `MoveRequest.component_name`. */ + componentName: string + /** Where it should end up, and the frame that pose is expressed in. */ + destination: { referenceFrame: string; pose: Pose } + worldState?: WorldState + constraints?: Constraints +} + +/** + * Builds the `plan` command. RDK `protojson.Unmarshal`s the string, so the payload uses protojson + * field names (`componentName`, and `oX`/`oY`/`oZ` for the pose's orientation vector) and the + * message's own units — millimetres, with `theta` in degrees, which is what `toDestinationPose` + * already produces. + * + * `worldState` and `constraints` are passed through so a preview plans against the same inputs the + * subsequent `move` would, rather than against a quietly different problem. + */ +export const planCommand = ({ + service, + componentName, + destination, + worldState, + constraints, +}: PlanRequest): Record => { + const moveRequest: Record = { + name: service, + componentName, + destination: { + referenceFrame: destination.referenceFrame, + pose: { + x: destination.pose.x, + y: destination.pose.y, + z: destination.pose.z, + oX: destination.pose.oX, + oY: destination.pose.oY, + oZ: destination.pose.oZ, + theta: destination.pose.theta, + }, + }, + } + + // The SDK exports each of these under one name as both a class and a `PlainMessage` type alias + // (`sdk/dist/types.d.ts:51`), and it is the alias that lands in a type position — so what arrives + // here is a plain object with no `toJson` on it. Rebuilding is what reaches protojson, the + // encoding RDK unmarshals the request string with. + if (worldState) moveRequest.worldState = new WorldState(worldState).toJson() + if (constraints) moveRequest.constraints = new Constraints(constraints).toJson() + + return { plan: JSON.stringify(moveRequest) } +} + +/** + * Selects RDK's own `defaultExecuteEpsilon` rather than naming a tolerance here. `builtin.go:376` + * reads any value ≤ 0 — or anything that is not a float — as "use the default", which is the right + * answer: how far an arm may have drifted before its plan is stale is a property of the arm, not + * something a viewer should be deciding for it. + */ +const RDK_DEFAULT_EPSILON = 0 + +/** + * Builds the `execute` command for a trajectory a previous `plan` produced. + * + * `executeCheckStart` is what arms RDK's own start-state guard. Its *presence* is the switch + * (`builtin.go:376`); omit it and epsilon is `math.MaxFloat64`, so the comparison at `builtin.go:621` + * can never trip and the trajectory runs from wherever the components happen to be. Since `execute` + * never replans, that is precisely the case worth refusing: the plan was validated from one starting + * configuration, and running it from a different one flies a path nothing checked. + * + * A failed check is an RPC error, not a field in the reply — `resp[executeCheckStart]` is a constant + * string echoed back whenever the key was sent, so it says only that the check was asked for. + */ +export const executeCommand = (trajectory: TrajectoryStep[]): Record => ({ + execute: trajectory, + executeCheckStart: RDK_DEFAULT_EPSILON, +}) + +const isTrajectory = (value: unknown): value is TrajectoryStep[] => + Array.isArray(value) && + value.every( + (step) => + typeof step === 'object' && + step !== null && + Object.values(step as Record).every( + (inputs) => Array.isArray(inputs) && inputs.every((input) => typeof input === 'number') + ) + ) + +export class PlanCommandError extends Error { + constructor(message: string) { + super(message) + this.name = 'PlanCommandError' + } +} + +/** + * Reads a `plan` reply. Throws rather than returning an empty trajectory: a silent empty result + * would render as "planned successfully, nothing to show". + */ +export const parsePlanResult = (value: JsonValue): PlanResult => { + if (typeof value !== 'object' || value === null || Array.isArray(value)) { + throw new PlanCommandError('Motion service returned an unexpected plan response.') + } + + const trajectory = (value as Record).plan + + if (trajectory === undefined) { + throw new PlanCommandError('Motion service returned no trajectory for this move.') + } + + // Told apart from an absent key deliberately. An RDK older than ~v0.101 answers a plan that + // *succeeded* with `[{"Value": 0.1}]`, because `Input` was a struct rather than a float alias — + // and the same versions take `component_name` as a `ResourceName`, so the request would not have + // unmarshalled either. There is no capability or version RPC to probe with, so the shape of the + // reply is the only evidence available for saying so. + if (!isTrajectory(trajectory)) { + throw new PlanCommandError( + 'Motion service returned a trajectory this client cannot read. The machine may be running a version of RDK older than v0.101.' + ) + } + + if (trajectory.length === 0) { + throw new PlanCommandError('Motion service returned an empty trajectory.') + } + + return { trajectory } +} + +const sameInputs = (a: TrajectoryStep, b: TrajectoryStep): boolean => { + const names = Object.keys(a) + if (names.length !== Object.keys(b).length) return false + + return names.every((name) => { + const left = a[name] + const right = b[name] + return ( + left !== undefined && + right !== undefined && + left.length === right.length && + left.every((value, index) => value === right[index]) + ) + }) +} + +/** + * Whether the planner answered "there is nothing to do". + * + * RDK seeds its trajectory with the start configuration before it plans towards the goal, so a move + * whose goal is already satisfied comes back as that one configuration twice — never as an empty + * plan, and never as an error. Exact comparison rather than a tolerance, because the two steps are + * the same node written out twice. + * + * Length two is load-bearing: a longer plan that happens to end where it began is a real move that + * goes somewhere and comes back, and hiding it would be worse than showing it. + */ +export const isAlreadyAtGoal = (trajectory: TrajectoryStep[]): boolean => { + if (trajectory.length !== 2) return false + + const [first, last] = trajectory + return first !== undefined && last !== undefined && sameInputs(first, last) +} + +export { type TrajectoryStep } from '$lib/motion/jointPose' From 2fa41a468e94ee4eb704204c7cc67ebc27733a5f Mon Sep 17 00:00:00 2001 From: Devin C Date: Thu, 6 Aug 2026 16:34:24 -0400 Subject: [PATCH 2/5] cleanup --- .../MoveFrame/__tests__/planDoCommand.spec.ts | 172 ++++++++++++++++-- src/lib/plugins/MoveFrame/planDoCommand.ts | 63 +++++-- 2 files changed, 208 insertions(+), 27 deletions(-) diff --git a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts index c5bcd7288..a197ae103 100644 --- a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts +++ b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts @@ -14,7 +14,15 @@ import { type TrajectoryStep, } from '../planDoCommand' -const goal = new Pose(100, -200, 350).merge({ oX: 0, oY: 0, oZ: 1, theta: 45 }) +// Distinct orientation components, so a transposed `oX`/`oY` cannot pass unnoticed. +const goal = new Pose(100, -200, 350).merge({ oX: 0.6, oY: -0.8, oZ: 1, theta: 45 }) + +/** + * The message's own field shape with no prototype, which is what the SDK's `WorldState` *type* means + * (`sdk/dist/types.d.ts:51` aliases it to `PlainMessage`). A spread keeps the fields, including the + * `{case, value}` oneofs, and drops the methods. + */ +const asPlainMessage = (message: T): T => ({ ...message }) const request = (overrides: Partial[0]> = {}) => planCommand({ @@ -39,10 +47,38 @@ describe('planCommand', () => { expect(moveRequestOf(request()).destination).toEqual({ referenceFrame: 'world', // Millimetres, with `theta` in degrees — what `toDestinationPose` produces. - pose: { x: 100, y: -200, z: 350, oX: 0, oY: 0, oZ: 1, theta: 45 }, + pose: { x: 100, y: -200, z: 350, oX: 0.6, oY: -0.8, oZ: 1, theta: 45 }, }) }) + /** + * The whole payload, not just the fields we care about. RDK unmarshals this with + * `protojson.Unmarshal` and default options, so `DiscardUnknown` is false and *any* key that is + * not a `MoveRequest` field is a hard RPC error. A stray or misspelled key would break every real + * plan call while a field-by-field spec stayed green. + */ + it('sends exactly the fields `MoveRequest` declares and no others', () => { + expect(Object.keys(moveRequestOf(request())).toSorted()).toEqual([ + 'componentName', + 'destination', + 'name', + ]) + }) + + /** + * `JSON.stringify` writes `null` for a non-finite number, and Go's protojson skips a null scalar + * rather than rejecting it — so RDK would read the field's zero, plan a valid move to a + * destination nobody asked for, and hand back a trajectory that looks fine. `client.move` cannot + * do this: a proto double carries the NaN and the planner refuses it. + */ + it.each(['x', 'theta'])('refuses a goal whose %s is not finite', (field) => { + const broken = goal.clone().merge({ [field]: Number.NaN }) + + expect(() => request({ destination: { referenceFrame: 'world', pose: broken } })).toThrow( + PlanCommandError + ) + }) + it('omits world state and constraints when the panel`s fields are empty', () => { const moveRequest = moveRequestOf(request(parseMoveOptions('', ''))) expect(moveRequest.worldState).toBeUndefined() @@ -63,13 +99,45 @@ describe('planCommand', () => { }) expect(moveRequest.constraints).toEqual({ linearConstraint: [{ lineToleranceMm: 5 }] }) }) + + /** + * What the *type* promises. `parseMoveOptions` happens to return real message instances, for which + * the rebuild is a no-op — so with only that input, deleting the rebuild passes every test and + * then dies on `worldState.toJson is not a function` the first time a caller honours the signature. + */ + it('accepts the plain-message shape its signature actually describes', () => { + const options = parseMoveOptions( + '{"obstacles":[{"referenceFrame":"world","geometries":[{"sphere":{"radiusMm":50}}]}]}', + '{"linearConstraint":[{"lineToleranceMm":5}]}' + ) + + const moveRequest = moveRequestOf( + request({ + worldState: options.worldState && asPlainMessage(options.worldState), + constraints: options.constraints && asPlainMessage(options.constraints), + }) + ) + + // The sphere in particular: a geometry's shape is a oneof, which is the part a careless rebuild + // drops, leaving an obstacle with no volume and a preview that plans straight through it. + expect(moveRequest.worldState).toEqual({ + obstacles: [{ referenceFrame: 'world', geometries: [{ sphere: { radiusMm: 50 } }] }], + }) + expect(moveRequest.constraints).toEqual({ linearConstraint: [{ lineToleranceMm: 5 }] }) + }) }) describe('executeCommand', () => { const trajectory: TrajectoryStep[] = [{ 'left-arm': [0, 0.5] }, { 'left-arm': [0.1, 0.4] }] + // Against a literal, not against `trajectory` itself: the command holds the same array reference, + // so comparing it to its own source is a tautology that any in-place reordering or rounding of + // the steps would survive — and the arm would run whatever the mutation left behind. it('sends the trajectory back verbatim', () => { - expect(executeCommand(trajectory).execute).toEqual(trajectory) + expect(executeCommand(trajectory).execute).toEqual([ + { 'left-arm': [0, 0.5] }, + { 'left-arm': [0.1, 0.4] }, + ]) }) /** @@ -96,15 +164,52 @@ describe('parsePlanResult', () => { expect(trajectory).toEqual([{ 'left-arm': [0, 0.5], 'left-gripper': [] }]) }) - // Throwing beats an empty result, which would read as "planned fine, nothing to show". + /** + * Throwing beats an empty result, which would read as "planned fine, nothing to show". + * + * Each row asserts its own message, not just that something threw. The four paths say four + * different things to a user — one of them tells them to go and upgrade RDK — and a bare + * `toThrow(PlanCommandError)` cannot tell them apart, so any of them could drift into the + * version-blaming text unnoticed. + */ it.each([ - ['a non-object reply', 'nope' as JsonValue], - ['a reply with no plan key', { execute: true } as JsonValue], - ['a plan that is not an array', { plan: { arm: [0] } } as JsonValue], - ['joint values that are not numbers', { plan: [{ arm: ['0'] }] } as JsonValue], - ['an empty trajectory', { plan: [] } as JsonValue], - ])('rejects %s', (_label, value) => { + ['a non-object reply', 'nope' as JsonValue, /unexpected plan response/], + ['a reply with no plan key', { execute: true } as JsonValue, /no trajectory/], + ['a null plan, which is how a Go nil marshals', { plan: null } as JsonValue, /no trajectory/], + ['a plan that is not an array', { plan: { arm: [0] } } as JsonValue, /cannot read/], + ['joint values that are not numbers', { plan: [{ arm: ['0'] }] } as JsonValue, /cannot read/], + ['a null step', { plan: [{ arm: [0] }, null] } as JsonValue, /cannot read/], + ['a null joint value', { plan: [{ arm: [0, null] }] } as JsonValue, /cannot read/], + ['an empty trajectory', { plan: [] } as JsonValue, /empty trajectory/], + ])('rejects %s', (_label, value, message) => { expect(() => parsePlanResult(value)).toThrow(PlanCommandError) + expect(() => parsePlanResult(value)).toThrow(message) + }) + + /** + * Steps that are structurally present but carry nothing readable. `every` says yes to both by + * default — an array is `typeof 'object'`, and `Object.values({})` is vacuously fine — and + * downstream nothing complains either, because `jointValueAt` resolves a missing column to `0`. + * The result was a plausible-looking arm drawn at the zero configuration rather than a reply + * reported as unreadable. `[[], []]` also satisfied `isAlreadyAtGoal`, so the panel claimed the + * machine was already there. + */ + it.each([ + ['a step with no columns', { plan: [{ arm: [0] }, {}] } as JsonValue], + [ + 'a step that is an array', + { + plan: [ + [ + [0, 1], + [2, 3], + ], + ], + } as JsonValue, + ], + ['nothing but empty steps', { plan: [[], []] } as JsonValue], + ])('rejects %s rather than reading it as the zero configuration', (_label, value) => { + expect(() => parsePlanResult(value)).toThrow(/cannot read/) }) /** @@ -131,6 +236,35 @@ describe('isAlreadyAtGoal', () => { expect(isAlreadyAtGoal([{ arm: [0, 1.5] }, { arm: [0, 1.5] }])).toBe(true) }) + /** + * A real reply keys every frame in the system, including the zero-DoF ones RDK pads with `[]` — + * the shape `plan-gantry.json` carries. With only single-component steps the per-component loop + * is never a loop, so `every` and `some` behave identically and the difference between "all + * components held still" and "any one of them did" goes untested. Under `some`, the `_origin` + * frames alone satisfy it and every real move reads as already-at-goal. + */ + it('recognises a full multi-component step, padding and all', () => { + const step: TrajectoryStep = { + 'arm-1': [0, 0, 0, 0, 0, 0], + 'arm-1_origin': [], + 'gantry-1': [50], + 'gantry-1_origin': [], + } + + expect(isAlreadyAtGoal([step, { ...step }])).toBe(true) + }) + + it('is false when one component moves and the rest are held', () => { + const held = { 'arm-1_origin': [], 'gantry-1': [50], 'gantry-1_origin': [] } + + expect( + isAlreadyAtGoal([ + { 'arm-1': [0, 0, 0], ...held }, + { 'arm-1': [0.4, 0, 0], ...held }, + ]) + ).toBe(false) + }) + it.each<[string, TrajectoryStep[]]>([ ['a real move', [{ arm: [0] }, { arm: [1] }]], ['a single step', [{ arm: [0] }]], @@ -138,12 +272,24 @@ describe('isAlreadyAtGoal', () => { ['different components', [{ arm: [0] }, { gantry: [0] }]], ['a component only one step has', [{ arm: [0] }, { arm: [0], gripper: [1] }]], ['different joint counts', [{ arm: [0] }, { arm: [0, 0] }]], + // A plain `b[name]` lookup reads through to `Object.prototype`, where `toString.length` is 0 + // and matches an empty column, so two steps naming different components compared equal. + ['a component sharing a name with an Object member', [{ toString: [] }, { arm: [] }]], ])('is false for %s', (_label, trajectory) => { expect(isAlreadyAtGoal(trajectory)).toBe(false) }) - // The arm goes somewhere and comes back. Hiding that would be worse than showing it. - it('is false for a longer plan that ends where it started', () => { - expect(isAlreadyAtGoal([{ arm: [0] }, { arm: [1] }, { arm: [0] }])).toBe(false) + /** + * The arm goes somewhere and comes back. Hiding that would be worse than showing it. + * + * The second case is the one that pins the length check itself: destructuring reads indices 0 and + * 1, so a plan whose *first two* steps differ stays false even if the length test is loosened. + * Here the seeded start is repeated, so only `length !== 2` keeps it false. + */ + it.each<[string, TrajectoryStep[]]>([ + ['ending where it started', [{ arm: [0] }, { arm: [1] }, { arm: [0] }]], + ['repeating its seed before moving', [{ arm: [0] }, { arm: [0] }, { arm: [1] }]], + ])('is false for a longer plan %s', (_label, trajectory) => { + expect(isAlreadyAtGoal(trajectory)).toBe(false) }) }) diff --git a/src/lib/plugins/MoveFrame/planDoCommand.ts b/src/lib/plugins/MoveFrame/planDoCommand.ts index b7af57250..9dfbb57df 100644 --- a/src/lib/plugins/MoveFrame/planDoCommand.ts +++ b/src/lib/plugins/MoveFrame/planDoCommand.ts @@ -52,6 +52,15 @@ export const planCommand = ({ worldState, constraints, }: PlanRequest): Record => { + // `JSON.stringify` writes `null` for a non-finite number, and Go's protojson *skips* a JSON null + // for a scalar field rather than rejecting it, leaving the field at its zero. A `NaN` in the goal + // would therefore reach RDK as 0 mm, plan successfully, and preview a move to somewhere the user + // never asked for. Only this path can do that: `client.move` sends a proto double, which carries + // the NaN and gets refused. + if (!destination.pose.isFinite()) { + throw new PlanCommandError('The move target is not a finite pose.') + } + const moveRequest: Record = { name: service, componentName, @@ -104,17 +113,28 @@ export const executeCommand = (trajectory: TrajectoryStep[]): Record - Array.isArray(value) && - value.every( - (step) => - typeof step === 'object' && - step !== null && - Object.values(step as Record).every( - (inputs) => Array.isArray(inputs) && inputs.every((input) => typeof input === 'number') - ) +/** + * A step has to be a non-empty object of number arrays. + * + * The two structural cases are worth naming because `every` says yes to both by default. An array + * is `typeof 'object'`, so `[[0, 1], [2, 3]]` walked as a step and passed; and `Object.values({})` + * is empty, so `{}` passed vacuously. Neither errors downstream either: `jointValueAt` resolves a + * missing column to `0`, so a step with no readable columns draws a plausible arm at the zero + * configuration instead of reporting a reply this client cannot read. `[[], []]` was worse again — + * it parsed *and* satisfied `isAlreadyAtGoal`. + */ +const isTrajectoryStep = (step: unknown): step is TrajectoryStep => + typeof step === 'object' && + step !== null && + !Array.isArray(step) && + Object.keys(step).length > 0 && + Object.values(step as Record).every( + (inputs) => Array.isArray(inputs) && inputs.every((input) => typeof input === 'number') ) +const isTrajectory = (value: unknown): value is TrajectoryStep[] => + Array.isArray(value) && value.every((step) => isTrajectoryStep(step)) + export class PlanCommandError extends Error { constructor(message: string) { super(message) @@ -133,7 +153,10 @@ export const parsePlanResult = (value: JsonValue): PlanResult => { const trajectory = (value as Record).plan - if (trajectory === undefined) { + // `== null` rather than `=== undefined`: a Go nil trajectory marshals to JSON `null`, which is + // nothing to draw for the same reason an absent key is. Distinguishing them only sent a user on + // current RDK to go and upgrade it. + if (trajectory == null) { throw new PlanCommandError('Motion service returned no trajectory for this move.') } @@ -160,6 +183,11 @@ const sameInputs = (a: TrajectoryStep, b: TrajectoryStep): boolean => { if (names.length !== Object.keys(b).length) return false return names.every((name) => { + // `hasOwn` rather than testing `b[name]` for undefined: a plain index reads straight through to + // `Object.prototype`, so a component named `toString` matched a member function whose `length` + // happens to be 0, and two steps naming different components compared equal. + if (!Object.hasOwn(b, name)) return false + const left = a[name] const right = b[name] return ( @@ -174,10 +202,17 @@ const sameInputs = (a: TrajectoryStep, b: TrajectoryStep): boolean => { /** * Whether the planner answered "there is nothing to do". * - * RDK seeds its trajectory with the start configuration before it plans towards the goal, so a move - * whose goal is already satisfied comes back as that one configuration twice — never as an empty - * plan, and never as an error. Exact comparison rather than a tolerance, because the two steps are - * the same node written out twice. + * RDK seeds its trajectory with the start configuration (`plan_manager.go:51`) and then appends the + * IK solution, so a move whose goal is already satisfied comes back as two steps — never as an empty + * plan, and never as an error. + * + * Exact comparison rather than a tolerance, but not because the second step is a copy of the first: + * it is an nlopt output. It is bit-identical because nlopt runs with `SetStopVal(defaultGoalThreshold)` + * from exactly the start configuration, so when the goal is already met it short-circuits at x0 and + * hands the seed vector back unchanged. That makes the guard one that under-fires rather than one + * that mis-fires: a goal near enough to look identical on screen but far enough to clear the + * threshold gets a real two-step plan, and the user sees a preview that does not visibly move. A + * tolerance here would trade that for hiding real short moves, which is the worse mistake. * * Length two is load-bearing: a longer plan that happens to end where it began is a real move that * goes somewhere and comes back, and hiding it would be worse than showing it. From c0f802f678ead10724049e52392eaaca4d33758d Mon Sep 17 00:00:00 2001 From: Devin C Date: Thu, 6 Aug 2026 18:16:58 -0400 Subject: [PATCH 3/5] reject a non-finite joint value in a plan reply typeof NaN is 'number', so the trajectory guard let one through, and a single NaN is not a local defect: interpolateTrajectory sums every segment's frame cost, so the total goes NaN and every segment of the plan collapses to one frame with no error raised. planCommand already refuses a non-finite goal on the same reasoning. --- .../MoveFrame/__tests__/planDoCommand.spec.ts | 22 +++++++++++++++++++ src/lib/plugins/MoveFrame/planDoCommand.ts | 16 ++++++++++++-- 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts index a197ae103..7cbb6d8ab 100644 --- a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts +++ b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts @@ -212,6 +212,28 @@ describe('parsePlanResult', () => { expect(() => parsePlanResult(value)).toThrow(/cannot read/) }) + /** + * `typeof NaN === 'number'`, so a numeric-only check lets these through, and neither is a local + * defect once it is in. `interpolateTrajectory` sums every segment's frame cost to decide how + * finely to sample, so one non-finite value makes the total non-finite, which survives + * `Math.ceil` until the interior loop stops running: *every* segment of the plan collapses to + * one frame. That is the raw waypoint teleport interpolation exists to prevent, reached with no + * error raised anywhere and a `NaN` on the scrubber's coarsening readout. + * + * `planCommand` already refuses a non-finite goal on exactly this reasoning; this is the same + * rule applied to the reply. + */ + it.each([ + ['NaN', Number.NaN], + ['Infinity', Number.POSITIVE_INFINITY], + ['-Infinity', Number.NEGATIVE_INFINITY], + ])('rejects a trajectory carrying %s rather than sampling the whole plan away', (_label, bad) => { + const value = { plan: [{ arm: [0, 0] }, { arm: [0.5, bad] }] } as unknown as JsonValue + + expect(() => parsePlanResult(value)).toThrow(PlanCommandError) + expect(() => parsePlanResult(value)).toThrow(/cannot read/) + }) + /** * There is no capability or version RPC to probe a machine with, so the only evidence available is * what came back. An RDK older than ~v0.101 serialises `Input` as `{Value: number}`, which reaches diff --git a/src/lib/plugins/MoveFrame/planDoCommand.ts b/src/lib/plugins/MoveFrame/planDoCommand.ts index 9dfbb57df..b26019704 100644 --- a/src/lib/plugins/MoveFrame/planDoCommand.ts +++ b/src/lib/plugins/MoveFrame/planDoCommand.ts @@ -114,7 +114,7 @@ export const executeCommand = (trajectory: TrajectoryStep[]): Record typeof step === 'object' && @@ -129,7 +141,7 @@ const isTrajectoryStep = (step: unknown): step is TrajectoryStep => !Array.isArray(step) && Object.keys(step).length > 0 && Object.values(step as Record).every( - (inputs) => Array.isArray(inputs) && inputs.every((input) => typeof input === 'number') + (inputs) => Array.isArray(inputs) && inputs.every((input) => Number.isFinite(input)) ) const isTrajectory = (value: unknown): value is TrajectoryStep[] => From 54c84576f67e3b1b0ac5b9bfa2ef2df44367ebe6 Mon Sep 17 00:00:00 2001 From: Devin C Date: Mon, 10 Aug 2026 20:09:56 -0400 Subject: [PATCH 4/5] apply review findings for #927 --- .changeset/rotten-poems-clap.md | 2 +- .../MoveFrame/__tests__/planDoCommand.spec.ts | 43 ++++---- src/lib/plugins/MoveFrame/planDoCommand.ts | 98 ++++++++++++++++--- 3 files changed, 114 insertions(+), 29 deletions(-) diff --git a/.changeset/rotten-poems-clap.md b/.changeset/rotten-poems-clap.md index aa6484483..e04e181ae 100644 --- a/.changeset/rotten-poems-clap.md +++ b/.changeset/rotten-poems-clap.md @@ -2,4 +2,4 @@ '@viamrobotics/motion-tools': patch --- -Ask RDK to check the start state before executing a previewed plan +Add a client for the motion service's `plan` and `execute` do-commands diff --git a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts index 7cbb6d8ab..98fef085b 100644 --- a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts +++ b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts @@ -141,7 +141,7 @@ describe('executeCommand', () => { }) /** - * The key's presence is the switch (`builtin.go:376`). Without it epsilon is `math.MaxFloat64`, + * The key's presence is the switch (`builtin.go`). Without it epsilon is `math.MaxFloat64`, * so RDK compares the trajectory's first step against where the components actually are and can * never find them too far away — a plan validated from one configuration runs from any other. */ @@ -167,19 +167,23 @@ describe('parsePlanResult', () => { /** * Throwing beats an empty result, which would read as "planned fine, nothing to show". * - * Each row asserts its own message, not just that something threw. The four paths say four - * different things to a user — one of them tells them to go and upgrade RDK — and a bare - * `toThrow(PlanCommandError)` cannot tell them apart, so any of them could drift into the - * version-blaming text unnoticed. + * Each row asserts its own message, not just that something threw. A bare `toThrow(PlanCommandError)` + * cannot tell a non-array plan apart from a null step or a non-numeric joint value, so any of them + * could silently drift into sharing one message that only actually describes one cause — the defect + * this file used to have, where every one of these asserted the same RDK-version-blaming text. */ it.each([ ['a non-object reply', 'nope' as JsonValue, /unexpected plan response/], ['a reply with no plan key', { execute: true } as JsonValue, /no trajectory/], ['a null plan, which is how a Go nil marshals', { plan: null } as JsonValue, /no trajectory/], - ['a plan that is not an array', { plan: { arm: [0] } } as JsonValue, /cannot read/], - ['joint values that are not numbers', { plan: [{ arm: ['0'] }] } as JsonValue, /cannot read/], - ['a null step', { plan: [{ arm: [0] }, null] } as JsonValue, /cannot read/], - ['a null joint value', { plan: [{ arm: [0, null] }] } as JsonValue, /cannot read/], + ['a plan that is not an array', { plan: { arm: [0] } } as JsonValue, /not a list of steps/], + [ + 'joint values that are not numbers', + { plan: [{ arm: ['0'] }] } as JsonValue, + /non-numeric joint value/, + ], + ['a null step', { plan: [{ arm: [0] }, null] } as JsonValue, /null trajectory step/], + ['a null joint value', { plan: [{ arm: [0, null] }] } as JsonValue, /null joint value/], ['an empty trajectory', { plan: [] } as JsonValue, /empty trajectory/], ])('rejects %s', (_label, value, message) => { expect(() => parsePlanResult(value)).toThrow(PlanCommandError) @@ -193,9 +197,13 @@ describe('parsePlanResult', () => { * The result was a plausible-looking arm drawn at the zero configuration rather than a reply * reported as unreadable. `[[], []]` also satisfied `isAlreadyAtGoal`, so the panel claimed the * machine was already there. + * + * `{}` and `[]` share a message: both name zero components, and that is the whole of what is + * wrong with either. A non-empty array step is a different defect — a list where a map of + * component names was expected — so it gets its own. */ it.each([ - ['a step with no columns', { plan: [{ arm: [0] }, {}] } as JsonValue], + ['a step with no columns', { plan: [{ arm: [0] }, {}] } as JsonValue, /naming no components/], [ 'a step that is an array', { @@ -206,10 +214,11 @@ describe('parsePlanResult', () => { ], ], } as JsonValue, + /unnamed joint values/, ], - ['nothing but empty steps', { plan: [[], []] } as JsonValue], - ])('rejects %s rather than reading it as the zero configuration', (_label, value) => { - expect(() => parsePlanResult(value)).toThrow(/cannot read/) + ['nothing but empty steps', { plan: [[], []] } as JsonValue, /naming no components/], + ])('rejects %s rather than reading it as the zero configuration', (_label, value, message) => { + expect(() => parsePlanResult(value)).toThrow(message) }) /** @@ -218,7 +227,7 @@ describe('parsePlanResult', () => { * finely to sample, so one non-finite value makes the total non-finite, which survives * `Math.ceil` until the interior loop stops running: *every* segment of the plan collapses to * one frame. That is the raw waypoint teleport interpolation exists to prevent, reached with no - * error raised anywhere and a `NaN` on the scrubber's coarsening readout. + * error raised anywhere. * * `planCommand` already refuses a non-finite goal on exactly this reasoning; this is the same * rule applied to the reply. @@ -231,7 +240,7 @@ describe('parsePlanResult', () => { const value = { plan: [{ arm: [0, 0] }, { arm: [0.5, bad] }] } as unknown as JsonValue expect(() => parsePlanResult(value)).toThrow(PlanCommandError) - expect(() => parsePlanResult(value)).toThrow(/cannot read/) + expect(() => parsePlanResult(value)).toThrow(/non-finite joint value/) }) /** @@ -242,8 +251,8 @@ describe('parsePlanResult', () => { it('tells an unreadable trajectory apart from an absent one', () => { const old = { plan: [{ arm: [{ Value: 0.1 }] }] } as JsonValue - expect(() => parsePlanResult(old)).toThrow(/older than/) - expect(() => parsePlanResult({ execute: true })).not.toThrow(/older than/) + expect(() => parsePlanResult(old)).toThrow(/older joint-value format/) + expect(() => parsePlanResult({ execute: true })).not.toThrow(/older joint-value format/) }) }) diff --git a/src/lib/plugins/MoveFrame/planDoCommand.ts b/src/lib/plugins/MoveFrame/planDoCommand.ts index b26019704..05d9e6ed7 100644 --- a/src/lib/plugins/MoveFrame/planDoCommand.ts +++ b/src/lib/plugins/MoveFrame/planDoCommand.ts @@ -89,7 +89,7 @@ export const planCommand = ({ } /** - * Selects RDK's own `defaultExecuteEpsilon` rather than naming a tolerance here. `builtin.go:376` + * Selects RDK's own `defaultExecuteEpsilon` rather than naming a tolerance here. `builtin.go` * reads any value ≤ 0 — or anything that is not a float — as "use the default", which is the right * answer: how far an arm may have drifted before its plan is stale is a property of the arm, not * something a viewer should be deciding for it. @@ -100,7 +100,7 @@ const RDK_DEFAULT_EPSILON = 0 * Builds the `execute` command for a trajectory a previous `plan` produced. * * `executeCheckStart` is what arms RDK's own start-state guard. Its *presence* is the switch - * (`builtin.go:376`); omit it and epsilon is `math.MaxFloat64`, so the comparison at `builtin.go:621` + * (`builtin.go`); omit it and epsilon is `math.MaxFloat64`, so the comparison in `builtin.go` * can never trip and the trajectory runs from wherever the components happen to be. Since `execute` * never replans, that is precisely the case worth refusing: the plan was validated from one starting * configuration, and running it from a different one flies a path nothing checked. @@ -128,7 +128,7 @@ export const executeCommand = (trajectory: TrajectoryStep[]): Record const isTrajectory = (value: unknown): value is TrajectoryStep[] => Array.isArray(value) && value.every((step) => isTrajectoryStep(step)) +/** + * RDK older than ~v0.101 serialised `Input` as `{Value: number}`, a struct, rather than the float + * alias it is today, so a plan that *succeeded* comes back looking like this instead of a number. + * Checked on its own so that one explainable shape gets its own message instead of folding into the + * generic malformed-reply diagnosis below, which is not evidence of any particular RDK version. + */ +const isOldInputShape = (inputs: unknown): boolean => + Array.isArray(inputs) && + inputs.some((input) => typeof input === 'object' && input !== null && 'Value' in input) + +const hasOldInputShape = (trajectory: unknown): boolean => + Array.isArray(trajectory) && + trajectory.some( + (step) => + typeof step === 'object' && + step !== null && + !Array.isArray(step) && + Object.values(step as Record).some((value) => isOldInputShape(value)) + ) + +/** + * Diagnoses a trajectory that already failed {@link isTrajectory}, so the several structurally + * different ways a reply can be malformed each say what is actually wrong instead of collapsing into + * one message that guesses at an RDK version — a guess only {@link hasOldInputShape}'s shape + * supports, and `parsePlanResult` never reaches this function for that shape. + */ +const describeMalformedTrajectory = (value: unknown): string => { + if (!Array.isArray(value)) { + return 'Motion service returned a trajectory that is not a list of steps.' + } + + for (const step of value) { + if (Array.isArray(step)) { + return step.length === 0 + ? 'Motion service returned a trajectory step naming no components.' + : 'Motion service returned a trajectory step with unnamed joint values.' + } + + if (step === null || typeof step !== 'object') { + return 'Motion service returned a null trajectory step.' + } + + const columns = Object.entries(step) + if (columns.length === 0) { + return 'Motion service returned a trajectory step naming no components.' + } + + for (const [name, inputs] of columns) { + if (!Array.isArray(inputs)) { + return `Motion service returned a non-list joint value for component "${name}".` + } + for (const input of inputs) { + if (input === null) { + return `Motion service returned a null joint value for component "${name}".` + } + if (typeof input !== 'number') { + return `Motion service returned a non-numeric joint value for component "${name}".` + } + if (!Number.isFinite(input)) { + return `Motion service returned a non-finite joint value for component "${name}".` + } + } + } + } + + // Unreachable from `parsePlanResult`: every caller already knows `isTrajectory` returned false, + // and the walk above covers every way `isTrajectoryStep` can say no to a step. + return 'Motion service returned a trajectory this client cannot read.' +} + export class PlanCommandError extends Error { constructor(message: string) { super(message) @@ -172,17 +242,23 @@ export const parsePlanResult = (value: JsonValue): PlanResult => { throw new PlanCommandError('Motion service returned no trajectory for this move.') } - // Told apart from an absent key deliberately. An RDK older than ~v0.101 answers a plan that - // *succeeded* with `[{"Value": 0.1}]`, because `Input` was a struct rather than a float alias — - // and the same versions take `component_name` as a `ResourceName`, so the request would not have - // unmarshalled either. There is no capability or version RPC to probe with, so the shape of the - // reply is the only evidence available for saying so. - if (!isTrajectory(trajectory)) { + // Told apart from every other malformed shape deliberately. An RDK older than ~v0.101 answers a + // plan that *succeeded* with `[{"Value": 0.1}]`, because `Input` was a struct rather than a float + // alias — and the same versions take `component_name` as a `ResourceName`, so the request would + // not have unmarshalled either. There is no capability or version RPC to probe with, so the shape + // of the reply is the only evidence available for saying so, and it is evidence for this one + // shape only: every other way a reply can fail `isTrajectory` is not something an RDK upgrade + // explains, so each gets its own diagnosis instead of borrowing this one. + if (hasOldInputShape(trajectory)) { throw new PlanCommandError( - 'Motion service returned a trajectory this client cannot read. The machine may be running a version of RDK older than v0.101.' + 'Motion service returned a trajectory using an older joint-value format. The machine may be running an older version of RDK.' ) } + if (!isTrajectory(trajectory)) { + throw new PlanCommandError(describeMalformedTrajectory(trajectory)) + } + if (trajectory.length === 0) { throw new PlanCommandError('Motion service returned an empty trajectory.') } @@ -214,7 +290,7 @@ const sameInputs = (a: TrajectoryStep, b: TrajectoryStep): boolean => { /** * Whether the planner answered "there is nothing to do". * - * RDK seeds its trajectory with the start configuration (`plan_manager.go:51`) and then appends the + * RDK seeds its trajectory with the start configuration (`plan_manager.go`) and then appends the * IK solution, so a move whose goal is already satisfied comes back as two steps — never as an empty * plan, and never as an error. * From f0fc129b1dee196398222ac85d412738990c99dc Mon Sep 17 00:00:00 2001 From: Devin C Date: Tue, 11 Aug 2026 13:04:00 -0400 Subject: [PATCH 5/5] apply comment, test and description review for #927 --- .../MoveFrame/__tests__/planDoCommand.spec.ts | 102 +++----------- src/lib/plugins/MoveFrame/planDoCommand.ts | 128 ++++-------------- 2 files changed, 48 insertions(+), 182 deletions(-) diff --git a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts index 98fef085b..fa194a6cb 100644 --- a/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts +++ b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts @@ -18,9 +18,8 @@ import { const goal = new Pose(100, -200, 350).merge({ oX: 0.6, oY: -0.8, oZ: 1, theta: 45 }) /** - * The message's own field shape with no prototype, which is what the SDK's `WorldState` *type* means - * (`sdk/dist/types.d.ts:51` aliases it to `PlainMessage`). A spread keeps the fields, including the - * `{case, value}` oneofs, and drops the methods. + * The message's own field shape with no prototype, which is what the SDK's `WorldState` type means: + * it aliases to `PlainMessage`. A spread keeps the fields, including the `{case, value}` oneofs. */ const asPlainMessage = (message: T): T => ({ ...message }) @@ -46,17 +45,10 @@ describe('planCommand', () => { it('serializes the destination with protojson field names and units', () => { expect(moveRequestOf(request()).destination).toEqual({ referenceFrame: 'world', - // Millimetres, with `theta` in degrees — what `toDestinationPose` produces. pose: { x: 100, y: -200, z: 350, oX: 0.6, oY: -0.8, oZ: 1, theta: 45 }, }) }) - /** - * The whole payload, not just the fields we care about. RDK unmarshals this with - * `protojson.Unmarshal` and default options, so `DiscardUnknown` is false and *any* key that is - * not a `MoveRequest` field is a hard RPC error. A stray or misspelled key would break every real - * plan call while a field-by-field spec stayed green. - */ it('sends exactly the fields `MoveRequest` declares and no others', () => { expect(Object.keys(moveRequestOf(request())).toSorted()).toEqual([ 'componentName', @@ -65,12 +57,6 @@ describe('planCommand', () => { ]) }) - /** - * `JSON.stringify` writes `null` for a non-finite number, and Go's protojson skips a null scalar - * rather than rejecting it — so RDK would read the field's zero, plan a valid move to a - * destination nobody asked for, and hand back a trajectory that looks fine. `client.move` cannot - * do this: a proto double carries the NaN and the planner refuses it. - */ it.each(['x', 'theta'])('refuses a goal whose %s is not finite', (field) => { const broken = goal.clone().merge({ [field]: Number.NaN }) @@ -79,14 +65,12 @@ describe('planCommand', () => { ) }) - it('omits world state and constraints when the panel`s fields are empty', () => { + it("omits world state and constraints when the panel's fields are empty", () => { const moveRequest = moveRequestOf(request(parseMoveOptions('', ''))) expect(moveRequest.worldState).toBeUndefined() expect(moveRequest.constraints).toBeUndefined() }) - // The preview has to plan the same problem the subsequent move would, or it previews a - // different one. it('passes through the same world state and constraints `move` would receive', () => { const options = parseMoveOptions( '{"obstacles":[{"referenceFrame":"world","geometries":[{"sphere":{"radiusMm":50}}]}]}', @@ -101,9 +85,8 @@ describe('planCommand', () => { }) /** - * What the *type* promises. `parseMoveOptions` happens to return real message instances, for which - * the rebuild is a no-op — so with only that input, deleting the rebuild passes every test and - * then dies on `worldState.toJson is not a function` the first time a caller honours the signature. + * `parseMoveOptions` returns real message instances, for which the rebuild is a no-op, so with + * only that input deleting the rebuild passes every other test here. */ it('accepts the plain-message shape its signature actually describes', () => { const options = parseMoveOptions( @@ -118,8 +101,8 @@ describe('planCommand', () => { }) ) - // The sphere in particular: a geometry's shape is a oneof, which is the part a careless rebuild - // drops, leaving an obstacle with no volume and a preview that plans straight through it. + // A geometry's shape is a oneof, the part a careless rebuild drops, leaving an obstacle with no + // volume and a preview that plans straight through it. expect(moveRequest.worldState).toEqual({ obstacles: [{ referenceFrame: 'world', geometries: [{ sphere: { radiusMm: 50 } }] }], }) @@ -131,8 +114,7 @@ describe('executeCommand', () => { const trajectory: TrajectoryStep[] = [{ 'left-arm': [0, 0.5] }, { 'left-arm': [0.1, 0.4] }] // Against a literal, not against `trajectory` itself: the command holds the same array reference, - // so comparing it to its own source is a tautology that any in-place reordering or rounding of - // the steps would survive — and the arm would run whatever the mutation left behind. + // so comparing it to its own source is a tautology any in-place reordering would survive. it('sends the trajectory back verbatim', () => { expect(executeCommand(trajectory).execute).toEqual([ { 'left-arm': [0, 0.5] }, @@ -140,23 +122,17 @@ describe('executeCommand', () => { ]) }) - /** - * The key's presence is the switch (`builtin.go`). Without it epsilon is `math.MaxFloat64`, - * so RDK compares the trajectory's first step against where the components actually are and can - * never find them too far away — a plan validated from one configuration runs from any other. - */ it('arms the start-state check RDK will not run unasked', () => { expect(executeCommand(trajectory)).toHaveProperty('executeCheckStart') }) - // Anything ≤ 0 selects `defaultExecuteEpsilon`, so the tolerance stays RDK's to choose. it('defers the tolerance to RDK rather than naming one', () => { expect(executeCommand(trajectory).executeCheckStart).toBeLessThanOrEqual(0) }) }) describe('parsePlanResult', () => { - it('reads the trajectory RDK returns under the command`s own key', () => { + it("reads the trajectory RDK returns under the command's own key", () => { const { trajectory } = parsePlanResult({ plan: [{ 'left-arm': [0, 0.5], 'left-gripper': [] }], }) @@ -165,12 +141,8 @@ describe('parsePlanResult', () => { }) /** - * Throwing beats an empty result, which would read as "planned fine, nothing to show". - * - * Each row asserts its own message, not just that something threw. A bare `toThrow(PlanCommandError)` - * cannot tell a non-array plan apart from a null step or a non-numeric joint value, so any of them - * could silently drift into sharing one message that only actually describes one cause — the defect - * this file used to have, where every one of these asserted the same RDK-version-blaming text. + * Each row asserts its own message. A bare `toThrow(PlanCommandError)` cannot tell a non-array + * plan from a null step, so they could drift into sharing one message that fits only one cause. */ it.each([ ['a non-object reply', 'nope' as JsonValue, /unexpected plan response/], @@ -191,16 +163,8 @@ describe('parsePlanResult', () => { }) /** - * Steps that are structurally present but carry nothing readable. `every` says yes to both by - * default — an array is `typeof 'object'`, and `Object.values({})` is vacuously fine — and - * downstream nothing complains either, because `jointValueAt` resolves a missing column to `0`. - * The result was a plausible-looking arm drawn at the zero configuration rather than a reply - * reported as unreadable. `[[], []]` also satisfied `isAlreadyAtGoal`, so the panel claimed the - * machine was already there. - * - * `{}` and `[]` share a message: both name zero components, and that is the whole of what is - * wrong with either. A non-empty array step is a different defect — a list where a map of - * component names was expected — so it gets its own. + * `{}` and `[]` share a message: both name zero components, and that is the whole of what is wrong + * with either. A non-empty array step is a different defect, a list where a map was expected. */ it.each([ ['a step with no columns', { plan: [{ arm: [0] }, {}] } as JsonValue, /naming no components/], @@ -221,17 +185,6 @@ describe('parsePlanResult', () => { expect(() => parsePlanResult(value)).toThrow(message) }) - /** - * `typeof NaN === 'number'`, so a numeric-only check lets these through, and neither is a local - * defect once it is in. `interpolateTrajectory` sums every segment's frame cost to decide how - * finely to sample, so one non-finite value makes the total non-finite, which survives - * `Math.ceil` until the interior loop stops running: *every* segment of the plan collapses to - * one frame. That is the raw waypoint teleport interpolation exists to prevent, reached with no - * error raised anywhere. - * - * `planCommand` already refuses a non-finite goal on exactly this reasoning; this is the same - * rule applied to the reply. - */ it.each([ ['NaN', Number.NaN], ['Infinity', Number.POSITIVE_INFINITY], @@ -243,11 +196,6 @@ describe('parsePlanResult', () => { expect(() => parsePlanResult(value)).toThrow(/non-finite joint value/) }) - /** - * There is no capability or version RPC to probe a machine with, so the only evidence available is - * what came back. An RDK older than ~v0.101 serialises `Input` as `{Value: number}`, which reaches - * us as a successful plan we cannot read — worth saying, rather than reporting it as no plan. - */ it('tells an unreadable trajectory apart from an absent one', () => { const old = { plan: [{ arm: [{ Value: 0.1 }] }] } as JsonValue @@ -256,25 +204,16 @@ describe('parsePlanResult', () => { }) }) -/** - * RDK seeds the trajectory with the start configuration before planning towards the goal, so a move - * that is already satisfied comes back as two identical steps. It never comes back empty, and never - * as an error — so a check keyed on emptiness never fires and the user gets a two-frame scrubber - * that appears to do nothing. - */ describe('isAlreadyAtGoal', () => { - it('recognises the start configuration returned twice', () => { + it('recognizes the start configuration returned twice', () => { expect(isAlreadyAtGoal([{ arm: [0, 1.5] }, { arm: [0, 1.5] }])).toBe(true) }) /** - * A real reply keys every frame in the system, including the zero-DoF ones RDK pads with `[]` — - * the shape `plan-gantry.json` carries. With only single-component steps the per-component loop - * is never a loop, so `every` and `some` behave identically and the difference between "all - * components held still" and "any one of them did" goes untested. Under `some`, the `_origin` - * frames alone satisfy it and every real move reads as already-at-goal. + * With only single-component steps `every` and `some` behave identically, so "all components held + * still" versus "any one of them did" goes untested. RDK pads zero-DoF frames with `[]`. */ - it('recognises a full multi-component step, padding and all', () => { + it('recognizes a full multi-component step, padding and all', () => { const step: TrajectoryStep = { 'arm-1': [0, 0, 0, 0, 0, 0], 'arm-1_origin': [], @@ -311,11 +250,8 @@ describe('isAlreadyAtGoal', () => { }) /** - * The arm goes somewhere and comes back. Hiding that would be worse than showing it. - * - * The second case is the one that pins the length check itself: destructuring reads indices 0 and - * 1, so a plan whose *first two* steps differ stays false even if the length test is loosened. - * Here the seeded start is repeated, so only `length !== 2` keeps it false. + * The second case pins the length check itself: destructuring reads indices 0 and 1, so a plan + * whose first two steps differ stays false even if the length test is loosened. */ it.each<[string, TrajectoryStep[]]>([ ['ending where it started', [{ arm: [0] }, { arm: [1] }, { arm: [0] }]], diff --git a/src/lib/plugins/MoveFrame/planDoCommand.ts b/src/lib/plugins/MoveFrame/planDoCommand.ts index 05d9e6ed7..e74a80709 100644 --- a/src/lib/plugins/MoveFrame/planDoCommand.ts +++ b/src/lib/plugins/MoveFrame/planDoCommand.ts @@ -1,17 +1,7 @@ /** - * The builtin motion service's two `DoCommand` verbs, which have no place in the motion proto and - * so no generated client: - * - * - `plan` — a protojson `motion.v1.MoveRequest` **as a string**. Plans without executing and - * answers with `motionplan.Trajectory`. - * - `execute` — a `motionplan.Trajectory`. Runs it verbatim, with no replanning. - * - * Only `services/motion/builtin` implements them; any other motion service errors, which is the - * behaviour the panel surfaces. - * - * `motionplan.Trajectory` is `[]referenceframe.FrameSystemInputs`, and `FrameSystemInputs` is - * `map[string][]Input` where `Input` is a `float64` alias — so on the wire it is exactly the - * `trajectory` array a plan dump carries, and the replayer's kinematics read it unchanged. + * The builtin motion service's two `DoCommand` verbs, absent from the motion proto and so from any + * generated client. `plan` takes a protojson `MoveRequest` string; `execute` runs a trajectory + * verbatim. */ import type { JsonValue } from '@bufbuild/protobuf' @@ -37,13 +27,8 @@ export interface PlanRequest { } /** - * Builds the `plan` command. RDK `protojson.Unmarshal`s the string, so the payload uses protojson - * field names (`componentName`, and `oX`/`oY`/`oZ` for the pose's orientation vector) and the - * message's own units — millimetres, with `theta` in degrees, which is what `toDestinationPose` - * already produces. - * - * `worldState` and `constraints` are passed through so a preview plans against the same inputs the - * subsequent `move` would, rather than against a quietly different problem. + * RDK `protojson.Unmarshal`s the string, so the payload uses protojson field names + * (`componentName`, `oX`/`oY`/`oZ`) and the message's own units: millimeters, `theta` in degrees. */ export const planCommand = ({ service, @@ -52,11 +37,9 @@ export const planCommand = ({ worldState, constraints, }: PlanRequest): Record => { - // `JSON.stringify` writes `null` for a non-finite number, and Go's protojson *skips* a JSON null - // for a scalar field rather than rejecting it, leaving the field at its zero. A `NaN` in the goal - // would therefore reach RDK as 0 mm, plan successfully, and preview a move to somewhere the user - // never asked for. Only this path can do that: `client.move` sends a proto double, which carries - // the NaN and gets refused. + // `JSON.stringify` writes `null` for a non-finite number, and Go's protojson skips a null scalar + // rather than rejecting it, so a `NaN` goal would reach RDK as 0 mm and plan a move nobody asked + // for. if (!destination.pose.isFinite()) { throw new PlanCommandError('The move target is not a finite pose.') } @@ -78,10 +61,9 @@ export const planCommand = ({ }, } - // The SDK exports each of these under one name as both a class and a `PlainMessage` type alias - // (`sdk/dist/types.d.ts:51`), and it is the alias that lands in a type position — so what arrives - // here is a plain object with no `toJson` on it. Rebuilding is what reaches protojson, the - // encoding RDK unmarshals the request string with. + // The SDK exports each of these as both a class and a `PlainMessage` alias, and it is the alias + // that lands in a type position, so what arrives here has no `toJson`. Rebuilding is what reaches + // protojson. if (worldState) moveRequest.worldState = new WorldState(worldState).toJson() if (constraints) moveRequest.constraints = new Constraints(constraints).toJson() @@ -89,24 +71,14 @@ export const planCommand = ({ } /** - * Selects RDK's own `defaultExecuteEpsilon` rather than naming a tolerance here. `builtin.go` - * reads any value ≤ 0 — or anything that is not a float — as "use the default", which is the right - * answer: how far an arm may have drifted before its plan is stale is a property of the arm, not - * something a viewer should be deciding for it. + * Selects RDK's own `defaultExecuteEpsilon`: `builtin.go` reads any value at or below zero as "use + * the default". How far an arm may drift before its plan is stale is a property of the arm. */ const RDK_DEFAULT_EPSILON = 0 /** - * Builds the `execute` command for a trajectory a previous `plan` produced. - * - * `executeCheckStart` is what arms RDK's own start-state guard. Its *presence* is the switch - * (`builtin.go`); omit it and epsilon is `math.MaxFloat64`, so the comparison in `builtin.go` - * can never trip and the trajectory runs from wherever the components happen to be. Since `execute` - * never replans, that is precisely the case worth refusing: the plan was validated from one starting - * configuration, and running it from a different one flies a path nothing checked. - * - * A failed check is an RPC error, not a field in the reply — `resp[executeCheckStart]` is a constant - * string echoed back whenever the key was sent, so it says only that the check was asked for. + * Builds the `execute` command. `executeCheckStart`'s presence is the switch (`builtin.go`); omit it + * and epsilon is `math.MaxFloat64`, so the trajectory runs from wherever the components happen to be. */ export const executeCommand = (trajectory: TrajectoryStep[]): Record => ({ execute: trajectory, @@ -114,26 +86,8 @@ export const executeCommand = (trajectory: TrajectoryStep[]): Record typeof step === 'object' && @@ -148,10 +102,8 @@ const isTrajectory = (value: unknown): value is TrajectoryStep[] => Array.isArray(value) && value.every((step) => isTrajectoryStep(step)) /** - * RDK older than ~v0.101 serialised `Input` as `{Value: number}`, a struct, rather than the float - * alias it is today, so a plan that *succeeded* comes back looking like this instead of a number. - * Checked on its own so that one explainable shape gets its own message instead of folding into the - * generic malformed-reply diagnosis below, which is not evidence of any particular RDK version. + * RDK older than ~v0.101 serialized `Input` as the struct `{Value: number}` rather than the float + * alias it is today, so a plan that succeeded comes back looking like this. Given its own message. */ const isOldInputShape = (inputs: unknown): boolean => Array.isArray(inputs) && @@ -168,10 +120,8 @@ const hasOldInputShape = (trajectory: unknown): boolean => ) /** - * Diagnoses a trajectory that already failed {@link isTrajectory}, so the several structurally - * different ways a reply can be malformed each say what is actually wrong instead of collapsing into - * one message that guesses at an RDK version — a guess only {@link hasOldInputShape}'s shape - * supports, and `parsePlanResult` never reaches this function for that shape. + * Diagnoses a trajectory that already failed {@link isTrajectory}. Each structurally different way a + * reply can be malformed gets its own message rather than one that guesses at an RDK version. */ const describeMalformedTrajectory = (value: unknown): string => { if (!Array.isArray(value)) { @@ -236,19 +186,13 @@ export const parsePlanResult = (value: JsonValue): PlanResult => { const trajectory = (value as Record).plan // `== null` rather than `=== undefined`: a Go nil trajectory marshals to JSON `null`, which is - // nothing to draw for the same reason an absent key is. Distinguishing them only sent a user on - // current RDK to go and upgrade it. + // nothing to draw for the same reason an absent key is. if (trajectory == null) { throw new PlanCommandError('Motion service returned no trajectory for this move.') } - // Told apart from every other malformed shape deliberately. An RDK older than ~v0.101 answers a - // plan that *succeeded* with `[{"Value": 0.1}]`, because `Input` was a struct rather than a float - // alias — and the same versions take `component_name` as a `ResourceName`, so the request would - // not have unmarshalled either. There is no capability or version RPC to probe with, so the shape - // of the reply is the only evidence available for saying so, and it is evidence for this one - // shape only: every other way a reply can fail `isTrajectory` is not something an RDK upgrade - // explains, so each gets its own diagnosis instead of borrowing this one. + // Checked ahead of `isTrajectory` so this one explainable shape gets its own message. There is no + // version RPC to probe with, so the reply's shape is the only evidence available. if (hasOldInputShape(trajectory)) { throw new PlanCommandError( 'Motion service returned a trajectory using an older joint-value format. The machine may be running an older version of RDK.' @@ -271,9 +215,8 @@ const sameInputs = (a: TrajectoryStep, b: TrajectoryStep): boolean => { if (names.length !== Object.keys(b).length) return false return names.every((name) => { - // `hasOwn` rather than testing `b[name]` for undefined: a plain index reads straight through to - // `Object.prototype`, so a component named `toString` matched a member function whose `length` - // happens to be 0, and two steps naming different components compared equal. + // `hasOwn` rather than testing `b[name]`: a plain index reads through to `Object.prototype`, so + // a component named `toString` matched a member function whose `length` happens to be 0. if (!Object.hasOwn(b, name)) return false const left = a[name] @@ -288,22 +231,9 @@ const sameInputs = (a: TrajectoryStep, b: TrajectoryStep): boolean => { } /** - * Whether the planner answered "there is nothing to do". - * - * RDK seeds its trajectory with the start configuration (`plan_manager.go`) and then appends the - * IK solution, so a move whose goal is already satisfied comes back as two steps — never as an empty - * plan, and never as an error. - * - * Exact comparison rather than a tolerance, but not because the second step is a copy of the first: - * it is an nlopt output. It is bit-identical because nlopt runs with `SetStopVal(defaultGoalThreshold)` - * from exactly the start configuration, so when the goal is already met it short-circuits at x0 and - * hands the seed vector back unchanged. That makes the guard one that under-fires rather than one - * that mis-fires: a goal near enough to look identical on screen but far enough to clear the - * threshold gets a real two-step plan, and the user sees a preview that does not visibly move. A - * tolerance here would trade that for hiding real short moves, which is the worse mistake. - * - * Length two is load-bearing: a longer plan that happens to end where it began is a real move that - * goes somewhere and comes back, and hiding it would be worse than showing it. + * RDK seeds its trajectory with the start configuration, so a satisfied goal returns two + * bit-identical steps, never empty. Length two matters: a longer plan ending where it began is a + * real move. */ export const isAlreadyAtGoal = (trajectory: TrajectoryStep[]): boolean => { if (trajectory.length !== 2) return false