diff --git a/.changeset/rotten-poems-clap.md b/.changeset/rotten-poems-clap.md new file mode 100644 index 000000000..e04e181ae --- /dev/null +++ b/.changeset/rotten-poems-clap.md @@ -0,0 +1,5 @@ +--- +'@viamrobotics/motion-tools': patch +--- + +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 new file mode 100644 index 000000000..fa194a6cb --- /dev/null +++ b/src/lib/plugins/MoveFrame/__tests__/planDoCommand.spec.ts @@ -0,0 +1,262 @@ +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' + +// 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: + * it aliases to `PlainMessage`. A spread keeps the fields, including the `{case, value}` oneofs. + */ +const asPlainMessage = (message: T): T => ({ ...message }) + +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', + pose: { x: 100, y: -200, z: 350, oX: 0.6, oY: -0.8, oZ: 1, theta: 45 }, + }) + }) + + it('sends exactly the fields `MoveRequest` declares and no others', () => { + expect(Object.keys(moveRequestOf(request())).toSorted()).toEqual([ + 'componentName', + 'destination', + 'name', + ]) + }) + + 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() + expect(moveRequest.constraints).toBeUndefined() + }) + + 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 }] }) + }) + + /** + * `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( + '{"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), + }) + ) + + // 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 } }] }], + }) + 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 any in-place reordering would survive. + it('sends the trajectory back verbatim', () => { + expect(executeCommand(trajectory).execute).toEqual([ + { 'left-arm': [0, 0.5] }, + { 'left-arm': [0.1, 0.4] }, + ]) + }) + + it('arms the start-state check RDK will not run unasked', () => { + expect(executeCommand(trajectory)).toHaveProperty('executeCheckStart') + }) + + 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': [] }]) + }) + + /** + * 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/], + ['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, /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) + expect(() => parsePlanResult(value)).toThrow(message) + }) + + /** + * `{}` 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/], + [ + 'a step that is an array', + { + plan: [ + [ + [0, 1], + [2, 3], + ], + ], + } as JsonValue, + /unnamed joint values/, + ], + ['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) + }) + + 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(/non-finite joint value/) + }) + + it('tells an unreadable trajectory apart from an absent one', () => { + const old = { plan: [{ arm: [{ Value: 0.1 }] }] } as JsonValue + + expect(() => parsePlanResult(old)).toThrow(/older joint-value format/) + expect(() => parsePlanResult({ execute: true })).not.toThrow(/older joint-value format/) + }) +}) + +describe('isAlreadyAtGoal', () => { + it('recognizes the start configuration returned twice', () => { + expect(isAlreadyAtGoal([{ arm: [0, 1.5] }, { arm: [0, 1.5] }])).toBe(true) + }) + + /** + * 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('recognizes 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] }]], + ['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] }]], + // 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 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] }]], + ['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 new file mode 100644 index 000000000..e74a80709 --- /dev/null +++ b/src/lib/plugins/MoveFrame/planDoCommand.ts @@ -0,0 +1,245 @@ +/** + * 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' + +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 +} + +/** + * 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, + componentName, + destination, + worldState, + constraints, +}: PlanRequest): Record => { + // `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.') + } + + 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 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() + + return { plan: JSON.stringify(moveRequest) } +} + +/** + * 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. `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, + executeCheckStart: RDK_DEFAULT_EPSILON, +}) + +/** + * `every` says yes to two degenerate shapes by default: an array is `typeof 'object'`, and + * `Object.values({})` is vacuously fine. Finite, not merely numeric, since `typeof NaN === 'number'`. + */ +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) => Number.isFinite(input)) + ) + +const isTrajectory = (value: unknown): value is TrajectoryStep[] => + Array.isArray(value) && value.every((step) => isTrajectoryStep(step)) + +/** + * 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) && + 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}. 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)) { + 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) + 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 + + // `== 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. + if (trajectory == null) { + throw new PlanCommandError('Motion service returned no trajectory for this move.') + } + + // 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.' + ) + } + + if (!isTrajectory(trajectory)) { + throw new PlanCommandError(describeMalformedTrajectory(trajectory)) + } + + 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) => { + // `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] + const right = b[name] + return ( + left !== undefined && + right !== undefined && + left.length === right.length && + left.every((value, index) => value === right[index]) + ) + }) +} + +/** + * 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 + + const [first, last] = trajectory + return first !== undefined && last !== undefined && sameInputs(first, last) +} + +export { type TrajectoryStep } from '$lib/motion/jointPose'