1274 lines
38 KiB
HTML
1274 lines
38 KiB
HTML
<!doctype html>
|
|
<html lang="zh-CN">
|
|
<head>
|
|
<meta charset="UTF-8" />
|
|
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
|
|
<title>URDF KDL Three.js Test</title>
|
|
<style>
|
|
:root {
|
|
--bg: #eef2f5;
|
|
--surface: #ffffff;
|
|
--surface-2: #f6f8fa;
|
|
--line: #cfd8df;
|
|
--text: #17212b;
|
|
--muted: #5d6975;
|
|
--primary: #006d77;
|
|
--primary-strong: #005760;
|
|
--accent: #b84a36;
|
|
--ok: #1f7a4d;
|
|
--danger: #a7352a;
|
|
--shadow: 0 14px 36px rgba(23, 33, 43, 0.1);
|
|
}
|
|
|
|
* {
|
|
box-sizing: border-box;
|
|
}
|
|
|
|
html,
|
|
body {
|
|
width: 100%;
|
|
height: 100%;
|
|
}
|
|
|
|
body {
|
|
margin: 0;
|
|
font-family:
|
|
"Segoe UI",
|
|
"Microsoft YaHei",
|
|
Arial,
|
|
sans-serif;
|
|
color: var(--text);
|
|
background: var(--bg);
|
|
overflow: hidden;
|
|
}
|
|
|
|
button,
|
|
input,
|
|
select,
|
|
textarea {
|
|
font: inherit;
|
|
}
|
|
|
|
button {
|
|
min-height: 46px;
|
|
border: 1px solid transparent;
|
|
border-radius: 8px;
|
|
padding: 0 14px;
|
|
color: #fff;
|
|
background: var(--primary);
|
|
font-weight: 600;
|
|
cursor: pointer;
|
|
transition:
|
|
background 180ms ease,
|
|
border-color 180ms ease,
|
|
color 180ms ease,
|
|
opacity 180ms ease;
|
|
}
|
|
|
|
button:hover {
|
|
background: var(--primary-strong);
|
|
}
|
|
|
|
button:focus-visible,
|
|
input:focus-visible,
|
|
textarea:focus-visible {
|
|
outline: 3px solid rgba(0, 109, 119, 0.28);
|
|
outline-offset: 2px;
|
|
}
|
|
|
|
button.secondary {
|
|
color: var(--text);
|
|
border-color: var(--line);
|
|
background: #fff;
|
|
}
|
|
|
|
button.secondary:hover {
|
|
border-color: var(--primary);
|
|
color: var(--primary);
|
|
background: #f7fbfb;
|
|
}
|
|
|
|
button:disabled {
|
|
cursor: not-allowed;
|
|
opacity: 0.5;
|
|
}
|
|
|
|
label {
|
|
display: block;
|
|
margin: 0 0 6px;
|
|
color: var(--muted);
|
|
font-size: 13px;
|
|
font-weight: 600;
|
|
}
|
|
|
|
input,
|
|
textarea {
|
|
width: 100%;
|
|
min-height: 44px;
|
|
border: 1px solid var(--line);
|
|
border-radius: 8px;
|
|
padding: 9px 10px;
|
|
color: var(--text);
|
|
background: #fff;
|
|
}
|
|
|
|
input[type="range"] {
|
|
min-height: 28px;
|
|
padding: 0;
|
|
accent-color: var(--primary);
|
|
}
|
|
|
|
textarea {
|
|
min-height: 184px;
|
|
resize: vertical;
|
|
font:
|
|
12px/1.5 Consolas,
|
|
"Courier New",
|
|
monospace;
|
|
}
|
|
|
|
.app {
|
|
display: grid;
|
|
grid-template-columns: minmax(336px, 420px) minmax(0, 1fr);
|
|
height: 100dvh;
|
|
min-height: 0;
|
|
overflow: hidden;
|
|
}
|
|
|
|
.side {
|
|
display: flex;
|
|
flex-direction: column;
|
|
gap: 16px;
|
|
min-height: 0;
|
|
height: 100dvh;
|
|
overflow-x: hidden;
|
|
overflow-y: auto;
|
|
padding: 18px;
|
|
border-right: 1px solid var(--line);
|
|
background: var(--surface);
|
|
box-shadow: var(--shadow);
|
|
overscroll-behavior: contain;
|
|
z-index: 2;
|
|
}
|
|
|
|
.title-block {
|
|
display: grid;
|
|
gap: 8px;
|
|
}
|
|
|
|
.title-block h1 {
|
|
margin: 0;
|
|
font-size: 22px;
|
|
line-height: 1.2;
|
|
}
|
|
|
|
.status-row {
|
|
display: flex;
|
|
flex-wrap: wrap;
|
|
gap: 8px;
|
|
align-items: center;
|
|
}
|
|
|
|
.pill {
|
|
min-height: 28px;
|
|
display: inline-flex;
|
|
align-items: center;
|
|
border: 1px solid var(--line);
|
|
border-radius: 999px;
|
|
padding: 3px 10px;
|
|
color: var(--muted);
|
|
background: var(--surface-2);
|
|
font-size: 12px;
|
|
font-weight: 700;
|
|
}
|
|
|
|
.pill.ok {
|
|
border-color: rgba(31, 122, 77, 0.24);
|
|
color: var(--ok);
|
|
background: rgba(31, 122, 77, 0.08);
|
|
}
|
|
|
|
.pill.error {
|
|
border-color: rgba(167, 53, 42, 0.24);
|
|
color: var(--danger);
|
|
background: rgba(167, 53, 42, 0.08);
|
|
}
|
|
|
|
.panel {
|
|
display: grid;
|
|
gap: 12px;
|
|
border: 1px solid var(--line);
|
|
border-radius: 8px;
|
|
padding: 14px;
|
|
background: var(--surface-2);
|
|
}
|
|
|
|
.panel h2 {
|
|
margin: 0;
|
|
font-size: 15px;
|
|
line-height: 1.25;
|
|
}
|
|
|
|
.form-grid {
|
|
display: grid;
|
|
grid-template-columns: repeat(2, minmax(0, 1fr));
|
|
gap: 10px;
|
|
}
|
|
|
|
.field.full {
|
|
grid-column: 1 / -1;
|
|
}
|
|
|
|
.joint-row {
|
|
display: grid;
|
|
grid-template-columns: 34px minmax(0, 1fr) 84px;
|
|
gap: 8px;
|
|
align-items: center;
|
|
}
|
|
|
|
.joint-row span {
|
|
color: var(--muted);
|
|
font-size: 13px;
|
|
font-weight: 700;
|
|
}
|
|
|
|
.actions {
|
|
display: grid;
|
|
grid-template-columns: repeat(2, minmax(0, 1fr));
|
|
gap: 10px;
|
|
}
|
|
|
|
.scene-wrap {
|
|
position: relative;
|
|
min-width: 0;
|
|
min-height: 0;
|
|
height: 100dvh;
|
|
overflow: hidden;
|
|
background: #dfe7ec;
|
|
}
|
|
|
|
#scene {
|
|
display: block;
|
|
width: 100%;
|
|
height: 100%;
|
|
}
|
|
|
|
.scene-toolbar {
|
|
position: absolute;
|
|
top: 16px;
|
|
right: 16px;
|
|
display: flex;
|
|
flex-wrap: wrap;
|
|
justify-content: flex-end;
|
|
gap: 8px;
|
|
max-width: min(520px, calc(100% - 32px));
|
|
}
|
|
|
|
.metric {
|
|
min-width: 112px;
|
|
border: 1px solid rgba(207, 216, 223, 0.9);
|
|
border-radius: 8px;
|
|
padding: 8px 10px;
|
|
background: rgba(255, 255, 255, 0.86);
|
|
backdrop-filter: blur(8px);
|
|
}
|
|
|
|
.metric-label {
|
|
display: block;
|
|
color: var(--muted);
|
|
font-size: 11px;
|
|
font-weight: 700;
|
|
}
|
|
|
|
.metric-value {
|
|
display: block;
|
|
margin-top: 3px;
|
|
color: var(--text);
|
|
font-size: 13px;
|
|
font-variant-numeric: tabular-nums;
|
|
font-weight: 700;
|
|
word-break: break-word;
|
|
}
|
|
|
|
.request-output {
|
|
display: grid;
|
|
gap: 10px;
|
|
}
|
|
|
|
.split-output {
|
|
display: grid;
|
|
gap: 10px;
|
|
}
|
|
|
|
.toast {
|
|
position: fixed;
|
|
left: 50%;
|
|
bottom: 18px;
|
|
transform: translateX(-50%);
|
|
max-width: min(560px, calc(100vw - 32px));
|
|
border: 1px solid rgba(23, 33, 43, 0.12);
|
|
border-radius: 8px;
|
|
padding: 10px 12px;
|
|
color: #fff;
|
|
background: rgba(23, 33, 43, 0.92);
|
|
box-shadow: var(--shadow);
|
|
opacity: 0;
|
|
pointer-events: none;
|
|
transition:
|
|
opacity 180ms ease,
|
|
transform 180ms ease;
|
|
z-index: 10;
|
|
}
|
|
|
|
.toast.show {
|
|
opacity: 1;
|
|
transform: translateX(-50%) translateY(-4px);
|
|
}
|
|
|
|
@media (max-width: 940px) {
|
|
body {
|
|
overflow: auto;
|
|
}
|
|
|
|
.app {
|
|
grid-template-columns: 1fr;
|
|
height: auto;
|
|
min-height: 100dvh;
|
|
overflow: visible;
|
|
}
|
|
|
|
.side {
|
|
height: auto;
|
|
max-height: none;
|
|
overflow: visible;
|
|
border-right: 0;
|
|
border-bottom: 1px solid var(--line);
|
|
box-shadow: none;
|
|
}
|
|
|
|
.scene-wrap,
|
|
#scene {
|
|
height: 58dvh;
|
|
min-height: 360px;
|
|
}
|
|
}
|
|
|
|
@media (max-width: 560px) {
|
|
.side {
|
|
padding: 14px;
|
|
}
|
|
|
|
.form-grid,
|
|
.actions {
|
|
grid-template-columns: 1fr;
|
|
}
|
|
|
|
.joint-row {
|
|
grid-template-columns: 30px minmax(0, 1fr);
|
|
}
|
|
|
|
.joint-row input[type="number"] {
|
|
grid-column: 2;
|
|
}
|
|
}
|
|
|
|
@media (prefers-reduced-motion: reduce) {
|
|
*,
|
|
*::before,
|
|
*::after {
|
|
scroll-behavior: auto !important;
|
|
transition-duration: 0.01ms !important;
|
|
animation-duration: 0.01ms !important;
|
|
}
|
|
}
|
|
</style>
|
|
</head>
|
|
<body>
|
|
<main class="app">
|
|
<aside class="side" aria-label="运动学控制">
|
|
<section class="title-block">
|
|
<h1>URDF / Orocos KDL 运动学测试</h1>
|
|
<div class="status-row">
|
|
<span class="pill" id="wasm-status">WASM 未初始化</span>
|
|
<span class="pill" id="solver-status">等待请求</span>
|
|
</div>
|
|
</section>
|
|
|
|
<section class="panel">
|
|
<h2>机器人</h2>
|
|
<div class="form-grid">
|
|
<div class="field full">
|
|
<label for="robot-uuid">robot_uuid</label>
|
|
<input id="robot-uuid" value="abb_irb120_3_58" />
|
|
</div>
|
|
<div class="field full">
|
|
<label for="urdf-file">URDF 文件</label>
|
|
<input id="urdf-file" type="file" accept=".xml,.urdf" />
|
|
</div>
|
|
</div>
|
|
<div class="actions">
|
|
<button id="init-btn" type="button">初始化</button>
|
|
<button id="register-urdf-btn" class="secondary" type="button">
|
|
注册 URDF
|
|
</button>
|
|
<button id="reset-btn" class="secondary" type="button">零位</button>
|
|
</div>
|
|
</section>
|
|
|
|
<section class="panel">
|
|
<h2>关节角</h2>
|
|
<div id="joint-controls"></div>
|
|
<div class="actions">
|
|
<button id="fk-btn" type="button">正解</button>
|
|
<button id="singularity-btn" class="secondary" type="button">
|
|
奇异点
|
|
</button>
|
|
<button id="target-current-btn" class="secondary" type="button">
|
|
取当前位姿
|
|
</button>
|
|
</div>
|
|
</section>
|
|
|
|
<section class="panel">
|
|
<h2>逆解目标</h2>
|
|
<div class="form-grid">
|
|
<div class="field">
|
|
<label for="target-x">X</label>
|
|
<input id="target-x" type="number" step="0.001" value="0" />
|
|
</div>
|
|
<div class="field">
|
|
<label for="target-y">Y</label>
|
|
<input id="target-y" type="number" step="0.001" value="0" />
|
|
</div>
|
|
<div class="field">
|
|
<label for="target-z">Z</label>
|
|
<input id="target-z" type="number" step="0.001" value="0" />
|
|
</div>
|
|
<div class="field">
|
|
<label for="target-qx">QX</label>
|
|
<input id="target-qx" type="number" step="0.001" value="0" />
|
|
</div>
|
|
<div class="field">
|
|
<label for="target-qy">QY</label>
|
|
<input id="target-qy" type="number" step="0.001" value="0" />
|
|
</div>
|
|
<div class="field">
|
|
<label for="target-qz">QZ</label>
|
|
<input id="target-qz" type="number" step="0.001" value="0" />
|
|
</div>
|
|
<div class="field full">
|
|
<label for="target-qw">QW</label>
|
|
<input id="target-qw" type="number" step="0.001" value="1" />
|
|
</div>
|
|
</div>
|
|
<div class="actions">
|
|
<button id="ik-btn" type="button">逆解</button>
|
|
<button id="apply-ik-btn" class="secondary" type="button" disabled>
|
|
应用解
|
|
</button>
|
|
</div>
|
|
</section>
|
|
|
|
<section class="panel request-output">
|
|
<h2>JSON</h2>
|
|
<div class="split-output">
|
|
<div>
|
|
<label for="request-json">请求</label>
|
|
<textarea id="request-json" readonly></textarea>
|
|
</div>
|
|
<div>
|
|
<label for="response-json">响应</label>
|
|
<textarea id="response-json" readonly></textarea>
|
|
</div>
|
|
</div>
|
|
</section>
|
|
</aside>
|
|
|
|
<section class="scene-wrap" aria-label="三维视图">
|
|
<canvas id="scene"></canvas>
|
|
<div class="scene-toolbar">
|
|
<div class="metric">
|
|
<span class="metric-label">TCP</span>
|
|
<span class="metric-value" id="tcp-value">-</span>
|
|
</div>
|
|
<div class="metric">
|
|
<span class="metric-label">关节数</span>
|
|
<span class="metric-value" id="joint-count-value">0</span>
|
|
</div>
|
|
<div class="metric">
|
|
<span class="metric-label">奇异风险</span>
|
|
<span class="metric-value" id="singularity-value">-</span>
|
|
</div>
|
|
<div class="metric">
|
|
<span class="metric-label">three.js</span>
|
|
<span class="metric-value">0.184.0</span>
|
|
</div>
|
|
</div>
|
|
</section>
|
|
</main>
|
|
|
|
<div class="toast" id="toast" role="status" aria-live="polite"></div>
|
|
|
|
<script src="./wasm/smart_math.js"></script>
|
|
<script type="importmap">
|
|
{
|
|
"imports": {
|
|
"three": "https://unpkg.com/three@0.184.0/build/three.module.js",
|
|
"three/addons/": "https://unpkg.com/three@0.184.0/examples/jsm/"
|
|
}
|
|
}
|
|
</script>
|
|
<script type="module">
|
|
import * as THREE from "three";
|
|
import { OrbitControls } from "three/addons/controls/OrbitControls.js";
|
|
import SmartWasmAPI from "./wasm/smart_api_wrapper.js";
|
|
|
|
THREE.Object3D.DEFAULT_UP.set(0, 0, 1);
|
|
|
|
const api = new SmartWasmAPI();
|
|
const jointCount = 6;
|
|
const state = {
|
|
initialized: false,
|
|
joints: Array(jointCount).fill(0),
|
|
latestTcp: null,
|
|
latestIkJoints: null,
|
|
lastResponse: null,
|
|
};
|
|
|
|
const elements = {
|
|
canvas: document.getElementById("scene"),
|
|
wasmStatus: document.getElementById("wasm-status"),
|
|
solverStatus: document.getElementById("solver-status"),
|
|
robotUuid: document.getElementById("robot-uuid"),
|
|
urdfFile: document.getElementById("urdf-file"),
|
|
jointControls: document.getElementById("joint-controls"),
|
|
requestJson: document.getElementById("request-json"),
|
|
responseJson: document.getElementById("response-json"),
|
|
targetX: document.getElementById("target-x"),
|
|
targetY: document.getElementById("target-y"),
|
|
targetZ: document.getElementById("target-z"),
|
|
targetQx: document.getElementById("target-qx"),
|
|
targetQy: document.getElementById("target-qy"),
|
|
targetQz: document.getElementById("target-qz"),
|
|
targetQw: document.getElementById("target-qw"),
|
|
tcpValue: document.getElementById("tcp-value"),
|
|
jointCountValue: document.getElementById("joint-count-value"),
|
|
singularityValue: document.getElementById("singularity-value"),
|
|
toast: document.getElementById("toast"),
|
|
initBtn: document.getElementById("init-btn"),
|
|
registerUrdfBtn: document.getElementById("register-urdf-btn"),
|
|
resetBtn: document.getElementById("reset-btn"),
|
|
fkBtn: document.getElementById("fk-btn"),
|
|
singularityBtn: document.getElementById("singularity-btn"),
|
|
ikBtn: document.getElementById("ik-btn"),
|
|
applyIkBtn: document.getElementById("apply-ik-btn"),
|
|
targetCurrentBtn: document.getElementById("target-current-btn"),
|
|
};
|
|
|
|
const scene = new THREE.Scene();
|
|
scene.background = new THREE.Color(0xdfe7ec);
|
|
|
|
const camera = new THREE.PerspectiveCamera(45, 1, 0.01, 80);
|
|
camera.up.set(0, 0, 1);
|
|
camera.position.set(1.18, -1.68, 0.92);
|
|
|
|
const renderer = new THREE.WebGLRenderer({
|
|
canvas: elements.canvas,
|
|
antialias: true,
|
|
});
|
|
renderer.setPixelRatio(Math.min(window.devicePixelRatio || 1, 2));
|
|
renderer.shadowMap.enabled = true;
|
|
renderer.shadowMap.type = THREE.PCFSoftShadowMap;
|
|
|
|
const controls = new OrbitControls(camera, renderer.domElement);
|
|
controls.enableDamping = true;
|
|
controls.screenSpacePanning = false;
|
|
controls.target.set(0.18, 0, 0.34);
|
|
controls.update();
|
|
|
|
window.__robotKinematicsDebug = {
|
|
// 获取当前相机状态，用于验证按钮操作不会改变视角。
|
|
getCameraState() {
|
|
return {
|
|
position: camera.position.toArray(),
|
|
up: camera.up.toArray(),
|
|
target: controls.target.toArray(),
|
|
};
|
|
},
|
|
// 获取 three.js 默认上方向，用于验证场景为 Z 轴向上。
|
|
getDefaultUp() {
|
|
return THREE.Object3D.DEFAULT_UP.toArray();
|
|
},
|
|
};
|
|
|
|
const robotGroup = new THREE.Group();
|
|
const targetGroup = new THREE.Group();
|
|
scene.add(robotGroup, targetGroup);
|
|
|
|
const linkMaterial = new THREE.MeshStandardMaterial({
|
|
color: 0x006d77,
|
|
metalness: 0.18,
|
|
roughness: 0.42,
|
|
});
|
|
const jointMaterial = new THREE.MeshStandardMaterial({
|
|
color: 0xb84a36,
|
|
metalness: 0.08,
|
|
roughness: 0.5,
|
|
});
|
|
const tcpMaterial = new THREE.MeshStandardMaterial({
|
|
color: 0x1f7a4d,
|
|
emissive: 0x0c3221,
|
|
emissiveIntensity: 0.14,
|
|
});
|
|
const targetMaterial = new THREE.MeshStandardMaterial({
|
|
color: 0x6f3cc3,
|
|
metalness: 0.08,
|
|
roughness: 0.46,
|
|
});
|
|
|
|
scene.add(new THREE.HemisphereLight(0xffffff, 0x8ca2ad, 2.1));
|
|
const sun = new THREE.DirectionalLight(0xffffff, 2.2);
|
|
sun.position.set(3, -4, 5);
|
|
sun.castShadow = true;
|
|
sun.shadow.mapSize.set(2048, 2048);
|
|
scene.add(sun);
|
|
|
|
const grid = new THREE.GridHelper(2.6, 26, 0x60727e, 0xb9c4ca);
|
|
grid.rotation.x = Math.PI / 2;
|
|
scene.add(grid);
|
|
scene.add(new THREE.AxesHelper(0.36));
|
|
|
|
// 创建关节滑块和数值输入。
|
|
function createJointControls() {
|
|
elements.jointControls.innerHTML = "";
|
|
for (let index = 0; index < jointCount; index += 1) {
|
|
const row = document.createElement("div");
|
|
row.className = "joint-row";
|
|
|
|
const label = document.createElement("span");
|
|
label.textContent = `J${index + 1}`;
|
|
|
|
const range = document.createElement("input");
|
|
range.type = "range";
|
|
range.min = "-3.142";
|
|
range.max = "3.142";
|
|
range.step = "0.001";
|
|
range.value = "0";
|
|
range.dataset.index = String(index);
|
|
range.setAttribute("aria-label", `J${index + 1} 弧度滑块`);
|
|
|
|
const number = document.createElement("input");
|
|
number.type = "number";
|
|
number.min = "-3.142";
|
|
number.max = "3.142";
|
|
number.step = "0.001";
|
|
number.value = "0.000";
|
|
number.dataset.index = String(index);
|
|
number.setAttribute("aria-label", `J${index + 1} 弧度数值`);
|
|
|
|
row.append(label, range, number);
|
|
elements.jointControls.append(row);
|
|
|
|
range.addEventListener("input", handleJointInput);
|
|
number.addEventListener("change", handleJointInput);
|
|
}
|
|
}
|
|
|
|
// 响应关节输入变化，并保持滑块和数值框同步。
|
|
function handleJointInput(event) {
|
|
const index = Number(event.target.dataset.index);
|
|
const nextValue = clampJoint(Number(event.target.value));
|
|
state.joints[index] = nextValue;
|
|
syncJointControls();
|
|
}
|
|
|
|
// 限制关节输入范围。
|
|
function clampJoint(value) {
|
|
if (!Number.isFinite(value)) {
|
|
return 0;
|
|
}
|
|
return Math.max(-3.142, Math.min(3.142, value));
|
|
}
|
|
|
|
// 将当前关节状态写回所有控件。
|
|
function syncJointControls() {
|
|
const ranges = elements.jointControls.querySelectorAll(
|
|
'input[type="range"]',
|
|
);
|
|
const numbers = elements.jointControls.querySelectorAll(
|
|
'input[type="number"]',
|
|
);
|
|
|
|
ranges.forEach((input, index) => {
|
|
input.value = state.joints[index].toFixed(3);
|
|
});
|
|
numbers.forEach((input, index) => {
|
|
input.value = state.joints[index].toFixed(3);
|
|
});
|
|
}
|
|
|
|
// 生成接口需要的关节字符串。
|
|
function buildJointString(joints = state.joints) {
|
|
return joints.map((value) => Number(value).toFixed(6)).join(",");
|
|
}
|
|
|
|
// 生成正解请求。
|
|
function buildForwardAllJointsRequest() {
|
|
return {
|
|
msg: "threejs fk all joints",
|
|
req_code: `THREE_FK_${Date.now()}`,
|
|
req_from: "threejs_test",
|
|
req_cmd: "Cmd_Kinematics_forward_all_joints",
|
|
req_param: {
|
|
robot_uuid: elements.robotUuid.value.trim(),
|
|
joints_str: buildJointString(),
|
|
},
|
|
};
|
|
}
|
|
|
|
// 生成 TCP 正解请求。
|
|
function buildForwardPoseRequest() {
|
|
return {
|
|
msg: "threejs fk tcp",
|
|
req_code: `THREE_TCP_${Date.now()}`,
|
|
req_from: "threejs_test",
|
|
req_cmd: "Cmd_Kinematics_forward_pose_str",
|
|
req_param: {
|
|
robot_uuid: elements.robotUuid.value.trim(),
|
|
q_init_str: buildJointString(),
|
|
},
|
|
};
|
|
}
|
|
|
|
// 生成奇异点检测请求。
|
|
function buildSingularityRequest() {
|
|
return {
|
|
msg: "threejs singularity check",
|
|
req_code: `THREE_SINGULARITY_${Date.now()}`,
|
|
req_from: "threejs_test",
|
|
req_cmd: "Cmd_Kinematics_check_singularity",
|
|
req_param: {
|
|
robot_uuid: elements.robotUuid.value.trim(),
|
|
joints_str: buildJointString(),
|
|
},
|
|
};
|
|
}
|
|
|
|
// 生成逆解请求。
|
|
function buildInverseRequest() {
|
|
const pose = [
|
|
elements.targetX.value,
|
|
elements.targetY.value,
|
|
elements.targetZ.value,
|
|
elements.targetQx.value,
|
|
elements.targetQy.value,
|
|
elements.targetQz.value,
|
|
elements.targetQw.value,
|
|
]
|
|
.map((value) => Number(value || 0).toFixed(6))
|
|
.join(",");
|
|
|
|
return {
|
|
msg: "threejs ik",
|
|
req_code: `THREE_IK_${Date.now()}`,
|
|
req_from: "threejs_test",
|
|
req_cmd: "Cmd_Kinematics_inverse_pose_str_NoDifference",
|
|
req_param: {
|
|
robot_uuid: elements.robotUuid.value.trim(),
|
|
pose_str: pose,
|
|
q_init_str: buildJointString(),
|
|
},
|
|
};
|
|
}
|
|
|
|
// 初始化 WASM 业务接口。
|
|
async function initializeWasm() {
|
|
if (state.initialized) {
|
|
return;
|
|
}
|
|
|
|
setBusy(true, "正在初始化");
|
|
try {
|
|
await api.initialize();
|
|
const response = await api.initializeBusinessApi();
|
|
state.initialized = response.success === true;
|
|
elements.wasmStatus.textContent = state.initialized
|
|
? "WASM 已初始化"
|
|
: "WASM 初始化失败";
|
|
elements.wasmStatus.className = `pill ${
|
|
state.initialized ? "ok" : "error"
|
|
}`;
|
|
renderJson(null, response);
|
|
await runForwardKinematics();
|
|
} catch (error) {
|
|
showError(error);
|
|
} finally {
|
|
setBusy(false);
|
|
}
|
|
}
|
|
|
|
// 读取本地 URDF 文件并注册为当前 robot_uuid。
|
|
async function registerUrdfFromFile() {
|
|
await ensureInitialized();
|
|
const file = elements.urdfFile.files?.[0];
|
|
const robotUuid = elements.robotUuid.value.trim();
|
|
|
|
if (!file) {
|
|
showToast("请选择 URDF 文件");
|
|
return;
|
|
}
|
|
|
|
if (!robotUuid) {
|
|
showToast("请先填写 robot_uuid");
|
|
return;
|
|
}
|
|
|
|
setBusy(true, "正在注册 URDF");
|
|
try {
|
|
const urdfText = await file.text();
|
|
const request = {
|
|
msg: "threejs init robot from urdf",
|
|
req_code: `THREE_INIT_URDF_${Date.now()}`,
|
|
req_from: "threejs_test",
|
|
req_cmd: "Cmd_InitRobot",
|
|
req_param: {
|
|
robot_uuid: robotUuid,
|
|
force_update: true,
|
|
urdf_base64: encodeUtf8Base64(urdfText),
|
|
},
|
|
};
|
|
|
|
renderJson(request, null);
|
|
const response = await api.processBusinessRequest(request);
|
|
renderJson(request, response);
|
|
|
|
if (response.success && response.res_data?.success) {
|
|
state.latestIkJoints = null;
|
|
elements.applyIkBtn.disabled = true;
|
|
setStatus("URDF 注册完成", true);
|
|
await runForwardKinematics();
|
|
} else {
|
|
setStatus("URDF 注册失败", false);
|
|
showToast(response.res_data?.message || response.msg || "URDF 注册失败");
|
|
}
|
|
} catch (error) {
|
|
showError(error);
|
|
} finally {
|
|
setBusy(false);
|
|
}
|
|
}
|
|
|
|
// 将 URDF 文本编码成接口需要的 UTF-8 Base64 字符串。
|
|
function encodeUtf8Base64(value) {
|
|
const bytes = new TextEncoder().encode(value);
|
|
let binary = "";
|
|
bytes.forEach((byte) => {
|
|
binary += String.fromCharCode(byte);
|
|
});
|
|
return btoa(binary);
|
|
}
|
|
|
|
// 确保调用算法前已经完成初始化。
|
|
async function ensureInitialized() {
|
|
if (!state.initialized) {
|
|
await initializeWasm();
|
|
}
|
|
}
|
|
|
|
// 执行正解并刷新三维场景。
|
|
async function runForwardKinematics() {
|
|
await ensureInitialized();
|
|
setBusy(true, "正解计算中");
|
|
|
|
const skeletonRequest = buildForwardAllJointsRequest();
|
|
const tcpRequest = buildForwardPoseRequest();
|
|
|
|
try {
|
|
renderJson(skeletonRequest, null);
|
|
const [skeletonResponse, tcpResponse] = await Promise.all([
|
|
api.processBusinessRequest(skeletonRequest),
|
|
api.processBusinessRequest(tcpRequest),
|
|
]);
|
|
|
|
state.lastResponse = skeletonResponse;
|
|
state.latestTcp = extractTcp(tcpResponse);
|
|
renderRobot(extractObjStates(skeletonResponse));
|
|
updateTcpTargetFromCurrent();
|
|
renderJson(skeletonRequest, {
|
|
skeleton: skeletonResponse,
|
|
tcp: tcpResponse,
|
|
});
|
|
setStatus("正解完成", true);
|
|
} catch (error) {
|
|
showError(error);
|
|
} finally {
|
|
setBusy(false);
|
|
}
|
|
}
|
|
|
|
// 执行奇异点检测并显示风险等级。
|
|
async function runSingularityCheck() {
|
|
await ensureInitialized();
|
|
setBusy(true, "奇异点检测中");
|
|
|
|
const request = buildSingularityRequest();
|
|
try {
|
|
renderJson(request, null);
|
|
const response = await api.processBusinessRequest(request);
|
|
renderJson(request, response);
|
|
updateSingularityMetric(response?.res_data);
|
|
|
|
const ok = response.success && response.res_data?.success;
|
|
setStatus(ok ? "奇异点检测完成" : "奇异点检测失败", Boolean(ok));
|
|
} catch (error) {
|
|
showError(error);
|
|
} finally {
|
|
setBusy(false);
|
|
}
|
|
}
|
|
|
|
// 执行逆解并保存待应用的关节解。
|
|
async function runInverseKinematics() {
|
|
await ensureInitialized();
|
|
setBusy(true, "逆解计算中");
|
|
drawTargetMarker(readTargetPose());
|
|
|
|
const request = buildInverseRequest();
|
|
try {
|
|
renderJson(request, null);
|
|
const response = await api.processBusinessRequest(request);
|
|
const solution = extractIkSolution(response);
|
|
state.latestIkJoints = solution;
|
|
elements.applyIkBtn.disabled = !solution;
|
|
renderJson(request, response);
|
|
setStatus(solution ? "逆解完成" : "逆解无解", Boolean(solution));
|
|
if (!solution) {
|
|
showToast("未取得可应用的关节解");
|
|
}
|
|
} catch (error) {
|
|
showError(error);
|
|
} finally {
|
|
setBusy(false);
|
|
}
|
|
}
|
|
|
|
// 应用最近一次逆解结果。
|
|
async function applyInverseSolution() {
|
|
if (!state.latestIkJoints) {
|
|
return;
|
|
}
|
|
state.joints = state.latestIkJoints.slice(0, jointCount).map(clampJoint);
|
|
syncJointControls();
|
|
await runForwardKinematics();
|
|
}
|
|
|
|
// 从正解响应中抽取关节对象状态。
|
|
function extractObjStates(response) {
|
|
const frames =
|
|
response?.res_data?.OPERATION?.OPERATION?.frames ??
|
|
response?.res_data?.OPERATION?.frames ??
|
|
[];
|
|
const frame = frames[0] || {};
|
|
return Array.isArray(frame.objStates) ? frame.objStates : [];
|
|
}
|
|
|
|
// 从 TCP 正解响应中抽取末端位姿。
|
|
function extractTcp(response) {
|
|
const position = response?.res_data?.position;
|
|
const orientation = response?.res_data?.orientation;
|
|
if (!Array.isArray(position) || !Array.isArray(orientation)) {
|
|
return null;
|
|
}
|
|
|
|
return {
|
|
position: position.map(Number),
|
|
orientation: orientation.map(Number),
|
|
};
|
|
}
|
|
|
|
// 从逆解响应中抽取最后一组关节解。
|
|
function extractIkSolution(response) {
|
|
const joints = response?.res_data?.joints;
|
|
if (!Array.isArray(joints) || joints.length === 0) {
|
|
return null;
|
|
}
|
|
|
|
const last = joints[joints.length - 1];
|
|
if (!Array.isArray(last) || last.length < jointCount) {
|
|
return null;
|
|
}
|
|
|
|
return last.map(Number);
|
|
}
|
|
|
|
// 将对象状态转换为累积世界位姿。
|
|
function toWorldPoints(objStates) {
|
|
const matrix = new THREE.Matrix4();
|
|
const world = new THREE.Matrix4();
|
|
const position = new THREE.Vector3();
|
|
const quaternion = new THREE.Quaternion();
|
|
const scale = new THREE.Vector3(1, 1, 1);
|
|
const points = [new THREE.Vector3(0, 0, 0)];
|
|
|
|
for (const item of objStates) {
|
|
position.set(Number(item.tx) || 0, Number(item.ty) || 0, Number(item.tz) || 0);
|
|
quaternion.set(
|
|
Number(item.qx) || 0,
|
|
Number(item.qy) || 0,
|
|
Number(item.qz) || 0,
|
|
Number(item.qw) || 1,
|
|
);
|
|
matrix.compose(position, quaternion.normalize(), scale);
|
|
world.multiply(matrix);
|
|
points.push(new THREE.Vector3().setFromMatrixPosition(world));
|
|
}
|
|
|
|
return points;
|
|
}
|
|
|
|
// 渲染机器人骨架、关节和 TCP。
|
|
function renderRobot(objStates) {
|
|
clearGroup(robotGroup);
|
|
const points = toWorldPoints(objStates);
|
|
elements.jointCountValue.textContent = String(Math.max(0, points.length - 1));
|
|
|
|
points.forEach((point, index) => {
|
|
const material = index === points.length - 1 ? tcpMaterial : jointMaterial;
|
|
const radius = index === points.length - 1 ? 0.032 : 0.026;
|
|
const joint = new THREE.Mesh(new THREE.SphereGeometry(radius, 32, 16), material);
|
|
joint.position.copy(point);
|
|
joint.castShadow = true;
|
|
robotGroup.add(joint);
|
|
});
|
|
|
|
for (let index = 0; index < points.length - 1; index += 1) {
|
|
const link = createLink(points[index], points[index + 1]);
|
|
if (link) {
|
|
robotGroup.add(link);
|
|
}
|
|
}
|
|
|
|
if (state.latestTcp) {
|
|
const tcp = new THREE.AxesHelper(0.16);
|
|
tcp.position.fromArray(state.latestTcp.position);
|
|
const q = state.latestTcp.orientation;
|
|
tcp.quaternion.set(q[0], q[1], q[2], q[3]).normalize();
|
|
robotGroup.add(tcp);
|
|
elements.tcpValue.textContent = state.latestTcp.position
|
|
.map((value) => Number(value).toFixed(3))
|
|
.join(", ");
|
|
}
|
|
|
|
}
|
|
|
|
// 创建两个关节点之间的连杆圆柱。
|
|
function createLink(start, end) {
|
|
const direction = new THREE.Vector3().subVectors(end, start);
|
|
const length = direction.length();
|
|
if (length < 0.001) {
|
|
return null;
|
|
}
|
|
|
|
const geometry = new THREE.CylinderGeometry(0.014, 0.014, length, 20);
|
|
const mesh = new THREE.Mesh(geometry, linkMaterial);
|
|
const midpoint = new THREE.Vector3().addVectors(start, end).multiplyScalar(0.5);
|
|
mesh.position.copy(midpoint);
|
|
mesh.quaternion.setFromUnitVectors(
|
|
new THREE.Vector3(0, 1, 0),
|
|
direction.clone().normalize(),
|
|
);
|
|
mesh.castShadow = true;
|
|
mesh.receiveShadow = true;
|
|
return mesh;
|
|
}
|
|
|
|
// 清空三维分组。
|
|
function clearGroup(group) {
|
|
while (group.children.length > 0) {
|
|
const child = group.children.pop();
|
|
child.traverse((object) => {
|
|
if (object.geometry) {
|
|
object.geometry.dispose();
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|
|
// 将当前 TCP 位姿写入逆解目标输入框。
|
|
function updateTcpTargetFromCurrent() {
|
|
if (!state.latestTcp) {
|
|
return;
|
|
}
|
|
|
|
const [x, y, z] = state.latestTcp.position;
|
|
const [qx, qy, qz, qw] = state.latestTcp.orientation;
|
|
elements.targetX.value = Number(x).toFixed(6);
|
|
elements.targetY.value = Number(y).toFixed(6);
|
|
elements.targetZ.value = Number(z).toFixed(6);
|
|
elements.targetQx.value = Number(qx).toFixed(6);
|
|
elements.targetQy.value = Number(qy).toFixed(6);
|
|
elements.targetQz.value = Number(qz).toFixed(6);
|
|
elements.targetQw.value = Number(qw).toFixed(6);
|
|
drawTargetMarker(readTargetPose());
|
|
}
|
|
|
|
// 读取逆解目标输入框。
|
|
function readTargetPose() {
|
|
return {
|
|
position: [
|
|
Number(elements.targetX.value) || 0,
|
|
Number(elements.targetY.value) || 0,
|
|
Number(elements.targetZ.value) || 0,
|
|
],
|
|
orientation: [
|
|
Number(elements.targetQx.value) || 0,
|
|
Number(elements.targetQy.value) || 0,
|
|
Number(elements.targetQz.value) || 0,
|
|
Number(elements.targetQw.value) || 1,
|
|
],
|
|
};
|
|
}
|
|
|
|
// 绘制逆解目标点。
|
|
function drawTargetMarker(targetPose) {
|
|
clearGroup(targetGroup);
|
|
const marker = new THREE.Mesh(
|
|
new THREE.TorusGeometry(0.056, 0.004, 12, 48),
|
|
targetMaterial,
|
|
);
|
|
marker.position.fromArray(targetPose.position);
|
|
targetGroup.add(marker);
|
|
|
|
const axes = new THREE.AxesHelper(0.18);
|
|
axes.position.fromArray(targetPose.position);
|
|
const q = targetPose.orientation;
|
|
axes.quaternion.set(q[0], q[1], q[2], q[3]).normalize();
|
|
targetGroup.add(axes);
|
|
}
|
|
|
|
// 渲染请求与响应 JSON。
|
|
function renderJson(request, response) {
|
|
if (request) {
|
|
elements.requestJson.value = JSON.stringify(request, null, 2);
|
|
}
|
|
if (response !== null) {
|
|
elements.responseJson.value = response
|
|
? JSON.stringify(response, null, 2)
|
|
: "";
|
|
}
|
|
}
|
|
|
|
// 更新奇异点风险指标。
|
|
function updateSingularityMetric(result) {
|
|
if (!result?.success) {
|
|
elements.singularityValue.textContent = "-";
|
|
return;
|
|
}
|
|
|
|
const riskText = {
|
|
normal: "正常",
|
|
warning: "接近",
|
|
singular: "奇异",
|
|
}[result.risk_level] ?? result.risk_level;
|
|
const minSv = Number(result.min_singular_value);
|
|
elements.singularityValue.textContent = `${riskText} / ${Number.isFinite(minSv) ? minSv.toExponential(2) : "-"}`;
|
|
}
|
|
|
|
// 设置按钮忙碌状态。
|
|
function setBusy(isBusy, label) {
|
|
[
|
|
elements.initBtn,
|
|
elements.registerUrdfBtn,
|
|
elements.resetBtn,
|
|
elements.fkBtn,
|
|
elements.singularityBtn,
|
|
elements.ikBtn,
|
|
elements.targetCurrentBtn,
|
|
].forEach((button) => {
|
|
button.disabled = isBusy;
|
|
});
|
|
elements.applyIkBtn.disabled = isBusy || !state.latestIkJoints;
|
|
if (label) {
|
|
setStatus(label, null);
|
|
}
|
|
}
|
|
|
|
// 设置求解状态。
|
|
function setStatus(message, ok) {
|
|
elements.solverStatus.textContent = message;
|
|
elements.solverStatus.className =
|
|
ok === null ? "pill" : `pill ${ok ? "ok" : "error"}`;
|
|
}
|
|
|
|
// 显示错误状态。
|
|
function showError(error) {
|
|
const message = error?.message || String(error);
|
|
setStatus("请求失败", false);
|
|
elements.responseJson.value = message;
|
|
showToast(message);
|
|
}
|
|
|
|
// 显示短提示。
|
|
function showToast(message) {
|
|
elements.toast.textContent = message;
|
|
elements.toast.classList.add("show");
|
|
window.clearTimeout(showToast.timer);
|
|
showToast.timer = window.setTimeout(() => {
|
|
elements.toast.classList.remove("show");
|
|
}, 3600);
|
|
}
|
|
|
|
// 重置关节到零位。
|
|
async function resetJoints() {
|
|
state.joints = Array(jointCount).fill(0);
|
|
state.latestIkJoints = null;
|
|
elements.applyIkBtn.disabled = true;
|
|
syncJointControls();
|
|
await runForwardKinematics();
|
|
}
|
|
|
|
// 处理窗口尺寸变化。
|
|
function resizeRenderer() {
|
|
const rect = elements.canvas.getBoundingClientRect();
|
|
const width = Math.max(1, Math.floor(rect.width));
|
|
const height = Math.max(1, Math.floor(rect.height));
|
|
renderer.setSize(width, height, false);
|
|
camera.aspect = width / height;
|
|
camera.updateProjectionMatrix();
|
|
}
|
|
|
|
// 启动三维渲染循环。
|
|
function animate() {
|
|
resizeRenderer();
|
|
controls.update();
|
|
renderer.render(scene, camera);
|
|
window.requestAnimationFrame(animate);
|
|
}
|
|
|
|
elements.initBtn.addEventListener("click", initializeWasm);
|
|
elements.registerUrdfBtn.addEventListener("click", registerUrdfFromFile);
|
|
elements.resetBtn.addEventListener("click", resetJoints);
|
|
elements.fkBtn.addEventListener("click", runForwardKinematics);
|
|
elements.singularityBtn.addEventListener("click", runSingularityCheck);
|
|
elements.ikBtn.addEventListener("click", runInverseKinematics);
|
|
elements.applyIkBtn.addEventListener("click", applyInverseSolution);
|
|
elements.targetCurrentBtn.addEventListener("click", updateTcpTargetFromCurrent);
|
|
[
|
|
elements.targetX,
|
|
elements.targetY,
|
|
elements.targetZ,
|
|
elements.targetQx,
|
|
elements.targetQy,
|
|
elements.targetQz,
|
|
elements.targetQw,
|
|
].forEach((input) => input.addEventListener("change", () => drawTargetMarker(readTargetPose())));
|
|
|
|
createJointControls();
|
|
syncJointControls();
|
|
drawTargetMarker(readTargetPose());
|
|
animate();
|
|
initializeWasm();
|
|
</script>
|
|
</body>
|
|
</html>
|