2091 lines
89 KiB
JavaScript
2091 lines
89 KiB
JavaScript
/**
|
|
* MJCF Adapter
|
|
* Parses MJCF XML and converts to unified model
|
|
*/
|
|
import { UnifiedRobotModel, Link, Joint, JointLimits, VisualGeometry, CollisionGeometry, InertialProperties, GeometryType, Constraint } from './UnifiedRobotModel.js';
|
|
import * as THREE from 'three';
|
|
import { loadMeshFile, ensureMeshHasPhongMaterial, getLoaders } from './MeshLoader.js';
|
|
|
|
export class MJCFAdapter {
|
|
/**
|
|
* Process include tags in MJCF XML
|
|
* Replaces <include file="path"/> with the content of the referenced file
|
|
* @param {string} xmlContent - MJCF XML content
|
|
* @param {Map} fileMap - File map for loading included files
|
|
* @param {string} basePath - Base path for resolving relative paths
|
|
* @returns {Promise<string>} Processed XML content
|
|
*/
|
|
static async processIncludes(xmlContent, fileMap = null, basePath = null) {
|
|
const parser = new DOMParser();
|
|
const doc = parser.parseFromString(xmlContent, 'text/xml');
|
|
|
|
// Check for parse errors
|
|
const parseError = doc.querySelector('parsererror');
|
|
if (parseError) {
|
|
// If there's a parse error, return original content
|
|
console.warn('Initial XML parse error, skipping include processing:', parseError.textContent);
|
|
return xmlContent;
|
|
}
|
|
|
|
// Find all include elements
|
|
const includes = doc.querySelectorAll('include');
|
|
|
|
if (includes.length === 0) {
|
|
return xmlContent;
|
|
}
|
|
|
|
console.log(`Processing ${includes.length} include tag(s)...`);
|
|
|
|
// Process each include tag
|
|
for (const includeEl of includes) {
|
|
const filePath = includeEl.getAttribute('file');
|
|
|
|
if (!filePath) {
|
|
console.warn('Include tag missing file attribute');
|
|
includeEl.remove();
|
|
continue;
|
|
}
|
|
|
|
// Try to find the included file in fileMap
|
|
let includedContent = null;
|
|
|
|
if (fileMap) {
|
|
// Try different path variations
|
|
const pathVariations = [
|
|
filePath,
|
|
basePath ? basePath + '/' + filePath : filePath,
|
|
filePath.startsWith('/') ? filePath : '/' + filePath
|
|
];
|
|
|
|
for (const path of pathVariations) {
|
|
// Try exact match first
|
|
if (fileMap.has(path)) {
|
|
const file = fileMap.get(path);
|
|
try {
|
|
includedContent = await file.text();
|
|
console.log(`Found included file: ${path}`);
|
|
break;
|
|
} catch (e) {
|
|
console.warn(`Failed to read included file ${path}:`, e);
|
|
}
|
|
}
|
|
|
|
// Try case-insensitive match
|
|
for (const [key, value] of fileMap) {
|
|
if (key.toLowerCase() === path.toLowerCase()) {
|
|
try {
|
|
includedContent = await value.text();
|
|
console.log(`Found included file (case-insensitive): ${key}`);
|
|
break;
|
|
} catch (e) {
|
|
console.warn(`Failed to read included file ${key}:`, e);
|
|
}
|
|
}
|
|
}
|
|
if (includedContent) break;
|
|
}
|
|
}
|
|
|
|
if (!includedContent) {
|
|
console.warn(`Could not find included file: ${filePath}`);
|
|
includeEl.remove();
|
|
continue;
|
|
}
|
|
|
|
// Parse the included content
|
|
const includedDoc = parser.parseFromString(includedContent, 'text/xml');
|
|
const includedParseError = includedDoc.querySelector('parsererror');
|
|
|
|
if (includedParseError) {
|
|
console.warn(`Failed to parse included file ${filePath}:`, includedParseError.textContent);
|
|
includeEl.remove();
|
|
continue;
|
|
}
|
|
|
|
// Get the mujoco root element from included file
|
|
const includedRoot = includedDoc.querySelector('mujoco');
|
|
|
|
if (!includedRoot) {
|
|
console.warn(`Included file ${filePath} has no mujoco root element`);
|
|
includeEl.remove();
|
|
continue;
|
|
}
|
|
|
|
// Move all child elements from included mujoco to current document
|
|
// Insert them before the include element
|
|
const childNodes = Array.from(includedRoot.childNodes);
|
|
|
|
for (const child of childNodes) {
|
|
// Skip text nodes and comment nodes
|
|
if (child.nodeType === Node.TEXT_NODE ||
|
|
(child.nodeType === Node.COMMENT_NODE) ||
|
|
(child.nodeType === Node.PROCESSING_INSTRUCTION_NODE)) {
|
|
continue;
|
|
}
|
|
|
|
// Clone the node to avoid removing from included doc
|
|
const importedNode = doc.importNode(child, true);
|
|
|
|
// Insert before the include element
|
|
includeEl.parentNode.insertBefore(importedNode, includeEl);
|
|
}
|
|
|
|
console.log(`Successfully included content from: ${filePath}`);
|
|
|
|
// Remove the include element
|
|
includeEl.remove();
|
|
}
|
|
|
|
// Serialize the modified document back to string
|
|
const serializer = new XMLSerializer();
|
|
return serializer.serializeToString(doc);
|
|
}
|
|
|
|
/**
|
|
* Parse MJCF XML content and convert to unified model
|
|
* @param {string} xmlContent - MJCF XML content
|
|
* @param {Map} fileMap - File map (optional), for loading mesh files
|
|
* @param {string} basePath - Base path for resolving relative include paths (optional)
|
|
* @returns {Promise<UnifiedRobotModel>}
|
|
*/
|
|
static async parse(xmlContent, fileMap = null, basePath = null) {
|
|
// Process include tags first
|
|
const processedContent = await this.processIncludes(xmlContent, fileMap, basePath);
|
|
|
|
const parser = new DOMParser();
|
|
const doc = parser.parseFromString(processedContent, 'text/xml');
|
|
|
|
// Check parse errors
|
|
const parseError = doc.querySelector('parsererror');
|
|
if (parseError) {
|
|
throw new Error('MJCF XML parsing failed: ' + parseError.textContent);
|
|
}
|
|
|
|
const model = new UnifiedRobotModel();
|
|
model.name = 'mujoco_model';
|
|
|
|
// Parse default values and class definitions in default tags first
|
|
// (needed for mesh scale inheritance)
|
|
const { classDefaults, rootDefaults } = this.parseDefaults(doc);
|
|
|
|
// Parse mesh definitions in asset tags (build mesh name to file path mapping)
|
|
// Pass classDefaults and rootDefaults to inherit mesh scale
|
|
const meshMap = this.parseAssets(doc, classDefaults, rootDefaults);
|
|
|
|
// Parse material definitions in material tags
|
|
const materialMap = this.parseMaterials(doc);
|
|
|
|
// Get worldbody (root node)
|
|
const worldbody = doc.querySelector('worldbody');
|
|
if (!worldbody) {
|
|
throw new Error('MJCF file missing worldbody element');
|
|
}
|
|
|
|
// Parse geoms directly in worldbody (not inside any body element)
|
|
// These geoms belong to a special "worldbody" link
|
|
const worldbodyGeoms = worldbody.querySelectorAll(':scope > geom');
|
|
if (worldbodyGeoms.length > 0) {
|
|
const worldbodyLink = new Link('worldbody');
|
|
worldbodyLink.userData.isWorldbody = true;
|
|
const seenMeshes = new Set();
|
|
|
|
worldbodyGeoms.forEach((geomEl, geomIndex) => {
|
|
// Get inherited properties from default class
|
|
const inheritedProps = this.getGeomInheritedProperties(geomEl, classDefaults, rootDefaults);
|
|
|
|
const group = geomEl.getAttribute('group');
|
|
// Use inherited group if not explicitly defined
|
|
const groupNum = group !== null ? parseInt(group) :
|
|
(inheritedProps.group !== null ? inheritedProps.group : 0);
|
|
const geomName = (geomEl.getAttribute('name') || '').toLowerCase();
|
|
const hasRgba = geomEl.hasAttribute('rgba') || inheritedProps.rgba !== null;
|
|
const meshRef = geomEl.getAttribute('mesh');
|
|
|
|
// Use inherited contype/conaffinity if not explicitly defined
|
|
const contype = geomEl.getAttribute('contype');
|
|
const conaffinity = geomEl.getAttribute('conaffinity');
|
|
const density = geomEl.getAttribute('density');
|
|
const contypeNum = contype !== null ? parseInt(contype) :
|
|
(inheritedProps.contype !== null ? inheritedProps.contype : null);
|
|
const conaffinityNum = conaffinity !== null ? parseInt(conaffinity) :
|
|
(inheritedProps.conaffinity !== null ? inheritedProps.conaffinity : null);
|
|
const densityNum = density !== null ? parseFloat(density) :
|
|
(inheritedProps.density !== null ? inheritedProps.density : null);
|
|
|
|
// Determine if collision or visual (same logic as in parseBodies)
|
|
let isCollisionGeom = false;
|
|
if (!meshRef) {
|
|
isCollisionGeom = true;
|
|
} else {
|
|
if (contypeNum === 0 && conaffinityNum === 0) {
|
|
isCollisionGeom = false;
|
|
} else if (groupNum === 3) {
|
|
// group=3 is collision in MuJoCo convention
|
|
isCollisionGeom = true;
|
|
} else if (groupNum === 2 || groupNum === 1) {
|
|
// group=1,2 are visual
|
|
isCollisionGeom = false;
|
|
} else if (geomName.includes('collision')) {
|
|
isCollisionGeom = true;
|
|
} else if (seenMeshes.has(meshRef)) {
|
|
if (hasRgba || (contypeNum === 0 && conaffinityNum === 0)) {
|
|
return; // Skip duplicate visual
|
|
} else {
|
|
isCollisionGeom = true;
|
|
}
|
|
} else if (densityNum === 0 && groupNum === 1) {
|
|
isCollisionGeom = false;
|
|
} else if (hasRgba) {
|
|
isCollisionGeom = false;
|
|
} else {
|
|
isCollisionGeom = false;
|
|
}
|
|
}
|
|
|
|
const geom = this.parseGeom(geomEl, meshMap);
|
|
if (geom) {
|
|
if (isCollisionGeom) {
|
|
const collision = new CollisionGeometry();
|
|
collision.geometry = geom;
|
|
collision.name = geomEl.getAttribute('name') || `worldbody_collision_${geomIndex}`;
|
|
collision.origin = this.parseOrigin(geomEl);
|
|
worldbodyLink.collisions.push(collision);
|
|
} else {
|
|
if (meshRef) {
|
|
seenMeshes.add(meshRef);
|
|
}
|
|
const visual = new VisualGeometry();
|
|
visual.geometry = geom;
|
|
visual.name = geomEl.getAttribute('name') || `worldbody_geom_${geomIndex}`;
|
|
visual.origin = this.parseOrigin(geomEl);
|
|
|
|
// Parse rgba (priority: explicit > inherited)
|
|
let rgba = null;
|
|
if (geomEl.hasAttribute('rgba')) {
|
|
const rgbaStr = geomEl.getAttribute('rgba');
|
|
const rgbaVals = rgbaStr.split(' ').map(parseFloat);
|
|
if (rgbaVals.length >= 3) {
|
|
rgba = {
|
|
r: rgbaVals[0],
|
|
g: rgbaVals[1],
|
|
b: rgbaVals[2],
|
|
a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
|
|
};
|
|
}
|
|
} else if (inheritedProps.rgba) {
|
|
rgba = inheritedProps.rgba;
|
|
}
|
|
|
|
visual.userData = {
|
|
group: groupNum,
|
|
hasRgba: hasRgba || !!rgba,
|
|
rgba: rgba,
|
|
meshRef: meshRef,
|
|
geomType: geomEl.getAttribute('type') || (meshRef ? 'mesh' : 'box')
|
|
};
|
|
worldbodyLink.visuals.push(visual);
|
|
}
|
|
}
|
|
});
|
|
|
|
// Only add worldbody link if it has geometries
|
|
if (worldbodyLink.visuals.length > 0 || worldbodyLink.collisions.length > 0) {
|
|
model.addLink(worldbodyLink);
|
|
}
|
|
}
|
|
|
|
// Parse all bodies (links), pass meshMap, materialMap, classDefaults and rootDefaults
|
|
const bodyMap = new Map();
|
|
this.parseBodies(worldbody, null, bodyMap, model, null, meshMap, null, materialMap, classDefaults, rootDefaults);
|
|
|
|
// Parse all joints
|
|
this.parseJoints(worldbody, bodyMap, model, null, classDefaults);
|
|
|
|
// Parse equality constraints (closed-chain constraints for parallel mechanisms)
|
|
this.parseEquality(doc, model);
|
|
|
|
// Find root body
|
|
// Priority: worldbody link > bodies without parent joints > first link
|
|
const worldbodyLink = model.links.get('worldbody');
|
|
if (worldbodyLink) {
|
|
model.rootLink = 'worldbody';
|
|
} else {
|
|
const rootBodies = Array.from(model.links.keys()).filter(
|
|
name => !Array.from(model.joints.values()).some(j => j.child === name)
|
|
);
|
|
if (rootBodies.length > 0) {
|
|
model.rootLink = rootBodies[0];
|
|
} else if (model.links.size > 0) {
|
|
model.rootLink = Array.from(model.links.keys())[0];
|
|
}
|
|
}
|
|
|
|
// Create Three.js objects (asynchronously load mesh files)
|
|
await this.createThreeObject(model, fileMap, meshMap);
|
|
|
|
return model;
|
|
}
|
|
|
|
/**
|
|
* Parse mesh definitions in asset tags
|
|
* @param {Document} doc - XML document
|
|
* @param {Map} classDefaults - Class default properties map (optional)
|
|
* @param {object} rootDefaults - Root default properties (optional)
|
|
* @returns {Map<string, object>} Mapping from mesh names to mesh data
|
|
* Mesh data can be: { type: 'file', path: string, scale: [x,y,z] } or { type: 'vertex', vertices: Float32Array, scale: [x,y,z] }
|
|
*/
|
|
static parseAssets(doc, classDefaults = null, rootDefaults = null) {
|
|
const meshMap = new Map();
|
|
const asset = doc.querySelector('asset');
|
|
if (!asset) {
|
|
return meshMap;
|
|
}
|
|
|
|
const meshes = asset.querySelectorAll('mesh');
|
|
meshes.forEach((meshEl, index) => {
|
|
let name = meshEl.getAttribute('name');
|
|
const file = meshEl.getAttribute('file');
|
|
const vertex = meshEl.getAttribute('vertex');
|
|
const scale = meshEl.getAttribute('scale');
|
|
const meshClass = meshEl.getAttribute('class');
|
|
|
|
// Parse scale (priority: direct attribute > class inheritance > root defaults > [1,1,1])
|
|
let scaleVec = [1, 1, 1];
|
|
|
|
// First check direct scale attribute
|
|
if (scale) {
|
|
const scaleValues = scale.trim().split(/\s+/).map(parseFloat);
|
|
if (scaleValues.length === 1) {
|
|
scaleVec = [scaleValues[0], scaleValues[0], scaleValues[0]];
|
|
} else if (scaleValues.length === 3) {
|
|
scaleVec = scaleValues;
|
|
}
|
|
} else if (meshClass && classDefaults && classDefaults.has(meshClass)) {
|
|
// Try to inherit scale from class defaults
|
|
const classDefault = classDefaults.get(meshClass);
|
|
if (classDefault.mesh && classDefault.mesh.scale) {
|
|
scaleVec = classDefault.mesh.scale;
|
|
}
|
|
} else if (rootDefaults && rootDefaults.mesh && rootDefaults.mesh.scale) {
|
|
// Fall back to root defaults (e.g., robotis_op3)
|
|
scaleVec = rootDefaults.mesh.scale;
|
|
}
|
|
|
|
// If has vertex attribute, it's an inline-defined mesh
|
|
if (vertex) {
|
|
if (!name) {
|
|
name = `inline_mesh_${index}`;
|
|
}
|
|
|
|
// Parse vertex data
|
|
const vertexValues = vertex.trim().split(/\s+/).map(parseFloat);
|
|
const vertices = new Float32Array(vertexValues);
|
|
|
|
meshMap.set(name, {
|
|
type: 'vertex',
|
|
vertices: vertices,
|
|
scale: scaleVec
|
|
});
|
|
}
|
|
// If has file attribute, it's an external file
|
|
else if (file) {
|
|
// If no name, extract filename from file (remove path and extension)
|
|
if (!name) {
|
|
// Extract filename from path: "path/to/wheel.stl" -> "wheel"
|
|
const fileName = file.split('/').pop().split('\\').pop(); // Support / and \ path separators
|
|
name = fileName.split('.')[0]; // Remove extension
|
|
}
|
|
|
|
meshMap.set(name, {
|
|
type: 'file',
|
|
path: file,
|
|
scale: scaleVec
|
|
});
|
|
} else {
|
|
console.warn('MJCF mesh element missing file or vertex attribute, skipping');
|
|
return;
|
|
}
|
|
});
|
|
|
|
return meshMap;
|
|
}
|
|
|
|
/**
|
|
* Parse material definitions in asset tags
|
|
* @param {Document} doc - XML document
|
|
* @returns {Map<string, object>} Mapping from material names to material properties
|
|
*/
|
|
static parseMaterials(doc) {
|
|
const materialMap = new Map();
|
|
const asset = doc.querySelector('asset');
|
|
if (!asset) {
|
|
return materialMap;
|
|
}
|
|
|
|
const materials = asset.querySelectorAll('material');
|
|
materials.forEach((matEl) => {
|
|
const name = matEl.getAttribute('name');
|
|
if (!name) return;
|
|
|
|
const material = {};
|
|
|
|
// Parse rgba
|
|
const rgba = matEl.getAttribute('rgba');
|
|
if (rgba) {
|
|
const vals = rgba.split(' ').map(parseFloat);
|
|
if (vals.length >= 3) {
|
|
material.rgba = {
|
|
r: vals[0],
|
|
g: vals[1],
|
|
b: vals[2],
|
|
a: vals.length >= 4 ? vals[3] : 1.0
|
|
};
|
|
}
|
|
}
|
|
|
|
// Parse other material properties
|
|
const specular = matEl.getAttribute('specular');
|
|
if (specular) {
|
|
const vals = specular.split(' ').map(parseFloat);
|
|
material.specular = vals[0] || 0.5;
|
|
}
|
|
|
|
const shininess = matEl.getAttribute('shininess');
|
|
if (shininess) {
|
|
material.shininess = parseFloat(shininess);
|
|
}
|
|
|
|
materialMap.set(name, material);
|
|
});
|
|
|
|
return materialMap;
|
|
}
|
|
|
|
/**
|
|
* Parse default values and class definitions in default tags
|
|
* @param {Document} doc - XML document
|
|
* @returns {object} Object containing classDefaults Map and rootDefaults object
|
|
*/
|
|
static parseDefaults(doc) {
|
|
const classDefaults = new Map();
|
|
let rootDefaults = {};
|
|
|
|
// Recursively parse default tags
|
|
const parseDefaultElement = (defaultEl, parentDefaults = {}) => {
|
|
const className = defaultEl.getAttribute('class');
|
|
|
|
// Start from parent defaults, deep copy to avoid reference issues
|
|
const defaults = JSON.parse(JSON.stringify(parentDefaults || {}));
|
|
|
|
// Parse mesh default values
|
|
const meshEl = defaultEl.querySelector(':scope > mesh');
|
|
if (meshEl) {
|
|
if (!defaults.mesh) {
|
|
defaults.mesh = {};
|
|
}
|
|
|
|
// Parse scale
|
|
const scale = meshEl.getAttribute('scale');
|
|
if (scale) {
|
|
const scaleVals = scale.trim().split(/\s+/).map(parseFloat);
|
|
if (scaleVals.length === 1) {
|
|
defaults.mesh.scale = [scaleVals[0], scaleVals[0], scaleVals[0]];
|
|
} else if (scaleVals.length === 3) {
|
|
defaults.mesh.scale = scaleVals;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Parse joint default values
|
|
const jointEl = defaultEl.querySelector(':scope > joint');
|
|
if (jointEl) {
|
|
// If parent has joint defaults, inherit first
|
|
if (!defaults.joint) {
|
|
defaults.joint = {};
|
|
}
|
|
|
|
// Parse axis (if axis defined, completely replace parent axis)
|
|
const axis = jointEl.getAttribute('axis');
|
|
if (axis) {
|
|
const axisVals = axis.split(' ').map(parseFloat);
|
|
defaults.joint.axis = [axisVals[0] || 0, axisVals[1] || 0, axisVals[2] || 0];
|
|
}
|
|
|
|
// Parse range
|
|
const range = jointEl.getAttribute('range');
|
|
if (range) {
|
|
const rangeVals = range.split(' ').map(parseFloat);
|
|
defaults.joint.range = rangeVals;
|
|
}
|
|
|
|
// Parse damping
|
|
const damping = jointEl.getAttribute('damping');
|
|
if (damping) {
|
|
defaults.joint.damping = parseFloat(damping);
|
|
}
|
|
}
|
|
|
|
// Parse geom default values
|
|
const geomEl = defaultEl.querySelector(':scope > geom');
|
|
if (geomEl) {
|
|
if (!defaults.geom) {
|
|
defaults.geom = {};
|
|
}
|
|
|
|
// Parse contype
|
|
const contype = geomEl.getAttribute('contype');
|
|
if (contype !== null) {
|
|
defaults.geom.contype = parseInt(contype);
|
|
}
|
|
|
|
// Parse conaffinity
|
|
const conaffinity = geomEl.getAttribute('conaffinity');
|
|
if (conaffinity !== null) {
|
|
defaults.geom.conaffinity = parseInt(conaffinity);
|
|
}
|
|
|
|
// Parse group
|
|
const group = geomEl.getAttribute('group');
|
|
if (group !== null) {
|
|
defaults.geom.group = parseInt(group);
|
|
}
|
|
|
|
// Parse rgba
|
|
const rgba = geomEl.getAttribute('rgba');
|
|
if (rgba) {
|
|
const rgbaVals = rgba.split(' ').map(parseFloat);
|
|
if (rgbaVals.length >= 3) {
|
|
defaults.geom.rgba = {
|
|
r: rgbaVals[0],
|
|
g: rgbaVals[1],
|
|
b: rgbaVals[2],
|
|
a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
|
|
};
|
|
}
|
|
}
|
|
|
|
// Parse material
|
|
const material = geomEl.getAttribute('material');
|
|
if (material) {
|
|
defaults.geom.material = material;
|
|
}
|
|
|
|
// Parse type
|
|
const type = geomEl.getAttribute('type');
|
|
if (type) {
|
|
defaults.geom.type = type;
|
|
}
|
|
|
|
// Parse density
|
|
const density = geomEl.getAttribute('density');
|
|
if (density !== null) {
|
|
defaults.geom.density = parseFloat(density);
|
|
}
|
|
}
|
|
|
|
// If has class name, save to class map
|
|
if (className) {
|
|
classDefaults.set(className, defaults);
|
|
} else {
|
|
// No class name means this is a root default (inherits to all)
|
|
// Store the final computed defaults as rootDefaults
|
|
Object.assign(rootDefaults, defaults);
|
|
}
|
|
|
|
// Recursively process nested default tags
|
|
const nestedDefaults = defaultEl.querySelectorAll(':scope > default');
|
|
nestedDefaults.forEach(nested => {
|
|
parseDefaultElement(nested, defaults);
|
|
});
|
|
};
|
|
|
|
// Start parsing from root default tags
|
|
const rootDefaultElements = doc.querySelectorAll('mujoco > default');
|
|
rootDefaultElements.forEach(defaultEl => {
|
|
parseDefaultElement(defaultEl);
|
|
});
|
|
|
|
return { classDefaults, rootDefaults };
|
|
}
|
|
|
|
/**
|
|
* Get inherited geom properties from default class
|
|
* @param {Element} geomEl - geom element
|
|
* @param {Map} classDefaults - Class default properties map
|
|
* @param {object} rootDefaults - Root default properties
|
|
* @returns {object} Inherited properties object
|
|
*/
|
|
static getGeomInheritedProperties(geomEl, classDefaults, rootDefaults) {
|
|
const inherited = {
|
|
contype: null,
|
|
conaffinity: null,
|
|
group: null,
|
|
rgba: null,
|
|
material: null,
|
|
type: null,
|
|
density: null
|
|
};
|
|
|
|
// First apply root defaults
|
|
if (rootDefaults && rootDefaults.geom) {
|
|
Object.assign(inherited, rootDefaults.geom);
|
|
}
|
|
|
|
// Then apply class defaults (if geom has class attribute)
|
|
const className = geomEl.getAttribute('class');
|
|
if (className && classDefaults && classDefaults.has(className)) {
|
|
const classDefault = classDefaults.get(className);
|
|
if (classDefault.geom) {
|
|
Object.assign(inherited, classDefault.geom);
|
|
}
|
|
}
|
|
|
|
return inherited;
|
|
}
|
|
|
|
/**
|
|
* Recursively parse body elements, record parent-child relationships
|
|
*/
|
|
static parseBodies(element, parentName, bodyMap, model, parentLinkRef = null, meshMap = null, stats = null, materialMap = null, classDefaults = null, rootDefaults = null) {
|
|
// Initialize stats object (only on root call)
|
|
if (!stats) {
|
|
stats = { totalGeoms: 0, skippedCollisionGeoms: 0, visualGeoms: 0 };
|
|
}
|
|
|
|
const bodies = element.querySelectorAll(':scope > body');
|
|
|
|
bodies.forEach(bodyEl => {
|
|
const linkName = bodyEl.getAttribute('name') || `body_${bodyMap.size}`;
|
|
const link = new Link(linkName);
|
|
|
|
// Record parent link relationship (for building hierarchy later)
|
|
if (parentName) {
|
|
link.userData.parentName = parentName;
|
|
}
|
|
|
|
// Parse body's pos and quat (body's own position)
|
|
const bodyOrigin = this.parseOrigin(bodyEl);
|
|
link.userData.bodyOrigin = bodyOrigin;
|
|
|
|
// Parse geometries (geom)
|
|
const geoms = bodyEl.querySelectorAll(':scope > geom');
|
|
const seenMeshes = new Set(); // Track added meshes to avoid duplicates
|
|
|
|
geoms.forEach((geomEl, geomIndex) => {
|
|
stats.totalGeoms++;
|
|
|
|
// Get inherited properties from default class
|
|
const inheritedProps = this.getGeomInheritedProperties(geomEl, classDefaults, rootDefaults);
|
|
|
|
const group = geomEl.getAttribute('group');
|
|
// Use inherited group if not explicitly defined
|
|
const groupNum = group !== null ? parseInt(group) :
|
|
(inheritedProps.group !== null ? inheritedProps.group : 0);
|
|
const geomName = (geomEl.getAttribute('name') || '').toLowerCase();
|
|
const hasRgba = geomEl.hasAttribute('rgba') || inheritedProps.rgba !== null;
|
|
const meshRef = geomEl.getAttribute('mesh');
|
|
// Use inherited type if not explicitly defined
|
|
const geomType = geomEl.getAttribute('type') || inheritedProps.type || (meshRef ? 'mesh' : 'box');
|
|
|
|
// Check collision-related attributes (use inherited if not explicitly defined)
|
|
const contype = geomEl.getAttribute('contype');
|
|
const conaffinity = geomEl.getAttribute('conaffinity');
|
|
const density = geomEl.getAttribute('density');
|
|
const contypeNum = contype !== null ? parseInt(contype) :
|
|
(inheritedProps.contype !== null ? inheritedProps.contype : null);
|
|
const conaffinityNum = conaffinity !== null ? parseInt(conaffinity) :
|
|
(inheritedProps.conaffinity !== null ? inheritedProps.conaffinity : null);
|
|
const densityNum = density !== null ? parseFloat(density) :
|
|
(inheritedProps.density !== null ? inheritedProps.density : null);
|
|
|
|
// Determine geom type: visual or collision
|
|
let isCollisionGeom = false;
|
|
let skipReason = '';
|
|
|
|
// [Key Strategy]: Distinguish visual and collision geoms
|
|
// Basic geometries (box, cylinder, sphere) are usually simplified shapes for collision
|
|
if (!meshRef) {
|
|
// No mesh reference, basic geometry, treat as collision
|
|
isCollisionGeom = true;
|
|
} else {
|
|
// Has mesh reference, check if should be collision
|
|
|
|
// Strategy 1: Explicitly disabled collision (contype="0" conaffinity="0") = visual only
|
|
if (contypeNum === 0 && conaffinityNum === 0) {
|
|
// This is explicitly marked as visual-only (no collision)
|
|
isCollisionGeom = false;
|
|
}
|
|
// Strategy 2: group=2 is visual, group=3 is collision
|
|
// MuJoCo convention: group 0=default, 1=visual1, 2=visual2, 3=collision
|
|
else if (groupNum === 3) {
|
|
isCollisionGeom = true;
|
|
} else if (groupNum === 2 || groupNum === 1) {
|
|
isCollisionGeom = false;
|
|
}
|
|
// Strategy 3: Name contains collision (indicates collision-specific)
|
|
else if (geomName.includes('collision')) {
|
|
isCollisionGeom = true;
|
|
}
|
|
// Strategy 4: If same mesh already added as visual
|
|
else if (seenMeshes.has(meshRef)) {
|
|
// If current geom also has visual markers (rgba or contype="0"), skip duplicate visual
|
|
if (hasRgba || (contypeNum === 0 && conaffinityNum === 0)) {
|
|
stats.skippedCollisionGeoms++;
|
|
return;
|
|
} else {
|
|
// Same mesh, but current geom has no visual markers - treat as collision
|
|
isCollisionGeom = true;
|
|
}
|
|
}
|
|
// Strategy 5: If density="0" and group="1", likely visual-only (common pattern in MJCF)
|
|
else if (densityNum === 0 && groupNum === 1) {
|
|
// This pattern (density="0" group="1") is often used for visual-only geoms
|
|
isCollisionGeom = false;
|
|
}
|
|
// Strategy 6: Default: if has rgba, treat as visual
|
|
else if (hasRgba) {
|
|
isCollisionGeom = false;
|
|
}
|
|
// Strategy 7: Default for mesh: treat as visual (for display purposes)
|
|
else {
|
|
// No explicit markers, but it's a mesh - default to visual for display
|
|
// (collision might be handled by a separate geom with same mesh)
|
|
isCollisionGeom = false;
|
|
}
|
|
}
|
|
|
|
const geom = this.parseGeom(geomEl, meshMap);
|
|
if (geom) {
|
|
if (isCollisionGeom) {
|
|
// Add to collision list
|
|
const collision = new CollisionGeometry();
|
|
collision.geometry = geom;
|
|
collision.name = geomEl.getAttribute('name') || `collision_${geomIndex}`;
|
|
collision.origin = this.parseOrigin(geomEl);
|
|
link.collisions.push(collision);
|
|
} else {
|
|
// Add to visual list
|
|
stats.visualGeoms++;
|
|
|
|
// Record added mesh
|
|
if (meshRef) {
|
|
seenMeshes.add(meshRef);
|
|
}
|
|
|
|
const visual = new VisualGeometry();
|
|
visual.geometry = geom;
|
|
visual.name = geomEl.getAttribute('name') || `geom_${geomIndex}`;
|
|
visual.origin = this.parseOrigin(geomEl);
|
|
|
|
// Parse MJCF rgba color (priority: geom rgba > inherited rgba > material rgba)
|
|
let rgba = null;
|
|
let materialName = null;
|
|
|
|
// 1. First check geom's own rgba, then inherited rgba
|
|
if (geomEl.hasAttribute('rgba')) {
|
|
const rgbaStr = geomEl.getAttribute('rgba');
|
|
const rgbaVals = rgbaStr.split(' ').map(parseFloat);
|
|
if (rgbaVals.length >= 3) {
|
|
rgba = {
|
|
r: rgbaVals[0],
|
|
g: rgbaVals[1],
|
|
b: rgbaVals[2],
|
|
a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
|
|
};
|
|
}
|
|
}
|
|
|
|
// 2. If geom has no explicit rgba, check inherited rgba
|
|
if (!rgba && inheritedProps.rgba) {
|
|
rgba = inheritedProps.rgba;
|
|
}
|
|
|
|
// 3. If still no rgba, check if references material (explicit or inherited)
|
|
if (!rgba && materialMap) {
|
|
materialName = geomEl.getAttribute('material') || inheritedProps.material;
|
|
if (materialName && materialMap.has(materialName)) {
|
|
const mat = materialMap.get(materialName);
|
|
if (mat.rgba) {
|
|
rgba = mat.rgba;
|
|
}
|
|
}
|
|
}
|
|
|
|
visual.userData = {
|
|
group: groupNum,
|
|
hasRgba: hasRgba || !!rgba,
|
|
rgba: rgba,
|
|
materialName: materialName,
|
|
meshRef: meshRef,
|
|
geomType: geomType
|
|
};
|
|
link.visuals.push(visual);
|
|
}
|
|
}
|
|
});
|
|
|
|
// Parse inertial properties
|
|
const inertialEl = bodyEl.querySelector(':scope > inertial');
|
|
if (inertialEl) {
|
|
link.inertial = this.parseInertial(inertialEl);
|
|
}
|
|
|
|
model.addLink(link);
|
|
bodyMap.set(linkName, { link, element: bodyEl, parentName });
|
|
|
|
// Recursively parse child bodies
|
|
this.parseBodies(bodyEl, linkName, bodyMap, model, link, meshMap, stats, materialMap, classDefaults, rootDefaults);
|
|
});
|
|
}
|
|
|
|
/**
|
|
* Parse geom element
|
|
* @param {Element} geomEl - geom element
|
|
* @param {Map} meshMap - Mapping from mesh names to file paths
|
|
*/
|
|
static parseGeom(geomEl, meshMap = null) {
|
|
// In MJCF, if geom has mesh attribute, type should be mesh
|
|
const meshAttr = geomEl.getAttribute('mesh');
|
|
let type = geomEl.getAttribute('type');
|
|
|
|
// If has mesh attribute but no explicit type declaration, auto-set to mesh
|
|
if (meshAttr && !type) {
|
|
type = 'mesh';
|
|
}
|
|
|
|
// If no type attribute and no mesh attribute, default to sphere
|
|
if (!type) {
|
|
type = 'sphere';
|
|
}
|
|
|
|
const geometry = new GeometryType(type);
|
|
|
|
switch (type) {
|
|
case 'box':
|
|
const size = geomEl.getAttribute('size');
|
|
if (size) {
|
|
const sizes = size.split(' ').map(parseFloat);
|
|
// MJCF size is half-size, multiply by 2 to convert to full size
|
|
geometry.size = sizes.length === 1
|
|
? { x: sizes[0] * 2, y: sizes[0] * 2, z: sizes[0] * 2 }
|
|
: { x: (sizes[0] || 0.05) * 2, y: (sizes[1] || 0.05) * 2, z: (sizes[2] || 0.05) * 2 };
|
|
} else {
|
|
geometry.size = { x: 0.1, y: 0.1, z: 0.1 };
|
|
}
|
|
break;
|
|
|
|
case 'sphere':
|
|
// MJCF sphere size is radius
|
|
const radius = parseFloat(geomEl.getAttribute('size') || '0.1');
|
|
geometry.size = { radius };
|
|
break;
|
|
|
|
case 'cylinder':
|
|
case 'capsule':
|
|
// Handle fromto attribute for capsule/cylinder
|
|
const fromto = geomEl.getAttribute('fromto');
|
|
const radiusAttr = geomEl.getAttribute('size');
|
|
|
|
if (fromto) {
|
|
const ft = fromto.split(' ').map(parseFloat);
|
|
if (ft.length >= 6) {
|
|
const p1 = new THREE.Vector3(ft[0], ft[1], ft[2]);
|
|
const p2 = new THREE.Vector3(ft[3], ft[4], ft[5]);
|
|
const center = new THREE.Vector3().addVectors(p1, p2).multiplyScalar(0.5);
|
|
const height = p1.distanceTo(p2);
|
|
|
|
// Calculate rotation to align cylinder/capsule with the fromto vector
|
|
const direction = new THREE.Vector3().subVectors(p2, p1).normalize();
|
|
const defaultDir = new THREE.Vector3(0, 1, 0); // Default cylinder axis is Y
|
|
const quaternion = new THREE.Quaternion().setFromUnitVectors(defaultDir, direction);
|
|
const euler = new THREE.Euler().setFromQuaternion(quaternion);
|
|
|
|
// Store fromto data
|
|
geometry.fromto = {
|
|
p1: [ft[0], ft[1], ft[2]],
|
|
p2: [ft[3], ft[4], ft[5]],
|
|
center: [center.x, center.y, center.z],
|
|
height: height,
|
|
rpy: [euler.x, euler.y, euler.z]
|
|
};
|
|
|
|
// Parse radius - for fromto, size is just radius
|
|
const radiusVal = parseFloat(radiusAttr || '0.01');
|
|
geometry.size = { radius: radiusVal, height: height };
|
|
}
|
|
} else if (radiusAttr) {
|
|
const radii = radiusAttr.split(' ').map(parseFloat);
|
|
// MJCF cylinder/capsule size is [radius, half-height], height needs to be multiplied by 2
|
|
geometry.size = {
|
|
radius: radii[0] || 0.1,
|
|
height: (radii[1] || 0.1) * 2 // Multiply by 2 to get full height
|
|
};
|
|
} else {
|
|
geometry.size = { radius: 0.01, height: 0.1 };
|
|
}
|
|
break;
|
|
|
|
case 'mesh':
|
|
let meshRef = geomEl.getAttribute('mesh');
|
|
// If meshMap exists, try to find data corresponding to mesh name
|
|
if (meshMap && meshMap.has(meshRef)) {
|
|
const meshData = meshMap.get(meshRef);
|
|
if (meshData.type === 'file') {
|
|
// External file mesh
|
|
geometry.filename = meshData.path;
|
|
// Apply mesh scale from asset definition (class inheritance)
|
|
if (meshData.scale) {
|
|
geometry.meshScale = meshData.scale;
|
|
}
|
|
} else if (meshData.type === 'vertex') {
|
|
// Inline vertex mesh, store vertex data
|
|
geometry.inlineVertices = meshData.vertices;
|
|
geometry.inlineScale = meshData.scale;
|
|
}
|
|
} else {
|
|
// Otherwise directly use mesh attribute value (may be file path)
|
|
geometry.filename = meshRef;
|
|
if (meshMap && meshMap.size > 0) {
|
|
console.warn(`⚠️ mesh "${meshRef}" not defined in assets`);
|
|
}
|
|
}
|
|
geometry.size = null;
|
|
break;
|
|
}
|
|
|
|
return geometry;
|
|
}
|
|
|
|
/**
|
|
* Parse origin attribute (pos + quat or xyz + rpy)
|
|
*/
|
|
static parseOrigin(element) {
|
|
const origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
|
|
|
|
// Check pos attribute
|
|
const pos = element.getAttribute('pos');
|
|
if (pos) {
|
|
const xyz = pos.split(' ').map(parseFloat);
|
|
origin.xyz = [xyz[0] || 0, xyz[1] || 0, xyz[2] || 0];
|
|
}
|
|
|
|
// Check quat attribute (quaternion, needs to be converted to rpy)
|
|
const quat = element.getAttribute('quat');
|
|
if (quat) {
|
|
const q = quat.split(' ').map(parseFloat);
|
|
// MJCF uses wxyz order
|
|
const qw = q[0], qx = q[1], qy = q[2], qz = q[3];
|
|
|
|
// Save original quaternion (for inertia visualization)
|
|
origin.quat = { w: qw, x: qx, y: qy, z: qz };
|
|
|
|
// Convert to Euler angles
|
|
origin.rpy = this.quaternionToEuler(qw, qx, qy, qz);
|
|
} else {
|
|
// Check euler attribute
|
|
const euler = element.getAttribute('euler');
|
|
if (euler) {
|
|
const rpy = euler.split(' ').map(parseFloat);
|
|
origin.rpy = [rpy[0] || 0, rpy[1] || 0, rpy[2] || 0];
|
|
}
|
|
}
|
|
|
|
return origin;
|
|
}
|
|
|
|
/**
|
|
* Convert quaternion to Euler angles (simplified version)
|
|
*/
|
|
static quaternionToEuler(w, x, y, z) {
|
|
// Normalize quaternion first (MJCF may use non-normalized quaternions)
|
|
const norm = Math.sqrt(w * w + x * x + y * y + z * z);
|
|
if (norm > 0) {
|
|
w = w / norm;
|
|
x = x / norm;
|
|
y = y / norm;
|
|
z = z / norm;
|
|
}
|
|
|
|
// Simplified conversion (using standard formula)
|
|
const sinr_cosp = 2 * (w * x + y * z);
|
|
const cosr_cosp = 1 - 2 * (x * x + y * y);
|
|
const roll = Math.atan2(sinr_cosp, cosr_cosp);
|
|
|
|
const sinp = 2 * (w * y - z * x);
|
|
const pitch = Math.abs(sinp) >= 1
|
|
? Math.sign(sinp) * Math.PI / 2
|
|
: Math.asin(sinp);
|
|
|
|
const siny_cosp = 2 * (w * z + x * y);
|
|
const cosy_cosp = 1 - 2 * (y * y + z * z);
|
|
const yaw = Math.atan2(siny_cosp, cosy_cosp);
|
|
|
|
return [roll, pitch, yaw];
|
|
}
|
|
|
|
/**
|
|
* Parse inertial element
|
|
*
|
|
* MJCF inertia is defined in inertial frame, needs:
|
|
* 1. Transform to body frame via quat rotation
|
|
* 2. Then perform MJCF to Three.js coordinate system conversion
|
|
*/
|
|
static parseInertial(inertialEl) {
|
|
const inertial = new InertialProperties();
|
|
|
|
const mass = inertialEl.getAttribute('mass');
|
|
if (mass) inertial.mass = parseFloat(mass);
|
|
|
|
const origin = this.parseOrigin(inertialEl);
|
|
inertial.origin = origin;
|
|
|
|
// Parse inertia matrix
|
|
const diaginertia = inertialEl.getAttribute('diaginertia');
|
|
const fullinertia = inertialEl.getAttribute('fullinertia');
|
|
|
|
let mjcf_ixx = 0, mjcf_iyy = 0, mjcf_izz = 0;
|
|
let mjcf_ixy = 0, mjcf_ixz = 0, mjcf_iyz = 0;
|
|
|
|
if (diaginertia) {
|
|
const values = diaginertia.split(' ').map(parseFloat);
|
|
mjcf_ixx = values[0] || 0;
|
|
mjcf_iyy = values[1] || 0;
|
|
mjcf_izz = values[2] || 0;
|
|
}
|
|
|
|
if (fullinertia) {
|
|
const values = fullinertia.split(' ').map(parseFloat);
|
|
mjcf_ixx = values[0] || 0;
|
|
mjcf_iyy = values[1] || 0;
|
|
mjcf_izz = values[2] || 0;
|
|
mjcf_ixy = values[3] || 0;
|
|
mjcf_ixz = values[4] || 0;
|
|
mjcf_iyz = values[5] || 0;
|
|
}
|
|
|
|
// Save original diagonal inertia values (for visualization)
|
|
// These are principal moments of inertia in inertial frame
|
|
inertial.diagonalInertia = {
|
|
ixx: mjcf_ixx,
|
|
iyy: mjcf_iyy,
|
|
izz: mjcf_izz
|
|
};
|
|
|
|
// If quat exists, need to rotate inertia tensor
|
|
if (origin.quat) {
|
|
const rotated = this.rotateInertiaTensor(
|
|
mjcf_ixx, mjcf_iyy, mjcf_izz,
|
|
mjcf_ixy, mjcf_ixz, mjcf_iyz,
|
|
origin.quat
|
|
);
|
|
mjcf_ixx = rotated.ixx;
|
|
mjcf_iyy = rotated.iyy;
|
|
mjcf_izz = rotated.izz;
|
|
mjcf_ixy = rotated.ixy;
|
|
mjcf_ixz = rotated.ixz;
|
|
mjcf_iyz = rotated.iyz;
|
|
}
|
|
|
|
// Coordinate system conversion: MJCF -> Three.js
|
|
// On top of quat rotation, need to rotate 180 degrees around Y-axis (split into two 90-degree rotations)
|
|
// This is the correct transformation from MJCF coordinate system (X-forward, Y-left, Z-up) to Three.js coordinate system (X-right, Y-up, Z-forward)
|
|
const coordRotated1 = this.rotateInertiaAroundAxis(
|
|
mjcf_ixx, mjcf_iyy, mjcf_izz,
|
|
mjcf_ixy, mjcf_ixz, mjcf_iyz,
|
|
'Y', 90
|
|
);
|
|
|
|
const coordRotated2 = this.rotateInertiaAroundAxis(
|
|
coordRotated1.ixx, coordRotated1.iyy, coordRotated1.izz,
|
|
coordRotated1.ixy, coordRotated1.ixz, coordRotated1.iyz,
|
|
'Y', 90
|
|
);
|
|
|
|
inertial.ixx = coordRotated2.ixx;
|
|
inertial.iyy = coordRotated2.iyy;
|
|
inertial.izz = coordRotated2.izz;
|
|
inertial.ixy = coordRotated2.ixy;
|
|
inertial.ixz = coordRotated2.ixz;
|
|
inertial.iyz = coordRotated2.iyz;
|
|
|
|
return inertial;
|
|
}
|
|
|
|
/**
|
|
* Rotate inertia tensor around specified axis
|
|
* @param {string} axis - 'X', 'Y', or 'Z'
|
|
* @param {number} degrees - Rotation angle (degrees)
|
|
*/
|
|
static rotateInertiaAroundAxis(ixx, iyy, izz, ixy, ixz, iyz, axis, degrees) {
|
|
const rad = degrees * Math.PI / 180;
|
|
const c = Math.cos(rad);
|
|
const s = Math.sin(rad);
|
|
|
|
let R;
|
|
if (axis === 'X') {
|
|
R = [
|
|
[1, 0, 0],
|
|
[0, c, -s],
|
|
[0, s, c]
|
|
];
|
|
} else if (axis === 'Y') {
|
|
R = [
|
|
[c, 0, s],
|
|
[0, 1, 0],
|
|
[-s, 0, c]
|
|
];
|
|
} else if (axis === 'Z') {
|
|
R = [
|
|
[c, -s, 0],
|
|
[s, c, 0],
|
|
[0, 0, 1]
|
|
];
|
|
}
|
|
|
|
// Inertia matrix
|
|
const I = [
|
|
[ixx, ixy, ixz],
|
|
[ixy, iyy, iyz],
|
|
[ixz, iyz, izz]
|
|
];
|
|
|
|
// Calculate R * I
|
|
const RI = [
|
|
[0, 0, 0],
|
|
[0, 0, 0],
|
|
[0, 0, 0]
|
|
];
|
|
|
|
for (let i = 0; i < 3; i++) {
|
|
for (let j = 0; j < 3; j++) {
|
|
for (let k = 0; k < 3; k++) {
|
|
RI[i][j] += R[i][k] * I[k][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
// Calculate (R * I) * R^T
|
|
const result = [
|
|
[0, 0, 0],
|
|
[0, 0, 0],
|
|
[0, 0, 0]
|
|
];
|
|
|
|
for (let i = 0; i < 3; i++) {
|
|
for (let j = 0; j < 3; j++) {
|
|
for (let k = 0; k < 3; k++) {
|
|
result[i][j] += RI[i][k] * R[j][k]; // R^T[k][j] = R[j][k]
|
|
}
|
|
}
|
|
}
|
|
|
|
return {
|
|
ixx: result[0][0],
|
|
iyy: result[1][1],
|
|
izz: result[2][2],
|
|
ixy: result[0][1],
|
|
ixz: result[0][2],
|
|
iyz: result[1][2]
|
|
};
|
|
}
|
|
|
|
/**
|
|
* Rotate inertia tensor: I_rotated = R * I * R^T
|
|
*/
|
|
static rotateInertiaTensor(ixx, iyy, izz, ixy, ixz, iyz, quat) {
|
|
const {w, x, y, z} = quat;
|
|
|
|
// Build rotation matrix R (from quaternion)
|
|
const r11 = 1 - 2*(y*y + z*z);
|
|
const r12 = 2*(x*y - w*z);
|
|
const r13 = 2*(x*z + w*y);
|
|
const r21 = 2*(x*y + w*z);
|
|
const r22 = 1 - 2*(x*x + z*z);
|
|
const r23 = 2*(y*z - w*x);
|
|
const r31 = 2*(x*z - w*y);
|
|
const r32 = 2*(y*z + w*x);
|
|
const r33 = 1 - 2*(x*x + y*y);
|
|
|
|
// Inertia matrix
|
|
const I = [
|
|
[ixx, ixy, ixz],
|
|
[ixy, iyy, iyz],
|
|
[ixz, iyz, izz]
|
|
];
|
|
|
|
// Calculate R * I
|
|
const RI = [
|
|
[0, 0, 0],
|
|
[0, 0, 0],
|
|
[0, 0, 0]
|
|
];
|
|
|
|
RI[0][0] = r11*I[0][0] + r12*I[1][0] + r13*I[2][0];
|
|
RI[0][1] = r11*I[0][1] + r12*I[1][1] + r13*I[2][1];
|
|
RI[0][2] = r11*I[0][2] + r12*I[1][2] + r13*I[2][2];
|
|
|
|
RI[1][0] = r21*I[0][0] + r22*I[1][0] + r23*I[2][0];
|
|
RI[1][1] = r21*I[0][1] + r22*I[1][1] + r23*I[2][1];
|
|
RI[1][2] = r21*I[0][2] + r22*I[1][2] + r23*I[2][2];
|
|
|
|
RI[2][0] = r31*I[0][0] + r32*I[1][0] + r33*I[2][0];
|
|
RI[2][1] = r31*I[0][1] + r32*I[1][1] + r33*I[2][1];
|
|
RI[2][2] = r31*I[0][2] + r32*I[1][2] + r33*I[2][2];
|
|
|
|
// Calculate (R * I) * R^T
|
|
const result = {
|
|
ixx: RI[0][0]*r11 + RI[0][1]*r12 + RI[0][2]*r13,
|
|
iyy: RI[1][0]*r21 + RI[1][1]*r22 + RI[1][2]*r23,
|
|
izz: RI[2][0]*r31 + RI[2][1]*r32 + RI[2][2]*r33,
|
|
ixy: RI[0][0]*r21 + RI[0][1]*r22 + RI[0][2]*r23,
|
|
ixz: RI[0][0]*r31 + RI[0][1]*r32 + RI[0][2]*r33,
|
|
iyz: RI[1][0]*r31 + RI[1][1]*r32 + RI[1][2]*r33
|
|
};
|
|
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* Parse joint element
|
|
*/
|
|
static parseJoints(element, bodyMap, model, parentBodyName = null, defaultsMap = null) {
|
|
const joints = element.querySelectorAll(':scope > joint');
|
|
|
|
joints.forEach(jointEl => {
|
|
const jointName = jointEl.getAttribute('name') || `joint_${model.joints.size}`;
|
|
const jointType = jointEl.getAttribute('type') || 'hinge';
|
|
|
|
// Map MJCF joint types to URDF types
|
|
let urdfType = 'revolute';
|
|
if (jointType === 'slide') urdfType = 'prismatic';
|
|
else if (jointType === 'free') urdfType = 'continuous';
|
|
else if (jointType === 'ball' || jointType === 'hinge') urdfType = 'revolute';
|
|
|
|
const joint = new Joint(jointName, urdfType);
|
|
|
|
// Joint types that don't require axis attribute
|
|
const jointTypesWithoutAxis = ['free', 'ball'];
|
|
const requiresAxis = !jointTypesWithoutAxis.includes(jointType);
|
|
|
|
// [Critical fix] In MJCF, joint is defined inside body, representing the connection relationship between this body and its parent body
|
|
// So: parent is parent body, child is current body
|
|
const currentBody = jointEl.parentElement;
|
|
const currentBodyName = currentBody.getAttribute('name');
|
|
|
|
// parent is the passed parent body name (or worldbody)
|
|
if (parentBodyName) {
|
|
joint.parent = parentBodyName;
|
|
} else {
|
|
// If no parent body, parent is worldbody
|
|
joint.parent = 'worldbody';
|
|
}
|
|
|
|
// child is current body
|
|
if (currentBodyName) {
|
|
joint.child = currentBodyName;
|
|
}
|
|
|
|
|
|
// [Important] Parse axis, consider class inheritance
|
|
let axisVals = null;
|
|
let axisSource = '';
|
|
|
|
// First try to get axis from joint element itself
|
|
const axis = jointEl.getAttribute('axis');
|
|
if (axis) {
|
|
axisVals = axis.split(' ').map(parseFloat);
|
|
axisSource = 'directly defined';
|
|
} else {
|
|
// If not, inherit from class or childclass
|
|
let className = jointEl.getAttribute('class');
|
|
|
|
// If joint has no class, check parent body's childclass
|
|
if (!className) {
|
|
className = currentBody.getAttribute('childclass');
|
|
}
|
|
|
|
if (className && defaultsMap) {
|
|
const defaults = defaultsMap.get(className);
|
|
if (defaults && defaults.joint && defaults.joint.axis) {
|
|
axisVals = defaults.joint.axis;
|
|
axisSource = `inherited from class="${className}"`;
|
|
}
|
|
}
|
|
|
|
// Only warn if axis is required for this joint type
|
|
if (!axisVals && requiresAxis) {
|
|
console.warn(` ⚠️ Joint "${jointName}" (type="${jointType}") has no axis attribute (class="${className || 'none'}")`);
|
|
}
|
|
}
|
|
|
|
// Set axis
|
|
if (axisVals) {
|
|
joint.axis = { xyz: [axisVals[0] || 0, axisVals[1] || 0, axisVals[2] || 0] };
|
|
}
|
|
|
|
// [Important] Parse limits, consider class inheritance
|
|
let rangeVals = null;
|
|
|
|
// First try to get range from joint element itself
|
|
const range = jointEl.getAttribute('range');
|
|
if (range) {
|
|
rangeVals = range.split(' ').map(parseFloat);
|
|
} else {
|
|
// If not, inherit from class or childclass
|
|
let className = jointEl.getAttribute('class');
|
|
|
|
// If joint has no class, check parent body's childclass
|
|
if (!className) {
|
|
className = currentBody.getAttribute('childclass');
|
|
}
|
|
|
|
if (className && defaultsMap) {
|
|
const defaults = defaultsMap.get(className);
|
|
if (defaults && defaults.joint && defaults.joint.range) {
|
|
rangeVals = defaults.joint.range;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Set limits
|
|
if (rangeVals && rangeVals.length >= 2) {
|
|
const limits = new JointLimits();
|
|
limits.lower = rangeVals[0];
|
|
limits.upper = rangeVals[1];
|
|
joint.limits = limits;
|
|
}
|
|
// If no range definition, joint.limits remains null (indicating unlimited/continuous)
|
|
|
|
// Parse joint's own origin (if any)
|
|
// joint's pos defines the offset of joint in this body's coordinate system
|
|
joint.origin = this.parseOrigin(jointEl);
|
|
|
|
model.addJoint(joint);
|
|
});
|
|
|
|
// Process freejoint elements (free-floating joints)
|
|
const freejoints = element.querySelectorAll(':scope > freejoint');
|
|
freejoints.forEach((freejointEl, index) => {
|
|
const freejointName = freejointEl.getAttribute('name') || `freejoint_${model.joints.size}`;
|
|
|
|
// Create a 'free' type joint (maps to continuous/floating in URDF terms)
|
|
const joint = new Joint(freejointName, 'continuous');
|
|
joint.type = 'free'; // Mark as free joint type
|
|
|
|
// Get parent body
|
|
const currentBody = freejointEl.parentElement;
|
|
const currentBodyName = currentBody.getAttribute('name');
|
|
|
|
// Parent is worldbody for freejoints
|
|
if (parentBodyName) {
|
|
joint.parent = parentBodyName;
|
|
} else {
|
|
joint.parent = 'worldbody';
|
|
}
|
|
|
|
// Child is current body
|
|
if (currentBodyName) {
|
|
joint.child = currentBodyName;
|
|
}
|
|
|
|
// Parse origin
|
|
joint.origin = this.parseOrigin(freejointEl);
|
|
|
|
model.addJoint(joint);
|
|
});
|
|
|
|
// Recursively process child bodies
|
|
// Find direct child bodies (use :scope > body to ensure only direct children are selected)
|
|
const bodies = element.querySelectorAll(':scope > body');
|
|
const currentElementName = element.getAttribute('name'); // Name of current body or worldbody
|
|
|
|
bodies.forEach(body => {
|
|
// Child body's parent body name is current element's name
|
|
// Note: worldbody has no name attribute, so first level body's parent is null or 'worldbody'
|
|
this.parseJoints(body, bodyMap, model, currentElementName || 'worldbody', defaultsMap);
|
|
});
|
|
}
|
|
|
|
/**
|
|
* Parse equality constraints (closed-chain constraints for parallel mechanisms)
|
|
*/
|
|
static parseEquality(doc, model) {
|
|
const equality = doc.querySelector('equality');
|
|
if (!equality) {
|
|
return; // No equality tag, skip
|
|
}
|
|
|
|
// Parse connect constraints (connect two bodies)
|
|
const connects = equality.querySelectorAll('connect');
|
|
connects.forEach((connectEl, index) => {
|
|
const name = connectEl.getAttribute('name') || `connect_${index}`;
|
|
const constraint = new Constraint(name, 'connect');
|
|
|
|
constraint.body1 = connectEl.getAttribute('body1');
|
|
constraint.body2 = connectEl.getAttribute('body2');
|
|
|
|
const anchor = connectEl.getAttribute('anchor');
|
|
if (anchor) {
|
|
constraint.anchor = anchor.trim().split(/\s+/).map(parseFloat);
|
|
}
|
|
|
|
const torquescale = connectEl.getAttribute('torquescale');
|
|
if (torquescale) {
|
|
constraint.torquescale = parseFloat(torquescale);
|
|
}
|
|
|
|
constraint.userData = {
|
|
body1: constraint.body1,
|
|
body2: constraint.body2,
|
|
anchor: constraint.anchor
|
|
};
|
|
|
|
model.addConstraint(constraint);
|
|
});
|
|
|
|
// Parse weld constraints (weld two bodies)
|
|
const welds = equality.querySelectorAll('weld');
|
|
welds.forEach((weldEl, index) => {
|
|
const name = weldEl.getAttribute('name') || `weld_${index}`;
|
|
const constraint = new Constraint(name, 'weld');
|
|
|
|
constraint.body1 = weldEl.getAttribute('body1');
|
|
constraint.body2 = weldEl.getAttribute('body2');
|
|
|
|
const anchor = weldEl.getAttribute('anchor');
|
|
if (anchor) {
|
|
constraint.anchor = anchor.trim().split(/\s+/).map(parseFloat);
|
|
}
|
|
|
|
const torquescale = weldEl.getAttribute('torquescale');
|
|
if (torquescale) {
|
|
constraint.torquescale = parseFloat(torquescale);
|
|
}
|
|
|
|
constraint.userData = {
|
|
body1: constraint.body1,
|
|
body2: constraint.body2,
|
|
anchor: constraint.anchor
|
|
};
|
|
|
|
model.addConstraint(constraint);
|
|
});
|
|
|
|
// Parse joint constraints (joint coupling)
|
|
const joints = equality.querySelectorAll('joint');
|
|
joints.forEach((jointEl, index) => {
|
|
const name = jointEl.getAttribute('name') || `joint_constraint_${index}`;
|
|
const constraint = new Constraint(name, 'joint');
|
|
|
|
constraint.joint1 = jointEl.getAttribute('joint1');
|
|
constraint.joint2 = jointEl.getAttribute('joint2');
|
|
|
|
const polycoef = jointEl.getAttribute('polycoef');
|
|
if (polycoef) {
|
|
constraint.polycoef = polycoef.trim().split(/\s+/).map(parseFloat);
|
|
} else {
|
|
constraint.polycoef = [0, 1]; // Default 1:1
|
|
}
|
|
|
|
constraint.userData = {
|
|
joint1: constraint.joint1,
|
|
joint2: constraint.joint2,
|
|
polycoef: constraint.polycoef
|
|
};
|
|
|
|
model.addConstraint(constraint);
|
|
});
|
|
|
|
// Parse distance constraints
|
|
const distances = equality.querySelectorAll('distance');
|
|
distances.forEach((distanceEl, index) => {
|
|
const name = distanceEl.getAttribute('name') || `distance_${index}`;
|
|
const constraint = new Constraint(name, 'distance');
|
|
|
|
constraint.body1 = distanceEl.getAttribute('body1');
|
|
constraint.body2 = distanceEl.getAttribute('body2');
|
|
|
|
constraint.userData = {
|
|
body1: constraint.body1,
|
|
body2: constraint.body2
|
|
};
|
|
|
|
model.addConstraint(constraint);
|
|
});
|
|
}
|
|
|
|
/**
|
|
* Create Three.js objects (recursively build hierarchy)
|
|
* @param {UnifiedRobotModel} model
|
|
* @param {Map} fileMap - File map for loading mesh files
|
|
* @param {Map} meshMap - Mesh name to file path mapping (optional)
|
|
*/
|
|
static async createThreeObject(model, fileMap = null, meshMap = null) {
|
|
// Preload loaders
|
|
await getLoaders();
|
|
|
|
const rootGroup = new THREE.Group();
|
|
rootGroup.name = model.name;
|
|
|
|
// Create Three.js objects for all links (but don't add to scene yet)
|
|
const linkObjects = new Map();
|
|
|
|
// Collect all unique mesh file paths (only need visual, as MJCF doesn't create collision separately)
|
|
const uniqueMeshFiles = new Set();
|
|
for (const [name, link] of model.links) {
|
|
for (const visual of link.visuals) {
|
|
if (visual.geometry.type === 'mesh' && visual.geometry.filename) {
|
|
uniqueMeshFiles.add(visual.geometry.filename);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Load all unique mesh files in parallel
|
|
const meshPromises = Array.from(uniqueMeshFiles).map(filename =>
|
|
this.loadMeshFile(filename, fileMap).catch(err => {
|
|
console.error(`Failed to load mesh: ${filename}`, err);
|
|
return null;
|
|
})
|
|
);
|
|
|
|
// Wait for all mesh loading to complete
|
|
const meshResults = await Promise.all(meshPromises);
|
|
const meshCache = new Map();
|
|
|
|
// Build mesh cache (filename -> geometry)
|
|
let index = 0;
|
|
for (const filename of uniqueMeshFiles) {
|
|
const result = meshResults[index++];
|
|
meshCache.set(filename, result);
|
|
}
|
|
|
|
// Create link groups
|
|
let totalVisuals = 0;
|
|
for (const [name, link] of model.links) {
|
|
const linkGroup = new THREE.Group();
|
|
linkGroup.name = name;
|
|
linkGroup.isURDFLink = true; // Mark as link for JointDragControls recognition
|
|
linkGroup.type = 'URDFLink'; // Set type
|
|
|
|
// [Critical] Do not apply body.pos on linkGroup!
|
|
// body.pos should be applied on the jointGroup that connects it
|
|
// linkGroup only needs to contain geometry, position is determined by jointGroup
|
|
|
|
let linkVisualCount = 0;
|
|
let linkCollisionCount = 0;
|
|
|
|
// Create visual geometry
|
|
for (const visual of link.visuals) {
|
|
const mesh = await this.createGeometryMesh(visual.geometry, fileMap, meshCache);
|
|
if (mesh) {
|
|
// Apply origin transformation
|
|
// Check if this geom has fromto data (for capsule/cylinder)
|
|
if (visual.geometry && visual.geometry.fromto) {
|
|
// Use fromto center position
|
|
mesh.position.set(...visual.geometry.fromto.center);
|
|
// Apply fromto rotation plus any explicit rotation
|
|
const fromtoRpy = visual.geometry.fromto.rpy;
|
|
mesh.rotation.set(
|
|
fromtoRpy[0] + visual.origin.rpy[0],
|
|
fromtoRpy[1] + visual.origin.rpy[1],
|
|
fromtoRpy[2] + visual.origin.rpy[2]
|
|
);
|
|
} else {
|
|
mesh.position.set(...visual.origin.xyz);
|
|
mesh.rotation.set(...visual.origin.rpy);
|
|
}
|
|
mesh.name = visual.name || 'visual';
|
|
|
|
// If MJCF defines rgba color, apply to mesh
|
|
if (visual.userData && visual.userData.rgba) {
|
|
const rgba = visual.userData.rgba;
|
|
const color = new THREE.Color(rgba.r, rgba.g, rgba.b);
|
|
|
|
mesh.traverse((child) => {
|
|
if (child.isMesh && child.material) {
|
|
// Handle material arrays and single materials
|
|
if (Array.isArray(child.material)) {
|
|
child.material = child.material.map(mat => {
|
|
const clonedMat = mat.clone();
|
|
clonedMat.color = color;
|
|
if (rgba.a < 1.0) {
|
|
clonedMat.transparent = true;
|
|
clonedMat.opacity = rgba.a;
|
|
}
|
|
// Save original properties before enhancing (for lighting toggle)
|
|
if (clonedMat.isMeshPhongMaterial || clonedMat.isMeshStandardMaterial) {
|
|
if (clonedMat.userData.originalShininess === undefined) {
|
|
clonedMat.userData.originalShininess = clonedMat.shininess !== undefined ? clonedMat.shininess : 30;
|
|
// Save original specular - if material had no specular, save null
|
|
if (!clonedMat.specular) {
|
|
clonedMat.userData.originalSpecular = null;
|
|
} else if (clonedMat.specular.isColor) {
|
|
const spec = clonedMat.specular;
|
|
if (spec.r < 0.1 && spec.g < 0.1 && spec.b < 0.1) {
|
|
clonedMat.userData.originalSpecular = null; // Likely default
|
|
} else {
|
|
clonedMat.userData.originalSpecular = spec.clone();
|
|
}
|
|
} else if (typeof clonedMat.specular === 'number') {
|
|
if (clonedMat.specular === 0x111111 || clonedMat.specular < 0x111111) {
|
|
clonedMat.userData.originalSpecular = null;
|
|
} else {
|
|
clonedMat.userData.originalSpecular = new THREE.Color(clonedMat.specular);
|
|
}
|
|
} else {
|
|
clonedMat.userData.originalSpecular = null;
|
|
}
|
|
}
|
|
// Enhance material for better lighting (MuJoCo style) - default enabled
|
|
if (clonedMat.shininess === undefined || clonedMat.shininess < 50) {
|
|
clonedMat.shininess = 50;
|
|
}
|
|
if (!clonedMat.specular ||
|
|
(clonedMat.specular.isColor && clonedMat.specular.r < 0.2) ||
|
|
(typeof clonedMat.specular === 'number' && clonedMat.specular < 0x333333)) {
|
|
clonedMat.specular = new THREE.Color(0.3, 0.3, 0.3);
|
|
}
|
|
}
|
|
return clonedMat;
|
|
});
|
|
} else {
|
|
// Clone material to avoid affecting other instances
|
|
child.material = child.material.clone();
|
|
child.material.color = color;
|
|
if (rgba.a < 1.0) {
|
|
child.material.transparent = true;
|
|
child.material.opacity = rgba.a;
|
|
}
|
|
// Save original properties before enhancing (for lighting toggle)
|
|
if (child.material.isMeshPhongMaterial || child.material.isMeshStandardMaterial) {
|
|
if (child.material.userData.originalShininess === undefined) {
|
|
child.material.userData.originalShininess = child.material.shininess !== undefined ? child.material.shininess : 30;
|
|
// Save original specular - if material had no specular, save null
|
|
if (!child.material.specular) {
|
|
child.material.userData.originalSpecular = null;
|
|
} else if (child.material.specular.isColor) {
|
|
const spec = child.material.specular;
|
|
if (spec.r < 0.1 && spec.g < 0.1 && spec.b < 0.1) {
|
|
child.material.userData.originalSpecular = null; // Likely default
|
|
} else {
|
|
child.material.userData.originalSpecular = spec.clone();
|
|
}
|
|
} else if (typeof child.material.specular === 'number') {
|
|
if (child.material.specular === 0x111111 || child.material.specular < 0x111111) {
|
|
child.material.userData.originalSpecular = null;
|
|
} else {
|
|
child.material.userData.originalSpecular = new THREE.Color(child.material.specular);
|
|
}
|
|
} else {
|
|
child.material.userData.originalSpecular = null;
|
|
}
|
|
}
|
|
// Enhance material for better lighting (MuJoCo style) - default enabled
|
|
if (child.material.shininess === undefined || child.material.shininess < 50) {
|
|
child.material.shininess = 50;
|
|
}
|
|
if (!child.material.specular ||
|
|
(child.material.specular.isColor && child.material.specular.r < 0.2) ||
|
|
(typeof child.material.specular === 'number' && child.material.specular < 0x333333)) {
|
|
child.material.specular = new THREE.Color(0.3, 0.3, 0.3);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
});
|
|
}
|
|
|
|
linkGroup.add(mesh);
|
|
visual.threeObject = mesh;
|
|
totalVisuals++;
|
|
linkVisualCount++;
|
|
}
|
|
}
|
|
|
|
// Create collision geometry
|
|
for (const collision of link.collisions) {
|
|
const mesh = await this.createGeometryMesh(collision.geometry, fileMap, meshCache);
|
|
if (mesh) {
|
|
// Apply origin transformation
|
|
// Check if this geom has fromto data (for capsule/cylinder)
|
|
if (collision.geometry && collision.geometry.fromto) {
|
|
// Use fromto center position
|
|
mesh.position.set(...collision.geometry.fromto.center);
|
|
// Apply fromto rotation plus any explicit rotation
|
|
const fromtoRpy = collision.geometry.fromto.rpy;
|
|
mesh.rotation.set(
|
|
fromtoRpy[0] + collision.origin.rpy[0],
|
|
fromtoRpy[1] + collision.origin.rpy[1],
|
|
fromtoRpy[2] + collision.origin.rpy[2]
|
|
);
|
|
} else {
|
|
mesh.position.set(...collision.origin.xyz);
|
|
mesh.rotation.set(...collision.origin.rpy);
|
|
}
|
|
mesh.name = collision.name || 'collision';
|
|
|
|
// Create collision body container (similar to URDF handling)
|
|
const colliderGroup = new THREE.Group();
|
|
colliderGroup.name = `${name}_collider_${linkCollisionCount}`;
|
|
colliderGroup.isURDFCollider = true; // Mark as collision body
|
|
colliderGroup.add(mesh);
|
|
|
|
linkGroup.add(colliderGroup);
|
|
collision.threeObject = colliderGroup;
|
|
linkCollisionCount++;
|
|
}
|
|
}
|
|
|
|
link.threeObject = linkGroup;
|
|
linkObjects.set(name, linkGroup);
|
|
}
|
|
|
|
|
|
// Build hierarchy based on body parent-child relationships (MJCF bodies are nested)
|
|
const bodyMap = new Map();
|
|
for (const [name, link] of model.links) {
|
|
bodyMap.set(name, { link, parentName: link.userData.parentName });
|
|
}
|
|
|
|
// Find root body (body without parent)
|
|
const rootLinks = Array.from(model.links.keys()).filter(
|
|
name => !bodyMap.get(name).parentName
|
|
);
|
|
|
|
// Recursively build hierarchy
|
|
function buildHierarchy(linkName, parentGroup) {
|
|
const linkGroup = linkObjects.get(linkName);
|
|
if (!linkGroup) return;
|
|
|
|
// Add current link to parent group
|
|
parentGroup.add(linkGroup);
|
|
|
|
// Find all joints with this link as parent
|
|
const childJoints = Array.from(model.joints.values()).filter(
|
|
j => j.parent === linkName && j.child
|
|
);
|
|
|
|
// Process child joints and child bodies
|
|
childJoints.forEach(joint => {
|
|
const childLinkName = joint.child;
|
|
if (!childLinkName) return;
|
|
|
|
// Get child link's body origin (in MJCF, body.pos defines connection position)
|
|
const childLink = model.links.get(childLinkName);
|
|
const bodyOrigin = childLink.userData.bodyOrigin || { xyz: [0, 0, 0], rpy: [0, 0, 0] };
|
|
|
|
// Create joint transformation group
|
|
const jointGroup = new THREE.Group();
|
|
jointGroup.name = joint.name || `joint_${childLinkName}`;
|
|
jointGroup.isURDFJoint = true; // Mark as joint for JointDragControls recognition
|
|
jointGroup.type = 'URDFJoint'; // Set type
|
|
jointGroup.jointType = joint.type; // Set joint type
|
|
|
|
// Store joint axis information (for JointDragControls use)
|
|
if (joint.axis && joint.axis.xyz) {
|
|
const mjcfAxis = joint.axis.xyz;
|
|
jointGroup.axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[1], mjcfAxis[2]).normalize();
|
|
} else {
|
|
// If no axis defined, use default value (0, 1, 0)
|
|
jointGroup.axis = new THREE.Vector3(0, 1, 0);
|
|
}
|
|
|
|
// [Critical] Apply body.pos + joint.pos as jointGroup position
|
|
// body.pos defines body position relative to parent body (i.e., connection position)
|
|
// joint.pos defines joint offset in body coordinate system (usually 0)
|
|
jointGroup.position.set(
|
|
bodyOrigin.xyz[0] + joint.origin.xyz[0],
|
|
bodyOrigin.xyz[1] + joint.origin.xyz[1],
|
|
bodyOrigin.xyz[2] + joint.origin.xyz[2]
|
|
);
|
|
jointGroup.rotation.set(...bodyOrigin.rpy);
|
|
|
|
// Recursively build child link
|
|
buildHierarchy(childLinkName, jointGroup);
|
|
|
|
linkGroup.add(jointGroup);
|
|
joint.threeObject = jointGroup;
|
|
});
|
|
|
|
// Process direct child bodies (find via bodyMap)
|
|
for (const [childName, bodyData] of bodyMap.entries()) {
|
|
if (bodyData.parentName === linkName) {
|
|
// Check if joint connection already exists
|
|
const hasJoint = Array.from(model.joints.values()).some(
|
|
j => j.parent === linkName && j.child === childName
|
|
);
|
|
if (!hasJoint) {
|
|
// If no joint, create fixed connection group to apply body position and rotation
|
|
const childLink = model.links.get(childName);
|
|
const childBodyOrigin = childLink.userData.bodyOrigin || { xyz: [0, 0, 0], rpy: [0, 0, 0] };
|
|
|
|
// Mark this as fixed-connected child body (for structure graph display)
|
|
childLink.userData.isFixedConnection = true;
|
|
|
|
// Create fixed connection group
|
|
const fixedGroup = new THREE.Group();
|
|
fixedGroup.position.set(...childBodyOrigin.xyz);
|
|
fixedGroup.rotation.set(...childBodyOrigin.rpy);
|
|
|
|
// Recursively build child body and add to fixed group
|
|
buildHierarchy(childName, fixedGroup);
|
|
|
|
linkGroup.add(fixedGroup);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Start building from root link
|
|
if (rootLinks.length > 0) {
|
|
rootLinks.forEach(rootName => {
|
|
// Root link needs to apply its own body.pos (because it has no parent joint)
|
|
const rootLink = model.links.get(rootName);
|
|
const rootLinkGroup = linkObjects.get(rootName);
|
|
if (rootLink.userData.bodyOrigin) {
|
|
rootLinkGroup.position.set(...rootLink.userData.bodyOrigin.xyz);
|
|
rootLinkGroup.rotation.set(...rootLink.userData.bodyOrigin.rpy);
|
|
}
|
|
buildHierarchy(rootName, rootGroup);
|
|
});
|
|
} else if (model.links.size > 0) {
|
|
// If no root link found, use first link
|
|
const firstLink = Array.from(model.links.keys())[0];
|
|
const firstLinkObj = model.links.get(firstLink);
|
|
const firstLinkGroup = linkObjects.get(firstLink);
|
|
if (firstLinkObj.userData.bodyOrigin) {
|
|
firstLinkGroup.position.set(...firstLinkObj.userData.bodyOrigin.xyz);
|
|
firstLinkGroup.rotation.set(...firstLinkObj.userData.bodyOrigin.rpy);
|
|
}
|
|
buildHierarchy(firstLink, rootGroup);
|
|
}
|
|
|
|
model.threeObject = rootGroup;
|
|
|
|
// Mark model type as MJCF (also set on model)
|
|
if (!rootGroup.userData) rootGroup.userData = {};
|
|
rootGroup.userData.type = 'mjcf';
|
|
|
|
if (!model.userData) model.userData = {};
|
|
model.userData.type = 'mjcf';
|
|
}
|
|
|
|
/**
|
|
* Create Three.js Mesh based on geometry type
|
|
* @param {GeometryType} geometry
|
|
* @param {Map} fileMap - File map for loading mesh files
|
|
* @param {Map} meshCache - Cache of loaded meshes (optional)
|
|
* @returns {Promise<THREE.Mesh|null>}
|
|
*/
|
|
static async createGeometryMesh(geometry, fileMap = null, meshCache = null) {
|
|
let threeGeometry = null;
|
|
|
|
switch (geometry.type) {
|
|
case 'box':
|
|
if (geometry.size) {
|
|
threeGeometry = new THREE.BoxGeometry(
|
|
geometry.size.x,
|
|
geometry.size.y,
|
|
geometry.size.z
|
|
);
|
|
}
|
|
break;
|
|
|
|
case 'sphere':
|
|
if (geometry.size && geometry.size.radius) {
|
|
threeGeometry = new THREE.SphereGeometry(geometry.size.radius, 32, 32);
|
|
}
|
|
break;
|
|
|
|
case 'cylinder':
|
|
if (geometry.size) {
|
|
// Three.js CylinderGeometry defaults to Y-axis
|
|
threeGeometry = new THREE.CylinderGeometry(
|
|
geometry.size.radius,
|
|
geometry.size.radius,
|
|
geometry.size.height,
|
|
32
|
|
);
|
|
// MJCF cylinder defaults to Z-axis, Three.js Cylinder is Y-axis aligned
|
|
// Rotate to align with Z-axis
|
|
threeGeometry.rotateX(Math.PI / 2);
|
|
|
|
// If fromto is defined, the mesh will be positioned and rotated by fromto data
|
|
// in the calling code
|
|
}
|
|
break;
|
|
|
|
case 'capsule':
|
|
if (geometry.size) {
|
|
// Three.js doesn't have native CapsuleGeometry in older versions
|
|
// Use a combination of cylinder and spheres, or CapsuleGeometry if available
|
|
const { radius, height } = geometry.size;
|
|
|
|
// Check if CapsuleGeometry is available (Three.js r133+)
|
|
if (typeof THREE.CapsuleGeometry !== 'undefined') {
|
|
threeGeometry = new THREE.CapsuleGeometry(radius, height, 4, 16);
|
|
// CapsuleGeometry is Y-axis aligned, MJCF capsule is Z-axis aligned
|
|
threeGeometry.rotateX(Math.PI / 2);
|
|
} else {
|
|
// Fallback: create a cylinder with sphere caps
|
|
const cylinderHeight = Math.max(0, height - 2 * radius);
|
|
const cylinder = new THREE.CylinderGeometry(radius, radius, cylinderHeight, 16);
|
|
cylinder.rotateX(Math.PI / 2); // Align with Z-axis
|
|
threeGeometry = cylinder;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case 'mesh':
|
|
// Load mesh file
|
|
if (geometry.filename) {
|
|
let cachedMesh = null;
|
|
|
|
// If already cached, get it
|
|
if (meshCache && meshCache.has(geometry.filename)) {
|
|
cachedMesh = meshCache.get(geometry.filename);
|
|
} else if (fileMap) {
|
|
cachedMesh = await this.loadMeshFile(geometry.filename, fileMap);
|
|
}
|
|
|
|
if (!cachedMesh) {
|
|
console.error(`❌ Cannot load mesh file: ${geometry.filename}`);
|
|
return null;
|
|
}
|
|
|
|
// loadMeshFile may return Group/Scene (OBJ/DAE/GLTF) or BufferGeometry (STL)
|
|
// If Group/Scene, need to clone (because Three.js objects can only have one parent)
|
|
if (cachedMesh.isGroup || cachedMesh.isObject3D) {
|
|
threeGeometry = cachedMesh.clone(true); // Deep clone (including materials)
|
|
|
|
// Apply mesh scale from MJCF class inheritance (e.g., scale="0.001 0.001 0.001")
|
|
if (geometry.meshScale) {
|
|
const [sx, sy, sz] = geometry.meshScale;
|
|
threeGeometry.scale.set(sx, sy, sz);
|
|
}
|
|
|
|
// Check cloned mesh material situation
|
|
let meshCount = 0;
|
|
let materialCount = 0;
|
|
threeGeometry.traverse((child) => {
|
|
if (child.isMesh) {
|
|
meshCount++;
|
|
if (child.material) {
|
|
materialCount++;
|
|
}
|
|
}
|
|
});
|
|
|
|
// Ensure mesh uses lighting-compatible material
|
|
ensureMeshHasPhongMaterial(threeGeometry);
|
|
return threeGeometry;
|
|
}
|
|
// If BufferGeometry (e.g., STL), create a mesh and apply scale
|
|
if (geometry.meshScale) {
|
|
const [sx, sy, sz] = geometry.meshScale;
|
|
// Scale the geometry directly
|
|
threeGeometry = cachedMesh.clone();
|
|
threeGeometry.scale(sx, sy, sz);
|
|
} else {
|
|
threeGeometry = cachedMesh;
|
|
}
|
|
} else {
|
|
console.warn('⚠️ Mesh type geometry missing filename');
|
|
return null;
|
|
}
|
|
break;
|
|
}
|
|
|
|
if (!threeGeometry) return null;
|
|
|
|
// Create default material for BufferGeometry (basic geometries: box, sphere, cylinder, stl, etc.)
|
|
// Enhanced for better lighting (MuJoCo style) with reflections
|
|
const envMap = typeof window !== 'undefined' && window.app?.sceneManager?.environmentManager?.getEnvironmentMap();
|
|
const material = new THREE.MeshPhongMaterial({
|
|
color: 0xf0f0f0, // Near white
|
|
shininess: 50, // Increased for better highlights
|
|
specular: new THREE.Color(0.3, 0.3, 0.3), // Enhanced specular reflection
|
|
envMap: envMap || null,
|
|
reflectivity: envMap ? 0.3 : 0
|
|
});
|
|
// Save original properties for lighting toggle
|
|
material.userData.originalShininess = 30;
|
|
material.userData.originalSpecular = null; // New material, no original specular
|
|
return new THREE.Mesh(threeGeometry, material);
|
|
}
|
|
|
|
/**
|
|
* Load mesh file from fileMap (using universal loader)
|
|
*/
|
|
static async loadMeshFile(meshPath, fileMap) {
|
|
return loadMeshFile(meshPath, fileMap);
|
|
}
|
|
|
|
/**
|
|
* Set joint angle
|
|
*/
|
|
static setJointAngle(joint, angle) {
|
|
joint.currentValue = angle;
|
|
|
|
if (joint.threeObject) {
|
|
// Rotate based on joint type and axis
|
|
if (joint.type === 'revolute' || joint.type === 'continuous') {
|
|
// Use axis stored on threeObject (already converted), if not available convert from joint.axis
|
|
let axis;
|
|
if (joint.threeObject.axis) {
|
|
axis = joint.threeObject.axis.clone().normalize();
|
|
} else if (joint.axis && joint.axis.xyz) {
|
|
// If no pre-stored axis, need coordinate system conversion
|
|
const mjcfAxis = joint.axis.xyz;
|
|
axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[2], -mjcfAxis[1]).normalize();
|
|
} else {
|
|
console.warn('Joint has no axis definition:', joint.name);
|
|
return;
|
|
}
|
|
|
|
// Save initial rotation (only save on first call)
|
|
if (!joint.threeObject.userData.initialQuaternion) {
|
|
joint.threeObject.userData.initialQuaternion = joint.threeObject.quaternion.clone();
|
|
}
|
|
|
|
// Set rotation using quaternion: initial rotation * joint rotation
|
|
const rotationQuat = new THREE.Quaternion();
|
|
rotationQuat.setFromAxisAngle(axis, angle);
|
|
|
|
// Combine rotations: apply initial rotation first, then joint rotation
|
|
joint.threeObject.quaternion.copy(joint.threeObject.userData.initialQuaternion);
|
|
joint.threeObject.quaternion.multiply(rotationQuat);
|
|
|
|
// Update matrix
|
|
joint.threeObject.updateMatrixWorld(true);
|
|
} else if (joint.type === 'prismatic') {
|
|
// Use axis stored on threeObject (already converted) or convert from joint.axis
|
|
let axis;
|
|
if (joint.threeObject.axis) {
|
|
axis = joint.threeObject.axis.clone().normalize();
|
|
} else if (joint.axis && joint.axis.xyz) {
|
|
// If no pre-stored axis, need coordinate system conversion
|
|
const mjcfAxis = joint.axis.xyz;
|
|
axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[2], -mjcfAxis[1]).normalize();
|
|
} else {
|
|
console.warn('Joint has no axis definition:', joint.name);
|
|
return;
|
|
}
|
|
|
|
// Save initial position (only save on first call)
|
|
if (!joint.threeObject.userData.initialPosition) {
|
|
joint.threeObject.userData.initialPosition = joint.threeObject.position.clone();
|
|
}
|
|
|
|
// Translate joint: initial position + move along axis
|
|
joint.threeObject.position.copy(joint.threeObject.userData.initialPosition);
|
|
joint.threeObject.position.addScaledVector(axis, angle);
|
|
|
|
// Update matrix
|
|
joint.threeObject.updateMatrixWorld(true);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|