Repository navigation
[Bot] Place Articraft catalog assets using rest-pose bounds #8
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Merged
Merged
Changes from all commits
Commits
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,73 @@ | ||
| import { describe, expect, test } from 'bun:test' | ||
| import { bottomCentreFrame, objExtent, urdfRestBounds } from './index' | ||
|
|
||
| // A 1 × 0.5 × 0.2 m base standing on z = 0, a lid hinged on its top edge | ||
| // (joint origin rotated a quarter turn about Z) carrying a Z-up cylinder, and | ||
| // a handle modelled in millimetres (mesh scale 0.001). | ||
| const URDF = `<?xml version="1.0"?> | ||
| <!-- comment <link name="ignored"/> --> | ||
| <robot name="box"> | ||
| <material name="red"><color rgba="1 0 0 1"/></material> | ||
| <link name="base"> | ||
| <visual name="body"> | ||
| <origin xyz="0 0 0.1" rpy="0 0 0"/> | ||
| <geometry><box size="1 0.5 0.2"/></geometry> | ||
| <material name="red"/> | ||
| </visual> | ||
| <collision><geometry><box size="9 9 9"/></geometry></collision> | ||
| </link> | ||
| <link name="lid"> | ||
| <visual> | ||
| <origin xyz="0 0 0.2"/> | ||
| <geometry><cylinder radius="0.1" length="0.4"/></geometry> | ||
| </visual> | ||
| <visual> | ||
| <origin xyz="0.3 0 0" rpy="0 0 0"/> | ||
| <geometry><mesh filename="assets/handle.obj" scale="0.001 0.001 0.001"/></geometry> | ||
| </visual> | ||
| </link> | ||
| <joint name="hinge" type="revolute"> | ||
| <parent link="base"/> | ||
| <child link="lid"/> | ||
| <origin xyz="0.2 0 0.2" rpy="0 0 1.5707963267948966"/> | ||
| <axis xyz="0 1 0"/> | ||
| <limit lower="0" upper="1.5" effort="1" velocity="1"/> | ||
| </joint> | ||
| </robot>` | ||
|
|
||
| const handle = objExtent('# handle\nv 0 -10 0\nv 100 10 20\nvn 0 0 1\nf 1 2 1\n') | ||
|
|
||
| describe('rest-pose bounds', () => { | ||
| test('reads an OBJ vertex extent', () => { | ||
| expect(handle).toEqual({ min: [0, -10, 0], max: [100, 10, 20] }) | ||
| expect(objExtent('# no vertices\n')).toBeNull() | ||
| }) | ||
|
|
||
| test('counts every visual with link, joint and visual origins and mesh scale, Y up', async () => { | ||
| const bounds = await urdfRestBounds(URDF, async (filename) => | ||
| filename === 'assets/handle.obj' ? handle : null, | ||
| ) | ||
| // URDF frame: base x ±0.5, y ±0.25, z 0..0.2. The lid frame sits at | ||
| // (0.2, 0, 0.2) turned 90° about Z: its cylinder spans x 0.1..0.3, | ||
| // y -0.1..0.1, z 0.2..0.6; its handle (0.3 m along lid x = world y) | ||
| // spans x 0.19..0.21, y 0.3..0.4, z 0.2..0.22. | ||
| // Pascal frame (x, z, -y): x -0.5..0.5, y 0..0.6, z -0.4..0.25. | ||
| expect(bounds).not.toBeNull() | ||
| const expected = { min: [-0.5, 0, -0.4], max: [0.5, 0.6, 0.25] } | ||
| for (const key of ['min', 'max'] as const) { | ||
| for (const axis of [0, 1, 2]) { | ||
| expect(bounds?.[key][axis]).toBeCloseTo(expected[key][axis] ?? Number.NaN, 9) | ||
| } | ||
| } | ||
| }) | ||
|
|
||
| test('returns null when nothing is visible', async () => { | ||
| expect(await urdfRestBounds('<robot name="empty"><link name="a"/></robot>', async () => null)).toBeNull() | ||
| }) | ||
|
|
||
| test('stands an asset that extends below its origin on its bottom centre', () => { | ||
| const frame = bottomCentreFrame({ min: [-0.2, -0.35, 0.1], max: [0.6, 0.45, 0.5] }) | ||
| expect(frame.dimensions).toEqual([0.8, 0.8, 0.4]) | ||
| expect(frame.offset).toEqual([-0.2, 0.35, -0.3]) | ||
| }) | ||
| }) |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,227 @@ | ||
| import type { Vec3 } from './types' | ||
|
|
||
| /** Axis-aligned bounds in the frame Pascal renders an asset in: Y up, metres. */ | ||
| export type Bounds = { min: Vec3; max: Vec3 } | ||
|
|
||
| /** Where a mesh's vertices lie in its own frame, before the visual's `scale`. */ | ||
| export type MeshExtent = { min: Vec3; max: Vec3 } | ||
|
|
||
| type XmlElement = { name: string; attributes: Record<string, string>; children: XmlElement[] } | ||
| type Matrix = number[] // 4 × 4, row-major | ||
|
|
||
| const IDENTITY: Matrix = [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1] | ||
|
|
||
| /** | ||
| * The rest-pose bounds of a URDF (every joint at 0) in Pascal's frame: the | ||
| * URDF's Z-up axes turned Y-up, as the renderer turns the loaded robot | ||
| * (`rotation.x = -π/2`). Every visual counts (box, cylinder, sphere, mesh) | ||
| * through its link, joint and visual origins and the mesh `scale`; a | ||
| * rotated visual contributes the bounds of its turned box. `meshExtent` | ||
| * resolves a mesh `filename` as written in the URDF. Null when nothing is | ||
| * visible. | ||
| */ | ||
| export async function urdfRestBounds( | ||
| urdf: string, | ||
| meshExtent: (filename: string) => Promise<MeshExtent | null>, | ||
| ): Promise<Bounds | null> { | ||
| const robot = parseXml(urdf).find((element) => element.name === 'robot') | ||
| if (!robot) return null | ||
| const links = robot.children.filter((element) => element.name === 'link') | ||
| const jointByChild = new Map<string, XmlElement>() | ||
| for (const joint of robot.children.filter((element) => element.name === 'joint')) { | ||
| const child = childNamed(joint, 'child')?.attributes.link | ||
| if (child) jointByChild.set(child, joint) | ||
| } | ||
| const worldByLink = new Map<string, Matrix>() | ||
| const world = (link: string, visiting = new Set<string>()): Matrix => { | ||
| const known = worldByLink.get(link) | ||
| if (known) return known | ||
| const joint = jointByChild.get(link) | ||
| const parent = joint && childNamed(joint, 'parent')?.attributes.link | ||
| // A root link, or a cycle the URDF should not have, sits at the origin. | ||
| const matrix = | ||
| joint && parent && !visiting.has(parent) | ||
| ? multiply(world(parent, visiting.add(link)), originOf(joint)) | ||
| : IDENTITY | ||
| worldByLink.set(link, matrix) | ||
| return matrix | ||
| } | ||
|
|
||
| const min: Vec3 = [Infinity, Infinity, Infinity] | ||
| const max: Vec3 = [-Infinity, -Infinity, -Infinity] | ||
| for (const link of links) { | ||
| const linkWorld = world(link.attributes.name ?? '') | ||
| for (const visual of link.children.filter((element) => element.name === 'visual')) { | ||
| const extent = await visualExtent(visual, meshExtent) | ||
| if (!extent) continue | ||
| const matrix = multiply(linkWorld, originOf(visual)) | ||
| for (const x of [extent.min[0], extent.max[0]]) { | ||
| for (const y of [extent.min[1], extent.max[1]]) { | ||
| for (const z of [extent.min[2], extent.max[2]]) { | ||
| const point = transform(matrix, [x, y, z]) | ||
| for (const axis of [0, 1, 2] as const) { | ||
| min[axis] = Math.min(min[axis], point[axis]) | ||
| max[axis] = Math.max(max[axis], point[axis]) | ||
| } | ||
| } | ||
| } | ||
| } | ||
| } | ||
| } | ||
| if (!min.every(Number.isFinite)) return null | ||
| // URDF (x, y, z) Z up → Pascal (x, z, -y) Y up. | ||
| return { min: [min[0], min[2], -max[1]], max: [max[0], max[2], -min[1]] } | ||
| } | ||
|
|
||
| /** | ||
| * The footprint and the offset that stand an asset on its bottom centre at the | ||
| * node's origin: the renderer moves the loaded asset by `offset` (before the | ||
| * node's scale), so its rest pose rests on the floor, centred on the node, | ||
| * like the placeholder box. Rounded to 0.1 mm. | ||
| */ | ||
| export function bottomCentreFrame(bounds: Bounds): { dimensions: Vec3; offset: Vec3 } { | ||
| const [minX, minY, minZ] = bounds.min | ||
| const [maxX, maxY, maxZ] = bounds.max | ||
| return { | ||
| dimensions: [ | ||
| round(Math.max(maxX - minX, 0.001)), | ||
| round(Math.max(maxY - minY, 0.001)), | ||
| round(Math.max(maxZ - minZ, 0.001)), | ||
| ], | ||
| offset: [round(-(minX + maxX) / 2), round(-minY), round(-(minZ + maxZ) / 2)], | ||
| } | ||
| } | ||
|
|
||
| /** The extent of an OBJ's `v x y z` vertices, null when it has none. */ | ||
| export function objExtent(obj: string): MeshExtent | null { | ||
| const min: Vec3 = [Infinity, Infinity, Infinity] | ||
| const max: Vec3 = [-Infinity, -Infinity, -Infinity] | ||
| for (const line of obj.split('\n')) { | ||
| if (!line.startsWith('v ')) continue | ||
| const values = line.trim().split(/\s+/).slice(1, 4).map(Number) | ||
| if (values.length !== 3 || !values.every(Number.isFinite)) continue | ||
| for (const axis of [0, 1, 2] as const) { | ||
| min[axis] = Math.min(min[axis], values[axis] as number) | ||
| max[axis] = Math.max(max[axis], values[axis] as number) | ||
| } | ||
| } | ||
| return min.every(Number.isFinite) ? { min, max } : null | ||
| } | ||
|
|
||
| async function visualExtent( | ||
| visual: XmlElement, | ||
| meshExtent: (filename: string) => Promise<MeshExtent | null>, | ||
| ): Promise<MeshExtent | null> { | ||
| const shape = childNamed(visual, 'geometry')?.children[0] | ||
| if (!shape) return null | ||
| const number = (name: string) => Number(shape.attributes[name]) | ||
| if (shape.name === 'box') { | ||
| const [x, y, z] = vector(shape.attributes.size, [0, 0, 0]) | ||
| return { min: [-x / 2, -y / 2, -z / 2], max: [x / 2, y / 2, z / 2] } | ||
| } | ||
| if (shape.name === 'cylinder') { | ||
| const radius = number('radius') | ||
| const half = number('length') / 2 | ||
| if (!(Number.isFinite(radius) && Number.isFinite(half))) return null | ||
| return { min: [-radius, -radius, -half], max: [radius, radius, half] } | ||
| } | ||
| if (shape.name === 'sphere') { | ||
| const radius = number('radius') | ||
| return Number.isFinite(radius) | ||
| ? { min: [-radius, -radius, -radius], max: [radius, radius, radius] } | ||
| : null | ||
| } | ||
| if (shape.name === 'mesh' && shape.attributes.filename) { | ||
| const extent = await meshExtent(shape.attributes.filename) | ||
| if (!extent) return null | ||
| const scale = vector(shape.attributes.scale, [1, 1, 1]) | ||
| const a = extent.min.map((value, axis) => value * (scale[axis] ?? 1)) as Vec3 | ||
| const b = extent.max.map((value, axis) => value * (scale[axis] ?? 1)) as Vec3 | ||
| return { | ||
| min: [Math.min(a[0], b[0]), Math.min(a[1], b[1]), Math.min(a[2], b[2])], | ||
| max: [Math.max(a[0], b[0]), Math.max(a[1], b[1]), Math.max(a[2], b[2])], | ||
| } | ||
| } | ||
| return null | ||
| } | ||
|
|
||
| /** A URDF `<origin xyz rpy>` (fixed-axis roll, pitch, yaw) as a matrix. */ | ||
| function originOf(element: XmlElement): Matrix { | ||
| const origin = childNamed(element, 'origin') | ||
| const [x, y, z] = vector(origin?.attributes.xyz, [0, 0, 0]) | ||
| const [roll, pitch, yaw] = vector(origin?.attributes.rpy, [0, 0, 0]) | ||
| const [cr, sr, cp, sp, cy, sy] = [ | ||
| Math.cos(roll), | ||
| Math.sin(roll), | ||
| Math.cos(pitch), | ||
| Math.sin(pitch), | ||
| Math.cos(yaw), | ||
| Math.sin(yaw), | ||
| ] | ||
| // R = Rz(yaw) · Ry(pitch) · Rx(roll) | ||
| return [ | ||
| cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, x, | ||
| sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, y, | ||
| -sp, cp * sr, cp * cr, z, | ||
| 0, 0, 0, 1, | ||
| ] | ||
| } | ||
|
|
||
| function multiply(a: Matrix, b: Matrix): Matrix { | ||
| const out: Matrix = new Array(16).fill(0) | ||
| for (let row = 0; row < 4; row += 1) { | ||
| for (let column = 0; column < 4; column += 1) { | ||
| let sum = 0 | ||
| for (let k = 0; k < 4; k += 1) sum += (a[row * 4 + k] ?? 0) * (b[k * 4 + column] ?? 0) | ||
| out[row * 4 + column] = sum | ||
| } | ||
| } | ||
| return out | ||
| } | ||
|
|
||
| function transform(m: Matrix, [x, y, z]: Vec3): Vec3 { | ||
| const at = (index: number) => m[index] ?? 0 | ||
| return [ | ||
| at(0) * x + at(1) * y + at(2) * z + at(3), | ||
| at(4) * x + at(5) * y + at(6) * z + at(7), | ||
| at(8) * x + at(9) * y + at(10) * z + at(11), | ||
| ] | ||
| } | ||
|
|
||
| function vector(value: string | undefined, fallback: Vec3): Vec3 { | ||
| const parts = value?.trim().split(/\s+/).map(Number) | ||
| return parts?.length === 3 && parts.every(Number.isFinite) ? (parts as Vec3) : fallback | ||
| } | ||
|
|
||
| function childNamed(element: XmlElement, name: string): XmlElement | undefined { | ||
| return element.children.find((child) => child.name === name) | ||
| } | ||
|
|
||
| function round(value: number): number { | ||
| return Math.round(value * 10_000) / 10_000 + 0 | ||
| } | ||
|
|
||
| /** Elements of a small XML document (URDF): declarations and comments skipped. */ | ||
| function parseXml(source: string): XmlElement[] { | ||
| const roots: XmlElement[] = [] | ||
| const stack: XmlElement[] = [] | ||
| const tag = /<!--[\s\S]*?-->|<\?[\s\S]*?\?>|<!\[CDATA\[[\s\S]*?\]\]>|<![^>]*>|<\/\s*([\w:.-]+)\s*>|<([\w:.-]+)((?:\s+[\w:.-]+\s*=\s*(?:"[^"]*"|'[^']*'))*)\s*(\/?)>/g | ||
| for (const match of source.matchAll(tag)) { | ||
| const [, closing, name, rawAttributes, selfClosing] = match | ||
| if (closing) { | ||
| stack.pop() | ||
| continue | ||
| } | ||
| if (!name) continue | ||
| const attributes: Record<string, string> = {} | ||
| for (const attribute of (rawAttributes ?? '').matchAll(/([\w:.-]+)\s*=\s*(?:"([^"]*)"|'([^']*)')/g)) { | ||
| attributes[attribute[1] as string] = attribute[2] ?? attribute[3] ?? '' | ||
| } | ||
| const element: XmlElement = { name, attributes, children: [] } | ||
| const parent = stack.at(-1) | ||
| if (parent) parent.children.push(element) | ||
| else roots.push(element) | ||
| if (!selfClosing) stack.push(element) | ||
| } | ||
| return roots | ||
| } | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
When a sphere or cylinder visual has a non-axis-aligned joint or visual rotation, transforming the corners of its local AABB overestimates the actual bounds. For example, a unit sphere rotated 45° about X is reported as 2.828 m high rather than 2 m, so
bottomCentreFrameoffsets it upward by 1.414 m and leaves its real bottom about 0.414 m above the floor. Handle spheres analytically and compute the transformed cylinder support extents instead of treating every primitive extent as a box.Useful? React with 👍 / 👎.