diff --git a/src/lib/components/overlay/FloatingPanel.svelte b/src/lib/components/overlay/FloatingPanel.svelte
index 0cbe941c2..79e7ef59f 100644
--- a/src/lib/components/overlay/FloatingPanel.svelte
+++ b/src/lib/components/overlay/FloatingPanel.svelte
@@ -7,10 +7,19 @@
import { Icon } from '@viamrobotics/prime-core'
import * as floatingPanel from '@zag-js/floating-panel'
import { normalizeProps, useMachine } from '@zag-js/svelte'
+ import { untrack } from 'svelte'
interface Props {
title?: string
defaultSize?: { width: number; height: number }
+ /**
+ * Resizes an already-open panel, for content that changes shape at runtime. `defaultSize` is
+ * zag's uncontrolled initial value and is ignored after mount, and `api.setSize` shares the
+ * resize-drag's action so it no-ops outside a gesture — driving the machine's controlled
+ * `size` is what actually moves the panel. Pass a stable object reference: the size is
+ * re-applied whenever it changes, discarding a manual resize.
+ */
+ size?: { width: number; height: number }
minSize?: { width: number; height: number }
defaultPosition?: { x: number; y: number }
exitable?: boolean
@@ -29,6 +38,7 @@
let {
title = '',
defaultSize = { width: 700, height: 500 },
+ size,
defaultPosition,
exitable = true,
resizable = false,
@@ -43,9 +53,21 @@
const { dom } = useThrelte()
const id = $props.id()
+
+ // Stays undefined unless the caller opted in, which leaves the machine uncontrolled and every
+ // other panel behaving exactly as before.
+ let currentSize = $state(untrack(() => size))
+
+ // Not derived state: `size` is a push from the caller and a resize drag is a push from the
+ // machine, so whichever moved last wins — an assignment, not a computation.
+ $effect(() => {
+ if (size) currentSize = size
+ })
+
const floatingPanelService = useMachine(floatingPanel.machine, () => ({
id,
defaultSize,
+ size: currentSize,
defaultPosition: defaultPosition ?? {
x: dom.clientWidth / 2 - defaultSize.width / 2 + dom.clientLeft,
y: dom.clientHeight / 2 - defaultSize.width / 2 + dom.clientTop,
@@ -56,6 +78,11 @@
persistRect,
open: isOpen,
...props,
+ onSizeChange: (details: floatingPanel.SizeChangeDetails) => {
+ // Controlled panels have to adopt the drag's own result, or a resize snaps straight back.
+ if (currentSize) currentSize = details.size
+ props.onSizeChange?.(details)
+ },
}))
const api = $derived(floatingPanel.connect(floatingPanelService, normalizeProps))
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/MotionPlanReplayerScrubber.svelte b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerScrubber.svelte
index 714540669..a0d17ded6 100644
--- a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerScrubber.svelte
+++ b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerScrubber.svelte
@@ -1,160 +1,12 @@
-{#if ctx.totalSteps > 0}
-
-
seek(Number((e.currentTarget as HTMLInputElement).value))}
- />
-
-
-
seek(0)}
- disabled={ctx.currentStep <= 0}
- aria-label="Jump to start"
- title="Jump to start">
-
-
stepOnce('prev')}
- disabled={ctx.currentStep <= 0}
- aria-label="Previous step"
- title="Previous step">
-
-
{#if isPlaying} {:else} {/if}
-
-
stepOnce('next')}
- disabled={atEnd}
- aria-label="Next step"
- title="Next step">
-
-
seek(lastStepIdx)}
- disabled={atEnd}
- aria-label="Jump to end"
- title="Jump to end">
-
-
- {ctx.currentStep + 1} / {ctx.totalSteps}
-
-
-
-{/if}
-
-
+ ctx.setStep(index)}
+/>
diff --git a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte
index b59740e34..cbb8a8076 100644
--- a/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte
+++ b/src/lib/plugins/MotionPlanReplayer/MotionPlanReplayerUI.svelte
@@ -1,13 +1,15 @@
+
+{#if totalSteps > 0}
+
+
+ seek(Number((e.currentTarget as HTMLInputElement).value))}
+ />
+ {#if markOffset}
+
+ {/if}
+
+
+
+
seek(0)}
+ disabled={currentStep <= 0}
+ aria-label="Jump to start"
+ title="Jump to start">
+
+
stepOnce('prev')}
+ disabled={currentStep <= 0}
+ aria-label="Previous step"
+ title="Previous step">
+
+
{#if isPlaying} {:else} {/if}
+
+
stepOnce('next')}
+ disabled={atEnd}
+ aria-label="Next step"
+ title="Next step">
+
+
seek(lastStepIdx)}
+ disabled={atEnd}
+ aria-label="Jump to end"
+ title="Jump to end">
+
+
+ {#if markLabel && currentStep === markIndex}
+ {markLabel}
+ {/if}
+ {currentStep + 1} / {totalSteps}
+
+
+
+{/if}
+
+
diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateDetail.svelte b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateDetail.svelte
new file mode 100644
index 000000000..e47b531a2
--- /dev/null
+++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateDetail.svelte
@@ -0,0 +1,181 @@
+
+
+
+
+
+ {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]}
+ setPoseVisible(poseSet.kind, !shown)}
+ >
+
+ {poseSet.label}
+
+ {/each}
+
+ {#if pathLength > 0}
+ {@const shown = poseVisibility.path}
+ setPoseVisible('path', !shown)}
+ >
+
+ Path
+
+ {/if}
+
+
+ {#if pathLength > 0}
+
+
+ Interpolate
+ setPathSteps(Number((event.target as HTMLInputElement).value))}
+ />
+ steps from start to end
+
+
+
+
+ {:else if candidate.status === 'invalid' && solved}
+
+ No path to walk — this candidate's end pose is itself in collision, so the goal is the problem
+ rather than the route to it.
+
+ {/if}
+
+ {#if !hasStartPose}
+
+ Start pose unavailable — the request carries no start configuration.
+
+ {/if}
+
+ {#if detail}
+
+ {: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..6d59ed723
--- /dev/null
+++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKCandidateList.svelte
@@ -0,0 +1,139 @@
+
+
+
+
+
+ {ctx.totalCount} candidates
+ {ctx.seedBuckets.length} seeds
+
+
+
+ {#each ORDER as status (status)}
+ {@const active = ctx.statusFilter.has(status)}
+
ctx.toggleStatusFilter(status)}
+ >
+
+ {ctx.counts[status]}
+ {IK_STATUS_LABEL[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)}
+ ctx.setSortMode(mode)}
+ >
+ {label}
+
+ {/each}
+
+
+
+
+
+ {#if ctx.visibleCandidates.length === 0}
+
+
No candidates match this filter.
+
ctx.resetStatusFilter()}
+ >
+ Show all
+
+
+ {: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 @@
+
+
+
+
+
+
+ {#if isScored(cost)}
+ {cost.toFixed(4)}
+ {:else}
+ —
+ {/if}
+
+ {#if showSeed}
+ {candidate.seed}
+ {/if}
+
+
+ {candidate.seed} · #{candidate.indexInSeed + 1}{isScored(cost) ? '' : ' · unscored'}
+
+
diff --git a/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKInspectionView.svelte b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKInspectionView.svelte
new file mode 100644
index 000000000..8dee2f201
--- /dev/null
+++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/IKInspectionView.svelte
@@ -0,0 +1,62 @@
+
+
+
+
+
ctx.exit()}
+ >
+
+ Back to plans
+
+
+ {#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 @@
+
+
+
+
+
+ {bucket.seed}
+ {#each ORDER as status (status)}
+
+
+ {bucket.counts[status]}
+
+ {/each}
+
+
+ {#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..c23aabca8
--- /dev/null
+++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/draw-pose-set.ts
@@ -0,0 +1,111 @@
+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))
+}
+
+/**
+ * Spawns a set's root on its own. It has to exist before the first reconcile so `resolveOrphans` can
+ * parent the set's top-level frames onto it within the same flush.
+ */
+export const createDrawnSet = (world: World, rootName: string): DrawnSet => ({
+ root: world.spawn(traits.Name(rootName)),
+ entityMap: new Map(),
+})
+
+/**
+ * Draws one snapshot into an existing set, reusing the entities whose `Transform.uuid` it has seen
+ * before. Stepping a set along a trajectory therefore moves the arm in place rather than respawning
+ * it, since every snapshot of one path is built from the same frame descriptors.
+ */
+export const applySnapshot = (
+ world: World,
+ drawn: DrawnSet,
+ snapshot: Snapshot,
+ style: PoseStyle
+): void => {
+ // Reconcile garbage-collects only entities present in the map it is handed, so a set can never
+ // sweep anything outside itself.
+ const result = reconcileSnapshotEntities(world, snapshot, drawn.entityMap)
+
+ for (const spawned of result.spawned) {
+ spawned.entity.add(inspectRelations.PartOfInspection(drawn.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)
+ }
+
+ // Colour survives reconcile, but its metadata pass resets Opacity to the default — so opacity has
+ // to be re-applied to entities that merely survived the step, not only to newly spawned ones.
+ for (const entry of [...result.spawned, ...result.updated]) {
+ setOrAddOpacity(entry.entity, style.opacity)
+ }
+
+ drawn.entityMap = result.current
+}
+
+const drawSnapshot = (
+ world: World,
+ rootName: string,
+ snapshot: Snapshot,
+ style: PoseStyle
+): DrawnSet => {
+ const drawn = createDrawnSet(world, rootName)
+ applySnapshot(world, drawn, snapshot, style)
+ return drawn
+}
+
+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/interpolate-configuration.ts b/src/lib/plugins/MotionPlanReplayer/inspect-ik/interpolate-configuration.ts
new file mode 100644
index 000000000..cab906565
--- /dev/null
+++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/interpolate-configuration.ts
@@ -0,0 +1,127 @@
+export const MIN_PATH_STEPS = 2
+export const MAX_PATH_STEPS = 200
+export const DEFAULT_PATH_STEPS = 25
+
+/** A uniform step is treated as already being the last-good keyframe within this fraction. */
+const SPLICE_EPSILON = 1e-4
+
+export interface PathStep {
+ configuration: Record
+ /** Position along the start → end segment, 0 to 1. */
+ fraction: number
+ isLastGood: boolean
+}
+
+const lerp = (from: number, to: number, fraction: number): number => from + (to - from) * fraction
+
+/**
+ * Component arrays are unioned rather than intersected: the request's `start_state.configuration`
+ * lists every component (most with empty arrays) while `last_good_inputs` carries only the arm, and
+ * either side may be the shorter one.
+ */
+const lerpConfiguration = (
+ from: Record,
+ to: Record,
+ fraction: number
+): Record => {
+ const configuration: Record = {}
+
+ for (const key of new Set([...Object.keys(from), ...Object.keys(to)])) {
+ const a = from[key] ?? []
+ const b = to[key] ?? []
+ const length = Math.max(a.length, b.length)
+ const values: number[] = []
+
+ for (let index = 0; index < length; index += 1) {
+ values.push(lerp(a[index] ?? b[index] ?? 0, b[index] ?? a[index] ?? 0, fraction))
+ }
+
+ configuration[key] = values
+ }
+
+ return configuration
+}
+
+/**
+ * Recovers where along the segment the path check stopped accepting.
+ *
+ * RDK derives `last_good_inputs` by walking the straight joint-space line, so every joint yields the
+ * same fraction. Reading it off the joint that travels furthest keeps the division away from a
+ * near-zero denominator.
+ */
+const lastGoodFraction = (
+ from: Record,
+ to: Record,
+ lastGood: Record
+): number | null => {
+ let widestDelta = 0
+ let fraction: number | null = null
+
+ for (const [key, values] of Object.entries(lastGood)) {
+ const a = from[key]
+ const b = to[key]
+ if (!a || !b) continue
+
+ for (const [index, value] of values.entries()) {
+ const start = a[index]
+ const end = b[index]
+ if (start === undefined || end === undefined) continue
+
+ const delta = Math.abs(end - start)
+ if (delta <= widestDelta) continue
+
+ widestDelta = delta
+ fraction = (value - start) / (end - start)
+ }
+ }
+
+ if (fraction === null || !Number.isFinite(fraction)) return null
+
+ return Math.min(1, Math.max(0, fraction))
+}
+
+/**
+ * Samples the straight joint-space line from `start` to `end` — the same path RDK's check-path
+ * walks, which is why a failing candidate's collision shows up on it.
+ *
+ * `lastGood` is spliced in at its true fraction rather than left to land between samples. These
+ * paths fail in the first few percent of travel, so a uniform sampling steps clean over the
+ * boundary and the one configuration worth landing on becomes unreachable.
+ */
+export const buildInterpolatedPath = (
+ start: Record,
+ end: Record,
+ steps: number,
+ lastGood?: Record
+): PathStep[] => {
+ const count = Math.min(MAX_PATH_STEPS, Math.max(MIN_PATH_STEPS, Math.trunc(steps)))
+ const path: PathStep[] = []
+
+ for (let index = 0; index < count; index += 1) {
+ const fraction = index / (count - 1)
+ path.push({
+ configuration: lerpConfiguration(start, end, fraction),
+ fraction,
+ isLastGood: false,
+ })
+ }
+
+ const fraction = lastGood ? lastGoodFraction(start, end, lastGood) : null
+ if (fraction === null) return path
+
+ const coincident = path.find((step) => Math.abs(step.fraction - fraction) < SPLICE_EPSILON)
+ if (coincident) {
+ coincident.isLastGood = true
+ return path
+ }
+
+ path.push({
+ // Interpolation supplies the components `last_good_inputs` omits; its own values, being the
+ // exact ones RDK accepted, win where it has them.
+ configuration: { ...lerpConfiguration(start, end, fraction), ...lastGood },
+ fraction,
+ isLastGood: true,
+ })
+
+ return path.toSorted((a, b) => a.fraction - b.fraction)
+}
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..358d672b5
--- /dev/null
+++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/pose-sets.ts
@@ -0,0 +1,136 @@
+import type { IKCandidate } from './ik-candidates'
+
+export type PoseKind = 'start' | 'lastGood' | 'end' | 'path'
+
+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
+}
+
+/** Onion-skin of PATH_STYLE: same hue, faded, so a pose and where it came from read as one arm. */
+const START: PoseStyle = {
+ rgb: { r: 0, g: 0.47, b: 1 },
+ opacity: 0.25,
+ swatchClass: 'bg-info-medium',
+}
+
+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',
+}
+
+/**
+ * The replayer's own arm colour and opacity (useMotionPlanReplayer.svelte.ts), because this is the
+ * same thing: an arm being scrubbed along a trajectory. Staying out of the traffic-light palette
+ * also keeps it from reading as a verdict on the candidate.
+ */
+export const PATH_STYLE: PoseStyle = {
+ rgb: { r: 0, g: 0.47, b: 1 },
+ opacity: 0.6,
+ swatchClass: 'bg-info-bright',
+}
+
+/** 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 PREFIX: Record = {
+ start: 'ik-start',
+ lastGood: 'ik-last-good',
+ end: 'ik-end',
+ path: 'ik-path',
+}
+
+/**
+ * A red candidate's end pose is itself in collision, so interpolating towards it only animates the
+ * arm into a goal already known to be unreachable — the failure is the goal, not the route.
+ */
+export const supportsInterpolation = (
+ candidate: IKCandidate,
+ startConfiguration: Record
+): boolean =>
+ candidate.status !== 'invalid' &&
+ candidate.solution.configuration !== null &&
+ Object.keys(startConfiguration).length > 0
+
+/**
+ * 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..5256bb356
--- /dev/null
+++ b/src/lib/plugins/MotionPlanReplayer/inspect-ik/useIKInspection.svelte.ts
@@ -0,0 +1,438 @@
+import { useThrelte } from '@threlte/core'
+import { onDestroy } from 'svelte'
+
+import type { Transform } from '$lib/buf/common/v1/common_pb'
+import type { Snapshot } from '$lib/buf/draw/v1/snapshot_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 {
+ applySnapshot,
+ createDrawnSet,
+ 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 {
+ buildInterpolatedPath,
+ DEFAULT_PATH_STEPS,
+ MAX_PATH_STEPS,
+ MIN_PATH_STEPS,
+} from './interpolate-configuration'
+import { namespaceSnapshot } from './namespace-snapshot'
+import {
+ PATH_STYLE,
+ type PoseKind,
+ type PoseSet,
+ poseSetsForCandidate,
+ PREFIX,
+ supportsInterpolation,
+} 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,
+ path: 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 {
+ /** Whether the replayer panel is currently handed over to IK inspection. */
+ readonly isActive: 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
+ /** Requested interpolation resolution; the built path can be one longer once last-good splices in. */
+ readonly pathSteps: number
+ readonly pathStep: number
+ /** 0 when the selected candidate has no interpolatable path. */
+ readonly pathLength: number
+ readonly lastGoodStepIndex: number | null
+ inspect: (planName: string, planContent: string) => Promise
+ exit: () => void
+ 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
+ setPathSteps: (steps: number) => void
+ setPathStep: (index: number) => 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 pathSnapshots: Snapshot[] = []
+
+ let isActive = $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 pathSteps = $state(DEFAULT_PATH_STEPS)
+ let pathStep = $state(0)
+ let pathLength = $state(0)
+ let lastGoodStepIndex = $state(null)
+ 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 clearPath = () => {
+ const drawn = drawnSets.get('path')
+ if (drawn) destroyDrawnSet(drawn)
+ drawnSets.delete('path')
+ pathSnapshots = []
+ pathLength = 0
+ pathStep = 0
+ lastGoodStepIndex = null
+ }
+
+ const clearPoseSets = () => {
+ clearPath()
+ for (const drawn of drawnSets.values()) destroyDrawnSet(drawn)
+ drawnSets.clear()
+ }
+
+ const applyPathStep = (index: number) => {
+ const drawn = drawnSets.get('path')
+ const snapshot = pathSnapshots[index]
+ if (!drawn || !snapshot) return
+
+ applySnapshot(world, drawn, snapshot, PATH_STYLE)
+ pathStep = index
+ }
+
+ /**
+ * Rebuilds the scrubbable straight-line path for a candidate. Called on selection and whenever
+ * the resolution changes; the static pose ghosts are left alone either way.
+ */
+ const rebuildPath = (candidate: IKCandidate | undefined) => {
+ // A resolution change should leave the user roughly where they were looking, so position is
+ // held as a fraction rather than an index. A candidate change arrives via drawSelection,
+ // which has already cleared the path — so this reads 0 there and the new path starts at its
+ // beginning.
+ const heldFraction = pathLength > 1 ? pathStep / (pathLength - 1) : 0
+ clearPath()
+
+ const end = candidate?.solution.configuration
+ if (!candidate || !end || !parsedRequest) return
+ if (!supportsInterpolation(candidate, startConfiguration)) return
+
+ const path = buildInterpolatedPath(
+ startConfiguration,
+ end,
+ pathSteps,
+ candidate.solution.lastGoodInputs
+ )
+
+ pathSnapshots = parsedPlanToSnapshots({
+ ...parsedRequest,
+ trajectory: path.map((step) => step.configuration),
+ }).map((snapshot) => namespaceSnapshot(snapshot, PREFIX.path))
+
+ pathLength = pathSnapshots.length
+ const markIndex = path.findIndex((step) => step.isLastGood)
+ lastGoodStepIndex = markIndex === -1 ? null : markIndex
+
+ const drawn = createDrawnSet(world, PREFIX.path)
+ drawnSets.set('path', drawn)
+ setDrawnSetVisible(drawn, poseVisibility.path)
+ applyPathStep(Math.round(heldFraction * (pathLength - 1)))
+ }
+
+ const teardownScene = () => {
+ clearPoseSets()
+ if (drawnObstacles) destroyDrawnSet(drawnObstacles)
+ drawnObstacles = undefined
+ invalidate()
+ }
+
+ const ensureObstacles = () => {
+ if (drawnObstacles || obstacleTransforms.length === 0) return
+ drawnObstacles = drawObstacles(world, obstacleTransforms)
+ }
+
+ /**
+ * A candidate with a scrubbable path has the endpoint ghosts hidden: they are three more arms
+ * stacked around the one the user is actually moving, and the path already passes through every
+ * one of them. A red candidate has no path, so its ghosts are the whole picture.
+ */
+ const defaultVisibility = (candidate: IKCandidate | undefined): Record => {
+ const ghosts = !(candidate && supportsInterpolation(candidate, startConfiguration))
+ return { start: ghosts, lastGood: ghosts, end: ghosts, path: true }
+ }
+
+ const drawSelection = () => {
+ clearPoseSets()
+
+ const candidate = candidates.find((entry) => entry.id === selectedId)
+ // Reset per selection rather than carrying toggles over: the right default differs between a
+ // candidate that has a path and one that does not.
+ poseVisibility = defaultVisibility(candidate)
+
+ 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)
+ }
+
+ rebuildPath(candidate)
+ invalidate()
+ }
+
+ const clear = () => {
+ teardownScene()
+ parsedRequest = undefined
+ startConfiguration = {}
+ obstacleTransforms = []
+ candidates = []
+ selectedId = null
+ error = null
+ status = 'idle'
+ }
+
+ const exit = () => {
+ clear()
+ planName = null
+ isActive = false
+ }
+
+ const inspect = async (name: string, planContent: string) => {
+ clear()
+ planName = name
+ status = 'loading'
+ isActive = 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'
+ }
+ }
+
+ context = {
+ get isActive() {
+ return isActive
+ },
+ 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
+ },
+ get pathSteps() {
+ return pathSteps
+ },
+ get pathStep() {
+ return pathStep
+ },
+ get pathLength() {
+ return pathLength
+ },
+ get lastGoodStepIndex() {
+ return lastGoodStepIndex
+ },
+ inspect,
+ exit,
+ 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()
+ },
+ setPathSteps: (steps) => {
+ const next = Math.min(MAX_PATH_STEPS, Math.max(MIN_PATH_STEPS, Math.trunc(steps)))
+ if (!Number.isFinite(next) || next === pathSteps) return
+ pathSteps = next
+ rebuildPath(candidates.find((candidate) => candidate.id === selectedId))
+ invalidate()
+ },
+ setPathStep: (index) => {
+ applyPathStep(Math.max(0, Math.min(pathLength - 1, index)))
+ 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
+}