增加URDF文件注册测试入口

This commit is contained in:
zhangshun
2026-06-16 12:20:15 +08:00
parent eeef7594fd
commit aaf2c0cc41
2 changed files with 381 additions and 0 deletions

309
docs/urdf.xml Normal file
View File

@@ -0,0 +1,309 @@
<robot
xmlns:xacro="http://ros.org/wiki/xacro" name="ABB_IRB_120" uuid="8bc48ac0-6570-4232-aa91-3367771c1069">
<link name="base_link" uuid="9c88ec70-a337-4d16-94c5-92ccff72c1a5">
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_1.stl"/>
</geometry>
<material name="">
<color rgba="0.7372549 0.3490196 0.1607843 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_1.stl"/>
</geometry>
<material name="">
<color rgba="1 1 0 1"/>
</material>
</collision>
</link>
<link name="link_1" uuid="09059ca4-6c99-4a5f-a474-8ba964e612c4">
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_2.stl"/>
</geometry>
<material name="">
<color rgba="0.7372549 0.3490196 0.1607843 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_2.stl"/>
</geometry>
<material name="">
<color rgba="1 1 0 1"/>
</material>
</collision>
</link>
<link name="link_2" uuid="3f38944a-9e19-4aba-a46e-2cc2d2611b91">
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_3.stl"/>
</geometry>
<material name="">
<color rgba="0.7372549 0.3490196 0.1607843 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_3.stl"/>
</geometry>
<material name="">
<color rgba="1 1 0 1"/>
</material>
</collision>
</link>
<link name="link_3" uuid="801f8e99-88d8-4a92-b848-3c573c5e6ddb">
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_4.stl"/>
</geometry>
<material name="">
<color rgba="0.7372549 0.3490196 0.1607843 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_4.stl"/>
</geometry>
<material name="">
<color rgba="1 1 0 1"/>
</material>
</collision>
</link>
<link name="link_4" uuid="c0146e6e-b9c7-4ada-9e96-b7bb141f6feb">
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_5.stl"/>
</geometry>
<material name="">
<color rgba="0.7372549 0.3490196 0.1607843 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_5.stl"/>
</geometry>
<material name="">
<color rgba="1 1 0 1"/>
</material>
</collision>
</link>
<link name="link_5" uuid="5a650225-e5cf-44ae-b2fc-aad215668177">
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_6.stl"/>
</geometry>
<material name="">
<color rgba="0.7372549 0.3490196 0.1607843 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_6.stl"/>
</geometry>
<material name="">
<color rgba="1 1 0 1"/>
</material>
</collision>
</link>
<link name="link_6" uuid="4d20b9a3-23a5-48d8-bed6-b50439da66c0">
<inertial>
<mass value="1.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_7.stl"/>
</geometry>
<material name="">
<color rgba="0.7372549 0.3490196 0.1607843 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_7.stl"/>
</geometry>
<material name="">
<color rgba="1 1 0 1"/>
</material>
</collision>
</link>
<joint name="joint_1" innerId="D0F7B07F-0438-4392-B775-42512D10BBD4" type="revolute">
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
<parent link="base_link" uuid="9c88ec70-a337-4d16-94c5-92ccff72c1a5"/>
<child link="link_1" uuid="09059ca4-6c99-4a5f-a474-8ba964e612c4"/>
<axis xyz="0 0 1"/>
<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="joint_2" innerId="3D80EDAC-92C7-492C-91FD-8108A298E175" type="revolute">
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.290000"/>
<parent link="link_1" uuid="09059ca4-6c99-4a5f-a474-8ba964e612c4"/>
<child link="link_2" uuid="3f38944a-9e19-4aba-a46e-2cc2d2611b91"/>
<axis xyz="1 0 0"/>
<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="joint_3" innerId="0A098978-9EAC-4F7B-B8DB-A1FFF640B6FF" type="revolute">
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.270000"/>
<parent link="link_2" uuid="3f38944a-9e19-4aba-a46e-2cc2d2611b91"/>
<child link="link_3" uuid="801f8e99-88d8-4a92-b848-3c573c5e6ddb"/>
<axis xyz="1 0 0"/>
<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="joint_4" innerId="823DDDE0-DBFA-4F5E-B3A6-9A87C06C8DB9" type="revolute">
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 -0.134000 0.070000"/>
<parent link="link_3" uuid="801f8e99-88d8-4a92-b848-3c573c5e6ddb"/>
<child link="link_4" uuid="c0146e6e-b9c7-4ada-9e96-b7bb141f6feb"/>
<axis xyz="0 1 0"/>
<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="joint_5" innerId="A3DD936A-C88D-4267-8926-1C29C144D52E" type="revolute">
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 -0.168000 0.000000"/>
<parent link="link_4" uuid="c0146e6e-b9c7-4ada-9e96-b7bb141f6feb"/>
<child link="link_5" uuid="5a650225-e5cf-44ae-b2fc-aad215668177"/>
<axis xyz="1 0 0"/>
<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<joint name="joint_6" innerId="85949D90-0C7B-463A-A18D-281A71FA4C19" type="revolute">
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 -0.072000 0.000000"/>
<parent link="link_5" uuid="5a650225-e5cf-44ae-b2fc-aad215668177"/>
<child link="link_6" uuid="4d20b9a3-23a5-48d8-bed6-b50439da66c0"/>
<axis xyz="0 1 0"/>
<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="base" uuidBase="f25bddf0-6533-448a-a1f5-b1ebbabe7d3c"/>
<joint name="base_link-base" type="fixed">
<origin xyz="0.000000 0.000000 0.000000" rpy="0.000000 0.000000 0.000000"/>
<parent link="base"/>
<child link="base_link"/>
</joint>
<link name="flange"/>
<joint name="joint_6-flange" type="fixed">
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
<parent link="link_6"/>
<child link="flange"/>
</joint>
<link name="tool0" uuidTool="5272238a-a622-412b-aa83-3e748fe57798"/>
<joint name="link_6-tool0" type="fixed">
<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
<parent link="flange"/>
<child link="tool0"/>
</joint>
<transmission name="joint_1">
<type>transmission_interface/SimpleTransmission</type>
<joint>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_1_motor">
<hardwareInterface>1</hardwareInterface>
<mechanicalReduction/>
</actuator>
</transmission>
<transmission name="joint_2">
<type>transmission_interface/SimpleTransmission</type>
<joint>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_2_motor">
<hardwareInterface>1</hardwareInterface>
<mechanicalReduction/>
</actuator>
</transmission>
<transmission name="joint_3">
<type>transmission_interface/SimpleTransmission</type>
<joint>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_3_motor">
<hardwareInterface>1</hardwareInterface>
<mechanicalReduction/>
</actuator>
</transmission>
<transmission name="joint_4">
<type>transmission_interface/SimpleTransmission</type>
<joint>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_4_motor">
<hardwareInterface>1</hardwareInterface>
<mechanicalReduction/>
</actuator>
</transmission>
<transmission name="joint_5">
<type>transmission_interface/SimpleTransmission</type>
<joint>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_5_motor">
<hardwareInterface>1</hardwareInterface>
<mechanicalReduction/>
</actuator>
</transmission>
<transmission name="joint_6">
<type>transmission_interface/SimpleTransmission</type>
<joint>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint_6_motor">
<hardwareInterface>1</hardwareInterface>
<mechanicalReduction/>
</actuator>
</transmission>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/</robotNamespace>
</plugin>
</gazebo>
</robot>

View File

@@ -382,9 +382,16 @@
<label for="robot-uuid">robot_uuid</label> <label for="robot-uuid">robot_uuid</label>
<input id="robot-uuid" value="abb_irb120_3_58" /> <input id="robot-uuid" value="abb_irb120_3_58" />
</div> </div>
<div class="field full">
<label for="urdf-file">URDF 文件</label>
<input id="urdf-file" type="file" accept=".xml,.urdf" />
</div>
</div> </div>
<div class="actions"> <div class="actions">
<button id="init-btn" type="button">初始化</button> <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> <button id="reset-btn" class="secondary" type="button">零位</button>
</div> </div>
</section> </section>
@@ -507,6 +514,7 @@
wasmStatus: document.getElementById("wasm-status"), wasmStatus: document.getElementById("wasm-status"),
solverStatus: document.getElementById("solver-status"), solverStatus: document.getElementById("solver-status"),
robotUuid: document.getElementById("robot-uuid"), robotUuid: document.getElementById("robot-uuid"),
urdfFile: document.getElementById("urdf-file"),
jointControls: document.getElementById("joint-controls"), jointControls: document.getElementById("joint-controls"),
requestJson: document.getElementById("request-json"), requestJson: document.getElementById("request-json"),
responseJson: document.getElementById("response-json"), responseJson: document.getElementById("response-json"),
@@ -521,6 +529,7 @@
jointCountValue: document.getElementById("joint-count-value"), jointCountValue: document.getElementById("joint-count-value"),
toast: document.getElementById("toast"), toast: document.getElementById("toast"),
initBtn: document.getElementById("init-btn"), initBtn: document.getElementById("init-btn"),
registerUrdfBtn: document.getElementById("register-urdf-btn"),
resetBtn: document.getElementById("reset-btn"), resetBtn: document.getElementById("reset-btn"),
fkBtn: document.getElementById("fk-btn"), fkBtn: document.getElementById("fk-btn"),
ikBtn: document.getElementById("ik-btn"), ikBtn: document.getElementById("ik-btn"),
@@ -756,6 +765,67 @@
} }
} }
// 读取本地 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() { async function ensureInitialized() {
if (!state.initialized) { if (!state.initialized) {
@@ -1029,6 +1099,7 @@
function setBusy(isBusy, label) { function setBusy(isBusy, label) {
[ [
elements.initBtn, elements.initBtn,
elements.registerUrdfBtn,
elements.resetBtn, elements.resetBtn,
elements.fkBtn, elements.fkBtn,
elements.ikBtn, elements.ikBtn,
@@ -1095,6 +1166,7 @@
} }
elements.initBtn.addEventListener("click", initializeWasm); elements.initBtn.addEventListener("click", initializeWasm);
elements.registerUrdfBtn.addEventListener("click", registerUrdfFromFile);
elements.resetBtn.addEventListener("click", resetJoints); elements.resetBtn.addEventListener("click", resetJoints);
elements.fkBtn.addEventListener("click", runForwardKinematics); elements.fkBtn.addEventListener("click", runForwardKinematics);
elements.ikBtn.addEventListener("click", runInverseKinematics); elements.ikBtn.addEventListener("click", runInverseKinematics);