feat: 更改模型加载方式
This commit is contained in:
parent
b1e35350a2
commit
75103022c3
@ -149,9 +149,7 @@
|
||||
<div v-if="activeMode === 'artificial' && activeController === 2" class="model-container">
|
||||
<UrdfViewer
|
||||
ref="viewerRef"
|
||||
base-path="/inspection/elfin10/"
|
||||
model-color="#cbcbcb"
|
||||
:show-controls="true"
|
||||
modelPath="/inspection/elfin10/elfin10.urdf.xacro"
|
||||
/>
|
||||
</div>
|
||||
<div v-if="activeMode === 'artificial' && activeController === 3" class="monitor-container">监控</div>
|
||||
|
||||
@ -1,240 +0,0 @@
|
||||
<template>
|
||||
<div class="elfin-viewer">
|
||||
<div ref="viewerContainer" class="viewer-container">
|
||||
<!-- Three.js 渲染 -->
|
||||
</div>
|
||||
|
||||
<div v-if="isLoading" class="loading-overlay">
|
||||
<div class="spinner"></div>
|
||||
<p>正在加载机器人模型...</p>
|
||||
<p class="loading-detail">{{ loadingMessage }}</p>
|
||||
</div>
|
||||
</div>
|
||||
</template>
|
||||
|
||||
<script setup>
|
||||
import { ref, onMounted, onBeforeUnmount, nextTick, reactive } from 'vue';
|
||||
import * as THREE from 'three';
|
||||
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
|
||||
import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
|
||||
|
||||
import { XacroAdapter } from '@/adapters/XacroAdapter.js'
|
||||
|
||||
const props = defineProps({
|
||||
basePath: { type: String, default: '/inspection/elfin10' },
|
||||
modelColor: { type: String, default: '#cbcbcb' }
|
||||
});
|
||||
|
||||
const viewerContainer = ref(null);
|
||||
const isLoading = ref(true);
|
||||
const loadingMessage = ref('初始化...');
|
||||
|
||||
let scene, camera, renderer, controls;
|
||||
|
||||
const animationId = ref(null);
|
||||
const jointControls = reactive([]);
|
||||
|
||||
// ============ 场景初始化 ============
|
||||
const initScene = () => {
|
||||
const container = viewerContainer.value;
|
||||
const width = container.clientWidth || 800;
|
||||
const height = container.clientHeight || 600;
|
||||
scene = new THREE.Scene();
|
||||
scene.background = new THREE.Color('#000d2e');
|
||||
|
||||
camera = new THREE.PerspectiveCamera(35, width / height, 0.1, 1000);
|
||||
camera.position.set(0, 0, 4);
|
||||
|
||||
renderer = new THREE.WebGLRenderer({ antialias: true, alpha: true });
|
||||
renderer.setSize(width, height);
|
||||
|
||||
container.appendChild(renderer.domElement);
|
||||
|
||||
controls = new OrbitControls(camera, renderer.domElement);
|
||||
controls.enableDamping = true;
|
||||
controls.dampingFactor = 0.1;
|
||||
controls.target.set(0, 0.5, 0);
|
||||
controls.update();
|
||||
|
||||
const ambient = new THREE.AmbientLight(0xffffff, 0.6)
|
||||
scene.add(ambient);
|
||||
|
||||
const directionalLight = new THREE.DirectionalLight(0xffffff, 1.0);
|
||||
directionalLight.position.set(5, 10, 7);
|
||||
directionalLight.castShadow = true;
|
||||
scene.add(directionalLight);
|
||||
};
|
||||
|
||||
|
||||
// ============ 关节控制（无需修改） ============
|
||||
const updateJoint = (jointName, angle) => {
|
||||
if (!robotInstance) return;
|
||||
const joint = robotInstance.joints[jointName];
|
||||
if (!joint) {
|
||||
console.warn(`未找到关节: ${jointName}`);
|
||||
return;
|
||||
}
|
||||
|
||||
// ✅ 使用 setJointValue 触发变换矩阵更新
|
||||
if (joint._jointType === 'revolute' || joint._jointType === 'continuous') {
|
||||
joint.setJointValue(angle);
|
||||
} else if (joint._jointType === 'prismatic') {
|
||||
joint.setJointValue(angle);
|
||||
}
|
||||
|
||||
|
||||
// 同步滑块显示值
|
||||
const control = jointControls.find(c => c.name === jointName);
|
||||
if (control) control.value = angle;
|
||||
};
|
||||
|
||||
const resetJoints = () => {
|
||||
for (const joint of jointControls) {
|
||||
joint.value = 0;
|
||||
updateJoint(joint.name, 0);
|
||||
}
|
||||
};
|
||||
|
||||
const fileMap = new Map()
|
||||
const loadFiles = async () => {
|
||||
const fileNames = [
|
||||
'elfin10.urdf.xacro',
|
||||
'elfin10_gazebo.xacro',
|
||||
'elfin10_transmission.xacro',
|
||||
'meshes/elfin10/elfin_base.STL',
|
||||
'meshes/elfin10/elfin_end_link.STL',
|
||||
'meshes/elfin10/elfin_link1.STL',
|
||||
'meshes/elfin10/elfin_link2.STL',
|
||||
'meshes/elfin10/elfin_link3.STL',
|
||||
'meshes/elfin10/elfin_link4.STL',
|
||||
'meshes/elfin10/elfin_link5.STL',
|
||||
'meshes/elfin10/elfin_link6.STL'
|
||||
]
|
||||
const fetchPromises = fileNames.map(async (name) => {
|
||||
const url = `/inspection/elfin10/${name}`
|
||||
const response = await fetch(url)
|
||||
const blob = await response.blob()
|
||||
return new File([blob], name, { type: blob.type })
|
||||
})
|
||||
const fileList = await Promise.all(fetchPromises)
|
||||
fileList.forEach(item => {
|
||||
fileMap.set('elfin10/' + item.name, item);
|
||||
})
|
||||
}
|
||||
|
||||
const currentModel = ref()
|
||||
let robotInstance = null; // 保存 URDFRobot 对象
|
||||
|
||||
const loadRobot = async () => {
|
||||
isLoading.value = true;
|
||||
loadingMessage.value = '加载XACRO文件...';
|
||||
const xacroUrl = `${props.basePath}elfin10.urdf.xacro`;
|
||||
|
||||
const response = await fetch(xacroUrl);
|
||||
if (!response.ok) throw new Error(`HTTP ${response.status}`);
|
||||
const xacroContent = await response.text();
|
||||
const file = fileMap.get('elfin10/elfin10.urdf.xacro')
|
||||
const model = await XacroAdapter.parse(xacroContent, file.name, fileMap, file);
|
||||
|
||||
const robot = model.threeObject; // 这就是 URDFRobot
|
||||
|
||||
// 将机器人添加到场景
|
||||
scene.add(robot); // ⚠️ 注意：是 robot.scene，不是 robot
|
||||
robot.rotation.x = -Math.PI / 2;
|
||||
isLoading.value = false;
|
||||
currentModel.value = model
|
||||
|
||||
robotInstance = robot;
|
||||
|
||||
// 获取关节条目数组
|
||||
let jointEntries = [];
|
||||
if (robot.joints instanceof Map) {
|
||||
jointEntries = [...robot.joints.entries()];
|
||||
} else if (typeof robot.joints === 'object' && robot.joints !== null) {
|
||||
jointEntries = Object.entries(robot.joints);
|
||||
} else if (Array.isArray(robot.joints)) {
|
||||
jointEntries = robot.joints.map((j, i) => [i, j]); // 索引作为名称
|
||||
} else {
|
||||
console.error('未知的 joints 结构:', robot.joints);
|
||||
}
|
||||
|
||||
// 填充关节控制列表
|
||||
for (const [name, joint] of jointEntries) {
|
||||
if (joint._jointType === 'fixed') continue;
|
||||
const limits = joint.limits || {};
|
||||
let lower = limits.lower !== undefined ? limits.lower : -Math.PI;
|
||||
let upper = limits.upper !== undefined ? limits.upper : Math.PI;
|
||||
// 如果是连续关节，范围设为 -PI ~ PI
|
||||
if (joint._jointType === 'continuous') {
|
||||
lower = -Math.PI;
|
||||
upper = Math.PI;
|
||||
}
|
||||
// 初始角度一般为 0
|
||||
jointControls.push({
|
||||
name,
|
||||
min: lower,
|
||||
max: upper,
|
||||
value: 0, // 因为 joint.angle 初始为 0
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
// ============ 动画循环 ============
|
||||
const animate = () => {
|
||||
animationId.value = requestAnimationFrame(animate);
|
||||
controls?.update();
|
||||
renderer?.render(scene, camera);
|
||||
};
|
||||
|
||||
// ============ 生命周期 ============
|
||||
onMounted(async () => {
|
||||
await nextTick();
|
||||
await loadFiles()
|
||||
initScene();
|
||||
await loadRobot();
|
||||
animate();
|
||||
window.addEventListener('resize', onResize);
|
||||
});
|
||||
|
||||
onBeforeUnmount(() => {
|
||||
if (animationId.value) cancelAnimationFrame(animationId.value);
|
||||
window.removeEventListener('resize', onResize);
|
||||
renderer?.dispose();
|
||||
controls?.dispose();
|
||||
});
|
||||
|
||||
const onResize = () => {
|
||||
const container = viewerContainer.value;
|
||||
if (!container || !camera || !renderer) return;
|
||||
const width = container.clientWidth;
|
||||
const height = container.clientHeight;
|
||||
camera.aspect = width / height;
|
||||
camera.updateProjectionMatrix();
|
||||
renderer.setSize(width, height);
|
||||
};
|
||||
|
||||
defineExpose({ updateJoint, resetJoints, jointControls });
|
||||
</script>
|
||||
|
||||
<style scoped>
|
||||
.elfin-viewer {
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
position: relative;
|
||||
overflow: hidden;
|
||||
background: #0d0d1a;
|
||||
}
|
||||
|
||||
.viewer-container {
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
}
|
||||
|
||||
.viewer-container canvas {
|
||||
display: block;
|
||||
width: 100% !important;
|
||||
height: 100% !important;
|
||||
}
|
||||
|
||||
|
||||
</style>
|
||||
@ -95,7 +95,7 @@
|
||||
<MapCanvas :robotList="robotList" ref="mapCanvasRef" />
|
||||
</div>
|
||||
<div v-if="activeMode === 'artificial' && activeController === 2" class="model-container">
|
||||
<UrdfViewer ref="urdfViewerRef" base-path="/inspection/elfin10/" />
|
||||
<UrdfViewer ref="urdfViewerRef" modelPath="/inspection/elfin10/elfin10.urdf.xacro" />
|
||||
</div>
|
||||
<div v-if="activeMode === 'artificial' && activeController === 3" class="monitor-container">监控</div>
|
||||
</div>
|
||||
@ -115,7 +115,7 @@
|
||||
<script setup>
|
||||
import { onMounted, nextTick } from "vue";
|
||||
import SvgIcon from "@/components/SvgIcon";
|
||||
import UrdfViewer from './UrdfView.vue'
|
||||
import UrdfViewer from '../UrdfView.vue'
|
||||
import DirectionControl from './DirectionControl.vue'
|
||||
import RoboticArm from "./RoboticArm.vue";
|
||||
import { getRobotList } from '@/api/inspection/robot'
|
||||
|
||||
@ -3,501 +3,230 @@
|
||||
<div ref="viewerContainer" class="viewer-container">
|
||||
<!-- Three.js 渲染 -->
|
||||
</div>
|
||||
|
||||
<div v-if="isLoading" class="loading-overlay">
|
||||
<div class="spinner"></div>
|
||||
<p>正在加载机器人模型...</p>
|
||||
<p class="loading-detail">{{ loadingMessage }}</p>
|
||||
</div>
|
||||
|
||||
<div class="control-panel" v-if="!isLoading && jointControls.length > 0">
|
||||
<h4>关节控制</h4>
|
||||
<div class="joint-list">
|
||||
<div v-for="joint in jointControls" :key="joint.name" class="joint-item">
|
||||
<label :for="joint.name">{{ joint.displayName }}</label>
|
||||
<input
|
||||
type="range"
|
||||
:id="joint.name"
|
||||
:min="joint.min"
|
||||
:max="joint.max"
|
||||
:step="0.01"
|
||||
v-model.number="joint.value"
|
||||
@input="updateJoint(joint.name, joint.value)"
|
||||
/>
|
||||
<span class="angle">{{ (joint.value * 180 / Math.PI).toFixed(1) }}°</span>
|
||||
</div>
|
||||
</div>
|
||||
<button @click="resetJoints" class="reset-btn">重置关节</button>
|
||||
</div>
|
||||
</div>
|
||||
</template>
|
||||
|
||||
<script setup>
|
||||
import { ref, onMounted, onBeforeUnmount, nextTick, reactive } from 'vue';
|
||||
import { computed, ref, onMounted, onBeforeUnmount, nextTick, reactive } from 'vue';
|
||||
import * as THREE from 'three';
|
||||
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
|
||||
import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
|
||||
import URDFLoader from 'urdf-loader';
|
||||
|
||||
import { XacroAdapter } from '@/adapters/XacroAdapter.js'
|
||||
import { URDFAdapter } from '@/adapters/URDFAdapter.js'
|
||||
|
||||
const props = defineProps({
|
||||
basePath: { type: String, default: '/inspection/elfin10' },
|
||||
modelColor: { type: String, default: '#cbcbcb' },
|
||||
showControls: { type: Boolean, default: true }
|
||||
});
|
||||
modelPath: {
|
||||
type: String,
|
||||
default: '/inspection/elfin10/elfin10.urdf.xacro'
|
||||
},
|
||||
filePath: {
|
||||
type: String,
|
||||
default: ''
|
||||
},
|
||||
fileName: {
|
||||
type: String,
|
||||
default: ''
|
||||
}
|
||||
})
|
||||
|
||||
const viewerContainer = ref(null);
|
||||
const isLoading = ref(true);
|
||||
const loadingMessage = ref('初始化...');
|
||||
|
||||
let scene, camera, renderer, controls;
|
||||
let robotGroup = null;
|
||||
const jointMap = new Map();
|
||||
const animationId = ref(null);
|
||||
const jointControls = reactive([]);
|
||||
|
||||
// ============ XACRO 解析器 ============
|
||||
class XacroParser {
|
||||
constructor() {
|
||||
this.variables = new Map();
|
||||
this.macros = new Map();
|
||||
this.includes = [];
|
||||
this.robotXml = null;
|
||||
}
|
||||
parse(xacroContent) {
|
||||
const parser = new DOMParser();
|
||||
const xmlDoc = parser.parseFromString(xacroContent, 'text/xml');
|
||||
const properties = xmlDoc.getElementsByTagName('xacro:property');
|
||||
for (const prop of properties) {
|
||||
const name = prop.getAttribute('name');
|
||||
const value = prop.getAttribute('value');
|
||||
if (name && value) this.variables.set(name, this.evaluateExpression(value));
|
||||
}
|
||||
const includes = xmlDoc.getElementsByTagName('xacro:include');
|
||||
for (const inc of includes) {
|
||||
const filename = inc.getAttribute('filename');
|
||||
if (filename) this.includes.push(filename);
|
||||
}
|
||||
const macros = xmlDoc.getElementsByTagName('xacro:macro');
|
||||
for (const macro of macros) {
|
||||
const name = macro.getAttribute('name');
|
||||
const params = macro.getAttribute('params') || '';
|
||||
if (name) {
|
||||
this.macros.set(name, {
|
||||
params: params.split(/\s+/).filter(p => p),
|
||||
content: macro.innerHTML
|
||||
});
|
||||
}
|
||||
}
|
||||
this.robotXml = this.processRobotXml(xmlDoc);
|
||||
return this.robotXml;
|
||||
}
|
||||
processRobotXml(xmlDoc) {
|
||||
const doc = xmlDoc.cloneNode(true);
|
||||
const allElements = Array.from(doc.getElementsByTagName('*'));
|
||||
for (const elem of allElements) {
|
||||
if (elem.tagName.toLowerCase().startsWith('xacro:')) {
|
||||
elem.parentNode?.removeChild(elem);
|
||||
}
|
||||
}
|
||||
const remaining = Array.from(doc.getElementsByTagName('*'));
|
||||
for (const elem of remaining) {
|
||||
for (const attr of elem.attributes) {
|
||||
if (attr.value?.includes('${')) attr.value = this.replaceVariables(attr.value);
|
||||
}
|
||||
if (elem.textContent?.includes('${')) elem.textContent = this.replaceVariables(elem.textContent);
|
||||
}
|
||||
return doc;
|
||||
}
|
||||
replaceVariables(text) {
|
||||
return text.replace(/\$\{([^}]+)\}/g, (match, expr) => this.evaluateExpression(expr));
|
||||
}
|
||||
evaluateExpression(expr) {
|
||||
expr = expr.trim();
|
||||
if (!expr) return '0';
|
||||
try {
|
||||
for (const [key, value] of this.variables) {
|
||||
if (key.length === 0) continue;
|
||||
const regex = new RegExp('\\b' + this.escapeRegex(key) + '\\b', 'g');
|
||||
expr = expr.replace(regex, '(' + value + ')');
|
||||
}
|
||||
expr = expr.replace(/\bPI\b/g, '(' + Math.PI + ')');
|
||||
expr = expr.replace(/\bpi\b/g, '(' + Math.PI + ')');
|
||||
const sanitized = expr.replace(/\s+/g, '');
|
||||
if (/^[\d+\-*/().eE]+$/.test(sanitized)) {
|
||||
const result = Function('"use strict"; return (' + sanitized + ')')();
|
||||
if (typeof result === 'number' && isFinite(result)) return result.toString();
|
||||
}
|
||||
console.warn('表达式求值失败，返回原值:', expr);
|
||||
return expr;
|
||||
} catch (e) {
|
||||
console.warn('表达式求值异常:', expr, e);
|
||||
return expr;
|
||||
}
|
||||
}
|
||||
escapeRegex(str) {
|
||||
return str.replace(/[.*+?^${}()|[\]\\]/g, '\\$&');
|
||||
}
|
||||
getRobotXml() { return this.robotXml; }
|
||||
getIncludes() { return this.includes; }
|
||||
const normalizePublicPath = (path) => {
|
||||
let normalized = path.trim().replace(/\\/g, '/')
|
||||
normalized = normalized.replace(/^[a-zA-Z]:.*?\/public\//, '/')
|
||||
normalized = normalized.replace(/^\.?\/?public\//, '/')
|
||||
return normalized.startsWith('/') ? normalized : `/${normalized}`
|
||||
}
|
||||
|
||||
// ============ 工具函数 ============
|
||||
const cleanMeshPath = (path) => {
|
||||
if (!path) return '';
|
||||
path = path.replace(/^file:\/\//, '');
|
||||
path = path.replace(/^package:\/\/[^/]+\//, '');
|
||||
const findMatch = path.match(/\$\(find\s+([^)]+)\)/);
|
||||
if (findMatch) path = path.replace(/\$\(find\s+[^)]+\)/, '');
|
||||
path = path.replace(/\\/g, '/').replace(/^\/+/, '');
|
||||
return path;
|
||||
};
|
||||
const modelUrl = computed(() => normalizePublicPath(
|
||||
props.filePath && props.fileName
|
||||
? `${props.filePath.replace(/[\\/]?$/, '/')}${props.fileName}`
|
||||
: props.modelPath
|
||||
))
|
||||
|
||||
const parseXYZ = (str) => {
|
||||
if (!str) return { x: 0, y: 0, z: 0 };
|
||||
const [x, y, z] = str.trim().split(/\s+/).map(Number);
|
||||
return { x: x || 0, y: y || 0, z: z || 0 };
|
||||
};
|
||||
const modelDirectory = computed(() => modelUrl.value.slice(0, modelUrl.value.lastIndexOf('/') + 1))
|
||||
|
||||
const parseRPY = (str) => {
|
||||
if (!str) return { roll: 0, pitch: 0, yaw: 0 };
|
||||
const [r, p, y] = str.trim().split(/\s+/).map(Number);
|
||||
return { roll: r || 0, pitch: p || 0, yaw: y || 0 };
|
||||
};
|
||||
const viewerContainer = ref(null);
|
||||
|
||||
// ★ 正确转换 RPY → 四元数（URDF 固定轴 XYZ = Three.js 局部轴 ZYX）
|
||||
const rpyToQuaternion = (rpy) => {
|
||||
const euler = new THREE.Euler(rpy.roll, rpy.pitch, rpy.yaw, 'ZYX');
|
||||
return new THREE.Quaternion().setFromEuler(euler);
|
||||
};
|
||||
let scene, camera, renderer, controls;
|
||||
|
||||
// ============ STL 加载 ============
|
||||
const loadSTLWithRetry = (url, group, visualPos, visualQuat) => {
|
||||
return new Promise((resolve) => {
|
||||
const loader = new STLLoader();
|
||||
const loadMesh = (attemptUrl) => {
|
||||
loader.load(
|
||||
attemptUrl,
|
||||
(geometry) => {
|
||||
geometry.computeVertexNormals();
|
||||
const material = new THREE.MeshStandardMaterial({
|
||||
color: props.modelColor,
|
||||
roughness: 0.4,
|
||||
metalness: 0.3,
|
||||
});
|
||||
const mesh = new THREE.Mesh(geometry, material);
|
||||
mesh.position.set(visualPos.x, visualPos.y, visualPos.z);
|
||||
mesh.quaternion.copy(visualQuat); // 使用四元数
|
||||
mesh.castShadow = true;
|
||||
mesh.receiveShadow = true;
|
||||
while (group.children.length > 0) {
|
||||
const child = group.children[0];
|
||||
if (child.isMesh) {
|
||||
child.geometry?.dispose();
|
||||
child.material?.dispose();
|
||||
}
|
||||
group.remove(child);
|
||||
}
|
||||
group.add(mesh);
|
||||
resolve(mesh);
|
||||
},
|
||||
undefined,
|
||||
(error) => {
|
||||
console.warn(`加载STL失败: ${attemptUrl}`, error);
|
||||
if (!attemptUrl.startsWith('/') && !attemptUrl.startsWith('http')) {
|
||||
loadMesh('/' + attemptUrl);
|
||||
} else {
|
||||
resolve(null);
|
||||
}
|
||||
}
|
||||
);
|
||||
};
|
||||
loadMesh(url);
|
||||
});
|
||||
};
|
||||
|
||||
// ============ URDF 解析与构建（严格修复） ============
|
||||
const parseAndBuildRobot = async (urdfDoc) => {
|
||||
const links = urdfDoc.getElementsByTagName('link');
|
||||
const jointsRaw = urdfDoc.getElementsByTagName('joint');
|
||||
const joints = sortJointsTopologically(jointsRaw, links);
|
||||
const linkGroupMap = new Map();
|
||||
const mainGroup = new THREE.Group();
|
||||
mainGroup.name = 'robot';
|
||||
|
||||
loadingMessage.value = '创建链接结构...';
|
||||
for (const link of links) {
|
||||
const name = link.getAttribute('name');
|
||||
const group = new THREE.Group();
|
||||
group.name = name;
|
||||
const visual = link.getElementsByTagName('visual')[0];
|
||||
if (visual) {
|
||||
const origin = visual.getElementsByTagName('origin')[0];
|
||||
const geometry = visual.getElementsByTagName('geometry')[0];
|
||||
const meshTag = geometry?.getElementsByTagName('mesh')[0];
|
||||
if (meshTag) {
|
||||
let meshPath = cleanMeshPath(meshTag.getAttribute('filename'));
|
||||
let visualPos = { x: 0, y: 0, z: 0 };
|
||||
let visualQuat = new THREE.Quaternion();
|
||||
if (origin) {
|
||||
const rawXyz = parseXYZ(origin.getAttribute('xyz') || '0 0 0');
|
||||
const rawRpy = parseRPY(origin.getAttribute('rpy') || '0 0 0');
|
||||
visualPos = rawXyz;
|
||||
visualQuat = rpyToQuaternion(rawRpy); // ★ 使用正确顺序
|
||||
}
|
||||
group.userData = {
|
||||
meshPath: `${props.basePath}${meshPath}`,
|
||||
visualPos,
|
||||
visualQuat
|
||||
};
|
||||
}
|
||||
}
|
||||
linkGroupMap.set(name, group);
|
||||
mainGroup.add(group);
|
||||
}
|
||||
|
||||
loadingMessage.value = '加载3D模型文件...';
|
||||
const loadPromises = [];
|
||||
for (const [name, group] of linkGroupMap) {
|
||||
if (group.userData.meshPath) {
|
||||
loadPromises.push(
|
||||
loadSTLWithRetry(group.userData.meshPath, group, group.userData.visualPos, group.userData.visualQuat)
|
||||
);
|
||||
}
|
||||
}
|
||||
await Promise.allSettled(loadPromises);
|
||||
|
||||
loadingMessage.value = '构建关节结构...';
|
||||
const jointDataList = [];
|
||||
for (const joint of joints) {
|
||||
const type = joint.getAttribute('type');
|
||||
const name = joint.getAttribute('name');
|
||||
const parent = joint.getElementsByTagName('parent')[0]?.getAttribute('link');
|
||||
const child = joint.getElementsByTagName('child')[0]?.getAttribute('link');
|
||||
const origin = joint.getElementsByTagName('origin')[0];
|
||||
let xyz = { x: 0, y: 0, z: 0 };
|
||||
let rpy = { roll: 0, pitch: 0, yaw: 0 };
|
||||
if (origin) {
|
||||
xyz = parseXYZ(origin.getAttribute('xyz') || '0 0 0');
|
||||
rpy = parseRPY(origin.getAttribute('rpy') || '0 0 0');
|
||||
}
|
||||
const axis = joint.getElementsByTagName('axis')[0];
|
||||
const axisVec = parseXYZ(axis?.getAttribute('xyz') || '0 0 1');
|
||||
const limit = joint.getElementsByTagName('limit')[0];
|
||||
const min = limit ? parseFloat(limit.getAttribute('lower')) : -Math.PI;
|
||||
const max = limit ? parseFloat(limit.getAttribute('upper')) : Math.PI;
|
||||
jointDataList.push({
|
||||
name, type, parent, child, xyz, rpy,
|
||||
axis: new THREE.Vector3(axisVec.x, axisVec.y, axisVec.z),
|
||||
min, max
|
||||
});
|
||||
}
|
||||
|
||||
// 构建层级（Euler 顺序 ZYX）
|
||||
for (const jointData of jointDataList) {
|
||||
const { name, type, parent, child, xyz, rpy, axis, min, max } = jointData;
|
||||
const parentGroup = linkGroupMap.get(parent);
|
||||
const childGroup = linkGroupMap.get(child);
|
||||
if (!parentGroup || !childGroup) {
|
||||
console.warn(`关节 ${name} 的父或子链接不存在`);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (mainGroup.children.includes(childGroup)) mainGroup.remove(childGroup);
|
||||
if (parentGroup.children.includes(childGroup)) parentGroup.remove(childGroup);
|
||||
|
||||
if (type === 'revolute' || type === 'continuous') {
|
||||
const varGroup = new THREE.Group();
|
||||
varGroup.name = name + '_var';
|
||||
|
||||
const originGroup = new THREE.Group();
|
||||
originGroup.name = name + '_origin';
|
||||
originGroup.position.set(xyz.x, xyz.y, xyz.z);
|
||||
// ★ 使用正确顺序
|
||||
const euler = new THREE.Euler(rpy.roll, rpy.pitch, rpy.yaw, 'ZYX');
|
||||
originGroup.quaternion.setFromEuler(euler);
|
||||
|
||||
originGroup.add(childGroup);
|
||||
varGroup.add(originGroup);
|
||||
jointMap.set(name, {
|
||||
name,
|
||||
group: varGroup,
|
||||
axis: axis.clone().normalize(), // 仍为 Z-up 轴，稍后在 loadRobot 中统一变换
|
||||
min, max,
|
||||
value: 0,
|
||||
displayName: name.replace(/_/g, ' ').replace('elfin ', '')
|
||||
});
|
||||
parentGroup.add(varGroup);
|
||||
} else {
|
||||
// 固定关节
|
||||
childGroup.position.set(xyz.x, xyz.y, xyz.z);
|
||||
const euler = new THREE.Euler(rpy.roll, rpy.pitch, rpy.yaw, 'ZYX');
|
||||
childGroup.quaternion.setFromEuler(euler);
|
||||
parentGroup.add(childGroup);
|
||||
}
|
||||
}
|
||||
|
||||
return mainGroup;
|
||||
};
|
||||
|
||||
// ============ 拓扑排序 ============
|
||||
const sortJointsTopologically = (joints, links) => {
|
||||
const childToJoint = new Map();
|
||||
const allLinkNames = new Set();
|
||||
for (const link of links) allLinkNames.add(link.getAttribute('name'));
|
||||
const jointArray = Array.from(joints);
|
||||
for (const joint of jointArray) {
|
||||
const child = joint.getElementsByTagName('child')[0]?.getAttribute('link');
|
||||
if (child) childToJoint.set(child, joint);
|
||||
}
|
||||
const childLinks = new Set(childToJoint.keys());
|
||||
const rootLinks = [...allLinkNames].filter(name => !childLinks.has(name));
|
||||
const sorted = [];
|
||||
const queue = [...rootLinks];
|
||||
const visited = new Set();
|
||||
while (queue.length > 0) {
|
||||
const linkName = queue.shift();
|
||||
for (const joint of jointArray) {
|
||||
const parent = joint.getElementsByTagName('parent')[0]?.getAttribute('link');
|
||||
const child = joint.getElementsByTagName('child')[0]?.getAttribute('link');
|
||||
if (parent === linkName && !visited.has(joint.getAttribute('name'))) {
|
||||
visited.add(joint.getAttribute('name'));
|
||||
sorted.push(joint);
|
||||
if (child) queue.push(child);
|
||||
}
|
||||
}
|
||||
}
|
||||
for (const joint of jointArray) {
|
||||
if (!visited.has(joint.getAttribute('name'))) sorted.push(joint);
|
||||
}
|
||||
return sorted;
|
||||
};
|
||||
const animationId = ref(null);
|
||||
const jointControls = reactive([]);
|
||||
|
||||
// ============ 场景初始化 ============
|
||||
const initScene = () => {
|
||||
const container = viewerContainer.value;
|
||||
const width = container.clientWidth || 800;
|
||||
const height = container.clientHeight || 600;
|
||||
|
||||
scene = new THREE.Scene();
|
||||
scene.background = new THREE.Color(0x1a1a2e);
|
||||
camera = new THREE.PerspectiveCamera(45, width / height, 0.1, 1000);
|
||||
camera.position.set(2, 2, 3);
|
||||
scene.background = new THREE.Color('#050a14');
|
||||
|
||||
camera = new THREE.PerspectiveCamera(35, width / height, 0.1, 1000);
|
||||
camera.position.set(0, 0, 4);
|
||||
|
||||
renderer = new THREE.WebGLRenderer({ antialias: true, alpha: true });
|
||||
renderer.setSize(width, height);
|
||||
renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
|
||||
renderer.shadowMap.enabled = true;
|
||||
renderer.shadowMap.type = THREE.PCFSoftShadowMap;
|
||||
renderer.toneMapping = THREE.ACESFilmicToneMapping;
|
||||
renderer.toneMappingExposure = 1.2;
|
||||
|
||||
container.appendChild(renderer.domElement);
|
||||
|
||||
controls = new OrbitControls(camera, renderer.domElement);
|
||||
controls.enableDamping = true;
|
||||
controls.dampingFactor = 0.1;
|
||||
controls.target.set(0, 0.5, 0);
|
||||
controls.update();
|
||||
|
||||
const ambient = new THREE.AmbientLight(0x404060, 0.6);
|
||||
const ambient = new THREE.AmbientLight(0xffffff, 0.6)
|
||||
scene.add(ambient);
|
||||
const main = new THREE.DirectionalLight(0xffffff, 1.0);
|
||||
main.position.set(5, 10, 7);
|
||||
main.castShadow = true;
|
||||
scene.add(main);
|
||||
const fill = new THREE.DirectionalLight(0x8888ff, 0.4);
|
||||
fill.position.set(-5, 0, 5);
|
||||
scene.add(fill);
|
||||
const rim = new THREE.DirectionalLight(0xffffff, 0.3);
|
||||
rim.position.set(0, -5, 5);
|
||||
scene.add(rim);
|
||||
const grid = new THREE.GridHelper(3, 20, 0x444466, 0x333355);
|
||||
grid.position.y = -0.5;
|
||||
scene.add(grid);
|
||||
|
||||
const directionalLight = new THREE.DirectionalLight(0xffffff, 1.0);
|
||||
directionalLight.position.set(5, 10, 7);
|
||||
directionalLight.castShadow = true;
|
||||
scene.add(directionalLight);
|
||||
};
|
||||
|
||||
// ============ 关节控制（无需修改） ============
|
||||
const updateJoint = (jointName, angle) => {
|
||||
const joint = jointMap.get(jointName);
|
||||
if (!joint) return;
|
||||
const clampedAngle = Math.max(joint.min, Math.min(joint.max, angle));
|
||||
joint.value = clampedAngle;
|
||||
if (!robotInstance) return;
|
||||
|
||||
joint.group.quaternion.identity();
|
||||
const axis = joint.axis.clone().normalize();
|
||||
joint.group.quaternion.setFromAxisAngle(axis, clampedAngle); // 正负根据实际情况调整
|
||||
const joint = robotInstance.joints[jointName];
|
||||
|
||||
const control = jointControls.find(j => j.name === jointName);
|
||||
if (control) control.value = clampedAngle;
|
||||
if (!joint) {
|
||||
console.warn(`未找到关节: ${jointName}`);
|
||||
return;
|
||||
}
|
||||
|
||||
// ✅ 使用 setJointValue 触发变换矩阵更新
|
||||
if (joint._jointType === 'revolute' || joint._jointType === 'continuous') {
|
||||
joint.setJointValue(angle);
|
||||
} else if (joint._jointType === 'prismatic') {
|
||||
joint.setJointValue(angle);
|
||||
}
|
||||
|
||||
// 同步滑块显示值
|
||||
const control = jointControls.find(c => c.name === jointName);
|
||||
if (control) control.value = angle;
|
||||
};
|
||||
|
||||
const resetJoints = () => {
|
||||
for (const joint of jointControls) updateJoint(joint.name, 0);
|
||||
};
|
||||
|
||||
// ============ 加载机器人（轴变换加入） ============
|
||||
const loadRobot = async () => {
|
||||
try {
|
||||
isLoading.value = true;
|
||||
loadingMessage.value = '加载XACRO文件...';
|
||||
const xacroUrl = `${props.basePath}elfin10.urdf.xacro`;
|
||||
const response = await fetch(xacroUrl);
|
||||
if (!response.ok) throw new Error(`HTTP ${response.status}`);
|
||||
const xacroContent = await response.text();
|
||||
loadingMessage.value = '解析XACRO...';
|
||||
const parser = new XacroParser();
|
||||
const robotDoc = parser.parse(xacroContent);
|
||||
loadingMessage.value = '构建机器人模型...';
|
||||
robotGroup = await parseAndBuildRobot(robotDoc);
|
||||
if (robotGroup) {
|
||||
scene.add(robotGroup);
|
||||
// ★ 全局旋转：Z-up → Y-up
|
||||
robotGroup.rotation.x = -Math.PI / 2;
|
||||
|
||||
// ★ 关键：将所有关节轴从 Z-up 坐标系变换到 Y-up 坐标系
|
||||
const globalQuat = new THREE.Quaternion().setFromEuler(
|
||||
new THREE.Euler(-Math.PI / 2, 0, 0, 'XYZ')
|
||||
);
|
||||
const globalQuat1 = new THREE.Quaternion().setFromEuler(
|
||||
new THREE.Euler(-Math.PI / 2, 0, 0, 'ZXY')
|
||||
);
|
||||
for (const joint of jointMap.values()) {
|
||||
if (['joint2', 'joint6'].includes(joint.displayName)) {
|
||||
joint.axis.applyQuaternion(globalQuat).normalize();
|
||||
}
|
||||
if (['joint3', 'joint4', 'joint5'].includes(joint.displayName)) {
|
||||
joint.axis.applyQuaternion(globalQuat1).normalize();
|
||||
}
|
||||
}
|
||||
|
||||
jointControls.length = 0;
|
||||
for (const [name, joint] of jointMap) {
|
||||
jointControls.push({
|
||||
name,
|
||||
displayName: joint.displayName,
|
||||
value: 0,
|
||||
min: joint.min,
|
||||
max: joint.max
|
||||
});
|
||||
}
|
||||
fitCamera();
|
||||
}
|
||||
isLoading.value = false;
|
||||
loadingMessage.value = '加载完成';
|
||||
} catch (error) {
|
||||
console.error('加载机器人失败:', error);
|
||||
isLoading.value = false;
|
||||
loadingMessage.value = `加载失败: ${error.message}`;
|
||||
for (const joint of jointControls) {
|
||||
joint.value = 0;
|
||||
updateJoint(joint.name, 0);
|
||||
}
|
||||
};
|
||||
|
||||
const fitCamera = () => {
|
||||
if (!robotGroup) return;
|
||||
const box = new THREE.Box3().setFromObject(robotGroup);
|
||||
const center = box.getCenter(new THREE.Vector3());
|
||||
const size = box.getSize(new THREE.Vector3());
|
||||
const maxDim = Math.max(size.x, size.y, size.z);
|
||||
if (maxDim === 0) return;
|
||||
const distance = maxDim / (2 * Math.tan((camera.fov * Math.PI) / 360));
|
||||
const direction = new THREE.Vector3(1, 1, 1).normalize();
|
||||
camera.position.copy(center).add(direction.multiplyScalar(distance * 1.5));
|
||||
controls.target.copy(center);
|
||||
controls.update();
|
||||
const fileMap = new Map()
|
||||
const resolveReferencedUrl = (reference, parentUrl) => {
|
||||
const normalized = reference.replace(/\\/g, '/')
|
||||
const rosPath = normalized.match(/^(?:file:\/\/)?\$\(find\s+[^)]+\)\/(.+)$/)
|
||||
if (rosPath) return `${modelDirectory.value}${rosPath[1]}`
|
||||
|
||||
const packagePath = normalized.match(/^package:\/\/[^/]+\/(.+)$/)
|
||||
if (packagePath) return `${modelDirectory.value}${packagePath[1]}`
|
||||
|
||||
if (/^(?:https?:|blob:|data:)/i.test(normalized)) return normalized
|
||||
return new URL(normalized, new URL(parentUrl, window.location.origin)).pathname
|
||||
}
|
||||
|
||||
const loadFiles = async () => {
|
||||
fileMap.clear()
|
||||
if (!modelUrl.value.toLowerCase().endsWith('.xacro')) return
|
||||
|
||||
const pendingUrls = [modelUrl.value]
|
||||
const visitedUrls = new Set()
|
||||
while (pendingUrls.length) {
|
||||
const url = pendingUrls.shift()
|
||||
if (visitedUrls.has(url)) continue
|
||||
visitedUrls.add(url)
|
||||
|
||||
const response = await fetch(url)
|
||||
if (!response.ok) throw new Error(`加载模型依赖失败：${url} (HTTP ${response.status})`)
|
||||
const blob = await response.blob()
|
||||
const fileName = url.split('/').pop()
|
||||
const file = new File([blob], fileName, { type: blob.type })
|
||||
fileMap.set(url, file)
|
||||
|
||||
if (!/\.(?:xacro|urdf|xml)$/i.test(fileName)) continue
|
||||
const content = await blob.text()
|
||||
const references = [...content.matchAll(/\b(?:filename|url)=["']([^"']+)["']/gi)]
|
||||
references.forEach(([, reference]) => {
|
||||
const referencedUrl = resolveReferencedUrl(reference, url)
|
||||
if (!visitedUrls.has(referencedUrl)) pendingUrls.push(referencedUrl)
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
const currentModel = ref()
|
||||
let robotInstance = null; // 保存 URDFRobot 对象
|
||||
|
||||
const parseUrdf = async (urdfContent) => {
|
||||
const loader = new URDFLoader()
|
||||
loader.parseCollision = true
|
||||
|
||||
const packageNames = [...urdfContent.matchAll(/package:\/\/([^/]+)/g)].map(match => match[1])
|
||||
loader.packages = Object.fromEntries(
|
||||
[...new Set(packageNames)].map(packageName => [packageName, `${modelDirectory.value}${packageName}`])
|
||||
)
|
||||
|
||||
const robot = loader.parse(urdfContent, modelDirectory.value)
|
||||
return URDFAdapter.convert(robot, urdfContent)
|
||||
}
|
||||
|
||||
const loadRobot = async () => {
|
||||
const response = await fetch(modelUrl.value);
|
||||
if (!response.ok) throw new Error(`加载模型失败：${modelUrl.value} (HTTP ${response.status})`);
|
||||
const content = await response.text();
|
||||
const fileName = modelUrl.value.split('/').pop()
|
||||
const lowerCaseFileName = fileName.toLowerCase()
|
||||
let model
|
||||
if (lowerCaseFileName.endsWith('.xacro')) {
|
||||
const file = fileMap.get(modelUrl.value) || new File([content], fileName, { type: 'text/xml' })
|
||||
model = await XacroAdapter.parse(content, file.name, fileMap, file);
|
||||
} else if (lowerCaseFileName.endsWith('.urdf')) {
|
||||
model = await parseUrdf(content)
|
||||
} else {
|
||||
throw new Error(`不支持的模型格式：${fileName}，仅支持 .xacro 和 .urdf`)
|
||||
}
|
||||
|
||||
const robot = model.threeObject;
|
||||
|
||||
// 将机器人添加到场景
|
||||
scene.add(robot);
|
||||
robot.rotation.x = -Math.PI / 2;
|
||||
currentModel.value = model
|
||||
|
||||
robotInstance = robot;
|
||||
|
||||
// 获取关节条目数组
|
||||
let jointEntries = [];
|
||||
if (robot.joints instanceof Map) {
|
||||
jointEntries = [...robot.joints.entries()];
|
||||
} else if (typeof robot.joints === 'object' && robot.joints !== null) {
|
||||
jointEntries = Object.entries(robot.joints);
|
||||
} else if (Array.isArray(robot.joints)) {
|
||||
jointEntries = robot.joints.map((j, i) => [i, j]); // 索引作为名称
|
||||
} else {
|
||||
console.error('未知的 joints 结构:', robot.joints);
|
||||
}
|
||||
|
||||
// 填充关节控制列表
|
||||
for (const [name, joint] of jointEntries) {
|
||||
if (joint._jointType === 'fixed') continue;
|
||||
const limits = joint.limits || {};
|
||||
let lower = limits.lower !== undefined ? limits.lower : -Math.PI;
|
||||
let upper = limits.upper !== undefined ? limits.upper : Math.PI;
|
||||
// 如果是连续关节，范围设为 -PI ~ PI
|
||||
if (joint._jointType === 'continuous') {
|
||||
lower = -Math.PI;
|
||||
upper = Math.PI;
|
||||
}
|
||||
// 初始角度一般为 0
|
||||
jointControls.push({
|
||||
name,
|
||||
min: lower,
|
||||
max: upper,
|
||||
value: 0, // 因为 joint.angle 初始为 0
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
// ============ 动画循环 ============
|
||||
@ -510,6 +239,7 @@ const animate = () => {
|
||||
// ============ 生命周期 ============
|
||||
onMounted(async () => {
|
||||
await nextTick();
|
||||
await loadFiles()
|
||||
initScene();
|
||||
await loadRobot();
|
||||
animate();
|
||||
@ -533,56 +263,30 @@ const onResize = () => {
|
||||
renderer.setSize(width, height);
|
||||
};
|
||||
|
||||
defineExpose({ updateJoint, resetJoints, jointControls, jointMap });
|
||||
defineExpose({ updateJoint, resetJoints, jointControls });
|
||||
</script>
|
||||
|
||||
<style scoped>
|
||||
<style lang="scss" scoped>
|
||||
.elfin-viewer {
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
position: relative;
|
||||
overflow: hidden;
|
||||
background: #0d0d1a;
|
||||
background: #050a14;
|
||||
|
||||
.viewer-container {
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
|
||||
canvas {
|
||||
display: block;
|
||||
width: 100% !important;
|
||||
height: 100% !important;
|
||||
}
|
||||
}
|
||||
}
|
||||
.viewer-container {
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
}
|
||||
.viewer-container canvas {
|
||||
display: block;
|
||||
width: 100% !important;
|
||||
height: 100% !important;
|
||||
}
|
||||
.control-panel {
|
||||
position: absolute;
|
||||
top: 20px;
|
||||
right: 20px;
|
||||
background: rgba(20, 20, 40, 0.85);
|
||||
padding: 15px;
|
||||
border-radius: 8px;
|
||||
color: #ccc;
|
||||
max-height: 80%;
|
||||
overflow-y: auto;
|
||||
}
|
||||
.joint-item {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
margin-bottom: 10px;
|
||||
}
|
||||
.joint-item label {
|
||||
width: 120px;
|
||||
font-size: 0.9em;
|
||||
}
|
||||
.joint-item input[type=range] {
|
||||
flex: 1;
|
||||
}
|
||||
.angle {
|
||||
width: 50px;
|
||||
text-align: right;
|
||||
}
|
||||
.reset-btn {
|
||||
margin-top: 10px;
|
||||
width: 100%;
|
||||
}
|
||||
</style>
|
||||
|
||||
|
||||
|
||||
|
||||
</style>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user