chenlin
3ad29356c9
集成通用机器人数值接口、本机控制桥、LeRobot 插件和统一键盘遥操作。采用离线 CoACD 全臂碰撞配方 revision 4、局部装配区切分与结构自接触，限制直接关节位姿写入并保留安全看门狗。同步版本号、变更记录、来源许可证和兼容性验证。
823 lines
31 KiB
TypeScript
823 lines
31 KiB
TypeScript
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(
|
|
'<mujoco><option gravity="0 0 0" timestep=".002"/><worldbody><body pos="0 0 1"><joint name="slider" type="slide" axis="1 0 0" range="-1 1"/><geom type="sphere" size=".1" mass="1"/></body></worldbody><actuator><position name="servo" joint="slider" kp="40" kv="15" ctrlrange="-1 1"/></actuator></mujoco>',
|
|
);
|
|
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<string, string> };
|
|
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<string, number>();
|
|
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<typeof surfaceDistance> | 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;
|