444 lines
16 KiB
JavaScript
444 lines
16 KiB
JavaScript
/* Digital twin - the real X2 model, posed and shaded by live telemetry.
|
||
|
||
The robot already reports all 31 revolute joint positions and efforts at
|
||
100 Hz, and the dashboard already streams them. This tab draws them, which
|
||
turns a table of numbers into something you can check against the machine in
|
||
front of you: an arm folded in the picture but straight in the room means the
|
||
encoder, the model, or your idea of which robot you are connected to is wrong.
|
||
|
||
ON LOAD, AND WHY IT IS NOT TEMPERATURE
|
||
--------------------------------------
|
||
This robot does not publish joint temperature. aimdk_msgs/JointState carries
|
||
name, position, velocity, effort and error_code and nothing else; the
|
||
GetAllJointState service returns the same type; /diagnostics is the Orbbec
|
||
camera's own internals; and across all 53 aimdk_msgs types the only
|
||
temperature fields are pmu_temperature and battery_temperature. The
|
||
norealtime DCU joint topics may carry more, but their message type is not
|
||
installed anywhere on the robot, so nothing here can deserialise them.
|
||
|
||
So this shades by *effort*, which is honest and nearly as useful: motor
|
||
heating is I squared R, and current tracks torque, so the joint pulling
|
||
hardest is the joint getting hottest. Every number on screen is measured.
|
||
|
||
Load is shown as a percentage of each joint's RATED torque, taken from the
|
||
URDF's <limit effort="..."> and baked into model.json. That matters: rated
|
||
torque here runs from 0.6 Nm to 120 Nm, so an absolute Nm ramp would paint
|
||
every wrist permanently cold and every leg permanently hot regardless of what
|
||
the robot was doing.
|
||
|
||
Geometry is baked offline by build_model.py - 112 MB of vendor STL down to
|
||
1.4 MB. Nothing here parses a URDF at runtime.
|
||
*/
|
||
|
||
import { store, num } from '../core.js';
|
||
import { el, card, pageHead, button, badge, note, toggle, segmented, stat, emptyState } from '../ui.js';
|
||
import { RobotModel } from '../model3d.js';
|
||
|
||
const POSES = {
|
||
zero: {},
|
||
stand: {
|
||
left_hip_pitch_joint: -0.30, right_hip_pitch_joint: -0.30,
|
||
left_knee_joint: 0.62, right_knee_joint: 0.62,
|
||
left_ankle_pitch_joint: -0.32, right_ankle_pitch_joint: -0.32,
|
||
left_shoulder_roll_joint: -0.10, right_shoulder_roll_joint: 0.10,
|
||
left_elbow_joint: -0.30, right_elbow_joint: -0.30,
|
||
},
|
||
crouch: {
|
||
left_hip_pitch_joint: -1.20, right_hip_pitch_joint: -1.20,
|
||
left_knee_joint: 2.00, right_knee_joint: 2.00,
|
||
left_ankle_pitch_joint: -0.85, right_ankle_pitch_joint: -0.85,
|
||
waist_pitch_joint: 0.25,
|
||
left_elbow_joint: -0.60, right_elbow_joint: -0.60,
|
||
},
|
||
tpose: { left_shoulder_roll_joint: -1.57, right_shoulder_roll_joint: 1.57 },
|
||
wave: {
|
||
right_shoulder_pitch_joint: -0.4, right_shoulder_roll_joint: 1.30,
|
||
right_elbow_joint: -1.30, right_wrist_pitch_joint: 0.3,
|
||
left_shoulder_roll_joint: -0.10,
|
||
},
|
||
};
|
||
|
||
// Cool grey through blue, green, amber, red. Grey rather than deep blue at the
|
||
// bottom so an idle joint reads as "nothing happening" instead of "measured
|
||
// zero", which matters when half the robot is parked.
|
||
const LOAD_RAMP = [
|
||
[0.42, 0.46, 0.52],
|
||
[0.23, 0.51, 0.87],
|
||
[0.29, 0.76, 0.55],
|
||
[0.95, 0.75, 0.16],
|
||
[0.90, 0.28, 0.20],
|
||
];
|
||
|
||
function rampColour(t) {
|
||
const clamped = Math.max(0, Math.min(1, t));
|
||
const scaled = clamped * (LOAD_RAMP.length - 1);
|
||
const index = Math.min(LOAD_RAMP.length - 2, Math.floor(scaled));
|
||
const frac = scaled - index;
|
||
const a = LOAD_RAMP[index], b = LOAD_RAMP[index + 1];
|
||
return [a[0] + (b[0] - a[0]) * frac,
|
||
a[1] + (b[1] - a[1]) * frac,
|
||
a[2] + (b[2] - a[2]) * frac];
|
||
}
|
||
|
||
function css(colour) {
|
||
return `rgb(${colour.map((c) => Math.round(c * 255)).join(',')})`;
|
||
}
|
||
|
||
export default {
|
||
id: 'model',
|
||
label: 'Twin',
|
||
icon: 'motion',
|
||
|
||
async render() {
|
||
const root = el('div.stack');
|
||
|
||
const statusBadge = badge('loading…');
|
||
const canvas = el('canvas', {
|
||
style: { width: '100%', height: '520px', display: 'block', cursor: 'grab',
|
||
borderRadius: 'var(--radius-sm)', touchAction: 'none' },
|
||
});
|
||
|
||
const camera = { yaw: -0.9, pitch: 0.18, distance: 3.0, target: [0, 0, 0.75], fov: 0.75 };
|
||
|
||
let robot = null;
|
||
let source = 'live';
|
||
let shading = 'load'; // 'load' | 'plain'
|
||
let spinning = false;
|
||
let frame = null;
|
||
let disposed = false;
|
||
const manual = {};
|
||
|
||
// joint name -> {link, rated}. Built once the model is loaded: shading
|
||
// paints links, but telemetry is per joint, and the link a joint drives is
|
||
// its child in the URDF tree.
|
||
const jointMeta = new Map();
|
||
|
||
root.appendChild(pageHead(
|
||
'Digital twin',
|
||
'The real X2 model, posed by live joint telemetry and shaded by how hard each '
|
||
+ 'joint is working. Drag to orbit, scroll to zoom.',
|
||
[
|
||
segmented(
|
||
[{ value: 'load', label: 'Load' }, { value: 'plain', label: 'Plain' }],
|
||
shading,
|
||
(value) => { shading = value; legend.style.display = value === 'load' ? '' : 'none'; },
|
||
),
|
||
segmented(
|
||
[{ value: 'live', label: 'Follow robot' }, { value: 'pose', label: 'Poses' }],
|
||
source,
|
||
(value) => {
|
||
source = value;
|
||
poseRow.style.display = value === 'pose' ? '' : 'none';
|
||
statusBadge.textContent = value === 'live' ? 'following robot' : 'manual pose';
|
||
statusBadge.dataset.tone = value === 'live' ? 'good' : 'accent';
|
||
},
|
||
),
|
||
statusBadge,
|
||
],
|
||
));
|
||
|
||
/* -- Load tiles -------------------------------------------------------- */
|
||
|
||
const tiles = el('div.grid.cols-4');
|
||
const refs = {};
|
||
for (const [key, label] of [
|
||
['peak', 'Highest load'],
|
||
['total', 'Total torque'],
|
||
['busy', 'Joints under load'],
|
||
['faults', 'Joints reporting error'],
|
||
]) {
|
||
// sub: ' ' rather than '' so the .stat-sub element always exists and
|
||
// setTile can write into it. Same pattern as the Navigation tab.
|
||
const node = stat(label, '—', { sub: ' ' });
|
||
refs[key] = node;
|
||
tiles.appendChild(node);
|
||
}
|
||
root.appendChild(tiles);
|
||
|
||
function setTile(key, value, sub, tone = 'default', unit = '') {
|
||
const node = refs[key];
|
||
if (!node) return;
|
||
node.dataset.tone = tone;
|
||
const valueNode = node.querySelector('.stat-value');
|
||
valueNode.textContent = String(value);
|
||
if (unit) valueNode.appendChild(el('span.unit', { text: unit }));
|
||
node.querySelector('.stat-sub').textContent = sub || '';
|
||
}
|
||
|
||
/* -- Legend and hardest-working list ----------------------------------- */
|
||
|
||
const legend = el('div.row', {
|
||
style: { gap: '10px', alignItems: 'center', flexWrap: 'wrap' },
|
||
},
|
||
el('span', { text: 'Load', style: { fontSize: '11.5px', color: 'var(--text-3)' } }),
|
||
(() => {
|
||
const bar = el('div', {
|
||
style: {
|
||
height: '10px', width: '190px', borderRadius: '5px',
|
||
border: '1px solid var(--border)',
|
||
background: `linear-gradient(90deg, ${LOAD_RAMP.map(css).join(',')})`,
|
||
},
|
||
});
|
||
return bar;
|
||
})(),
|
||
el('span', { text: '0 % → 100 % of rated torque',
|
||
style: { fontSize: '11.5px', color: 'var(--text-3)' } }),
|
||
);
|
||
|
||
const poseRow = el('div.row', { style: { gap: '8px', flexWrap: 'wrap', display: 'none' } },
|
||
...Object.keys(POSES).map((name) => button(
|
||
name === 'tpose' ? 'T-pose' : name[0].toUpperCase() + name.slice(1),
|
||
() => applyPose(name),
|
||
{ size: 'sm' },
|
||
)),
|
||
);
|
||
|
||
const hardest = el('div.stack', { style: { gap: '5px' } });
|
||
|
||
root.appendChild(card('X2 Ultra', {
|
||
sub: '41 links · 31 revolute joints · baked from x2_ultra.urdf',
|
||
actions: [
|
||
toggle('Spin', false, (on) => { spinning = on; }),
|
||
button('Reset view', () => {
|
||
camera.yaw = -0.9; camera.pitch = 0.18; camera.distance = 3.0;
|
||
camera.target = [0, 0, 0.75];
|
||
}, { size: 'sm', style: 'ghost' }),
|
||
],
|
||
flush: true,
|
||
foot: el('div.stack', { style: { gap: '10px' } }, legend, poseRow),
|
||
}, canvas));
|
||
|
||
root.appendChild(card('Working hardest', {
|
||
sub: 'Measured torque against each joint’s rated limit, highest first',
|
||
}, hardest));
|
||
|
||
root.appendChild(note(
|
||
'This robot does not report joint temperature — aimdk_msgs/JointState carries only '
|
||
+ 'position, velocity, effort and an error code, and no service or topic on the unit '
|
||
+ 'exposes motor temperature. Colour here is measured torque as a share of each joint’s '
|
||
+ 'rated limit, which is the closest honest indicator of which joints are heating: motor '
|
||
+ 'heating rises with current, and current tracks torque.',
|
||
'info',
|
||
));
|
||
|
||
/* -- Boot -------------------------------------------------------------- */
|
||
|
||
try {
|
||
robot = new RobotModel(canvas);
|
||
} catch (err) {
|
||
root.replaceChildren(pageHead('Digital twin', 'The 3D view could not start.'),
|
||
emptyState('WebGL unavailable', err.message));
|
||
return { node: root };
|
||
}
|
||
|
||
try {
|
||
const [modelRes, geometryRes] = await Promise.all([
|
||
fetch('/model/model.json', { cache: 'force-cache' }),
|
||
fetch('/model/model.bin', { cache: 'force-cache' }),
|
||
]);
|
||
if (!modelRes.ok || !geometryRes.ok) {
|
||
throw new Error('The baked model is not on the server (web/model/).');
|
||
}
|
||
const model = await modelRes.json();
|
||
const geometry = await geometryRes.arrayBuffer();
|
||
robot.load(model, geometry);
|
||
|
||
for (const joint of model.joints) {
|
||
if (joint.type !== 'revolute') continue;
|
||
jointMeta.set(joint.name, {
|
||
link: joint.child,
|
||
// Fall back to a mid-range rating rather than 0 - dividing by zero
|
||
// would paint an unrated joint permanently red.
|
||
rated: joint.effort && joint.effort > 0 ? joint.effort : 40,
|
||
});
|
||
}
|
||
|
||
statusBadge.textContent = 'following robot';
|
||
statusBadge.dataset.tone = 'good';
|
||
} catch (err) {
|
||
robot.dispose();
|
||
root.replaceChildren(
|
||
pageHead('Digital twin', 'The 3D model could not be loaded.'),
|
||
emptyState('Model missing', err.message),
|
||
);
|
||
return { node: root };
|
||
}
|
||
|
||
/* -- Interaction -------------------------------------------------------- */
|
||
|
||
let dragging = null;
|
||
|
||
canvas.addEventListener('pointerdown', (event) => {
|
||
dragging = { x: event.clientX, y: event.clientY };
|
||
canvas.setPointerCapture(event.pointerId);
|
||
canvas.style.cursor = 'grabbing';
|
||
});
|
||
|
||
canvas.addEventListener('pointermove', (event) => {
|
||
if (!dragging) return;
|
||
camera.yaw -= (event.clientX - dragging.x) * 0.008;
|
||
camera.pitch = Math.max(-1.35, Math.min(1.35,
|
||
camera.pitch + (event.clientY - dragging.y) * 0.008));
|
||
dragging = { x: event.clientX, y: event.clientY };
|
||
});
|
||
|
||
for (const type of ['pointerup', 'pointercancel']) {
|
||
canvas.addEventListener(type, () => { dragging = null; canvas.style.cursor = 'grab'; });
|
||
}
|
||
|
||
canvas.addEventListener('wheel', (event) => {
|
||
event.preventDefault();
|
||
camera.distance = Math.max(0.8, Math.min(12,
|
||
camera.distance * (1 + Math.sign(event.deltaY) * 0.1)));
|
||
}, { passive: false });
|
||
|
||
function applyPose(name) {
|
||
source = 'pose';
|
||
poseRow.style.display = '';
|
||
statusBadge.textContent = 'manual pose';
|
||
statusBadge.dataset.tone = 'accent';
|
||
for (const key of robot.jointAngles.keys()) manual[key] = 0;
|
||
Object.assign(manual, POSES[name] || {});
|
||
}
|
||
|
||
/* -- Live telemetry ------------------------------------------------------ */
|
||
|
||
function readJoints() {
|
||
const groups = store.state?.joints || {};
|
||
const rows = [];
|
||
for (const list of Object.values(groups)) {
|
||
if (!Array.isArray(list)) continue;
|
||
for (const joint of list) {
|
||
if (joint && typeof joint.position === 'number') rows.push(joint);
|
||
}
|
||
}
|
||
return rows;
|
||
}
|
||
|
||
/* -- Frame loop ---------------------------------------------------------- */
|
||
|
||
const styles = getComputedStyle(document.documentElement);
|
||
|
||
function background() {
|
||
const raw = (styles.getPropertyValue('--surface-2') || '').trim();
|
||
const match = /^#?([0-9a-f]{6})$/i.exec(raw);
|
||
if (!match) return [0.09, 0.10, 0.12];
|
||
const value = parseInt(match[1], 16);
|
||
return [((value >> 16) & 255) / 255, ((value >> 8) & 255) / 255, (value & 255) / 255];
|
||
}
|
||
|
||
let lastPanel = 0;
|
||
|
||
function loop() {
|
||
if (disposed) return;
|
||
frame = requestAnimationFrame(loop);
|
||
|
||
if (spinning && !dragging) camera.yaw += 0.004;
|
||
|
||
const rows = source === 'live' ? readJoints() : [];
|
||
const angles = {};
|
||
if (source === 'live') {
|
||
for (const joint of rows) angles[joint.name] = joint.position;
|
||
} else {
|
||
Object.assign(angles, manual);
|
||
}
|
||
robot.setJoints(angles);
|
||
|
||
// Shading. In pose mode there is no live torque to show, so the model
|
||
// stays neutral rather than freezing the last real reading onto a pose
|
||
// the robot is not actually holding.
|
||
robot.linkColours.clear();
|
||
let loads = [];
|
||
if (shading === 'load' && source === 'live') {
|
||
for (const joint of rows) {
|
||
const meta = jointMeta.get(joint.name);
|
||
if (!meta) continue;
|
||
const effort = Math.abs(joint.effort || 0);
|
||
const share = effort / meta.rated;
|
||
robot.linkColours.set(meta.link, rampColour(share));
|
||
loads.push({ name: joint.name, effort, share, rated: meta.rated,
|
||
error: joint.error || 0 });
|
||
}
|
||
}
|
||
|
||
robot.render(camera, background());
|
||
|
||
const now = Date.now();
|
||
if (now - lastPanel > 300) {
|
||
lastPanel = now;
|
||
paintPanels(loads);
|
||
}
|
||
}
|
||
|
||
function paintPanels(loads) {
|
||
if (!loads.length) {
|
||
const why = source === 'live' ? 'Waiting for joint telemetry…'
|
||
: 'Manual pose — no live torque to show.';
|
||
hardest.replaceChildren(el('div', {
|
||
text: why, style: { fontSize: '12.5px', color: 'var(--text-3)' },
|
||
}));
|
||
for (const key of ['peak', 'total', 'busy', 'faults']) setTile(key, '—', '');
|
||
return;
|
||
}
|
||
|
||
loads.sort((a, b) => b.share - a.share);
|
||
const peak = loads[0];
|
||
const total = loads.reduce((sum, l) => sum + l.effort, 0);
|
||
const busy = loads.filter((l) => l.share >= 0.25).length;
|
||
const faults = loads.filter((l) => l.error).length;
|
||
|
||
setTile('peak', num(peak.share * 100, 0),
|
||
`${peak.name.replace(/_joint$/, '').replace(/_/g, ' ')} · ${num(peak.effort, 1)} Nm`,
|
||
peak.share > 0.8 ? 'critical' : peak.share > 0.5 ? 'warning' : 'good', ' %');
|
||
setTile('total', num(total, 0), 'sum across 31 joints', 'default', ' Nm');
|
||
setTile('busy', busy, 'above 25 % of rated',
|
||
busy > 6 ? 'warning' : 'default');
|
||
setTile('faults', faults, faults ? 'check the Motion tab' : 'all clear',
|
||
faults ? 'critical' : 'good');
|
||
|
||
hardest.replaceChildren(...loads.slice(0, 6).map((l) => el('div.row', {
|
||
style: { gap: '10px', alignItems: 'center' },
|
||
},
|
||
el('span', {
|
||
text: l.name.replace(/_joint$/, '').replace(/_/g, ' '),
|
||
style: { fontSize: '12px', width: '170px', flexShrink: '0' },
|
||
}),
|
||
el('div', {
|
||
style: {
|
||
flex: '1', height: '8px', borderRadius: '4px',
|
||
background: 'var(--surface-2)', overflow: 'hidden', minWidth: '60px',
|
||
},
|
||
},
|
||
el('div', {
|
||
style: {
|
||
width: `${Math.max(2, Math.min(100, l.share * 100))}%`, height: '100%',
|
||
background: css(rampColour(l.share)), transition: 'width .25s',
|
||
},
|
||
}),
|
||
),
|
||
el('span', {
|
||
text: `${num(l.effort, 1)} / ${num(l.rated, 0)} Nm`,
|
||
style: { fontSize: '11px', fontFamily: 'var(--mono)', color: 'var(--text-3)',
|
||
width: '110px', textAlign: 'right', flexShrink: '0' },
|
||
}),
|
||
el('span', {
|
||
text: `${num(l.share * 100, 0)}%`,
|
||
style: { fontSize: '11.5px', fontWeight: '600', width: '42px',
|
||
textAlign: 'right', flexShrink: '0', color: css(rampColour(l.share)) },
|
||
}),
|
||
)));
|
||
}
|
||
|
||
loop();
|
||
|
||
return {
|
||
node: root,
|
||
dispose: () => {
|
||
disposed = true;
|
||
if (frame) cancelAnimationFrame(frame);
|
||
robot.dispose();
|
||
},
|
||
};
|
||
},
|
||
};
|