From b7fe16c07c4476a345766bdd05aff2e8806a5696 Mon Sep 17 00:00:00 2001 From: Marcus <139352735+sucrammal@users.noreply.github.com> Date: Tue, 28 Jul 2026 16:33:43 -0400 Subject: [PATCH] DRAFT: New modal for inspect IK --- .../MotionPlanReplayer.svelte | 2 + .../MotionPlanReplayerUI.svelte | 31 +- .../inspect-ik/IKCandidateDetail.svelte | 102 + .../inspect-ik/IKCandidateList.svelte | 133 + .../inspect-ik/IKCandidateRow.svelte | 55 + .../inspect-ik/IKInspectionPanel.svelte | 75 + .../inspect-ik/IKSeedGroup.svelte | 58 + .../inspect-ik/IKStatusDot.svelte | 25 + .../inspect-ik/draw-pose-set.ts | 85 + .../fixtures/pirouette-request.json | 1625 +++++++++++ .../fixtures/pirouette-solutions.json | 2597 +++++++++++++++++ .../inspect-ik/ik-candidates.ts | 85 + .../inspect-ik/inspect-ik-client.ts | 37 + .../inspect-ik/namespace-snapshot.ts | 42 + .../inspect-ik/parse-ik-solutions.ts | 99 + .../inspect-ik/pose-sets.ts | 113 + .../inspect-ik/relations.ts | 11 + .../inspect-ik/useIKInspection.svelte.ts | 313 ++ .../inspect-ik/world-state-obstacles.ts | 71 + 19 files changed, 5556 insertions(+), 3 deletions(-) create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateDetail.svelte create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateList.svelte create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateRow.svelte create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/IKInspectionPanel.svelte create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/IKSeedGroup.svelte create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/IKStatusDot.svelte create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/draw-pose-set.ts create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/fixtures/pirouette-request.json create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/fixtures/pirouette-solutions.json create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/ik-candidates.ts create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/inspect-ik-client.ts create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/namespace-snapshot.ts create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/parse-ik-solutions.ts create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/pose-sets.ts create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/relations.ts create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/useIKInspection.svelte.ts create mode 100644 src/lib/plugins/MotionPlanReplayer/inspect-ik/world-state-obstacles.ts diff --git a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayer.svelte b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayer.svelte index bd4ce286b..74d96070c 100644 --- a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayer.svelte +++ b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayer.svelte @@ -5,6 +5,7 @@ import { useEnvironment } from '$lib/hooks/useEnvironment.svelte' + import { provideIKInspection } from './inspect-ik/useIKInspection.svelte' import MotionPlanReplayerUI from './MotionPlanReplayerUI.svelte' import { type ResolvePlanSnapshots } from './plan-dropper' import { type PlanEntry, provideMotionPlanReplayer } from './useMotionPlanReplayer.svelte' @@ -20,6 +21,7 @@ const { plans, children, resolvePlanSnapshots }: Props = $props() provideMotionPlanReplayer(untrack(() => plans)) + provideIKInspection() const environment = useEnvironment() diff --git a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte index b59740e34..abac63d80 100644 --- a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte +++ b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte @@ -1,13 +1,15 @@ + +
+
+ + {candidate.seed} · #{candidate.indexInSeed + 1} + {IK_STATUS_LABEL[candidate.status]} + + {#if isScored(cost)} + {cost.toFixed(4)} + {:else} + unscored + {/if} + +
+ +
+ Show + {#each poseSets as poseSet (poseSet.kind)} + {@const shown = poseVisibility[poseSet.kind]} + + {/each} +
+ + {#if !hasStartPose} +

+ Start pose unavailable — the request carries no start configuration. +

+ {/if} + + {#if detail} +
+

+ {detail} +

+ {#if clampable} + + {/if} +
+ {:else if !solved} +

+ The solver returned no configuration for this seed, so there is no pose to draw — it failed + before producing a candidate rather than producing one that collides. +

+ {:else} +

No failure detail was recorded for this candidate.

+ {/if} +
diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateList.svelte b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateList.svelte new file mode 100644 index 000000000..b519f60d8 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateList.svelte @@ -0,0 +1,133 @@ + + +
+
+
+ {ctx.totalCount} candidates + {ctx.seedBuckets.length} seeds +
+ +
+ {#each ORDER as status (status)} + {@const active = ctx.statusFilter.has(status)} + + {/each} + + + + + {#each ORDER as status (status)} + + {IK_STATUS_LABEL[status]} — {IK_STATUS_DESCRIPTION[ + status + ]} + + {/each} + + + +
+ Sort + {#each [['seed', 'Seed'], ['cost', 'Cost']] as const as [mode, label] (mode)} + + {/each} +
+
+
+ +
+ {#if ctx.visibleCandidates.length === 0} +
+

No candidates match this filter.

+ +
+ {:else if ctx.sortMode === 'seed'} + {#each ctx.seedBuckets as bucket (bucket.seedIndex)} + ctx.toggleSeed(bucket.seedIndex)} + onselect={(id) => ctx.select(id)} + /> + {/each} + {:else} +
+ {#each ctx.visibleCandidates as candidate (candidate.id)} + ctx.select(candidate.id)} + /> + {/each} +
+ {/if} +
+ + {#if ctx.selectedCandidate} + + {/if} +
diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateRow.svelte b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateRow.svelte new file mode 100644 index 000000000..8ea5eb839 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateRow.svelte @@ -0,0 +1,55 @@ + + + + +

+ {candidate.seed} · #{candidate.indexInSeed + 1}{isScored(cost) ? '' : ' · unscored'} +

+
diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKInspectionPanel.svelte b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKInspectionPanel.svelte new file mode 100644 index 000000000..d9e69e927 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKInspectionPanel.svelte @@ -0,0 +1,75 @@ + + + ctx.isOpen, (open) => ctx.setOpen(open)} + title="IK Inspection" + defaultSize={SIZE} + minSize={{ width: 320, height: 320 }} + {defaultPosition} + resizable + bodyClass="bg-white" +> + {#snippet headerPrefix()} + + {/snippet} + +
+ {#if ctx.status === 'loading'} +
+
+ {#each [0, 1, 2] as row (row)} +
+ {/each} +
+

Running IK inspection…

+
+ {:else if ctx.status === 'error'} +
+
+ {:else if ctx.status === 'ready'} +

+ Demo fixture — candidates come from pirouette-request, + not from {ctx.planName ?? 'the selected plan'}. +

+
+ +
+ {:else} +
+ Use a plan's bug icon to inspect its IK candidates. +
+ {/if} +
+
diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKSeedGroup.svelte b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKSeedGroup.svelte new file mode 100644 index 000000000..cb884f2c9 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKSeedGroup.svelte @@ -0,0 +1,58 @@ + + +
+ + + {#if expanded} +
+ {#each bucket.candidates as candidate (candidate.id)} + onselect(candidate.id)} + /> + {/each} +
+ {/if} +
diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKStatusDot.svelte b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKStatusDot.svelte new file mode 100644 index 000000000..03b8b0b1b --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKStatusDot.svelte @@ -0,0 +1,25 @@ + + + diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/draw-pose-set.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/draw-pose-set.ts new file mode 100644 index 000000000..b7f71d372 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/draw-pose-set.ts @@ -0,0 +1,85 @@ +import type { Entity, World } from 'koota' + +import type { Transform } from '$lib/buf/common/v1/common_pb' +import type { Snapshot } from '$lib/buf/draw/v1/snapshot_pb' + +import { traits } from '$lib/ecs' +import { reconcileSnapshotEntities, type SnapshotEntity } from '$lib/snapshot' + +import { transformsToSnapshot } from '../plan-to-snapshots' +import { namespaceSnapshot } from './namespace-snapshot' +import { OBSTACLE_STYLE, type PoseSet, type PoseStyle } from './pose-sets' +import * as inspectRelations from './relations' +import { OBSTACLE_PREFIX } from './world-state-obstacles' + +export interface DrawnSet { + root: Entity + entityMap: Map +} + +// koota's `set` writes the trait's store slot but will not add an absent trait — the entity's +// mask is untouched, so `has` stays false and nothing querying the trait ever sees the value. +// Plan transforms carry no color metadata, so `Color` is always absent on spawn; `Opacity` only +// happens to be present because `drawTransform` adds it unconditionally. Guard both rather than +// depend on that. +const setOrAddColor = (entity: Entity, value: PoseStyle['rgb']) => { + if (entity.has(traits.Color)) entity.set(traits.Color, value) + else entity.add(traits.Color(value)) +} + +const setOrAddOpacity = (entity: Entity, value: number) => { + if (entity.has(traits.Opacity)) entity.set(traits.Opacity, value) + else entity.add(traits.Opacity(value)) +} + +const drawSnapshot = ( + world: World, + rootName: string, + snapshot: Snapshot, + style: PoseStyle +): DrawnSet => { + // Spawned before the reconcile so `resolveOrphans` can parent the set's top-level frames onto + // it within the same flush. + const root = world.spawn(traits.Name(rootName)) + + // A fresh empty map. Reconcile garbage-collects only entities present in the map it is handed, + // so passing an empty one guarantees nothing already in the scene gets swept. + const result = reconcileSnapshotEntities(world, snapshot, new Map()) + + for (const spawned of result.spawned) { + spawned.entity.add(inspectRelations.PartOfInspection(root)) + + // Frames without geometry carry `ReferenceFrame` and render as axes, which the plan colour + // would not apply to anyway. + if (!spawned.entity.has(traits.ReferenceFrame)) setOrAddColor(spawned.entity, style.rgb) + setOrAddOpacity(spawned.entity, style.opacity) + } + + return { root, entityMap: result.current } +} + +export const drawPoseSet = (world: World, poseSet: PoseSet, snapshot: Snapshot): DrawnSet => + drawSnapshot(world, poseSet.prefix, namespaceSnapshot(snapshot, poseSet.prefix), poseSet.style) + +/** + * Obstacles keep their `obstacle/` names and stay parented to `world` — they are the fixed scene, + * not one of the candidate poses, so they are drawn once and survive candidate changes. + */ +export const drawObstacles = (world: World, transforms: Transform[]): DrawnSet => + drawSnapshot(world, OBSTACLE_PREFIX, transformsToSnapshot(transforms), OBSTACLE_STYLE) + +export const destroyDrawnSet = (drawn: DrawnSet): void => { + // `PartOfInspection` is autoDestroy: 'source', so the root takes every member with it. + if (drawn.root.isAlive()) drawn.root.destroy() + drawn.entityMap.clear() +} + +/** + * Toggles the root only. Every frame in a pose set is a `ChildOf` descendant of it, and + * `useInheritedInvisible` walks that chain, so one trait hides the whole arm. + */ +export const setDrawnSetVisible = (drawn: DrawnSet, visible: boolean): void => { + if (!drawn.root.isAlive()) return + if (visible) drawn.root.remove(traits.Invisible) + else drawn.root.add(traits.Invisible) +} diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/fixtures/pirouette-request.json b/src/lib/plugins/MotionPlanReplayer/inspect-ik/fixtures/pirouette-request.json new file mode 100644 index 000000000..0be6c6baa --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/fixtures/pirouette-request.json @@ -0,0 +1,1625 @@ +{ + "frame_system": { + "name": "builtin", + "world": { + "frame_type": "static", + "frame": { + "id": "world", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "frames": { + "arm": { + "frame_type": "model", + "frame": { + "name": "arm", + "model": { + "name": "UR5e", + "kinematic_param_type": "SVA", + "links": [ + { + "id": "base_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 162.5 + }, + "orientation": null, + "geometry": { + "type": "", + "x": 0, + "y": 0, + "z": 0, + "r": 60, + "l": 260, + "translation": { + "X": 0, + "Y": 0, + "Z": 130 + }, + "orientation": { + "type": "" + }, + "Label": "" + }, + "parent": "world" + }, + { + "id": "shoulder_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": null, + "geometry": { + "type": "", + "x": 0, + "y": 0, + "z": 0, + "r": 55, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "" + }, + "Label": "" + }, + "parent": "shoulder_pan_joint" + }, + { + "id": "upper_arm_link", + "translation": { + "X": -425, + "Y": 0, + "Z": 0 + }, + "orientation": null, + "geometry": { + "type": "", + "x": 0, + "y": 0, + "z": 0, + "r": 65, + "l": 550, + "translation": { + "X": -212.5, + "Y": -130, + "Z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": 0, + "x": -1, + "y": 0, + "z": 0 + } + }, + "Label": "" + }, + "parent": "shoulder_lift_joint" + }, + { + "id": "forearm_link", + "translation": { + "X": -392.2, + "Y": 0, + "Z": 0 + }, + "orientation": null, + "geometry": { + "type": "", + "x": 0, + "y": 0, + "z": 0, + "r": 50, + "l": 490, + "translation": { + "X": -196.1, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": 0, + "x": -1, + "y": 0, + "z": 0 + } + }, + "Label": "" + }, + "parent": "elbow_joint" + }, + { + "id": "wrist_1_link", + "translation": { + "X": 0, + "Y": -133.3, + "Z": 0 + }, + "orientation": null, + "geometry": { + "type": "", + "x": 0, + "y": 0, + "z": 0, + "r": 40, + "l": 230, + "translation": { + "X": 0, + "Y": -80.65, + "Z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": 0, + "x": 0, + "y": -1, + "z": 0 + } + }, + "Label": "" + }, + "parent": "wrist_1_joint" + }, + { + "id": "wrist_2_link", + "translation": { + "X": 0, + "Y": 0, + "Z": -99.7 + }, + "orientation": null, + "geometry": { + "type": "", + "x": 0, + "y": 0, + "z": 0, + "r": 70, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": 0, + "x": 0, + "y": 0, + "z": -1 + } + }, + "Label": "" + }, + "parent": "wrist_2_joint" + }, + { + "id": "ee_link", + "translation": { + "X": 0, + "Y": -99.6, + "Z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": 90, + "x": 0, + "y": -1, + "z": 0 + } + }, + "geometry": { + "type": "", + "x": 0, + "y": 0, + "z": 0, + "r": 40, + "l": 170, + "translation": { + "X": 0, + "Y": -49.85, + "Z": 0 + }, + "orientation": { + "type": "ov_degrees", + "value": { + "th": 0, + "x": 0, + "y": -1, + "z": 0 + } + }, + "Label": "" + }, + "parent": "wrist_3_joint" + } + ], + "joints": [ + { + "id": "shoulder_pan_joint", + "type": "revolute", + "parent": "base_link", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 360, + "min": -360 + }, + { + "id": "shoulder_lift_joint", + "type": "revolute", + "parent": "shoulder_link", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + }, + { + "id": "elbow_joint", + "type": "revolute", + "parent": "upper_arm_link", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 180, + "min": -180 + }, + { + "id": "wrist_1_joint", + "type": "revolute", + "parent": "forearm_link", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + }, + { + "id": "wrist_2_joint", + "type": "revolute", + "parent": "wrist_1_link", + "axis": { + "X": 0, + "Y": 0, + "Z": -1 + }, + "max": 360, + "min": -360 + }, + { + "id": "wrist_3_joint", + "type": "revolute", + "parent": "wrist_2_link", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + } + ], + "original_file": { + "bytes": "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", + "extension": "json" + } + }, + "limits": [ + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + }, + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + }, + { + "Min": -3.141592653589793, + "Max": 3.141592653589793 + }, + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + }, + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + }, + { + "Min": -6.283185307179586, + "Max": 6.283185307179586 + } + ], + "internal_fs": { + "name": "internal", + "world": { + "frame_type": "static", + "frame": { + "id": "world", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "frames": { + "base_link": { + "frame_type": "static", + "frame": { + "id": "base_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 162.5 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 60, + "l": 260, + "translation": { + "X": 0, + "Y": 0, + "Z": 130 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "base_link" + } + } + }, + "ee_link": { + "frame_type": "static", + "frame": { + "id": "ee_link", + "translation": { + "X": 0, + "Y": -99.6, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.7071067811865476, + "X": 0.7071067811865475, + "Y": 1.1102230246251565e-16, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 40, + "l": 170, + "translation": { + "X": 0, + "Y": -49.85, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.5000000000000001, + "X": 0.4999999999999999, + "Y": 0.5, + "Z": -0.5 + } + }, + "Label": "ee_link" + } + } + }, + "elbow_joint": { + "frame_type": "rotational", + "frame": { + "id": "elbow_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 180, + "min": -180 + } + }, + "forearm_link": { + "frame_type": "static", + "frame": { + "id": "forearm_link", + "translation": { + "X": -392.2, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 50, + "l": 490, + "translation": { + "X": -196.09999999999997, + "Y": -7.888609052210118e-31, + "Z": -1.4210854715202004e-14 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 4.329780281177461e-17, + "X": -0.7071067811865475, + "Y": 4.32978028117746e-17, + "Z": 0.7071067811865476 + } + }, + "Label": "forearm_link" + } + } + }, + "shoulder_lift_joint": { + "frame_type": "rotational", + "frame": { + "id": "shoulder_lift_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + } + }, + "shoulder_link": { + "frame_type": "static", + "frame": { + "id": "shoulder_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "sphere", + "x": 0, + "y": 0, + "z": 0, + "r": 55, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "shoulder_link" + } + } + }, + "shoulder_pan_joint": { + "frame_type": "rotational", + "frame": { + "id": "shoulder_pan_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 360, + "min": -360 + } + }, + "upper_arm_link": { + "frame_type": "static", + "frame": { + "id": "upper_arm_link", + "translation": { + "X": -425, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 65, + "l": 550, + "translation": { + "X": -212.5, + "Y": -130, + "Z": -1.9900510486144466e-15 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 4.329780281177461e-17, + "X": -0.7071067811865475, + "Y": 4.32978028117746e-17, + "Z": 0.7071067811865476 + } + }, + "Label": "upper_arm_link" + } + } + }, + "wrist_1_joint": { + "frame_type": "rotational", + "frame": { + "id": "wrist_1_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + } + }, + "wrist_1_link": { + "frame_type": "static", + "frame": { + "id": "wrist_1_link", + "translation": { + "X": 0, + "Y": -133.3, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 40, + "l": 230, + "translation": { + "X": 0, + "Y": -80.65, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.5000000000000001, + "X": 0.4999999999999999, + "Y": 0.5, + "Z": -0.5 + } + }, + "Label": "wrist_1_link" + } + } + }, + "wrist_2_joint": { + "frame_type": "rotational", + "frame": { + "id": "wrist_2_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": 0, + "Z": -1 + }, + "max": 360, + "min": -360 + } + }, + "wrist_2_link": { + "frame_type": "static", + "frame": { + "id": "wrist_2_link", + "translation": { + "X": 0, + "Y": 0, + "Z": -99.7 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "sphere", + "x": 0, + "y": 0, + "z": 0, + "r": 70, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 6.123233995736757e-17, + "X": 0, + "Y": 1, + "Z": 0 + } + }, + "Label": "wrist_2_link" + } + } + }, + "wrist_3_joint": { + "frame_type": "rotational", + "frame": { + "id": "wrist_3_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + } + } + }, + "parents": { + "base_link": "world", + "ee_link": "wrist_3_joint", + "elbow_joint": "upper_arm_link", + "forearm_link": "elbow_joint", + "shoulder_lift_joint": "shoulder_link", + "shoulder_link": "shoulder_pan_joint", + "shoulder_pan_joint": "base_link", + "upper_arm_link": "shoulder_lift_joint", + "wrist_1_joint": "forearm_link", + "wrist_1_link": "wrist_1_joint", + "wrist_2_joint": "wrist_1_link", + "wrist_2_link": "wrist_2_joint", + "wrist_3_joint": "wrist_2_link" + } + }, + "primary_output_frame": "ee_link" + } + }, + "arm:base_link": { + "frame_type": "named", + "frame": { + "name": "arm:base_link", + "inner_frame": { + "frame_type": "static", + "frame": { + "id": "base_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 162.5 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 60, + "l": 260, + "translation": { + "X": 0, + "Y": 0, + "Z": 130 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "base_link" + } + } + } + } + }, + "arm:ee_link": { + "frame_type": "named", + "frame": { + "name": "arm:ee_link", + "inner_frame": { + "frame_type": "static", + "frame": { + "id": "ee_link", + "translation": { + "X": 0, + "Y": -99.6, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.7071067811865476, + "X": 0.7071067811865475, + "Y": 1.1102230246251565e-16, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 40, + "l": 170, + "translation": { + "X": 0, + "Y": -49.85, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.5000000000000001, + "X": 0.4999999999999999, + "Y": 0.5, + "Z": -0.5 + } + }, + "Label": "ee_link" + } + } + } + } + }, + "arm:elbow_joint": { + "frame_type": "named", + "frame": { + "name": "arm:elbow_joint", + "inner_frame": { + "frame_type": "rotational", + "frame": { + "id": "elbow_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 180, + "min": -180 + } + } + } + }, + "arm:forearm_link": { + "frame_type": "named", + "frame": { + "name": "arm:forearm_link", + "inner_frame": { + "frame_type": "static", + "frame": { + "id": "forearm_link", + "translation": { + "X": -392.2, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 50, + "l": 490, + "translation": { + "X": -196.09999999999997, + "Y": -7.888609052210118e-31, + "Z": -1.4210854715202004e-14 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 4.329780281177461e-17, + "X": -0.7071067811865475, + "Y": 4.32978028117746e-17, + "Z": 0.7071067811865476 + } + }, + "Label": "forearm_link" + } + } + } + } + }, + "arm:shoulder_lift_joint": { + "frame_type": "named", + "frame": { + "name": "arm:shoulder_lift_joint", + "inner_frame": { + "frame_type": "rotational", + "frame": { + "id": "shoulder_lift_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + } + } + } + }, + "arm:shoulder_link": { + "frame_type": "named", + "frame": { + "name": "arm:shoulder_link", + "inner_frame": { + "frame_type": "static", + "frame": { + "id": "shoulder_link", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "sphere", + "x": 0, + "y": 0, + "z": 0, + "r": 55, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "shoulder_link" + } + } + } + } + }, + "arm:shoulder_pan_joint": { + "frame_type": "named", + "frame": { + "name": "arm:shoulder_pan_joint", + "inner_frame": { + "frame_type": "rotational", + "frame": { + "id": "shoulder_pan_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": 0, + "Z": 1 + }, + "max": 360, + "min": -360 + } + } + } + }, + "arm:upper_arm_link": { + "frame_type": "named", + "frame": { + "name": "arm:upper_arm_link", + "inner_frame": { + "frame_type": "static", + "frame": { + "id": "upper_arm_link", + "translation": { + "X": -425, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 65, + "l": 550, + "translation": { + "X": -212.5, + "Y": -130, + "Z": -1.9900510486144466e-15 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 4.329780281177461e-17, + "X": -0.7071067811865475, + "Y": 4.32978028117746e-17, + "Z": 0.7071067811865476 + } + }, + "Label": "upper_arm_link" + } + } + } + } + }, + "arm:wrist_1_joint": { + "frame_type": "named", + "frame": { + "name": "arm:wrist_1_joint", + "inner_frame": { + "frame_type": "rotational", + "frame": { + "id": "wrist_1_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + } + } + } + }, + "arm:wrist_1_link": { + "frame_type": "named", + "frame": { + "name": "arm:wrist_1_link", + "inner_frame": { + "frame_type": "static", + "frame": { + "id": "wrist_1_link", + "translation": { + "X": 0, + "Y": -133.3, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "capsule", + "x": 0, + "y": 0, + "z": 0, + "r": 40, + "l": 230, + "translation": { + "X": 0, + "Y": -80.65, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 0.5000000000000001, + "X": 0.4999999999999999, + "Y": 0.5, + "Z": -0.5 + } + }, + "Label": "wrist_1_link" + } + } + } + } + }, + "arm:wrist_2_joint": { + "frame_type": "named", + "frame": { + "name": "arm:wrist_2_joint", + "inner_frame": { + "frame_type": "rotational", + "frame": { + "id": "wrist_2_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": 0, + "Z": -1 + }, + "max": 360, + "min": -360 + } + } + } + }, + "arm:wrist_2_link": { + "frame_type": "named", + "frame": { + "name": "arm:wrist_2_link", + "inner_frame": { + "frame_type": "static", + "frame": { + "id": "wrist_2_link", + "translation": { + "X": 0, + "Y": 0, + "Z": -99.7 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "sphere", + "x": 0, + "y": 0, + "z": 0, + "r": 70, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 6.123233995736757e-17, + "X": 0, + "Y": 1, + "Z": 0 + } + }, + "Label": "wrist_2_link" + } + } + } + } + }, + "arm:wrist_3_joint": { + "frame_type": "named", + "frame": { + "name": "arm:wrist_3_joint", + "inner_frame": { + "frame_type": "rotational", + "frame": { + "id": "wrist_3_joint", + "type": "revolute", + "parent": "", + "axis": { + "X": 0, + "Y": -1, + "Z": 0 + }, + "max": 360, + "min": -360 + } + } + } + }, + "arm_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "arm_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "floor": { + "frame_type": "static", + "frame": { + "id": "floor", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "floor_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "floor_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": -50 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 2000, + "y": 2000, + "z": 10, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "floor_origin" + } + } + }, + "gripper": { + "frame_type": "static", + "frame": { + "id": "gripper", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "gripper_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "gripper_origin", + "translation": { + "X": 0, + "Y": 0, + "Z": 100 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "geometry": { + "type": "box", + "x": 60, + "y": 60, + "z": 30, + "r": 0, + "l": 0, + "translation": { + "X": 0, + "Y": 0, + "Z": -15 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + }, + "Label": "gripper_origin" + } + } + }, + "pallet": { + "frame_type": "static", + "frame": { + "id": "pallet", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "pallet_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "pallet_origin", + "translation": { + "X": 200, + "Y": 500, + "Z": 100 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "pick-station": { + "frame_type": "static", + "frame": { + "id": "pick-station", + "translation": { + "X": 0, + "Y": 0, + "Z": 0 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + }, + "pick-station_origin": { + "frame_type": "tail_geometry_static", + "frame": { + "id": "pick-station_origin", + "translation": { + "X": 400, + "Y": -300, + "Z": 200 + }, + "orientation": { + "type": "quaternion", + "value": { + "W": 1, + "X": 0, + "Y": 0, + "Z": 0 + } + } + } + } + }, + "parents": { + "arm": "arm_origin", + "arm:base_link": "arm_origin", + "arm:ee_link": "arm:wrist_3_joint", + "arm:elbow_joint": "arm:upper_arm_link", + "arm:forearm_link": "arm:elbow_joint", + "arm:shoulder_lift_joint": "arm:shoulder_link", + "arm:shoulder_link": "arm:shoulder_pan_joint", + "arm:shoulder_pan_joint": "arm:base_link", + "arm:upper_arm_link": "arm:shoulder_lift_joint", + "arm:wrist_1_joint": "arm:forearm_link", + "arm:wrist_1_link": "arm:wrist_1_joint", + "arm:wrist_2_joint": "arm:wrist_1_link", + "arm:wrist_2_link": "arm:wrist_2_joint", + "arm:wrist_3_joint": "arm:wrist_2_link", + "arm_origin": "world", + "floor": "floor_origin", + "floor_origin": "world", + "gripper": "gripper_origin", + "gripper_origin": "arm", + "pallet": "pallet_origin", + "pallet_origin": "world", + "pick-station": "pick-station_origin", + "pick-station_origin": "world" + } + }, + "goals": [ + { + "poses": { + "gripper": { + "referenceFrame": "world", + "pose": { + "x": 400, + "y": -300, + "z": 420, + "oX": 1.2246467991473515e-16, + "oZ": -1 + } + } + }, + "configuration": null + } + ], + "start_state": { + "poses": null, + "configuration": { + "arm": [ + 4.081750377684086, -1.4964197133000183, 1.5901956311183134, + -1.6645721653113386, -1.5707959818938046, 2.510953637167213 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + } + }, + "world_state": { + "obstacles": [ + { + "referenceFrame": "world", + "geometries": [ + { + "center": { + "x": 200, + "y": 500, + "z": 100, + "oZ": 1 + }, + "box": { + "dimsMm": { + "x": 350, + "y": 350, + "z": 100 + } + }, + "label": "pallet" + }, + { + "center": { + "x": 200, + "y": -500, + "z": 220, + "oZ": 1 + }, + "box": { + "dimsMm": { + "x": 400, + "y": 400, + "z": 40 + } + }, + "label": "pick-station" + } + ] + } + ] + }, + "constraints": { + "linear_constraints": [], + "pseudolinear_constraints": [], + "orientation_constraints": [ + { + "OrientationToleranceDegs": 30 + } + ], + "collision_specifications": [] + }, + "planner_options": { + "goal_metric_type": "squared_norm", + "max_ik_solutions": 100, + "min_ik_score": 0.01, + "resolution": 2, + "timeout": 30, + "goal_threshold": 0.1, + "frame_step": 0.01, + "input_ident_dist": 0.0001, + "iter_before_rand": 50, + "position_seeds": 0, + "return_partial_plan": false, + "configuration_distance_metric": "fs_config_l2", + "collision_buffer_mm": 1e-8, + "rseed": 0, + "meshes_as_octrees": false + } +} diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/fixtures/pirouette-solutions.json b/src/lib/plugins/MotionPlanReplayer/inspect-ik/fixtures/pirouette-solutions.json new file mode 100644 index 000000000..8dc71f956 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/fixtures/pirouette-solutions.json @@ -0,0 +1,2597 @@ +[ + { + "seed": "start · full limits", + "solutions": [ + { + "cost": 10.651956098914805, + "configuration": { + "arm": [ + -0.3736384529843958, -2.9840002348518087, + 1.5090402833317484, 3.045755520297502, + -4.712386919585232, -5.086024714885804 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.38919560062994, + "configuration": { + "arm": [ + 2.228227204912571, 4.691979215156888, + 1.5090413752136582, 4.794553277702239, + -1.5707965099395806, 0.6574288984058989 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.75728 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.17282778739626584 -1.889626178436074e-07 0.9641943258899853 -0.20114635921445922}", + "last_good_inputs": { + "arm": [ + 4.006169821124471, -1.2440772327221639, + 1.5868864284503565, -1.4011903511302024, + -1.5707960034257684, 2.435373016751703 + ] + } + }, + { + "cost": 8.592302864290106, + "configuration": { + "arm": [ + -0.373637494816117, 4.228153412958453, + -0.878635919903603, -4.920313848993856, + -1.5707964195213875, 4.338751548069538 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.78945 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-1.3690423385548887e-07 0.2654673753033997 0.9641198435098715 7.821760187187972e-08}", + "last_good_inputs": { + "arm": [ + 3.710468054975736, -1.019371952778479, + 1.3844596685331538, -1.9358839722848817, + -1.5707960183627698, 2.663270129742407 + ] + } + }, + { + "cost": 14.669677173433206, + "configuration": { + "arm": [ + -4.054958453139637, 4.691977815357016, + 1.5090444388372222, 4.794549700883186, + -1.5707936158276619, -5.6257530277769 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 31.11110 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.2230485663197744 -9.424901960891813e-08 0.9633708819561094 -0.1488821039003417}", + "last_good_inputs": { + "arm": [ + 3.742720843066431, -1.238569816272642, + 1.5868143314399346, -1.3954420875532334, + -1.5707958833077154, 2.1719241927945414 + ] + } + }, + { + "cost": 6.777793118506012, + "configuration": { + "arm": [ + 2.2282267088842103, 4.691979819269963, + 1.5090424228047976, -1.4886346207005694, + -1.570793796844969, 0.65743017302692 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "obstacle constraint: violation between gripper_origin and pallet geometries", + "last_good_inputs": { + "arm": [ + 3.980238939433383, -1.1575012804817841, + 1.5857511349226443, -1.6549366405557793, + -1.5707958622258156, 2.409442210125047 + ] + } + }, + { + "cost": 12.107385722491026, + "configuration": { + "arm": [ + -4.054957321748623, 6.037540282229422, + -0.8786356659726481, -3.5881083160555236, + -4.712388684572695, 3.7990240842423884 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 7.534474453875696, + "configuration": { + "arm": [ + -0.37363771066678825, -2.984000675112053, + 1.5090406621057215, 3.0457556413468017, + -4.712388614422924, 1.1971580412276441 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.412462910783247, + "configuration": { + "arm": [ + 5.909547843776799, -2.055032081776799, + -0.8786357025529459, -4.920314337346383, + -1.5707963009284178, -1.9444334524295366 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.65099 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.12291496043222869 -0.18519082190542938 0.9644398815252064 0.14301044335145965}", + "last_good_inputs": { + "arm": [ + 4.187709651080765, -1.5288030390088172, + 1.4470749740938926, -1.8533108419510513, + -1.5707960003885648, 2.2526703276253723 + ] + } + }, + { + "cost": 10.442162663989686, + "configuration": { + "arm": [ + -0.3736371031305519, 3.3872377895301415, + 0.8786362788469669, 0.446515770287204, + 4.712389441303686, -1.944433069585175 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 7.607846697253402, + "configuration": { + "arm": [ + -0.3736352022505601, 3.2991846233531223, + 1.5090419954846277, -3.2374317344146792, + -4.712395272315822, 1.1971595696444832 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + } + ] + }, + { + "seed": "start · sensitivity 3%", + "solutions": [ + { + "cost": 10.442162610648165, + "configuration": { + "arm": [ + -0.3736378171137583, 3.3872377518876533, + 0.878635509362026, 0.446515396061152, 4.712388263315598, + -1.9444340018825828 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.365879170160442, + "configuration": { + "arm": [ + -4.054957352545147, -1.086561088464691, + 0.87863625547171, -4.504464460428971, 1.570795952945761, + 3.7990242198643673 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.54874 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.0028900763715690833 0.23651311569371225 0.9646753677864913 0.1159940875623282}", + "last_good_inputs": { + "arm": [ + 3.6673810025335234, -1.4755472833315526, + 1.5539588110622364, -1.809196309970107, + -1.4108075037306786, 2.5765498242490126 + ] + } + }, + { + "cost": 5.5328233283170425, + "configuration": { + "arm": [ + -0.3736365294423404, -2.8959479297545827, + 0.8786369054910028, 0.4465153081414669, + -1.5707947821799204, 4.338753116742279 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.061264937670561, + "configuration": { + "arm": [ + 5.909546893511973, -1.5503877744760366, + -1.5090410621821366, -1.6529600838328302, + -4.7123862374166565, -5.086027506766407 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.78838 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.11695729163831059 -0.23830470997304815 0.9641222908463919 0.00025579366338202274}", + "last_good_inputs": { + "arm": [ + 4.175406066924854, -1.4991850189139961, + 1.431391767494654, -1.6639771660950844, + -1.7317700280445623, 2.1216868347507796 + ] + } + }, + { + "cost": 8.804052317043903, + "configuration": { + "arm": [ + -0.37363741564458, 3.29918475398896, 1.5090413532058191, + 3.045755615021412, 1.5707961485204263, + 1.1971589355524137 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.365879694147397, + "configuration": { + "arm": [ + -4.054957724925695, -1.0865621112977328, + 0.8786379063841915, -4.5044664140733435, + 1.5707951807689207, 3.799024292941036 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.54874 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.0028901164369905656 0.23651312552084283 0.9646753641591301 0.11599409669372182}", + "last_good_inputs": { + "arm": [ + 3.667380983569699, -1.4755473354202724, + 1.553958895136483, -1.8091964094612554, + -1.4108075430544993, 2.5765498279705095 + ] + } + }, + { + "cost": 6.9220724118696015, + "configuration": { + "arm": [ + 2.228225585152741, -1.0865603991613078, + 0.8786358037262757, -4.504460556481893, + -4.7123919121223885, -2.4841603394693244 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.41881 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.22209764170824312 -0.13210996063164 0.9649734338399735 -0.04523127051756916}", + "last_good_inputs": { + "arm": [ + 3.8937116306156887, -1.4548397828801491, + 1.5180084022524545, -1.952676784705453, + -1.8895086124967044, 2.0042029438852453 + ] + } + }, + { + "cost": 12.503435600527146, + "configuration": { + "arm": [ + -4.054958695618554, 5.196625931728509, + 0.878634858622766, -4.50445980729934, 1.570791715437851, + -2.4841628686947823 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.13782 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.18232853235641389 0.16267081310869266 0.9656138493024388 0.0887941827297259}", + "last_good_inputs": { + "arm": [ + 3.3409080392198027, -0.8870220388298283, + 1.5254084620176385, -1.9231421820972832, + -1.2847563613034223, 2.0561513627137287 + ] + } + }, + { + "cost": 9.411262089804117, + "configuration": { + "arm": [ + -0.37363727514732525, 4.228154687503318, + -0.8786377447820016, 1.3628735607678155, + -1.5707954921149754, -1.944433776353323 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.00000 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.21805714448347535 -1.4026550173203563e-07 0.9659258004564713 -0.13942176929094738}", + "last_good_inputs": { + "arm": [ + 3.720238660239432, -1.0319257377962388, + 1.389873624740437, -1.4189241568356177, + -1.5707959421529787, 2.149441939140327 + ] + } + }, + { + "cost": 12.217155214105867, + "configuration": { + "arm": [ + -4.054957000468022, 4.691979108106254, + 1.5090424105316922, -1.4886343878334247, + 4.712388456052036, 0.6574312110593298 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.13781 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.20220753395308266 0.16267080330447295 0.9656138576707752 0.015498414081915999}", + "last_good_inputs": { + "arm": [ + 3.7176076092174024, -1.2194697660457252, + 1.5865637740241592, -1.6566984067513701, + -1.2896040857511668, 2.4280027878506565 + ] + } + } + ] + }, + { + "seed": "start · sensitivity 25%", + "solutions": [ + { + "cost": 7.595345420660517, + "configuration": { + "arm": [ + -0.37363682058935765, 3.299185316937823, + 1.5090407266156414, -3.2374308916322834, + 1.5707966017573072, 1.1971577491399108 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 12.308515636952421, + "configuration": { + "arm": [ + -4.054959280733298, 4.691981729363136, + 1.5090371526698543, 4.794561580234015, + -1.5707934976553557, 0.6574258222925053 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.99532 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.1760522419933894 0.11737172194978762 0.9636413773795046 -0.16317102309470874}", + "last_good_inputs": { + "arm": [ + 3.7803907607056644, -1.2672196598680496, + 1.5871897615461483, -1.4253449895503996, + -1.5707958898849732, 2.44230445883852 + ] + } + }, + { + "cost": 14.59298101741498, + "configuration": { + "arm": [ + -4.054958104965163, 4.691978328243295, + 1.5090447714162092, -1.4886395016170153, + 4.712389923954626, -5.625753084232944 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.54942 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.2465597390500627 0.03532610160129595 0.9646737554088296 0.08581903781006316}", + "last_good_inputs": { + "arm": [ + 3.6045977197509513, -1.133519828147787, + 1.5854367844073876, -1.6542551263909924, + -1.2023375491434338, 2.0338010825172037 + ] + } + }, + { + "cost": 10.442163171355721, + "configuration": { + "arm": [ + -0.3736362408852365, 3.38723852927098, + 0.8786371144882761, 0.44651639748892413, + 4.712391445391104, -1.944431292106537 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.55669831245379, + "configuration": { + "arm": [ + -4.0549568462341625, -1.0865608884939733, + 0.8786354461594081, 1.778720203843172, + 1.5707991954901692, 3.7990223109100243 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.04822 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.1092502089000105 0.23507397718467393 0.9658168360983117 0.00156723161339659}", + "last_good_inputs": { + "arm": [ + 3.6924943222188613, -1.4768122695207169, + 1.5561549432576252, -1.4998467587622801, + -1.4205036200127812, 2.5725742064357733 + ] + } + }, + { + "cost": 14.592980907396463, + "configuration": { + "arm": [ + -4.054957344860978, 4.691978431032734, + 1.5090440180978506, -1.4886362789421717, + 4.712388686192901, -5.625754485752635 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.54942 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.24655978896438013 0.03532604626313074 0.9646737502940139 0.08581897467889783}", + "last_good_inputs": { + "arm": [ + 3.604597764324962, -1.1335198221200113, + 1.585436740231311, -1.654254937406974, + -1.2023376217282262, 2.0338010003293205 + ] + } + }, + { + "cost": 9.782627835198635, + "configuration": { + "arm": [ + -0.3736373811252857, -2.0550309345705693, + -0.8786373628366784, 1.3628736703218904, + 4.712389139914974, -1.94443396339343 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.24000 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.13985814809110605 -1.6518868714747136e-07 0.965381595881782 0.2201773665598771}", + "last_good_inputs": { + "arm": [ + 3.716408581461718, -1.5422258334442036, + 1.3877513256140042, -1.4163216067894138, + -1.0555748019054847, 2.14561185392124 + ] + } + }, + { + "cost": 9.031235481620874, + "configuration": { + "arm": [ + -0.3736384334988178, -1.5503894514765677, + -1.509038468970165, 4.630220989520482, + 1.5707982732505812, 1.1971573261348214 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.55013 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.2040181034733739 0.16498649227499337 0.9646721588399251 0.023742299405062714}", + "last_good_inputs": { + "arm": [ + 3.672672762286055, -1.5013750186016577, + 1.3056352147429242, -1.0866071251471394, + -1.28234620996438, 2.3903256325614444 + ] + } + }, + { + "cost": 5.026053463966332, + "configuration": { + "arm": [ + 5.909547588678186, -1.5503880493287112, + -1.5090405582383668, -1.652961112116691, + -4.712387529264098, 1.1971562232264599 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.41883 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.2229354992270385 -0.13211023152128135 0.964973395242968 0.04090227848229194}", + "last_good_inputs": { + "arm": [ + 4.267179080248705, -1.5018947618826393, + 1.2757803655314037, -1.6633942323785482, + -1.889508167858907, 2.377669841550035 + ] + } + }, + { + "cost": 7.027019827588537, + "configuration": { + "arm": [ + 2.22822687639683, -1.5912050198833916, + 1.5090406485161256, 4.794554595578257, + -1.5707940288257152, 0.6574289666414195 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.05826 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.1833602871181342 -1.8106706265957132e-07 0.9657941083715812 -0.1833596066277019}", + "last_good_inputs": { + "arm": [ + 3.930589237773281, -1.5041497771378856, + 1.5835771665177467, -1.1378084294717987, + -1.5707958226144654, 2.3597924019010317 + ] + } + } + ] + }, + { + "seed": "Seed cache 0", + "solutions": [ + { + "cost": 11.720547779862805, + "configuration": { + "arm": [ + -0.37363688574089604, 4.732797414736696, + -1.5090414485310972, -1.6529614658844136, + 1.5707946694852062, -5.086024984746715 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.04165 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.2002033833297705 -0.1308049529542472 0.9658316157491073 0.09988973723771245}", + "last_good_inputs": { + "arm": [ + 3.6325295131073525, -0.8683498871673909, + 1.2777105701949845, -1.663401499749285, + -1.254040560845673, 1.7449772802469656 + ] + } + }, + { + "cost": 9.870185882607693, + "configuration": { + "arm": [ + -0.37363760219471315, -2.0550319678977944, + -0.878635792050774, -4.92031399645751, 4.71238870043685, + -1.9444338852390362 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.45601 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.029238805062782657 -0.035111965518315344 0.964888214055706 0.25865609703297326}", + "last_good_inputs": { + "arm": [ + 3.823337874851116, -1.5288192240666894, + 1.4470034085745063, -1.8534051915178165, + -1.2063712703186267, 2.2525411608676507 + ] + } + }, + { + "cost": 8.903561181889621, + "configuration": { + "arm": [ + -0.3736377960798283, -2.0550300982575314, + -0.8786373667219466, 1.3628759200942964, + 4.712388398471403, 4.338745394665077 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.45599 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.14216818628662292 0.18406194931646835 0.9648882777348431 0.12206562624949942}", + "last_good_inputs": { + "arm": [ + 3.8233378636057793, -1.5288191156275541, + 1.4470033172435783, -1.4889801763578117, + -1.2063712878326225, 2.616965559102089 + ] + } + }, + { + "cost": 6.060877496877919, + "configuration": { + "arm": [ + -0.3736375971536865, -2.9840006675335493, + 1.509041267751084, -3.2374298506845616, + 1.570797159417415, 1.1971588789144747 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.411264681915078, + "configuration": { + "arm": [ + -0.37363911299788355, 4.2281552033938645, + -0.8786377250813987, 1.3628752126112356, + -1.5708024125046767, -1.9444355079735194 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.00001 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.21805708935487042 -2.10620450231282e-07 0.9659257768668863 -0.13942201894299042}", + "last_good_inputs": { + "arm": [ + 3.720238511115593, -1.031925695936699, + 1.3898736263389508, -1.418924022804463, + -1.5707965036758271, 2.149441798636057 + ] + } + }, + { + "cost": 9.624413475020816, + "configuration": { + "arm": [ + -0.37363523037423835, 3.38723672982546, + 0.8786408554654872, 0.44651615052379373, + 4.712391920984488, 4.338752298311065 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 11.59405448219547, + "configuration": { + "arm": [ + -0.3736374789561311, 3.299184964044252, + 1.5090410635607447, 3.045755103754528, + -4.71238898529748, -5.0860263013449725 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 4.653283557592129, + "configuration": { + "arm": [ + 2.2282275761079515, -0.15759319887614068, + -1.5090364235443385, 0.09584060094768622, + -1.5707948732936134, 0.6574343132701096 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 2.6721300670442427, + "configuration": { + "arm": [ + 2.2282275699693086, -1.5912053318382506, + 1.5090412877843629, -1.4886318163056027, + -1.5707957126331542, 0.6574324421618585 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": true + }, + { + "cost": 12.369755125953832, + "configuration": { + "arm": [ + 2.228227987758861, -1.591205196260639, + 1.5090409438630503, 4.794553253525228, + 4.712388658970535, -5.6257540125622985 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.71049 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.22759733392518666 -0.13311210914410027 0.964302480882932 0.024522343941371195}", + "last_good_inputs": { + "arm": [ + 3.987358033752709, -1.501246751784124, + 1.5860627535266103, -1.3356352226854022, + -1.2508189862942318, 2.096584266116173 + ] + } + } + ] + }, + { + "seed": "Seed cache 1", + "solutions": [ + { + "cost": 4.940600506854566, + "configuration": { + "arm": [ + 2.228227880739555, -1.0865608369826576, + 0.8786360631352572, -4.504464245317324, + -4.7123890352042705, 3.7990242138583254 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.68219 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.2045342844349829 0.16572847988351172 0.9643678998809297 0.026350543171158713}", + "last_good_inputs": { + "arm": [ + 3.911620293423477, -1.4587998164399707, + 1.5248834002406415, -1.9252385881138203, + -1.8591547645648137, 2.6291823374432086 + ] + } + }, + { + "cost": 7.169885117439539, + "configuration": { + "arm": [ + 2.2282281204449648, -1.0865608192244054, + 0.8786360468965729, 1.7787210440356322, + 1.5707969654893892, -2.4841615408996534 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.41882 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.2220979556646862 -0.13211006371973336 0.9649733478261935 0.04523126284599463}", + "last_good_inputs": { + "arm": [ + 3.8937118878192476, -1.454839825495246, + 1.518008426921905, -1.3152525643630952, + -1.2520836538984081, 2.0042028220010093 + ] + } + }, + { + "cost": 6.909571458910106, + "configuration": { + "arm": [ + 2.2282274531726487, -1.086560214715948, + 0.8786361355557025, -4.50446334780328, + 1.5707951543687042, -2.484161174831772 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.68221 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.03444656687398772 -0.16572884702872143 0.964367810807896 0.20332758979221824}", + "last_good_inputs": { + "arm": [ + 3.9116202541782057, -1.4587997593237028, + 1.524883406887929, -1.9252385057333041, + -1.2824373751837193, 2.052464837998224 + ] + } + }, + { + "cost": 2.6721303127781106, + "configuration": { + "arm": [ + 2.2282280481799845, -1.5912051588382323, + 1.509041009691801, -1.4886318720963434, + -1.5707964606233729, 0.6574316117219466 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": true + }, + { + "cost": 9.453028507106467, + "configuration": { + "arm": [ + 2.228228267348298, -1.591205108543095, + 1.5090410287645017, 4.794553222884604, + 4.712388157986595, 0.657432093529513 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.55397 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.2608619375987625 0.035336515629725916 0.9646633079793911 -0.01127308359002739}", + "last_good_inputs": { + "arm": [ + 3.973778215916953, -1.5019411926345665, + 1.5854681785540137, -1.2883124339601186, + -1.2047852552988298, 2.4029815084116186 + ] + } + }, + { + "cost": 7.169884247572685, + "configuration": { + "arm": [ + 2.2282282241241713, -1.0865616164644931, + 0.8786368329659168, 1.7787195046230013, + 1.570797105389879, -2.4841614831426826 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.41882 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.22209789557350534 -0.1321100795137162 0.9649733571895069 0.045231312020681265}", + "last_good_inputs": { + "arm": [ + 3.893711898337428, -1.454839906374675, + 1.5180085066680702, -1.315252720535391, + -1.2520836397056048, 2.004202827860412 + ] + } + }, + { + "cost": 5.304676701647277, + "configuration": { + "arm": [ + 2.228231024212523, -1.086557770806609, + 0.8786307443431408, 1.7787263815994745, + -4.712382472827537, 3.799024681008788 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.41879 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.028346537265101366 0.1321096338110106 0.9649734885024822 -0.22487704410799522}", + "last_good_inputs": { + "arm": [ + 3.8937121824043626, -1.4548395162354695, + 1.5180078889817017, -1.315252022871111, + -1.889507654887082, 2.6416275111801264 + ] + } + }, + { + "cost": 6.909571847106741, + "configuration": { + "arm": [ + 2.2282281188283224, -1.0865606700899575, + 0.8786352623313833, -4.504464233827058, + 1.570797047161925, -2.4841601671404274 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.68223 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{0.03444658327351555 -0.1657288552364049 0.9643677814151029 0.20332771973177627}", + "last_good_inputs": { + "arm": [ + 3.9116203152770352, -1.458799801121317, + 1.5248833267369044, -1.9252385870591582, + -1.282437201449076, 2.0524649304916323 + ] + } + }, + { + "cost": 5.2921781913441, + "configuration": { + "arm": [ + 2.2282281953733314, -1.0865608365517987, + 0.8786356094381066, 1.7787216452446057, + 1.5707978684224622, 3.7990248321109323 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.68223 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.2045346175140891 0.165728549731585 0.9643678218146037 -0.02635037553408718}", + "last_good_inputs": { + "arm": [ + 3.911620322302906, -1.4587998164004232, + 1.5248833585969417, -1.348521042603305, + -1.282437126067674, 2.629182394191033 + ] + } + }, + { + "cost": 7.027022720283706, + "configuration": { + "arm": [ + 2.2282276693996454, -1.5912042576780214, + 1.5090390214808445, 4.794558318842148, + -1.5707964638779344, 0.6574303648918124 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.05827 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.18336028241532354 -1.8302454607659293e-07 0.9657940774274197 -0.18335977431993633}", + "last_good_inputs": { + "arm": [ + 3.9305893024453553, -1.5041497149774476, + 1.5835770338274906, -1.1378081258269765, + -1.5707960212012482, 2.3597925159331026 + ] + } + } + ] + }, + { + "seed": "Seed cache 2", + "solutions": [ + { + "cost": 10.452718498945213, + "configuration": { + "arm": [ + -4.054955401851624, 4.691978534674866, + 1.509044968129802, -1.4886389915669158, + -1.5708008563473446, 0.6574322875545272 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.54941 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.15080926807317963 0.1846028568275688 0.9646738222114692 -0.11217293191428152}", + "last_good_inputs": { + "arm": [ + 3.6045978782668686, -1.133519816042232, + 1.5854367959430613, -1.654255096480647, + -1.5707962677413887, 2.402259483949185 + ] + } + }, + { + "cost": 12.656725364542774, + "configuration": { + "arm": [ + -4.0549578192335405, 5.196624547642287, + 0.878635959769509, 1.7787218016226285, + -4.712389975782374, -2.4841612429079234 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.54875 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.1571235454206217 0.05947015210416703 0.9646753347919005 -0.20292114460374994}", + "last_good_inputs": { + "arm": [ + 3.667380978766985, -1.155570237048327, + 1.553958796003328, -1.4892192318100717, + -1.7307845649159077, 2.2565727867930163 + ] + } + }, + { + "cost": 12.644224320174414, + "configuration": { + "arm": [ + -4.054957393545306, 5.1966243960129805, + 0.8786359873430881, 1.778720895029097, + 1.5707962239001343, -2.4841612882062454 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.04823 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.23123424628317177 0.09299394485672534 0.9658167942287625 -0.07127951705443919}", + "last_good_inputs": { + "arm": [ + 3.6924942960357665, -1.1762277883174521, + 1.556154969147585, -1.4998467256962869, + -1.420503762172181, 2.2719898058607666 + ] + } + }, + { + "cost": 11.533108172563667, + "configuration": { + "arm": [ + -4.054957089355044, 5.196624099772897, + 0.8786362716971128, -4.504464922529592, + 1.5707970549227184, 3.799024436348588 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.04822 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.10925018461737748 0.23507403161043885 0.9658168256871184 0.001567176806102512}", + "last_good_inputs": { + "arm": [ + 3.6924943105880783, -1.1762278024894313, + 1.556154982750941, -1.8004312324004834, + -1.420503722416471, 2.572574308115705 + ] + } + }, + { + "cost": 11.699140646152673, + "configuration": { + "arm": [ + -4.054958008737735, 5.196623246256051, + 0.878637803015962, 1.7787199724802372, + -4.712390537754422, 3.7990235541115527 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.41444 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.08582027013354858 0.172609329903885 0.9649834575967576 -0.1778983615128115}", + "last_good_inputs": { + "arm": [ + 3.7678341590721334, -1.2382004633171453, + 1.5627435544168338, -1.53172910443975, + -1.6919994755612668, 2.5606476926048805 + ] + } + }, + { + "cost": 11.533107075981833, + "configuration": { + "arm": [ + -4.0549605108821725, 5.196621511910969, + 0.8786414563468735, -4.504467515210506, + 1.5707908985086467, 3.7990186205891434 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.04821 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.10925007968784266 0.2350739658009587 0.9658168537105906 0.001567092653798962}", + "last_good_inputs": { + "arm": [ + 3.6924941469039103, -1.176227926291468, + 1.5561552307820254, -1.800431356433058, + -1.42050401693628, 2.5725740298926447 + ] + } + }, + { + "cost": 12.644224342485888, + "configuration": { + "arm": [ + -4.054957492279878, 5.196624415046245, + 0.8786360875369239, 1.7787209670334194, + 1.5707960765040105, -2.4841612137037243 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.04823 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.23123424802942874 0.09299394844580665 0.9658167930594639 -0.07127952255070176}", + "last_good_inputs": { + "arm": [ + 3.6924942913123533, -1.17622778740691, + 1.5561549739408087, -1.4998467222516356, + -1.4205037692235387, 2.2719898094249307 + ] + } + }, + { + "cost": 11.533108025795338, + "configuration": { + "arm": [ + -4.05495777019379, 5.1966236398357095, + 0.8786372380179417, -4.504465400588566, 1.570795694951, + 3.799023988394337 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.04822 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.10925016195314234 0.2350740352781181 0.965816827393055 0.0015671552852444565}", + "last_good_inputs": { + "arm": [ + 3.6924942780170893, -1.1762278244925992, + 1.5561550289792525, -1.8004312552705886, + -1.4205037874768465, 2.5725742866857946 + ] + } + }, + { + "cost": 14.669681422205146, + "configuration": { + "arm": [ + -4.054957471757169, 4.691981296113241, + 1.509039266110577, 4.794555861744819, + -1.5707962320119964, -5.6257540166944535 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 31.11111 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.22304861347625046 -1.504157593814175e-07 0.9633708560640112 -0.1488822007924179}", + "last_good_inputs": { + "arm": [ + 3.742720883957367, -1.2385696712411325, + 1.5868141159096578, -1.3954418308506653, + -1.570795992315396, 2.1719241515896437 + ] + } + }, + { + "cost": 12.308507099392822, + "configuration": { + "arm": [ + -4.054957165840207, 4.691979367071568, + 1.509041885704453, 4.794551853484722, + -1.5707971431767083, 0.6574310059625245 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.99530 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.1760520715857419 0.1173717663563741 0.963641406283751 -0.1631710043121274}", + "last_good_inputs": { + "arm": [ + 3.7803908390350385, -1.26721974736033, + 1.587189936843726, -1.4253453498003734, + -1.5707960249042825, 2.4423046508262987 + ] + } + } + ] + }, + { + "seed": "Seed cache 3", + "solutions": [ + { + "cost": 12.656725381306616, + "configuration": { + "arm": [ + -4.0549581472835134, 5.196623740266164, + 0.878637274282298, 1.778721069976685, + -4.712389362874262, -2.4841628851239563 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": true, + "checkpath_valid": false, + "first_error": "gripper orientation constraint violated dist: 30.54875 \u003e 30.00000 from: \u0026{-1.626604524119997e-07 -2.0686098148815533e-07 0.9999999999999631 6.966321910796991e-08} to: \u0026{6.123233995736757e-17 0 1 0} currPose: \u0026{-0.1571235601170187 0.05947012344658773 0.9646753391915126 -0.20292112070738536}", + "last_good_inputs": { + "arm": [ + 3.6673809620607365, -1.1555702781647035, + 1.553958862946109, -1.489219269069819, + -1.7307845337029946, 2.2565727031616443 + ] + } + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + } + ] + }, + { + "seed": "Seed cache 4", + "solutions": [ + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + } + ] + }, + { + "seed": "Seed cache 5", + "solutions": [ + { + "cost": 5.545324669870563, + "configuration": { + "arm": [ + 5.909547571957678, -2.895947960321564, + 0.8786363449002493, 0.44651540785931715, + -1.570796595376777, -1.9444357046340939 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.355438207468268, + "configuration": { + "arm": [ + 5.909547034137733, -2.9839986738080637, + 1.5090361759067779, 3.0457548303099764, + -4.712386015294655, 1.197158542590824 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 5.545322675345279, + "configuration": { + "arm": [ + 5.909547212391416, -2.8959479790261073, + 0.8786354361460105, 0.4465152870709163, + -1.5707977628772833, -1.9444333189243592 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.355440692502052, + "configuration": { + "arm": [ + 5.909547637346169, -2.9840005012409248, + 1.509041163858935, 3.0457557338574732, + -4.712388475009866, 1.197158702443589 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.84977032497073, + "configuration": { + "arm": [ + 5.909548275458104, -2.984000785010834, + 1.5090427117162177, 3.0457553609460173, + 1.570795361430671, -5.086025817021532 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.641456892727833, + "configuration": { + "arm": [ + 5.909547030258615, -2.8959485339089848, + 0.8786412497268992, -5.836669314982257, + -1.5707992656058585, -1.944435345109489 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.355440905935287, + "configuration": { + "arm": [ + 5.909548010393154, -2.9840009056960546, + 1.509041107409044, 3.045755627979551, -4.71238928634594, + 1.197160280388952 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.849771519365763, + "configuration": { + "arm": [ + 5.909548146910135, -2.9840010347509804, + 1.5090426508813939, 3.0457561249250027, + 1.5707957800785763, -5.086026676204755 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.862271370527074, + "configuration": { + "arm": [ + 5.90954771628707, -2.9840005398559786, + 1.5090412844395285, 3.0457555993657754, + -4.712388784934209, -5.086026586235518 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 8.795123175341008, + "configuration": { + "arm": [ + 5.909548806092467, -2.9840008613109203, + 1.5090427872599113, -3.237430336345951, + -4.712391293856063, -5.086029418691408 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + } + ] + }, + { + "seed": "Seed cache 6", + "solutions": [ + { + "cost": 5.5453226041462464, + "configuration": { + "arm": [ + 5.909547571382554, -2.895947709514948, + 0.8786353632800225, 0.4465147755944308, + -1.5707975334688073, -1.9444334028261199 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 8.795118651617662, + "configuration": { + "arm": [ + 5.909547733039979, -2.984000511933758, + 1.5090413442322628, -3.2374302071043797, + -4.712388157954265, -5.0860259257507785 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 7.3655422592851165, + "configuration": { + "arm": [ + 5.909547504811115, -2.895947755099408, + 0.8786357700680186, 0.4465153629979247, + 4.712388462465842, 4.338751183399966 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.342942083961242, + "configuration": { + "arm": [ + 5.909546960605698, -2.9840007293609156, + 1.5090421262972638, 3.0457565874990444, + 1.5707986480456289, 1.1971596018146577 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + } + ] + }, + { + "seed": "Seed cache 7", + "solutions": [ + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + }, + { + "cost": -1, + "configuration": null, + "configuration_valid": false + } + ] + }, + { + "seed": "Seed cache 8", + "solutions": [ + { + "cost": 6.863842615566254, + "configuration": { + "arm": [ + -0.37363741956504837, -2.895948012303375, + 0.8786360706788132, 0.44651510244398235, + -1.5707964770788696, -1.9444345798424452 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 8.399939843578572, + "configuration": { + "arm": [ + -0.37363576767696555, -2.8959492327269696, + 0.8786353696794545, 0.44651263291936855, + 4.712391193583996, 4.3387548269351255 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.663045502705751, + "configuration": { + "arm": [ + -0.3736373427012013, -2.9840001986080007, + 1.509039926898302, -3.2374302171738516, + 1.5707958778026947, -5.086025950109175 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 7.534474431453026, + "configuration": { + "arm": [ + -0.373636697556837, -2.98400113632787, + 1.509041179639572, 3.0457539251522894, + -4.712392751350702, 1.1971590512568255 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.164453044651578, + "configuration": { + "arm": [ + -0.37363764945334255, -2.895947722732025, + 0.878635564998343, -5.836670017524166, + 4.712388733873699, 4.338751214533779 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.675546726772701, + "configuration": { + "arm": [ + -0.3736370868526188, -2.9840010195072595, + 1.5090426180457208, -3.2374293249473336, + -4.712389946588151, -5.086026792175183 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 8.399939501082956, + "configuration": { + "arm": [ + -0.3736371484331296, -2.8959487347882567, + 0.8786368792596455, 0.44651295560922616, + 4.712389995669618, 4.338754562083356 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.0608767969493815, + "configuration": { + "arm": [ + -0.3736374277507892, -2.9840005537179954, + 1.509041209868871, -3.2374298439737763, + 1.570796133370334, 1.1971588945042113 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.327330361862147, + "configuration": { + "arm": [ + -0.37363765267174454, -2.895947855995494, + 0.8786362293654312, 0.4465150752101046, + 4.71238887224054, -1.9444344919974075 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.675546521834736, + "configuration": { + "arm": [ + -0.37363739933474105, -2.9840015988295483, + 1.5090432083984824, -3.2374285310070756, + -4.712389141109657, -5.086026738215047 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + } + ] + }, + { + "seed": "Seed cache 9", + "solutions": [ + { + "cost": 7.521977575255319, + "configuration": { + "arm": [ + -0.3736402795087443, -2.9840006943070794, + 1.5090400665710204, 3.045755772686928, + 1.5708020301509193, 1.1971626666913304 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 10.639460273755022, + "configuration": { + "arm": [ + -0.3736395000490705, -2.98399969893395, + 1.5090386422920332, 3.045756889571019, + 1.570800849752134, -5.086026673991787 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.327330277978287, + "configuration": { + "arm": [ + -0.37363796010698475, -2.8959480408654596, + 0.8786356405846724, 0.44651563221042906, + 4.712388253205772, -1.9444344731725225 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.07337741196238, + "configuration": { + "arm": [ + -0.3736379354116039, -2.984000672048243, + 1.509041166631658, -3.2374297405132175, + -4.712388160657027, 1.1971579458458856 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.863842417933423, + "configuration": { + "arm": [ + -0.3736375277729669, -2.895947451989923, + 0.8786353341581805, 0.44651544036255475, + -1.5707961234269643, -1.944434081887619 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.060877575677823, + "configuration": { + "arm": [ + -0.3736374369137544, -2.9840013933831404, + 1.5090435627589642, -3.2374290878284007, + 1.5707960381253063, 1.1971550503426307 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 9.663044174085472, + "configuration": { + "arm": [ + -0.37363641862781877, -2.9839992349995863, + 1.509038606672657, -3.2374311842043375, + 1.570794106445414, -5.086025549125828 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.0608789943784425, + "configuration": { + "arm": [ + -0.37363837092061586, -2.9840002546918174, + 1.5090416728090092, -3.2374295898366587, + 1.5707991401627615, 1.1971586305172148 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 5.532823687169383, + "configuration": { + "arm": [ + -0.37363772294233344, -2.89594749564896, + 0.8786365676753057, 0.44651697322850137, + -1.5707970636757966, 4.338749556969685 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + }, + { + "cost": 6.073378633959247, + "configuration": { + "arm": [ + -0.37363823448125033, -2.984001181400317, + 1.5090425829859018, -3.2374293228072735, + -4.712389020294109, 1.1971554420103376 + ], + "arm_origin": [], + "floor": [], + "floor_origin": [], + "gripper": [], + "gripper_origin": [], + "pallet": [], + "pallet_origin": [], + "pick-station": [], + "pick-station_origin": [] + }, + "configuration_valid": false, + "error": "obstacle constraint: violation between arm:upper_arm_link and pick-station geometries" + } + ] + } +] diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/ik-candidates.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/ik-candidates.ts new file mode 100644 index 000000000..b6567414e --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/ik-candidates.ts @@ -0,0 +1,85 @@ +import { + classifyIKSolution, + type IKSeedGroup, + type IKSolution, + type IKStatus, + isScored, +} from './parse-ik-solutions' + +export interface IKCandidate { + /** `seedIndex:indexInSeed` — stable across filter and sort, so rows keep their DOM. */ + id: string + seed: string + seedIndex: number + indexInSeed: number + status: IKStatus + solution: IKSolution +} + +export type IKStatusCounts = Record + +export interface IKSeedBucket { + seed: string + seedIndex: number + candidates: IKCandidate[] + counts: IKStatusCounts +} + +const emptyCounts = (): IKStatusCounts => ({ valid: 0, 'path-invalid': 0, invalid: 0 }) + +export const toCandidates = (groups: IKSeedGroup[]): IKCandidate[] => + groups.flatMap((group, seedIndex) => + group.solutions.map((solution, indexInSeed) => ({ + id: `${seedIndex}:${indexInSeed}`, + seed: group.seed, + seedIndex, + indexInSeed, + status: classifyIKSolution(solution), + solution, + })) + ) + +export const countByStatus = (candidates: IKCandidate[]): IKStatusCounts => { + const counts = emptyCounts() + for (const candidate of candidates) counts[candidate.status] += 1 + return counts +} + +export const filterByStatus = ( + candidates: IKCandidate[], + allowed: ReadonlySet +): IKCandidate[] => candidates.filter((candidate) => allowed.has(candidate.status)) + +/** Unscored candidates sort last; a raw -1 would otherwise lead the list. */ +export const sortByCost = (candidates: IKCandidate[]): IKCandidate[] => + candidates.toSorted((a, b) => { + const left = a.solution.cost + const right = b.solution.cost + if (isScored(left) && isScored(right)) return left - right + if (isScored(left)) return -1 + if (isScored(right)) return 1 + return 0 + }) + +export const groupBySeed = (candidates: IKCandidate[]): IKSeedBucket[] => { + const buckets = new Map() + + for (const candidate of candidates) { + let bucket = buckets.get(candidate.seedIndex) + if (!bucket) { + bucket = { + seed: candidate.seed, + seedIndex: candidate.seedIndex, + candidates: [], + counts: emptyCounts(), + } + buckets.set(candidate.seedIndex, bucket) + } + bucket.candidates.push(candidate) + bucket.counts[candidate.status] += 1 + } + + // Sorting rather than trusting Map insertion order, which follows the (possibly cost-sorted) + // candidate list rather than the file. + return [...buckets.values()].toSorted((a, b) => a.seedIndex - b.seedIndex) +} diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/inspect-ik-client.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/inspect-ik-client.ts new file mode 100644 index 000000000..b7a5b3c39 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/inspect-ik-client.ts @@ -0,0 +1,37 @@ +import { type IKSeedGroup, parseIKSolutions } from './parse-ik-solutions' + +export interface InspectIKResult { + /** Raw request JSON, parseable by `parsePlan`. */ + requestContent: string + seedGroups: IKSeedGroup[] +} + +/** + * MOCK — stands in for the RDK inspect-ik endpoint, which has not merged yet. + * + * It ignores the plan it is handed and always returns the bundled pirouette pair. That pairing is + * the point: the request and the solutions describe the same scene, which is what makes the + * candidate poses drawable at all. An arbitrary uploaded plan would not match the solutions. + * + * When the real route lands, this file is the only thing replaced — everything downstream consumes + * `InspectIKResult` and does not care where it came from. + */ +// The parameter is the whole point of the real signature, so it stays even though the mock has +// nothing to do with it — the real implementation drops the disable, not the argument. +// eslint-disable-next-line @typescript-eslint/no-unused-vars +export const inspectIK = async (planContent: string): Promise => { + // Dynamic so the two fixtures (~116 KB) land in their own chunk instead of the entry bundle. + const [request, solutions] = await Promise.all([ + import('./fixtures/pirouette-request.json?raw'), + import('./fixtures/pirouette-solutions.json?raw'), + ]) + + // The real call is a network round trip. Without a delay the loading state would never be + // visible once the chunk is cached, and the mockup is partly about how that reads. + await new Promise((resolve) => setTimeout(resolve, 500)) + + return { + requestContent: request.default, + seedGroups: parseIKSolutions(solutions.default), + } +} diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/namespace-snapshot.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/namespace-snapshot.ts new file mode 100644 index 000000000..65fc45b59 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/namespace-snapshot.ts @@ -0,0 +1,42 @@ +import { PoseInFrame, Transform } from '$lib/buf/common/v1/common_pb' +import { Snapshot } from '$lib/buf/draw/v1/snapshot_pb' + +import { transformsToSnapshot } from '../plan-to-snapshots' + +export const NAMESPACE_SEPARATOR = '/' + +export const namespacedFrameName = (prefix: string, frame: string): string => + `${prefix}${NAMESPACE_SEPARATOR}${frame}` + +/** + * Prefixes every frame name in a snapshot so two copies of one kinematic chain can coexist. + * + * `resolveOrphans` (src/lib/ecs/hierarchy.ts) indexes name -> entity across the entire world, one + * slot per name. Without a prefix the second copy's links parent onto the first copy's and the two + * collapse into a single chain. + * + * World-parented frames are re-parented onto `prefix` itself, which the caller spawns as the pose + * set's root: `parentTraits` yields no parent for 'world', so they would otherwise become a dozen + * separate tree roots rather than one group that can be hidden with a single `Invisible`. + */ +export const namespaceSnapshot = (snapshot: Snapshot, prefix: string): Snapshot => { + const transforms = snapshot.transforms.map((transform) => { + const parent = transform.poseInObserverFrame?.referenceFrame ?? '' + + return new Transform({ + referenceFrame: namespacedFrameName(prefix, transform.referenceFrame), + poseInObserverFrame: new PoseInFrame({ + referenceFrame: + parent === '' || parent === 'world' ? prefix : namespacedFrameName(prefix, parent), + pose: transform.poseInObserverFrame?.pose, + }), + physicalObject: transform.physicalObject, + // Each pose set is built by its own `parsedPlanToSnapshots` call, and + // `buildFrameDescriptors` mints fresh uuids per call, so these are already unique + // across sets and can carry through untouched. + uuid: transform.uuid, + }) + }) + + return transformsToSnapshot(transforms) +} diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/parse-ik-solutions.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/parse-ik-solutions.ts new file mode 100644 index 000000000..1e8431993 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/parse-ik-solutions.ts @@ -0,0 +1,99 @@ +import { z } from 'zod' + +/** Traffic-light class: `valid` is green, `path-invalid` yellow, `invalid` red. */ +export type IKStatus = 'valid' | 'path-invalid' | 'invalid' + +/** + * A seed the solver returned nothing for reports `cost: -1` with a null configuration and no + * error, rather than being omitted. The sentinel has to survive parsing: rendered literally it + * would lead every cost-sorted list with a value that isn't a cost. + */ +export const UNSCORED_COST = -1 + +export const isScored = (cost: number): boolean => cost !== UNSCORED_COST + +/** No configuration means there is nothing to draw — distinct from a configuration that collides. */ +export const hasSolution = (solution: Pick): boolean => + solution.configuration !== null + +const SolutionSchema = z + .object({ + cost: z.number(), + configuration: z.record(z.string(), z.array(z.number())).nullable(), + configuration_valid: z.boolean(), + checkpath_valid: z.boolean().optional(), + error: z.string().optional(), + first_error: z.string().optional(), + last_good_inputs: z.record(z.string(), z.array(z.number())).optional(), + }) + .transform((solution) => ({ + cost: solution.cost, + configuration: solution.configuration, + configurationValid: solution.configuration_valid, + // Absent means the path check never ran, which only happens when the configuration was + // already rejected — the same outcome as an explicit false. + checkpathValid: solution.checkpath_valid ?? false, + error: solution.error, + firstError: solution.first_error, + lastGoodInputs: solution.last_good_inputs, + })) + +export type IKSolution = z.infer + +const SeedGroupSchema = z.object({ + seed: z.string(), + solutions: z.array(SolutionSchema), +}) + +export type IKSeedGroup = z.infer + +export class IKSolutionsParseError extends Error { + constructor(message: string) { + super(message) + this.name = 'IKSolutionsParseError' + } +} + +export const IK_STATUS_LABEL: Record = { + valid: 'Valid', + 'path-invalid': 'Path blocked', + invalid: 'Goal blocked', +} + +/** Legend-length. The full reasoning lives in the MotionPlanFailure Debugging guide. */ +export const IK_STATUS_DESCRIPTION: Record = { + valid: 'End pose and the direct joint-space path are both clear.', + 'path-invalid': 'End pose is reachable, but interpolating straight to it collides.', + invalid: 'The end pose itself collides — the goal is the problem, not the route.', +} + +export const classifyIKSolution = (solution: { + configurationValid: boolean + checkpathValid: boolean +}): IKStatus => { + if (!solution.configurationValid) return 'invalid' + return solution.checkpathValid ? 'valid' : 'path-invalid' +} + +const IKSolutionsSchema = z + .string() + .transform((content, ctx) => { + try { + return JSON.parse(content) as unknown + } catch { + ctx.addIssue({ code: 'custom', message: 'IK solutions contain invalid JSON' }) + return z.NEVER + } + }) + .pipe(z.array(SeedGroupSchema).min(1, 'IK solutions contain no seed groups')) + +export const parseIKSolutions = (content: string): IKSeedGroup[] => { + const result = IKSolutionsSchema.safeParse(content) + if (result.success) return result.data + + const issue = result.error.issues[0] + const path = issue?.path.join('.') + throw new IKSolutionsParseError( + path ? `${issue!.message} (at ${path})` : (issue?.message ?? 'IK solutions are invalid') + ) +} diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/pose-sets.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/pose-sets.ts new file mode 100644 index 000000000..f8b6b528d --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/pose-sets.ts @@ -0,0 +1,113 @@ +import type { IKCandidate } from './ik-candidates' + +export type PoseKind = 'start' | 'lastGood' | 'end' + +export interface PoseStyle { + /** Scene colour, 0-1 floats. Three.js materials can't read Tailwind tokens. */ + rgb: { r: number; g: number; b: number } + opacity: number + /** The same role as a DOM class, kept alongside the scene colour so the two can't drift. */ + swatchClass: string +} + +export interface PoseSet { + kind: PoseKind + /** Frame-name namespace, and the name of the set's root entity. */ + prefix: string + label: string + configuration: Record + style: PoseStyle +} + +const START: PoseStyle = { + rgb: { r: 0.29, g: 0.47, b: 0.7 }, + opacity: 0.3, + swatchClass: 'bg-info-dark', +} + +const LAST_GOOD: PoseStyle = { + rgb: { r: 0.95, g: 0.66, b: 0.15 }, + opacity: 0.5, + swatchClass: 'bg-warning-dark', +} + +const END_VALID: PoseStyle = { + rgb: { r: 0.13, g: 0.65, b: 0.35 }, + opacity: 0.6, + swatchClass: 'bg-success-dark', +} + +const END_FAILED: PoseStyle = { + rgb: { r: 0.85, g: 0.25, b: 0.25 }, + opacity: 0.6, + swatchClass: 'bg-danger-dark', +} + +/** Deliberately desaturated: obstacles are the fixed world, not one of the candidate poses. */ +export const OBSTACLE_STYLE: PoseStyle = { + rgb: { r: 0.45, g: 0.45, b: 0.48 }, + opacity: 0.35, + swatchClass: 'bg-medium', +} + +export const POSE_KINDS: PoseKind[] = ['start', 'lastGood', 'end'] + +const PREFIX: Record = { + start: 'ik-start', + lastGood: 'ik-last-good', + end: 'ik-end', +} + +/** + * Which poses a candidate is worth drawing. A green candidate only needs start and end; a yellow + * one also needs the last configuration the path check accepted, which is where the useful + * information is — the end pose itself was fine. + */ +export const poseSetsForCandidate = ( + candidate: IKCandidate, + startConfiguration: Record +): PoseSet[] => { + const { solution, status } = candidate + const sets: PoseSet[] = [] + + // An empty start configuration would draw an all-zeros arm that reads as a real pose. Leaving + // the set out is honest about not knowing where the arm started. + if (Object.keys(startConfiguration).length > 0) { + sets.push({ + kind: 'start', + prefix: PREFIX.start, + label: 'Start', + configuration: startConfiguration, + style: START, + }) + } + + // A seed the solver returned nothing for has no pose to show — only the start. 35 of the 130 + // demo candidates are like this, all of them `cost: -1` with no error. + if (!solution.configuration) return sets + + if (status === 'path-invalid' && solution.lastGoodInputs) { + sets.push({ + kind: 'lastGood', + prefix: PREFIX.lastGood, + label: 'Last good', + configuration: solution.lastGoodInputs, + style: LAST_GOOD, + }) + } + + // Only a red candidate's end pose actually collides. A yellow one reached a valid configuration + // and failed on the way there, so drawing its end pose as a failure would point the user at the + // goal when the goal is fine. + const endFailed = status === 'invalid' + + sets.push({ + kind: 'end', + prefix: PREFIX.end, + label: endFailed ? 'Failing pose' : 'End', + configuration: solution.configuration, + style: endFailed ? END_FAILED : END_VALID, + }) + + return sets +} diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/relations.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/relations.ts new file mode 100644 index 000000000..1c7fe460c --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/relations.ts @@ -0,0 +1,11 @@ +import { relation } from 'koota' + +/** + * Members are the source, the set's root is the target, so destroying the root destroys the set — + * the same shape as the replayer's `PartOfPlan`. + * + * Kept separate from `PartOfPlan` rather than reused: the replayer queries that relation every + * scrub step to preserve user-edited display state, and inspection entities have no business + * showing up in it. + */ +export const PartOfInspection = relation({ autoDestroy: 'source' }) diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/useIKInspection.svelte.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/useIKInspection.svelte.ts new file mode 100644 index 000000000..626231ba0 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/useIKInspection.svelte.ts @@ -0,0 +1,313 @@ +import { useThrelte } from '@threlte/core' +import { onDestroy } from 'svelte' + +import type { Transform } from '$lib/buf/common/v1/common_pb' + +import { useWorld } from '$lib/ecs' + +import type { IKStatus } from './parse-ik-solutions' + +import { type ParsedPlan, parsePlan } from '../parse-plan' +import { parsedPlanToSnapshots } from '../plan-to-snapshots' +import { + destroyDrawnSet, + type DrawnSet, + drawObstacles, + drawPoseSet, + setDrawnSetVisible, +} from './draw-pose-set' +import { + countByStatus, + filterByStatus, + groupBySeed, + type IKCandidate, + type IKSeedBucket, + type IKStatusCounts, + sortByCost, + toCandidates, +} from './ik-candidates' +import { inspectIK } from './inspect-ik-client' +import { type PoseKind, type PoseSet, poseSetsForCandidate } from './pose-sets' +import { worldStateObstacleTransforms } from './world-state-obstacles' + +export type IKSortMode = 'seed' | 'cost' +export type IKInspectionStatus = 'idle' | 'loading' | 'ready' | 'error' + +const ALL_STATUSES: IKStatus[] = ['valid', 'path-invalid', 'invalid'] + +const allVisible = (): Record => ({ start: true, lastGood: true, end: true }) + +/** + * `parsePlan` drops everything but the frame system, and this mockup deliberately does not widen + * it — see viam-kb `visualization/local-viz-motion-plan-parsing-improvements.md` §3. The request is + * a single JSON object, so the two fields the panel needs are one parse away. + */ +const readRequestExtras = ( + content: string +): { startConfiguration: Record; worldState: unknown } => { + try { + const raw = JSON.parse(content) as { + start_state?: { configuration?: Record } + world_state?: unknown + } + return { + startConfiguration: raw.start_state?.configuration ?? {}, + worldState: raw.world_state, + } + } catch { + return { startConfiguration: {}, worldState: undefined } + } +} + +export interface IKInspectionContext { + readonly isOpen: boolean + readonly status: IKInspectionStatus + readonly error: string | null + readonly planName: string | null + readonly candidates: IKCandidate[] + readonly counts: IKStatusCounts + readonly totalCount: number + readonly statusFilter: ReadonlySet + readonly sortMode: IKSortMode + readonly visibleCandidates: IKCandidate[] + readonly seedBuckets: IKSeedBucket[] + readonly expandedSeeds: ReadonlySet + readonly selectedCandidate: IKCandidate | undefined + readonly poseSets: PoseSet[] + readonly poseVisibility: Record + setOpen: (open: boolean) => void + inspect: (planName: string, planContent: string) => Promise + select: (id: string | null) => void + toggleStatusFilter: (status: IKStatus) => void + resetStatusFilter: () => void + setSortMode: (mode: IKSortMode) => void + toggleSeed: (seedIndex: number) => void + setPoseVisible: (kind: PoseKind, visible: boolean) => void + clear: () => void +} + +// One inspection panel per app, so the context is published to a module-level variable like the +// replayer's own hook rather than through Svelte context. +let context: IKInspectionContext | undefined + +export const provideIKInspection = (): IKInspectionContext => { + const world = useWorld() + const { invalidate } = useThrelte() + + // Proto objects and ECS handles live outside $state — a Svelte 5 deep proxy over a Snapshot or + // a 130-element candidate list is pure overhead, and koota entities must not be proxied. + let parsedRequest: ParsedPlan | undefined + let startConfiguration: Record = {} + let obstacleTransforms: Transform[] = [] + const drawnSets = new Map() + let drawnObstacles: DrawnSet | undefined + + let isOpen = $state(false) + let status = $state('idle') + let error = $state(null) + let planName = $state(null) + let selectedId = $state(null) + let sortMode = $state('seed') + let poseVisibility = $state>(allVisible()) + let candidates = $state.raw([]) + let statusFilter = $state.raw>(new Set(ALL_STATUSES)) + let expandedSeeds = $state.raw>(new Set([0])) + + const counts = $derived(countByStatus(candidates)) + const filtered = $derived(filterByStatus(candidates, statusFilter)) + const visibleCandidates = $derived(sortMode === 'cost' ? sortByCost(filtered) : filtered) + const seedBuckets = $derived(groupBySeed(visibleCandidates)) + const selectedCandidate = $derived(candidates.find((candidate) => candidate.id === selectedId)) + const poseSets = $derived( + selectedCandidate ? poseSetsForCandidate(selectedCandidate, startConfiguration) : [] + ) + + const clearPoseSets = () => { + for (const drawn of drawnSets.values()) destroyDrawnSet(drawn) + drawnSets.clear() + } + + const teardownScene = () => { + clearPoseSets() + if (drawnObstacles) destroyDrawnSet(drawnObstacles) + drawnObstacles = undefined + invalidate() + } + + const ensureObstacles = () => { + if (drawnObstacles || obstacleTransforms.length === 0) return + drawnObstacles = drawObstacles(world, obstacleTransforms) + } + + const drawSelection = () => { + clearPoseSets() + + const candidate = candidates.find((entry) => entry.id === selectedId) + if (!candidate || !parsedRequest) { + invalidate() + return + } + + for (const poseSet of poseSetsForCandidate(candidate, startConfiguration)) { + // One call per pose set, each with a single-step trajectory: a solution's configuration + // is already exactly the shape of one trajectory step, and a fresh call mints fresh + // frame uuids so the sets stay independent. + const [snapshot] = parsedPlanToSnapshots({ + ...parsedRequest, + trajectory: [poseSet.configuration], + }) + if (!snapshot) continue + + const drawn = drawPoseSet(world, poseSet, snapshot) + setDrawnSetVisible(drawn, poseVisibility[poseSet.kind]) + drawnSets.set(poseSet.kind, drawn) + } + + invalidate() + } + + const clear = () => { + teardownScene() + parsedRequest = undefined + startConfiguration = {} + obstacleTransforms = [] + candidates = [] + selectedId = null + error = null + status = 'idle' + } + + const inspect = async (name: string, planContent: string) => { + clear() + planName = name + status = 'loading' + isOpen = true + + try { + const result = await inspectIK(planContent) + + parsedRequest = parsePlan(result.requestContent) + const extras = readRequestExtras(result.requestContent) + startConfiguration = extras.startConfiguration + obstacleTransforms = worldStateObstacleTransforms(extras.worldState) + candidates = toCandidates(result.seedGroups) + + ensureObstacles() + status = 'ready' + invalidate() + } catch (error_) { + console.warn('[InspectIK] inspection failed:', error_) + error = error_ instanceof Error ? error_.message : 'IK inspection failed.' + status = 'error' + } + } + + const setOpen = (open: boolean) => { + if (open === isOpen) return + isOpen = open + + // Geometry the user cannot reach from a closed panel is worse than a redraw on reopen. + if (open) { + ensureObstacles() + drawSelection() + } else { + teardownScene() + } + } + + context = { + get isOpen() { + return isOpen + }, + get status() { + return status + }, + get error() { + return error + }, + get planName() { + return planName + }, + get candidates() { + return candidates + }, + get counts() { + return counts + }, + get totalCount() { + return candidates.length + }, + get statusFilter() { + return statusFilter + }, + get sortMode() { + return sortMode + }, + get visibleCandidates() { + return visibleCandidates + }, + get seedBuckets() { + return seedBuckets + }, + get expandedSeeds() { + return expandedSeeds + }, + get selectedCandidate() { + return selectedCandidate + }, + get poseSets() { + return poseSets + }, + get poseVisibility() { + return poseVisibility + }, + setOpen, + inspect, + select: (id) => { + selectedId = id + drawSelection() + }, + toggleStatusFilter: (value) => { + const next = new Set(statusFilter) + if (next.has(value)) next.delete(value) + else next.add(value) + statusFilter = next + }, + resetStatusFilter: () => { + statusFilter = new Set(ALL_STATUSES) + }, + setSortMode: (mode) => { + sortMode = mode + }, + toggleSeed: (seedIndex) => { + const next = new Set(expandedSeeds) + if (next.has(seedIndex)) next.delete(seedIndex) + else next.add(seedIndex) + expandedSeeds = next + }, + setPoseVisible: (kind, visible) => { + poseVisibility = { ...poseVisibility, [kind]: visible } + const drawn = drawnSets.get(kind) + if (drawn) setDrawnSetVisible(drawn, visible) + invalidate() + }, + clear, + } + + // Only clear if we still own the singleton + const instance = context + onDestroy(() => { + clear() + if (context === instance) context = undefined + }) + + return context +} + +export const useIKInspection = (): IKInspectionContext => { + if (context === undefined) { + throw new Error('useIKInspection must be used with a mounted ') + } + + return context +} diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/world-state-obstacles.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/world-state-obstacles.ts new file mode 100644 index 000000000..84eb10439 --- /dev/null +++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/world-state-obstacles.ts @@ -0,0 +1,71 @@ +import type { JsonValue } from '@bufbuild/protobuf' + +import { UuidTool } from 'uuid-tool' + +import { Geometry, PoseInFrame, Transform } from '$lib/buf/common/v1/common_pb' +// Not the generated Pose: this one defaults to the 0,0,1,0 orientation vector, where the proto's +// zero value is a degenerate all-zero axis. +import { Pose } from '$lib/math' + +/** Frame names here are shared with `frame_system.frames` (`pallet`, `pick-station`) and the ECS + * name index is global, so obstacles get their own namespace. */ +export const OBSTACLE_PREFIX = 'obstacle' + +type WorldStateJson = { + obstacles?: Array<{ referenceFrame?: string; geometries?: unknown[] }> +} + +const newUuid = (): Uint8Array => + Uint8Array.from(UuidTool.toBytes(crypto.randomUUID())) + +/** + * Builds drawable transforms for obstacles that live only in `world_state`. + * + * `world_state` is protobuf-JSON (viam.common.v1.WorldState): geometry type is a oneof keyed by + * field name, dimensions sit under `box.dimsMm`, the pose is an orientation vector in `center`, and + * the label is lowercase. That is a different encoding from the Go-marshalled geometry + * `parseGeometry` in build-frame-descriptors.ts reads, which is why this decodes through + * protobuf-es rather than reusing it. + * + * Obstacles RDK already flattened into `frame_system.frames` carry inline geometry and render + * through the normal descriptor path; this covers only the ones it did not flatten. + */ +export const worldStateObstacleTransforms = (worldState: unknown): Transform[] => { + const obstacles = (worldState as WorldStateJson | undefined)?.obstacles + if (!Array.isArray(obstacles)) return [] + + const transforms: Transform[] = [] + + for (const obstacle of obstacles) { + const geometries = obstacle?.geometries + if (!Array.isArray(geometries)) continue + + for (const [index, raw] of geometries.entries()) { + let geometry: Geometry + try { + // protobuf-es rejects unknown fields by default, which would drop every obstacle the + // moment RDK adds a field ahead of our vendored proto. + geometry = Geometry.fromJson(raw as JsonValue, { ignoreUnknownFields: true }) + } catch (error) { + console.warn('[InspectIK] skipping unreadable world_state geometry:', error) + continue + } + + transforms.push( + new Transform({ + referenceFrame: `${OBSTACLE_PREFIX}/${geometry.label || `geometry-${index}`}`, + // The geometry's own `center` already positions it within the obstacle's + // reference frame, so the transform itself is identity. + poseInObserverFrame: new PoseInFrame({ + referenceFrame: obstacle.referenceFrame || 'world', + pose: new Pose(), + }), + physicalObject: geometry, + uuid: newUuid(), + }) + ) + } + } + + return transforms +}