462 lines
19 KiB
JavaScript
462 lines
19 KiB
JavaScript
|
/**
|
||
|
|
* URDF Adapter
|
||
|
|
* Converts urdf-loaders result to unified model
|
||
|
|
*/
|
||
|
|
import * as THREE from 'three';
|
||
|
|
import { UnifiedRobotModel, Link, Joint, JointLimits, VisualGeometry, CollisionGeometry, InertialProperties } from '../models/UnifiedRobotModel.js';
|
||
|
|
|
||
|
|
export class URDFAdapter {
|
||
|
|
/**
|
||
|
|
* Convert urdf-loaders robot object to unified model
|
||
|
|
* @param {THREE.Group} robot - Robot object returned by urdf-loaders
|
||
|
|
* @param {string} urdfXML - Original URDF XML content (optional, for extracting inertial data)
|
||
|
|
* @returns {UnifiedRobotModel}
|
||
|
|
*/
|
||
|
|
static convert(robot, urdfXML = null) {
|
||
|
|
const model = new UnifiedRobotModel();
|
||
|
|
model.name = robot.name || 'robot';
|
||
|
|
model.threeObject = robot;
|
||
|
|
|
||
|
|
// Mark model type as URDF
|
||
|
|
if (!robot.userData) robot.userData = {};
|
||
|
|
robot.userData.type = 'urdf';
|
||
|
|
|
||
|
|
if (!robot.links || !robot.joints) {
|
||
|
|
console.warn('URDF model missing links or joints information');
|
||
|
|
return model;
|
||
|
|
}
|
||
|
|
|
||
|
|
// If XML provided, parse inertial data
|
||
|
|
let inertialData = {};
|
||
|
|
if (urdfXML) {
|
||
|
|
inertialData = this.parseInertialFromXML(urdfXML);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Convert links
|
||
|
|
Object.values(robot.links).forEach(urdfLink => {
|
||
|
|
const link = this.convertLink(urdfLink);
|
||
|
|
|
||
|
|
// If urdf-loader didn't parse inertial, get from XML
|
||
|
|
if (!link.inertial && inertialData[urdfLink.name]) {
|
||
|
|
link.inertial = inertialData[urdfLink.name];
|
||
|
|
}
|
||
|
|
|
||
|
|
model.addLink(link);
|
||
|
|
});
|
||
|
|
|
||
|
|
// Convert joints
|
||
|
|
Object.values(robot.joints).forEach(urdfJoint => {
|
||
|
|
const joint = this.convertJoint(urdfJoint);
|
||
|
|
model.addJoint(joint);
|
||
|
|
});
|
||
|
|
|
||
|
|
// If XML provided, supplement effort and velocity from XML (urdf-loaders may not have parsed)
|
||
|
|
if (urdfXML) {
|
||
|
|
this.supplementJointLimitsFromXML(model, urdfXML);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Find root link (link that is not a child of any joint)
|
||
|
|
const allChildren = new Set(
|
||
|
|
Array.from(model.joints.values()).map(j => j.child).filter(c => c)
|
||
|
|
);
|
||
|
|
const rootLinks = Array.from(model.links.keys()).filter(
|
||
|
|
name => !allChildren.has(name)
|
||
|
|
);
|
||
|
|
if (rootLinks.length > 0) {
|
||
|
|
model.rootLink = rootLinks[0];
|
||
|
|
}
|
||
|
|
|
||
|
|
// Enhance materials for better lighting (MuJoCo style)
|
||
|
|
this.enhanceMaterials(robot);
|
||
|
|
|
||
|
|
return model;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Enhance materials for better lighting (MuJoCo style)
|
||
|
|
* Applies consistent shininess and specular properties to all materials
|
||
|
|
* Directly modifies materials in place to ensure changes persist
|
||
|
|
*/
|
||
|
|
static enhanceMaterials(robotObject) {
|
||
|
|
robotObject.traverse((child) => {
|
||
|
|
if (child.isMesh && child.material) {
|
||
|
|
// Handle material arrays
|
||
|
|
if (Array.isArray(child.material)) {
|
||
|
|
child.material = child.material.map(mat => {
|
||
|
|
return this.enhanceSingleMaterial(mat);
|
||
|
|
});
|
||
|
|
} else {
|
||
|
|
child.material = this.enhanceSingleMaterial(child.material);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
});
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Enhance a single material with better lighting properties
|
||
|
|
* Returns enhanced material (may be cloned or modified in place)
|
||
|
|
* Saves original properties for lighting toggle
|
||
|
|
*/
|
||
|
|
static enhanceSingleMaterial(material) {
|
||
|
|
if (material.isMeshPhongMaterial || material.isMeshStandardMaterial) {
|
||
|
|
// Save original properties if not already saved (for lighting toggle)
|
||
|
|
if (material.userData.originalShininess === undefined) {
|
||
|
|
material.userData.originalShininess = material.shininess !== undefined ? material.shininess : 30;
|
||
|
|
// IMPORTANT: Save original color to preserve URDF material colors
|
||
|
|
if (!material.userData.originalColor && material.color) {
|
||
|
|
material.userData.originalColor = material.color.clone();
|
||
|
|
}
|
||
|
|
// Save original specular - if material had no specular, save null
|
||
|
|
if (!material.specular) {
|
||
|
|
material.userData.originalSpecular = null;
|
||
|
|
} else if (material.specular.isColor) {
|
||
|
|
const spec = material.specular;
|
||
|
|
if (spec.r < 0.1 && spec.g < 0.1 && spec.b < 0.1) {
|
||
|
|
material.userData.originalSpecular = null; // Likely default
|
||
|
|
} else {
|
||
|
|
material.userData.originalSpecular = spec.clone();
|
||
|
|
}
|
||
|
|
} else if (typeof material.specular === 'number') {
|
||
|
|
if (material.specular === 0x111111 || material.specular < 0x111111) {
|
||
|
|
material.userData.originalSpecular = null;
|
||
|
|
} else {
|
||
|
|
material.userData.originalSpecular = new THREE.Color(material.specular);
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
material.userData.originalSpecular = null;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// IMPORTANT: Preserve original color from URDF material definitions
|
||
|
|
// Do not modify material.color - urdf-loaders already sets the correct color
|
||
|
|
|
||
|
|
// Apply enhanced lighting (default enabled)
|
||
|
|
// Increase shininess for better highlights
|
||
|
|
if (material.shininess === undefined || material.shininess < 50) {
|
||
|
|
material.shininess = 50;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Enhance specular reflection - ensure it's a Color object with proper values
|
||
|
|
if (!material.specular) {
|
||
|
|
material.specular = new THREE.Color(0.3, 0.3, 0.3);
|
||
|
|
} else if (material.specular.isColor) {
|
||
|
|
// If it's already a Color object, update values
|
||
|
|
if (material.specular.r < 0.2 || material.specular.g < 0.2 || material.specular.b < 0.2) {
|
||
|
|
material.specular.setRGB(0.3, 0.3, 0.3);
|
||
|
|
}
|
||
|
|
} else if (typeof material.specular === 'number') {
|
||
|
|
// Convert number to Color object
|
||
|
|
if (material.specular < 0x333333) {
|
||
|
|
material.specular = new THREE.Color(0.3, 0.3, 0.3);
|
||
|
|
} else {
|
||
|
|
// Convert hex to Color
|
||
|
|
material.specular = new THREE.Color(material.specular);
|
||
|
|
if (material.specular.r < 0.2) {
|
||
|
|
material.specular.setRGB(0.3, 0.3, 0.3);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Mark material as needing update
|
||
|
|
material.needsUpdate = true;
|
||
|
|
return material;
|
||
|
|
} else if (material.type === 'MeshBasicMaterial') {
|
||
|
|
// Convert MeshBasicMaterial to MeshPhongMaterial
|
||
|
|
const oldMaterial = material;
|
||
|
|
const envMap = typeof window !== 'undefined' && window.app?.sceneManager?.environmentManager?.getEnvironmentMap();
|
||
|
|
const newMaterial = new THREE.MeshPhongMaterial({
|
||
|
|
color: oldMaterial.color,
|
||
|
|
map: oldMaterial.map,
|
||
|
|
transparent: oldMaterial.transparent,
|
||
|
|
opacity: oldMaterial.opacity,
|
||
|
|
side: oldMaterial.side,
|
||
|
|
shininess: 50,
|
||
|
|
specular: new THREE.Color(0.3, 0.3, 0.3),
|
||
|
|
envMap: envMap || null,
|
||
|
|
reflectivity: envMap ? 0.3 : 0
|
||
|
|
});
|
||
|
|
// Save original properties for lighting toggle
|
||
|
|
newMaterial.userData.originalShininess = 30;
|
||
|
|
newMaterial.userData.originalSpecular = null; // MeshBasicMaterial had no specular
|
||
|
|
return newMaterial;
|
||
|
|
}
|
||
|
|
|
||
|
|
return material;
|
||
|
|
}
|
||
|
|
|
||
|
|
static convertLink(urdfLink) {
|
||
|
|
const link = new Link(urdfLink.name);
|
||
|
|
link.threeObject = urdfLink;
|
||
|
|
|
||
|
|
// Convert inertial properties
|
||
|
|
if (urdfLink.inertial) {
|
||
|
|
link.inertial = this.convertInertial(urdfLink.inertial);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Note: urdf-loaders has already converted visual and collision to Three.js objects
|
||
|
|
// We mainly extract metadata, actual meshes are in threeObject
|
||
|
|
|
||
|
|
return link;
|
||
|
|
}
|
||
|
|
|
||
|
|
static convertJoint(urdfJoint) {
|
||
|
|
// URDF joint object has jointType property (not type)
|
||
|
|
const jointType = urdfJoint.jointType || urdfJoint.type || 'fixed';
|
||
|
|
const joint = new Joint(urdfJoint.name, jointType);
|
||
|
|
|
||
|
|
// Extract parent name (urdf-loader returns Three.js object reference)
|
||
|
|
joint.parent = urdfJoint.parent?.name || null;
|
||
|
|
|
||
|
|
// child may not be in urdfJoint.child, but in Three.js children array
|
||
|
|
// In Three.js scene graph, Joint is parent node, Child Link is Joint's child node
|
||
|
|
if (urdfJoint.child && urdfJoint.child.name) {
|
||
|
|
joint.child = urdfJoint.child.name;
|
||
|
|
} else if (urdfJoint.children && urdfJoint.children.length > 0) {
|
||
|
|
// Find Link object from children array
|
||
|
|
const childLink = urdfJoint.children.find(child =>
|
||
|
|
child.isURDFLink || child.type === 'URDFLink'
|
||
|
|
);
|
||
|
|
if (childLink) {
|
||
|
|
joint.child = childLink.name;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
joint.threeObject = urdfJoint;
|
||
|
|
|
||
|
|
// Convert origin
|
||
|
|
if (urdfJoint.origin) {
|
||
|
|
joint.origin = {
|
||
|
|
xyz: urdfJoint.origin.xyz || [0, 0, 0],
|
||
|
|
rpy: urdfJoint.origin.rpy || [0, 0, 0]
|
||
|
|
};
|
||
|
|
}
|
||
|
|
|
||
|
|
// Convert axis
|
||
|
|
if (urdfJoint.axis) {
|
||
|
|
joint.axis = { xyz: urdfJoint.axis.xyz || [0, 0, 1] };
|
||
|
|
}
|
||
|
|
|
||
|
|
// Convert limits
|
||
|
|
if (urdfJoint.limit) {
|
||
|
|
joint.limits = this.convertLimits(urdfJoint.limit);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Get current value
|
||
|
|
if (urdfJoint.angle !== undefined) {
|
||
|
|
joint.currentValue = urdfJoint.angle;
|
||
|
|
} else if (urdfJoint.jointValue !== undefined) {
|
||
|
|
joint.currentValue = urdfJoint.jointValue;
|
||
|
|
}
|
||
|
|
|
||
|
|
return joint;
|
||
|
|
}
|
||
|
|
|
||
|
|
static convertLimits(limit) {
|
||
|
|
const limits = new JointLimits();
|
||
|
|
if (limit.lower !== undefined) limits.lower = limit.lower;
|
||
|
|
if (limit.upper !== undefined) limits.upper = limit.upper;
|
||
|
|
if (limit.effort !== undefined) limits.effort = limit.effort;
|
||
|
|
if (limit.velocity !== undefined) limits.velocity = limit.velocity;
|
||
|
|
return limits;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Supplement joint effort and velocity information from URDF XML
|
||
|
|
* (because urdf-loaders may not have parsed these attributes)
|
||
|
|
*/
|
||
|
|
static supplementJointLimitsFromXML(model, urdfXML) {
|
||
|
|
try {
|
||
|
|
const parser = new DOMParser();
|
||
|
|
const doc = parser.parseFromString(urdfXML, 'text/xml');
|
||
|
|
|
||
|
|
const jointElements = doc.querySelectorAll('joint');
|
||
|
|
jointElements.forEach(jointEl => {
|
||
|
|
const jointName = jointEl.getAttribute('name');
|
||
|
|
const joint = model.joints.get(jointName);
|
||
|
|
|
||
|
|
if (joint) {
|
||
|
|
const limitEl = jointEl.querySelector('limit');
|
||
|
|
if (limitEl) {
|
||
|
|
if (!joint.limits) {
|
||
|
|
joint.limits = new JointLimits();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Read effort and velocity
|
||
|
|
const effort = limitEl.getAttribute('effort');
|
||
|
|
const velocity = limitEl.getAttribute('velocity');
|
||
|
|
|
||
|
|
if (effort !== null) {
|
||
|
|
joint.limits.effort = parseFloat(effort);
|
||
|
|
}
|
||
|
|
if (velocity !== null) {
|
||
|
|
joint.limits.velocity = parseFloat(velocity);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Also ensure lower and upper are correctly set
|
||
|
|
const lower = limitEl.getAttribute('lower');
|
||
|
|
const upper = limitEl.getAttribute('upper');
|
||
|
|
if (lower !== null) {
|
||
|
|
joint.limits.lower = parseFloat(lower);
|
||
|
|
}
|
||
|
|
if (upper !== null) {
|
||
|
|
joint.limits.upper = parseFloat(upper);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
});
|
||
|
|
} catch (error) {
|
||
|
|
console.error('Failed to supplement joint limit information from XML:', error);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static convertInertial(inertial) {
|
||
|
|
const props = new InertialProperties();
|
||
|
|
if (inertial.mass !== undefined) props.mass = inertial.mass;
|
||
|
|
if (inertial.origin) {
|
||
|
|
props.origin = {
|
||
|
|
xyz: inertial.origin.xyz || [0, 0, 0],
|
||
|
|
rpy: inertial.origin.rpy || [0, 0, 0]
|
||
|
|
};
|
||
|
|
}
|
||
|
|
if (inertial.ixx !== undefined) props.ixx = inertial.ixx;
|
||
|
|
if (inertial.iyy !== undefined) props.iyy = inertial.iyy;
|
||
|
|
if (inertial.izz !== undefined) props.izz = inertial.izz;
|
||
|
|
if (inertial.ixy !== undefined) props.ixy = inertial.ixy;
|
||
|
|
if (inertial.ixz !== undefined) props.ixz = inertial.ixz;
|
||
|
|
if (inertial.iyz !== undefined) props.iyz = inertial.iyz;
|
||
|
|
return props;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Parse inertial data from URDF XML
|
||
|
|
* @param {string} xmlContent - URDF XML content
|
||
|
|
* @returns {Object} Mapping from link names to inertial data
|
||
|
|
*/
|
||
|
|
static parseInertialFromXML(xmlContent) {
|
||
|
|
const inertialData = {};
|
||
|
|
|
||
|
|
try {
|
||
|
|
const parser = new DOMParser();
|
||
|
|
const xmlDoc = parser.parseFromString(xmlContent, 'text/xml');
|
||
|
|
|
||
|
|
// Get all link elements
|
||
|
|
const links = xmlDoc.getElementsByTagName('link');
|
||
|
|
|
||
|
|
for (let i = 0; i < links.length; i++) {
|
||
|
|
const linkElement = links[i];
|
||
|
|
const linkName = linkElement.getAttribute('name');
|
||
|
|
|
||
|
|
// Find inertial element
|
||
|
|
const inertialElement = linkElement.getElementsByTagName('inertial')[0];
|
||
|
|
if (!inertialElement) continue;
|
||
|
|
|
||
|
|
const inertial = new InertialProperties();
|
||
|
|
|
||
|
|
// Parse mass
|
||
|
|
const massElement = inertialElement.getElementsByTagName('mass')[0];
|
||
|
|
if (massElement) {
|
||
|
|
inertial.mass = parseFloat(massElement.getAttribute('value'));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Parse origin
|
||
|
|
const originElement = inertialElement.getElementsByTagName('origin')[0];
|
||
|
|
if (originElement) {
|
||
|
|
const xyz = originElement.getAttribute('xyz');
|
||
|
|
const rpy = originElement.getAttribute('rpy');
|
||
|
|
inertial.origin = {
|
||
|
|
xyz: xyz ? xyz.split(' ').map(parseFloat) : [0, 0, 0],
|
||
|
|
rpy: rpy ? rpy.split(' ').map(parseFloat) : [0, 0, 0]
|
||
|
|
};
|
||
|
|
} else {
|
||
|
|
inertial.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
|
||
|
|
}
|
||
|
|
|
||
|
|
// Parse inertia
|
||
|
|
const inertiaElement = inertialElement.getElementsByTagName('inertia')[0];
|
||
|
|
if (inertiaElement) {
|
||
|
|
inertial.ixx = parseFloat(inertiaElement.getAttribute('ixx')) || 0;
|
||
|
|
inertial.iyy = parseFloat(inertiaElement.getAttribute('iyy')) || 0;
|
||
|
|
inertial.izz = parseFloat(inertiaElement.getAttribute('izz')) || 0;
|
||
|
|
inertial.ixy = parseFloat(inertiaElement.getAttribute('ixy')) || 0;
|
||
|
|
inertial.ixz = parseFloat(inertiaElement.getAttribute('ixz')) || 0;
|
||
|
|
inertial.iyz = parseFloat(inertiaElement.getAttribute('iyz')) || 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
inertialData[linkName] = inertial;
|
||
|
|
}
|
||
|
|
} catch (error) {
|
||
|
|
console.error('Failed to parse URDF XML inertial data:', error);
|
||
|
|
}
|
||
|
|
|
||
|
|
return inertialData;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Set joint angle (using urdf-loaders' setJointValue method)
|
||
|
|
* Reference URDFClasses.js setJointValue implementation
|
||
|
|
*/
|
||
|
|
static setJointAngle(joint, angle, ignoreLimits = false) {
|
||
|
|
joint.currentValue = angle;
|
||
|
|
|
||
|
|
// URDF format: use urdf-loader's setJointValue method
|
||
|
|
if (joint.threeObject) {
|
||
|
|
// If ignoring limits, temporarily modify URDF joint object's limit values
|
||
|
|
let originalLimits = null;
|
||
|
|
if (ignoreLimits && joint.threeObject.limit) {
|
||
|
|
originalLimits = {
|
||
|
|
lower: joint.threeObject.limit.lower,
|
||
|
|
upper: joint.threeObject.limit.upper
|
||
|
|
};
|
||
|
|
joint.threeObject.limit.lower = -Math.PI * 2;
|
||
|
|
joint.threeObject.limit.upper = Math.PI * 2;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Prefer setJointValue method (urdf-loader's standard method)
|
||
|
|
if (typeof joint.threeObject.setJointValue === 'function') {
|
||
|
|
joint.threeObject.setJointValue(angle);
|
||
|
|
|
||
|
|
// Restore original limits
|
||
|
|
if (originalLimits && joint.threeObject.limit) {
|
||
|
|
joint.threeObject.limit.lower = originalLimits.lower;
|
||
|
|
joint.threeObject.limit.upper = originalLimits.upper;
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
} else if (typeof joint.threeObject.setAngle === 'function') {
|
||
|
|
joint.threeObject.setAngle(angle);
|
||
|
|
|
||
|
|
// Restore original limits
|
||
|
|
if (originalLimits && joint.threeObject.limit) {
|
||
|
|
joint.threeObject.limit.lower = originalLimits.lower;
|
||
|
|
joint.threeObject.limit.upper = originalLimits.upper;
|
||
|
|
}
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Restore original limits (if none of the above methods executed)
|
||
|
|
if (originalLimits && joint.threeObject.limit) {
|
||
|
|
joint.threeObject.limit.lower = originalLimits.lower;
|
||
|
|
joint.threeObject.limit.upper = originalLimits.upper;
|
||
|
|
}
|
||
|
|
|
||
|
|
// If none available, manually set rotation
|
||
|
|
// Note: For URDF, we usually shouldn't reach here, as urdf-loader should provide setJointValue
|
||
|
|
console.warn(`Joint ${joint.name} has no setJointValue or setAngle method, attempting manual setup`);
|
||
|
|
if (joint.type === 'revolute' || joint.type === 'continuous') {
|
||
|
|
const axis = joint.axis ? new THREE.Vector3(...joint.axis.xyz).normalize() : new THREE.Vector3(0, 0, 1);
|
||
|
|
// Note: joint.threeObject may not be a Three.js object, cannot directly set rotation
|
||
|
|
if (joint.threeObject.rotation) {
|
||
|
|
joint.threeObject.rotation.setFromAxisAngle(axis, angle);
|
||
|
|
}
|
||
|
|
} else if (joint.type === 'prismatic') {
|
||
|
|
const axis = joint.axis ? new THREE.Vector3(...joint.axis.xyz).normalize() : new THREE.Vector3(1, 0, 0);
|
||
|
|
if (joint.threeObject.position) {
|
||
|
|
joint.threeObject.position.copy(axis.multiplyScalar(angle));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
console.warn(`Joint ${joint.name} has no threeObject`);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|