import { MainThreadPhysicsAdapter } from '../src/simulation/PhysicsAdapter'; import { SimulationSession } from '../src/simulation/SimulationSession'; import { ModelBindings } from '../src/simulation/ModelBindings'; import { LEKIWI_PROFILE as p, bodyToWheels } from '../src/robot/profiles/lekiwi'; import type { ProjectManifest } from '../src/project/types'; import { ExternalControlClient } from '../src/robot/ExternalControlClient'; import { validateDescriptor, validateValues } from '../src/robot/validation'; import { sha256 } from '../src/robot/registry'; import single from '../../contracts/fixtures/single-joint.json'; import { Quaternion, Vector3 } from 'three'; import { surfaceDistance, type Surface } from './meshDistance'; const adapter = new MainThreadPhysicsAdapter(); let bindings: ModelBindings; let currentManifest: ProjectManifest; let frame: number | undefined; let bridge: ExternalControlClient | undefined; function session() { if (!adapter.session) throw new Error('not initialized'); return adapter.session; } function read() { const s = session(), id = bindings.resolve(bindings.bodies, p.baseBody); const quat = Array.from(s.data.xquat.slice(id * 4, id * 4 + 4), Number); const [w, x, y, z] = quat; return { time: Number(s.data.time), position: Array.from(s.data.xpos.slice(id * 3, id * 3 + 3), Number), quaternion: quat, roll: Math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)), pitch: Math.asin(Math.max(-1, Math.min(1, 2 * (w * y - z * x)))), yaw: Math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)), arm: p.arm.map((j) => Number(s.data.qpos[bindings.scalarJoint(j.joint).qposAddress])), wheel: p.wheels.map((j) => Number(s.data.qvel[bindings.scalarJoint(j).qvelAddress])), finite: Array.from(s.data.qpos, Number).every(Number.isFinite), contacts: Number(s.data.ncon), nu: s.model.nu, njnt: s.model.njnt, }; } function contactPairs() { const s = session(); const result: { geom1: string; geom2: string; body1: number; body2: number; distance: number; position: number[]; }[] = []; for (let i = 0; i < s.data.ncon; i++) { const contact = s.data.contact.get(i)!; if ( contact.geom1 < 0 || contact.geom2 < 0 || contact.geom1 >= s.model.ngeom || contact.geom2 >= s.model.ngeom ) { const message = `非法接触几何 id: ${contact.geom1}/${contact.geom2}; ${i}/${s.data.ncon}`; contact.delete(); throw new Error(message); } const a = s.model.geom(contact.geom1), b = s.model.geom(contact.geom2); try { result.push({ geom1: a.name, geom2: b.name, body1: Number(s.model.geom_bodyid[contact.geom1]), body2: Number(s.model.geom_bodyid[contact.geom2]), distance: contact.dist, position: Array.from(contact.pos, Number), }); } finally { a.delete(); b.delete(); contact.delete(); } } return result; } function step(seconds: number) { const s = session(), steps = Math.round(seconds / Number(s.model.opt.timestep)); let maxContacts = 0; for (let i = 0; i < steps; i++) { s.singleStep(); maxContacts = Math.max(maxContacts, Number(s.data.ncon)); } return { ...read(), maxContacts }; } const harness = { async bootSingle() { const data = new TextEncoder().encode( '', ); const manifest: ProjectManifest = { id: 'single', name: 'single', entries: [{ path: 'single.xml', format: 'mjcf', label: 'single' }], maps: [], files: [{ path: 'single.xml', data, size: data.length, mimeType: '', source: 'file' }], totalBytes: data.length, }; await adapter.load(manifest, 'single.xml', { urdfMode: 'mjcf', baseMode: 'fixed', map: { kind: 'none' }, }); const s = session(), b = new ModelBindings(s.model), j = b.scalarActuator('servo', 'slider', 'position'); const descriptor = validateDescriptor({ ...single.descriptor, modelFingerprint: await sha256(data), }); const values = (v: unknown) => validateValues(v, descriptor.actionChannels, true); s.configureRobotAdapter({ describe: () => descriptor, validateAction: values, applyAction(v) { const accepted = values(v); s.data.ctrl[j.controlAddress] = accepted['slider.position']; return accepted; }, readObservation: () => ({ 'slider.position': Number(s.data.qpos[j.qposAddress]) }), safeStop() { s.data.ctrl[j.controlAddress] = Math.max( -1, Math.min(1, Number(s.data.qpos[j.qposAddress])), ); }, reset() { s.data.qpos[j.qposAddress] = 0; s.data.ctrl[j.controlAddress] = 0; }, dispose() {}, }); return descriptor; }, async connectBridge(endpoint: string, token: string) { bridge?.disconnect(); bridge = new ExternalControlClient(() => adapter); session().setPaused(false); session().setExternalControlEnabled(true); const tick = (now: number) => { adapter.advance(now); frame = requestAnimationFrame(tick); }; if (frame === undefined) frame = requestAnimationFrame(tick); await bridge.connect(endpoint, token); return bridge.status(); }, externalState() { return { control: session().externalControlStatus(), observation: session().robotObservation(), bridge: bridge?.status(), }; }, authorize() { session().setPaused(false); session().setExternalControlEnabled(true); bridge?.sync(); }, pause() { session().setPaused(true); bridge?.sync(); }, async boot(assetBase: string) { const response = await fetch(`${assetBase}/source-manifest.json`); if (!response.ok) throw new Error('Run examples/lekiwi/prepare_assets.py first'); const source = (await response.json()) as { files: Record }; const files = await Promise.all( Object.keys(source.files) .filter((f) => f.startsWith('URDF/')) .map(async (path) => { const response = await fetch(`${assetBase}/${path}`); if (!response.ok) throw new Error(`Missing ${path}`); const data = new Uint8Array(await response.arrayBuffer()); return { path, data, size: data.byteLength, source: 'file' as const, mimeType: '' }; }), ); const manifest: ProjectManifest = { id: 'lekiwi-physics', name: 'LeKiwi physics', files, entries: [{ path: 'URDF/LeKiwi.urdf', format: 'urdf', label: 'LeKiwi' }], maps: [], totalBytes: files.reduce((s, f) => s + f.size, 0), }; currentManifest = manifest; await adapter.load(manifest, 'URDF/LeKiwi.urdf', { robotProfileId: p.id, urdfMode: 'mjcf', baseMode: 'floating', map: { kind: 'none' }, enhancements: { addActuators: false, addSensors: false, sensorType: 'camera' }, }); bindings = new ModelBindings(session().model); return { version: session().module.mj_versionString(), ...read() }; }, reset() { session().reset(); return step(1); }, step, read, contactPairs, adjacentArmChecks() { const s = session(), joint = bindings.scalarJoint('arm_elbow_flex'); const upper = bindings.resolve(bindings.bodies, 'SO_ARM100_08k_116_Square-v1'); const forearm = bindings.resolve(bindings.bodies, 'SO_ARM100_08k_Mirror-v1'); const touching = () => contactPairs().filter( (c) => (c.body1 === upper && c.body2 === forearm) || (c.body2 === upper && c.body1 === forearm), ); s.reset(); const before = Array.from(s.data.qpos, Number); const teleportAccepted = s.setJointPosition(joint.id, -1.3); let jointResetError = ''; try { s.resetJoints(); } catch (error) { jointResetError = String(error); } const after = Array.from(s.data.qpos, Number); const editable = s.snapshot().joints.find((j) => j.id === joint.id)!.editable; // Bearing/structural hull partitioning must leave these pan poses reachable. const samples = [0, 0.8, -0.8].map((pan) => { s.reset(); harness.arm([pan, 0, 0, 0, 0, 0], 1.5); const neutral = { actual: Number(s.data.qpos[joint.qposAddress]), panActual: read().arm[0], contacts: touching(), }; s.setActuator(bindings.resolve(bindings.actuators, 'arm_elbow_flex_servo'), -1.3); let maxPenetration = 0, maxContacts = 0; for (let i = 0; i < Math.ceil(3 / Number(s.model.opt.timestep)); i++) { s.singleStep(); const contacts = touching(); maxContacts = Math.max(maxContacts, contacts.length); for (const c of contacts) maxPenetration = Math.max(maxPenetration, -c.distance); } const contacts = touching(); const anchor = Array.from(s.data.xanchor.slice(joint.id * 3, joint.id * 3 + 3), Number); const axis = Array.from(s.data.xaxis.slice(joint.id * 3, joint.id * 3 + 3), Number); const radii = contacts.map((c) => { const offset = c.position.map((x, i) => x - anchor[i]); const axial = offset.reduce((sum, x, i) => sum + x * axis[i], 0); return Math.hypot(...offset.map((x, i) => x - axial * axis[i])); }); const blocked = { actual: Number(s.data.qpos[joint.qposAddress]), target: -1.3, contacts, maxPenetration, maxContacts, minContactRadius: Math.min(...radii), finite: read().finite, }; harness.arm([pan, 0, 0.3, 0, 0, 0], 2); return { pan, neutral, blocked, released: { actual: Number(s.data.qpos[joint.qposAddress]), contacts: touching() }, }; }); s.reset(); return { teleportAccepted, jointResetError, editable, before, after, samples }; }, jointSweepChecks() { const s = session(), dt = Number(s.model.opt.timestep); const results = []; for (const spec of p.arm) for (const target of [spec.min * 0.6, spec.max * 0.6]) { s.reset(); step(0.5); const motor = bindings.resolve(bindings.actuators, `${spec.joint}_servo`); const joint = bindings.scalarJoint(spec.joint); const initial = Number(s.data.qpos[joint.qposAddress]); const duration = Math.abs(target - initial) / 0.4; // bounded 0.4 rad/s target ramp let maxPenetration = 0, maxContacts = 0; const start = Number(s.data.time), steps = Math.ceil((duration + 0.75) / dt); for (let i = 0; i < steps; i++) { if (i % 10 === 0) s.setActuator(motor, initial + (target - initial) * Math.min(1, (i * dt) / duration)); s.singleStep(); const contacts = contactPairs().filter( (c) => c.geom1.startsWith('__lekiwi_cad_') && c.geom2.startsWith('__lekiwi_cad_'), ); maxContacts = Math.max(maxContacts, contacts.length); for (const c of contacts) maxPenetration = Math.max(maxPenetration, -c.distance); } results.push({ joint: spec.joint, target, actual: Number(s.data.qpos[joint.qposAddress]), maxPenetration, maxContacts, finite: read().finite, elapsed: Number(s.data.time) - start, expectedTime: steps * dt, contacts: contactPairs().filter( (c) => c.geom1.startsWith('__lekiwi_cad_') && c.geom2.startsWith('__lekiwi_cad_'), ), }); } s.reset(); return results; }, visualCollisionCoverage() { const xml = new DOMParser().parseFromString(harness.xml(), 'application/xml'); const arm = xml.querySelector('body[name="Base_08q-v1"]')!; const original = new DOMParser().parseFromString( new TextDecoder().decode( currentManifest.files.find((f) => f.path === 'URDF/LeKiwi.urdf')!.data, ), 'application/xml', ); const visuals = Array.from(original.querySelectorAll('link > visual')).filter((v) => arm.querySelector(`geom[name="${v.getAttribute('name')}"]`), ); const probeBody = xml.createElement('body'); probeBody.setAttribute('name', '__coverage_probe_body'); probeBody.setAttribute('mocap', 'true'); probeBody.setAttribute('pos', '10 10 10'); const geom = xml.createElement('geom'); geom.setAttribute('name', '__coverage_probe'); geom.setAttribute('type', 'sphere'); geom.setAttribute('size', '.0015'); probeBody.append(geom); xml.querySelector('worldbody')!.append(probeBody); const path = 'URDF/lekiwi-coverage-test.xml'; adapter.workspace!.writeGenerated( path, new TextEncoder().encode(new XMLSerializer().serializeToString(xml)), ); const module = session().module; session().dispose(); adapter.session = new SimulationSession(module, adapter.workspace!.path(path)); const s = session(); bindings = new ModelBindings(s.model); const mocap = Number( s.model.body_mocapid[bindings.resolve(bindings.bodies, '__coverage_probe_body')], ); const samples = visuals.map((v) => { const name = v.getAttribute('name')!, link = v.parentElement!.getAttribute('name')!; const sourceMesh = v.querySelector('mesh')!; const scale = (sourceMesh.getAttribute('scale') ?? '1 1 1').split(/\s+/).map(Number); const file = currentManifest.files.find( (f) => f.path === `URDF/${sourceMesh.getAttribute('filename')}`, )!; const data = new DataView(file.data.buffer, file.data.byteOffset, file.data.byteLength); const extrema: number[][] = []; for (let triangle = 0; triangle < data.getUint32(80, true); triangle++) for (let vertex = 0; vertex < 3; vertex++) { const point = [0, 1, 2].map( (a) => data.getFloat32(84 + triangle * 50 + 12 + vertex * 12 + a * 4, true) * scale[a], ); for (let a = 0; a < 3; a++) for (let sign = 0; sign < 2; sign++) { const i = a * 2 + sign, factor = sign ? 1 : -1; if (!extrema[i] || point[a] * factor > extrema[i][a] * factor) extrema[i] = point; } } const visual = xml.querySelector(`geom[name="${name}"]`)!; const body = bindings.resolve(bindings.bodies, visual.parentElement!.getAttribute('name')!); const q = (visual.getAttribute('quat') ?? '1 0 0 0').split(/\s+/).map(Number); const pos = (visual.getAttribute('pos') ?? '0 0 0').split(/\s+/).map(Number); const probes = extrema.map((point) => { const local = new Vector3(...point) .applyQuaternion(new Quaternion(q[1], q[2], q[3], q[0])) .add(new Vector3(...pos)) .toArray(); const world = [0, 1, 2].map( (row) => Number(s.data.xpos[body * 3 + row]) + local.reduce( (sum, value, col) => sum + value * Number(s.data.xmat[body * 9 + row * 3 + col]), 0, ), ); s.data.mocap_pos.set(world, mocap * 3); module.mj_forward(s.model, s.data); const contacts = contactPairs().filter( (c) => (c.geom1 === '__coverage_probe' && c.geom2.startsWith(`__lekiwi_cad_${link}__`)) || (c.geom2 === '__coverage_probe' && c.geom1.startsWith(`__lekiwi_cad_${link}__`)), ); return { point, contacts }; }); return { visual: name, probes }; }); s.data.mocap_pos.set([10, 10, 10], mocap * 3); module.mj_forward(s.model, s.data); return samples; }, drive(x: number, y: number, w: number, seconds: number) { const wheels = bodyToWheels(x, y, w); p.wheels.forEach((j, i) => session().setActuator(bindings.resolve(bindings.actuators, `${j}_servo`), wheels[i]), ); return step(seconds); }, arm(targets: number[], seconds: number) { p.arm.forEach((j, i) => session().setActuator(bindings.resolve(bindings.actuators, `${j.joint}_servo`), targets[i]), ); return step(seconds); }, wall() { const xml = new DOMParser().parseFromString( new TextDecoder().decode(adapter.exportMjcf()), 'application/xml', ); const wall = xml.createElement('geom'); for (const [k, v] of Object.entries({ name: 'test_wall', type: 'box', pos: '.32 0 .12', size: '.03 .5 .12', })) wall.setAttribute(k, v); xml.querySelector('worldbody')!.append(wall); adapter.workspace!.writeGenerated( 'URDF/lekiwi-test.xml', new TextEncoder().encode(new XMLSerializer().serializeToString(xml)), ); const module = session().module; session().dispose(); adapter.session = new SimulationSession( module, adapter.workspace!.path('URDF/lekiwi-test.xml'), ); bindings = new ModelBindings(session().model); return step(1); }, armSelfCollisionChecks() { const s = session(); const supportGroups = new Set([ bindings.resolve(bindings.bodies, p.baseBody), bindings.resolve(bindings.bodies, 'Rotation_Pitch_08i-v1'), ]); const upperArm = bindings.resolve(bindings.bodies, 'SO_ARM100_08k_116_Square-v1'); const joint = bindings.scalarJoint('arm_shoulder_lift'); const geomNames = Array.from({ length: s.model.ngeom }, (_, i) => { const geom = s.model.geom(i); try { return geom.name; } finally { geom.delete(); } }); const contacts = () => { const touching: { distance: number; geom1: string; geom2: string }[] = []; for (let i = 0; i < s.data.ncon; i++) { const contact = s.data.contact.get(i)!; try { // The fixed base/mounting plate and rotating shoulder clip can all // stop the upper arm before it reaches the original Base/Square pair. const a = Number(s.model.body_weldid[Number(s.model.geom_bodyid[contact.geom1])]); const b = Number(s.model.body_weldid[Number(s.model.geom_bodyid[contact.geom2])]); if ( ((supportGroups.has(a) && b === upperArm) || (a === upperArm && supportGroups.has(b))) && geomNames[contact.geom1].startsWith('__lekiwi_cad_') && geomNames[contact.geom2].startsWith('__lekiwi_cad_') ) touching.push({ distance: contact.dist, geom1: geomNames[contact.geom1], geom2: geomNames[contact.geom2], }); } finally { contact.delete(); } } return touching; }; const samples = [0, 0.8, -0.8].map((pan) => { s.reset(); harness.arm([pan, 0, 0, 0, 0, 0], 1.5); const neutral = { actual: Number(s.data.qpos[joint.qposAddress]), contacts: contacts() }; const target = 0.6; s.setActuator(bindings.resolve(bindings.actuators, 'arm_shoulder_lift_servo'), target); let maxContacts = 0, maxPenetration = 0; for (let i = 0; i < Math.ceil(3 / Number(s.model.opt.timestep)); i++) { s.singleStep(); const touching = contacts(); maxContacts = Math.max(maxContacts, touching.length); for (const contact of touching) maxPenetration = Math.max(maxPenetration, -contact.distance); } const blocked = { actual: Number(s.data.qpos[joint.qposAddress]), target, contacts: contacts(), allArmContacts: contactPairs().filter( (c) => c.geom1.startsWith('__lekiwi_cad_') && c.geom2.startsWith('__lekiwi_cad_'), ), maxContacts, maxPenetration, finite: read().finite, }; harness.arm([pan, -0.3, 0, 0, 0, 0], 2); const released = { actual: Number(s.data.qpos[joint.qposAddress]), contacts: contacts() }; return { pan, neutral, blocked, released }; }); s.reset(); return samples; }, gripperCollisionChecks() { const xml = new DOMParser().parseFromString(harness.xml(), 'application/xml'); const probe = xml.createElement('body'); probe.setAttribute('name', 'gripper_probe_body'); probe.setAttribute('mocap', 'true'); probe.setAttribute('pos', '10 10 10'); const sphere = xml.createElement('geom'); sphere.setAttribute('name', 'gripper_probe'); sphere.setAttribute('type', 'sphere'); sphere.setAttribute('size', '.008'); probe.append(sphere); xml.querySelector('worldbody')!.append(probe); const path = 'URDF/lekiwi-gripper-test.xml'; adapter.workspace!.writeGenerated( path, new TextEncoder().encode(new XMLSerializer().serializeToString(xml)), ); const module = session().module; session().dispose(); adapter.session = new SimulationSession(module, adapter.workspace!.path(path)); const s = session(); bindings = new ModelBindings(s.model); const geoms = new Map(); for (let i = 0; i < s.model.ngeom; i++) { const geom = s.model.geom(i); try { geoms.set(geom.name, i); } finally { geom.delete(); } } const probeId = geoms.get('gripper_probe')!; const fixedBody = bindings.resolve(bindings.bodies, 'Wrist_Roll_08c-v1'); const movingBody = bindings.resolve(bindings.bodies, 'Moving_Jaw_08d-v1'); const motor = bindings.resolve(bindings.actuators, 'arm_gripper_servo'); const joint = bindings.scalarJoint('arm_gripper'); const spec = p.arm[5]; const sourcePoint = (visualName: string, point: number[]) => { // Independent CAD surface samples, NOT points taken from the collision recipe. const visual = xml.querySelector(`geom[name="${visualName}"]`)!; const body = bindings.resolve(bindings.bodies, visual.parentElement!.getAttribute('name')!); const q = (visual.getAttribute('quat') ?? '1 0 0 0').split(/\s+/).map(Number); const pos = (visual.getAttribute('pos') ?? '0 0 0').split(/\s+/).map(Number); const local = new Vector3(...point) .multiplyScalar(0.001) .applyQuaternion(new Quaternion(q[1], q[2], q[3], q[0])) .add(new Vector3(...pos)) .toArray(); return [0, 1, 2].map( (row) => Number(s.data.xpos[body * 3 + row]) + local.reduce( (sum, value, col) => sum + value * Number(s.data.xmat[body * 9 + row * 3 + col]), 0, ), ); }; const contacts = () => { const result: { other: string; distance: number }[] = []; for (let i = 0; i < s.data.ncon; i++) { const contact = s.data.contact.get(i)!; try { if (contact.geom1 !== probeId && contact.geom2 !== probeId) continue; const other = contact.geom1 === probeId ? contact.geom2 : contact.geom1; result.push({ other: [...geoms].find(([, id]) => id === other)![0], distance: contact.dist, }); } finally { contact.delete(); } } return result; }; const place = (position: number[]) => { s.data.mocap_pos.set(position); module.mj_forward(s.model, s.data); return contacts(); }; step(0.5); const coverage = [ { name: 'fixed_tip', visual: 'Wrist_Roll_08c-v1_visual', point: [-12, 0, 99] }, { name: 'moving_tip', visual: 'Moving_Jaw_08d-v1_visual', point: [-10, -78, 0] }, ].map(({ name, visual, point }) => ({ name, contacts: place(sourcePoint(visual, point)) })); place([10, 10, 10]); // Select actual collision parts contacted by independent CAD tip probes. // No hull indices or generator vertices are used as the test oracle. const fingerIds = coverage.map((sample, i) => sample.contacts .filter((c) => c.other.startsWith( `__lekiwi_cad_${i === 0 ? 'Wrist_Roll_08c-v1' : 'Moving_Jaw_08d-v1'}__`, ), ) .map((c) => geoms.get(c.other)!), ); const surface = (geom: number): Surface => { const m = s.model, d = s.data, mesh = Number(m.geom_dataid[geom]); const va = Number(m.mesh_vertadr[mesh]), fa = Number(m.mesh_faceadr[mesh]); return { vertices: Array.from({ length: Number(m.mesh_vertnum[mesh]) }, (_, i) => { const local = [0, 1, 2].map((c) => Number(m.mesh_vert[3 * (va + i) + c])); return new Vector3( ...[0, 1, 2].map( (r) => Number(d.geom_xpos[3 * geom + r]) + local.reduce((sum, v, c) => sum + v * Number(d.geom_xmat[9 * geom + 3 * r + c]), 0), ), ); }), faces: Array.from({ length: Number(m.mesh_facenum[mesh]) }, (_, i) => [0, 1, 2].map((c) => Number(m.mesh_face[3 * (fa + i) + c])), ), }; }; const closestPair = () => { // Independent triangle geometry: WASM 3.11 mj_geomDistance returned scalar 0 // for a separated thin hull pair (its witness points were >50 mm apart). // Do not silently discard that pair, loosen gap assertions or tune the solver. const surfaces = fingerIds.map((ids) => ids.map(surface)); let closest: ReturnType | undefined; for (const fixed of surfaces[0]) for (const moving of surfaces[1]) { const result = surfaceDistance(fixed, moving); if (!closest || result.distance < closest.distance) closest = result; } if (!closest) throw new Error('CAD 指尖没有对应碰撞体'); return closest; }; const openings = [0, 0.5, 1].map((opening) => { const target = spec.min + opening * (spec.max - spec.min); s.setActuator(motor, target); step(1); return { opening, target, actual: Number(s.data.qpos[joint.qposAddress]), separation: closestPair().distance, contacts: contactPairs().filter( (c) => (c.body1 === fixedBody && c.body2 === movingBody) || (c.body1 === movingBody && c.body2 === fixedBody), ), }; }); // Put a rigid object in the jaw space of the half-open pose; then close on it. s.setActuator(motor, (spec.min + spec.max) / 2); step(1); const closest = closestPair(); const midpoint = [0, 1, 2].map((axis) => (closest.from[axis] + closest.to[axis]) / 2); s.setActuator(motor, spec.max); step(1); const gapContacts = place(midpoint); s.setActuator(motor, spec.min); let maxContacts = 0, maxPenetration = 0; for (let i = 0; i < Math.ceil(2 / Number(s.model.opt.timestep)); i++) { s.singleStep(); const touching = contacts(); maxContacts = Math.max(maxContacts, touching.length); for (const contact of touching) maxPenetration = Math.max(maxPenetration, -contact.distance); } const obstruction = { maxContacts, maxPenetration, actual: Number(s.data.qpos[joint.qposAddress]), target: spec.min, contacts: contacts(), }; place([10, 10, 10]); // Deliberately overlap the fingers to check structural pairs bypass the // default parent filter. This is a query, never a normal control command. s.data.qpos[joint.qposAddress] = -0.24; module.mj_forward(s.model, s.data); let fingerSelfContacts = 0; for (let i = 0; i < s.data.ncon; i++) { const contact = s.data.contact.get(i)!; try { if ( (s.model.geom_bodyid[contact.geom1] === fixedBody && s.model.geom_bodyid[contact.geom2] === movingBody) || (s.model.geom_bodyid[contact.geom2] === fixedBody && s.model.geom_bodyid[contact.geom1] === movingBody) ) fingerSelfContacts++; } finally { contact.delete(); } } s.reset(); return { coverage, openings, gapContacts, obstruction, fingerSelfContacts, finite: read().finite, }; }, async runtimeChecks() { const s = session(); await s.loadPythonController( `OLD = None\nMOTOR = 0\ndef init(model):\n global MOTOR\n MOTOR=model.actuator('arm_shoulder_pan_servo')\n return MOTOR\ndef step(ctx, state):\n global OLD\n OLD=ctx\n ctx.set_control(state, 0.1)\ndef dispose(state):\n if OLD is not None:\n OLD.set_control(MOTOR, 1.2)\n`, 'scoped.py', ); s.setControllerEnabled(true); s.singleStep(); const motor = bindings.resolve(bindings.actuators, 'arm_shoulder_pan_servo'); const pythonTarget = Number(s.data.ctrl[motor]); s.setPaused(false); s.setExternalControlEnabled(true); const identity = s.claimExternalControlLease('browser-lease'); const values = Object.fromEntries(s.describeRobot()!.actionChannels.map((c) => [c.id, 0])); values['arm_gripper.pos'] = 0.25; values['arm_shoulder_pan.pos'] = 0.3; values['x.vel'] = 0.1; const pending = s.sendRobotAction({ protocolVersion: 1, ...identity, actionSeq: 1, values }); s.singleStep(); const accepted = await pending; let manualBlocked = false; try { s.setActuator(motor, 1); } catch { manualBlocked = true; } s.removeController(); // old Python dispose tries to write 1.2 through its saved ctx const afterOldDispose = Number(s.data.ctrl[motor]); const measured = s.robotObservation(); s.setPaused(true); const held = Number(s.data.ctrl[motor]); const actual = Number(s.data.qpos[bindings.scalarJoint('arm_shoulder_pan').qposAddress]); const paused = s.snapshot(); s.setPaused(false); s.setExternalControlEnabled(true); const nextIdentity = s.claimExternalControlLease('next-lease'); const cancel = s .sendRobotAction({ protocolVersion: 1, ...nextIdentity, actionSeq: 1, values }) .catch((e) => e.code); s.reset(); return { pythonTarget, accepted, manualBlocked, afterOldDispose, measured, held, actual, paused: paused.externalControl, pausedOwner: paused.controlOwner, reset: s.robotObservation(), cancel: await cancel, }; }, async reimport() { const data = adapter.exportMjcf(); const file = { path: 'URDF/export.xml', data, size: data.length, mimeType: '', source: 'file' as const, }; const manifest: ProjectManifest = { ...currentManifest, files: [...currentManifest.files, file], entries: [...currentManifest.entries, { path: file.path, format: 'mjcf', label: 'export' }], }; await adapter.load(manifest, file.path, { robotProfileId: p.id, urdfMode: 'mjcf', baseMode: 'floating', map: { kind: 'none' }, }); adapter.releaseRetired(); bindings = new ModelBindings(session().model); return { robot: adapter.describeRobot(), ...read() }; }, xml() { return new TextDecoder().decode(adapter.exportMjcf()); }, dispose() { if (frame !== undefined) cancelAnimationFrame(frame); frame = undefined; bridge?.disconnect(); bridge = undefined; adapter.dispose(); }, }; export type PhysicsHarness = typeof harness; declare global { interface Window { lekiwiPhysics: PhysicsHarness; } } window.lekiwiPhysics = harness;