322 lines
11 KiB
JavaScript
322 lines
11 KiB
JavaScript
/* Motion - joint-level control and end effectors. */
|
|
|
|
import {
|
|
store, command, num, RAD2DEG, DEG2RAD,
|
|
} from '../core.js';
|
|
import {
|
|
el, card, pageHead, button, note, segmented, range, table, badge, emptyState,
|
|
} from '../ui.js';
|
|
import { bars } from '../charts.js';
|
|
|
|
export default {
|
|
id: 'motion',
|
|
label: 'Motion',
|
|
icon: 'motion',
|
|
|
|
async render() {
|
|
const spec = store.spec;
|
|
const root = el('div.stack');
|
|
|
|
root.appendChild(pageHead(
|
|
'Motion',
|
|
'Drive joints and end effectors directly. Limits shown are the published X2 '
|
|
+ 'Ultra ranges; the server clamps anything outside them before publishing.',
|
|
));
|
|
|
|
/* ==================================================================
|
|
Joint control
|
|
================================================================== */
|
|
|
|
const groups = spec?.joint_groups || [];
|
|
let activeGroup = groups[0]?.key || 'arm';
|
|
let controlMode = 'position';
|
|
|
|
const sliders = new Map(); // joint name -> range control
|
|
const targets = new Map(); // joint name -> radians
|
|
const editorHost = el('div');
|
|
const stateHost = el('div');
|
|
|
|
const stiffness = range({ min: 0, max: 200, step: 1, value: 60, precision: 0, unit: ' N·m/rad' });
|
|
const damping = range({ min: 0, max: 20, step: 0.5, value: 3, precision: 1, unit: ' N·m·s/rad' });
|
|
|
|
function buildEditor() {
|
|
const group = groups.find((g) => g.key === activeGroup);
|
|
if (!group) { editorHost.replaceChildren(emptyState('Unknown joint group')); return; }
|
|
|
|
sliders.clear();
|
|
const rows = el('div.stack', { style: { gap: '10px' } });
|
|
|
|
for (const joint of group.joints) {
|
|
const fixed = joint.min_deg === joint.max_deg;
|
|
const current = targets.get(joint.name) ?? 0;
|
|
|
|
const control = range({
|
|
min: joint.min_deg, max: joint.max_deg,
|
|
step: fixed ? 1 : 0.5,
|
|
value: current * RAD2DEG,
|
|
precision: 1, unit: '°',
|
|
onInput: (deg) => targets.set(joint.name, deg * DEG2RAD),
|
|
});
|
|
if (fixed) {
|
|
control.querySelector('input').disabled = true;
|
|
control.style.opacity = '.5';
|
|
}
|
|
sliders.set(joint.name, control);
|
|
|
|
rows.appendChild(el('div', {},
|
|
el('div.row.between', { style: { marginBottom: '3px' } },
|
|
el('span', { text: joint.label, style: { fontSize: '12px', fontWeight: '550' } }),
|
|
el('span', {
|
|
text: fixed ? 'fixed' : `${joint.min_deg}° … ${joint.max_deg}°`,
|
|
style: { fontSize: '10.5px', color: 'var(--text-3)', fontFamily: 'var(--mono)' },
|
|
}),
|
|
),
|
|
control,
|
|
));
|
|
}
|
|
|
|
editorHost.replaceChildren(rows);
|
|
}
|
|
|
|
async function sendTargets() {
|
|
const group = groups.find((g) => g.key === activeGroup);
|
|
if (!group) return;
|
|
const payload = {};
|
|
for (const joint of group.joints) {
|
|
if (joint.min_deg === joint.max_deg) continue;
|
|
payload[joint.name] = targets.get(joint.name) ?? 0;
|
|
}
|
|
await command('/api/joints', {
|
|
group: activeGroup,
|
|
mode: controlMode,
|
|
targets: payload,
|
|
stiffness: stiffness.getValue(),
|
|
damping: damping.getValue(),
|
|
}, { successTitle: `${group.label} command sent` });
|
|
}
|
|
|
|
function zeroTargets() {
|
|
const group = groups.find((g) => g.key === activeGroup);
|
|
for (const joint of group?.joints || []) {
|
|
targets.set(joint.name, 0);
|
|
sliders.get(joint.name)?.setValue(0);
|
|
}
|
|
}
|
|
|
|
function syncFromRobot() {
|
|
const live = store.state?.joints?.[activeGroup] || [];
|
|
for (const joint of live) {
|
|
targets.set(joint.name, joint.position);
|
|
sliders.get(joint.name)?.setValue(joint.position * RAD2DEG);
|
|
}
|
|
}
|
|
|
|
const groupTabs = segmented(
|
|
groups.map((g) => ({ value: g.key, label: g.label })),
|
|
activeGroup,
|
|
(value) => { activeGroup = value; buildEditor(); paintState(); },
|
|
);
|
|
|
|
const modeTabs = segmented(
|
|
(spec?.joint_modes || []).map((m) => ({ value: m.id, label: m.label })),
|
|
controlMode,
|
|
(value) => { controlMode = value; },
|
|
);
|
|
|
|
buildEditor();
|
|
|
|
root.appendChild(el('div.grid.pair', {},
|
|
card('Joint targets', { sub: 'JointCommandArray', actions: [groupTabs] },
|
|
el('div.stack', {},
|
|
el('div.row.between', {},
|
|
el('span', { text: 'Control mode', style: { fontSize: '12px', color: 'var(--text-2)' } }),
|
|
modeTabs,
|
|
),
|
|
editorHost,
|
|
el('div.grid.cols-2', { style: { gap: '10px' } },
|
|
el('div', {},
|
|
el('div', { text: 'Stiffness', style: { fontSize: '12px', color: 'var(--text-2)', marginBottom: '4px' } }),
|
|
stiffness,
|
|
),
|
|
el('div', {},
|
|
el('div', { text: 'Damping', style: { fontSize: '12px', color: 'var(--text-2)', marginBottom: '4px' } }),
|
|
damping,
|
|
),
|
|
),
|
|
el('div.row', {},
|
|
button('Send targets', sendTargets, { style: 'primary' }),
|
|
button('Sync from robot', syncFromRobot),
|
|
button('Zero all', zeroTargets, { style: 'ghost' }),
|
|
),
|
|
note('Head pitch has a published range of 0° on the X2 Ultra — the axis exists in the '
|
|
+ 'message but is not articulated, so its slider is disabled.', 'info'),
|
|
),
|
|
),
|
|
card('Measured joint state', { sub: 'JointStateArray' }, stateHost),
|
|
));
|
|
|
|
/* ==================================================================
|
|
End effectors
|
|
================================================================== */
|
|
|
|
let handSide = 'right';
|
|
const handSliders = [];
|
|
const handHost = el('div');
|
|
const handStateHost = el('div');
|
|
|
|
function buildHand() {
|
|
const joints = spec?.dexhand_joints || [];
|
|
handSliders.length = 0;
|
|
const rows = el('div.stack', { style: { gap: '8px' } });
|
|
|
|
joints.forEach((name, index) => {
|
|
const control = range({ min: 0, max: 1.6, step: 0.02, value: 0, precision: 2, unit: ' rad' });
|
|
handSliders.push(control);
|
|
rows.appendChild(el('div', {},
|
|
el('div', { text: name, style: { fontSize: '11.5px', color: 'var(--text-2)', marginBottom: '2px' } }),
|
|
control,
|
|
));
|
|
});
|
|
|
|
handHost.replaceChildren(rows);
|
|
}
|
|
|
|
async function sendHand() {
|
|
await command('/api/hand', {
|
|
side: handSide,
|
|
positions: handSliders.map((s) => s.getValue()),
|
|
}, { successTitle: `${handSide} hand` });
|
|
}
|
|
|
|
async function applyHandPreset(preset) {
|
|
preset.positions.forEach((value, index) => handSliders[index]?.setValue(value));
|
|
await command('/api/hand', { side: handSide, preset: preset.key },
|
|
{ successTitle: `${preset.label} · ${handSide}` });
|
|
}
|
|
|
|
buildHand();
|
|
|
|
const handSendBtn = button('Send hand pose', sendHand, { style: 'primary' });
|
|
|
|
const presetRow = el('div.row.tight', {},
|
|
...(spec?.hand_presets || []).map((preset) =>
|
|
button(preset.label, () => applyHandPreset(preset), { size: 'sm' })),
|
|
);
|
|
|
|
const sideTabs = segmented(
|
|
[{ value: 'left', label: 'Left' }, { value: 'right', label: 'Right' }],
|
|
handSide,
|
|
(value) => { handSide = value; paintState(); },
|
|
);
|
|
|
|
root.appendChild(el('div.grid.pair', {},
|
|
card('Dexterous hand', { sub: 'HandCommandArray · OmniHand', actions: [sideTabs] },
|
|
el('div.stack', {},
|
|
presetRow,
|
|
handHost,
|
|
handSendBtn,
|
|
note('Disable native motor control first — run "aima em stop-app mc" on the robot — or the '
|
|
+ 'built-in controller and these commands will fight each other.', 'warning', '⚠'),
|
|
),
|
|
),
|
|
card('Hand state', { sub: 'HandStateArray' }, handStateHost),
|
|
));
|
|
|
|
/* ==================================================================
|
|
Live state
|
|
================================================================== */
|
|
|
|
function paintState() {
|
|
const s = store.state;
|
|
if (!s) return;
|
|
|
|
const group = groups.find((g) => g.key === activeGroup);
|
|
const live = s.joints?.[activeGroup] || [];
|
|
const limits = Object.fromEntries((group?.joints || []).map((j) => [j.name, j]));
|
|
|
|
if (!live.length) {
|
|
stateHost.replaceChildren(emptyState(
|
|
'No joint state yet',
|
|
`Nothing has been received on ${group?.state_topic || 'the joint state topic'}.`,
|
|
));
|
|
} else {
|
|
const rows = live.map((joint) => {
|
|
const limit = limits[joint.name] || { min_deg: -180, max_deg: 180 };
|
|
const deg = joint.position * RAD2DEG;
|
|
const range_ = (limit.max_deg - limit.min_deg) || 1;
|
|
const headroom = Math.min(deg - limit.min_deg, limit.max_deg - deg) / range_;
|
|
return {
|
|
label: joint.label || joint.name,
|
|
value: deg,
|
|
min: limit.min_deg,
|
|
max: limit.max_deg,
|
|
colorIndex: 0,
|
|
tone: joint.error ? 'critical' : headroom < 0.03 ? 'warning' : 'default',
|
|
};
|
|
});
|
|
|
|
stateHost.replaceChildren(
|
|
bars(rows, { precision: 1, unit: '°' }),
|
|
el('div', { style: { marginTop: '14px' } },
|
|
table([
|
|
{ key: 'name', label: 'Joint', get: (r) => r.label || r.name },
|
|
{ key: 'pos', label: 'Position', align: 'right', get: (r) => `${num(r.position * RAD2DEG, 1)}°` },
|
|
{ key: 'vel', label: 'Velocity', align: 'right', get: (r) => num(r.velocity, 3) },
|
|
{ key: 'eff', label: 'Effort', align: 'right', get: (r) => num(r.effort, 2) },
|
|
{
|
|
key: 'err', label: 'Fault',
|
|
get: (r) => (r.error ? badge(`code ${r.error}`, 'critical') : badge('ok', 'good')),
|
|
},
|
|
], live),
|
|
),
|
|
);
|
|
}
|
|
|
|
const hand = s.hand_state?.[handSide] || [];
|
|
const attached = (s.hand_type || 'None') !== 'None';
|
|
|
|
handStateHost.replaceChildren(
|
|
el('div.row', { style: { marginBottom: '10px' } },
|
|
badge(s.hand_type || 'unknown', attached ? 'accent' : 'warning'),
|
|
badge(`${hand.length} joints reported`),
|
|
),
|
|
hand.length
|
|
? bars(hand.map((joint) => ({
|
|
label: joint.name, value: joint.position, min: 0, max: 1.6, colorIndex: 2,
|
|
})), { precision: 2, unit: ' rad', diverging: false })
|
|
: emptyState(
|
|
attached ? 'No hand state yet' : 'No hand hardware attached',
|
|
attached
|
|
? 'Nothing received on /aima/hal/joint/hand/state.'
|
|
: 'GetHandType reports NONE for both sides on this robot, and the state message '
|
|
+ 'carries no joints. Attach an OmniHand or OmniPicker to use this panel.',
|
|
),
|
|
);
|
|
|
|
// Sending hand commands with no hand attached would silently do nothing.
|
|
for (const control of handSliders) {
|
|
control.querySelector('input').disabled = !attached;
|
|
}
|
|
handSendBtn.disabled = !attached;
|
|
for (const btn of presetRow.children) btn.disabled = !attached;
|
|
}
|
|
|
|
paintState();
|
|
|
|
let last = 0;
|
|
const unsubscribe = store.on('state', () => {
|
|
const now = Date.now();
|
|
if (now - last < 600) return;
|
|
last = now;
|
|
paintState();
|
|
});
|
|
|
|
return {
|
|
node: root,
|
|
dispose: () => {
|
|
unsubscribe();
|
|
},
|
|
};
|
|
},
|
|
};
|