diff --git a/URDFEditor.js b/URDFEditor.js index a83ba58..da09f9b 100644 --- a/URDFEditor.js +++ b/URDFEditor.js @@ -119,14 +119,14 @@ export class URDFEditor { const urdfText = await fetch(this.urdfPath).then(res => res.text()); const robot = await this.loader.loadFromString(urdfText); - + //addOriginMarkers(robot); robot.rotation.x = -Math.PI / 2; this.scene.add(robot); this.robot = robot; - for (const jointName in this.robot.joints) { - console.log(jointName, this.robot.joints[jointName].transmission); - } + // for (const jointName in this.robot.joints) { + // console.log(jointName, this.robot.joints[jointName].transmission); + // } } @@ -162,31 +162,47 @@ export class URDFEditor { this.lastX = event.clientX; const angleDelta = deltaX * 0.005; - const jointName = this.draggedJoint.name; - - // ✅ Use transmission-derived limits if available + let jointName = this.draggedJoint.name; const { lower, upper } = getJointLimits(jointName, this.robot); const currentAngle = this.jointAngles[jointName] ?? 0; const proposedAngle = currentAngle + angleDelta; - - // ✅ Clamp to safe range const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle)); const actualDelta = clampedAngle - currentAngle; + // ✅ Rotate dragged joint this.draggedJoint.rotateOnAxis(this.worldAxis, actualDelta); this.jointAngles[jointName] = clampedAngle; + //console.log(jointName); + // ✅ Apply mimic relationships automatically + // ✅ Enforce mimics + for (const [name, jointObj] of Object.entries(this.robot.joints)) { + if (jointObj.type === 'URDFMimicJoint') { + const { mimicJoint, multiplier, offset } = jointObj; + if (mimicJoint === jointName) { + const mimicAngle = clampedAngle * multiplier + offset; + const { lower, upper } = getJointLimits(name, this.robot); + const mimicClamped = Math.max(lower, Math.min(upper, mimicAngle)); + const mimicDelta = mimicClamped - (this.jointAngles[name] ?? 0); + + const mimicNode = this.robot.getObjectByName(name); + mimicNode.rotateOnAxis(this.worldAxis, mimicDelta); + this.jointAngles[name] = mimicClamped; + } + } + } // ✅ Update gizmo indicator const gizmo = this.draggedJoint.userData.gizmo; if (gizmo?.indicator) this.draggedJoint.parent.remove(gizmo.indicator); - const newIndicator = createAngleIndicator(this.worldAxis, clampedAngle); newIndicator.position.copy(this.draggedJoint.position); this.draggedJoint.parent.add(newIndicator); this.draggedJoint.userData.gizmo.indicator = newIndicator; } + + // Hover highlighting if (intersects.length > 0) { const target = intersects[0].object; @@ -227,16 +243,24 @@ export class URDFEditor { // ✅ Use transmission-derived limits if available const { lower, upper } = getJointLimits(jointName, this.robot); - console.log(lower, upper); + //console.log(lower, upper); const jointTransmission = this.robot.joints[jointName].transmission; - if (!jointTransmission) return; - const encoderRange = jointTransmission.encoderRange / jointTransmission.mechanicalReduction; + const jointMimic = this.robot.joints[jointName].mimic; + console.log(this.robot.joints[jointName]); + + // Bail only if neither transmission nor mimic + if (!jointTransmission && !jointMimic) return; + + // If transmission exists, use its encoderRange + let encoderRange = 180; // sensible default + if (jointTransmission) { + encoderRange = jointTransmission.encoderRange / jointTransmission.mechanicalReduction; + } + const sector = createRotationSector(this.worldAxis, lower, upper, encoderRange); const indicator = createAngleIndicator(this.worldAxis, this.jointAngles[jointName] ?? 0); - sector.position.copy(this.draggedJoint.position); - indicator.position.copy(this.draggedJoint.position); this.draggedJoint.parent.add(sector); this.draggedJoint.parent.add(indicator); @@ -369,3 +393,20 @@ function loadObjWithMtl(objPath, mtlPath, onLoad) { }); }); } + +function addOriginMarkers(robot) { + console.log(robot.joints); + for (const [name, joint] of Object.entries(robot.joints)) { + const marker = new THREE.Mesh( + new THREE.SphereGeometry(0.01), + new THREE.MeshBasicMaterial({ color: 0x00ff00 }) + ); + joint.add(marker); + + const axes = new THREE.AxesHelper(0.05); + joint.add(axes); + + console.log("Added marker for joint:", name); + } + +} diff --git a/robots/LittleSophia/meshes/Little_Sophia_Foot_Left.glb b/robots/LittleSophia/meshes/Little_Sophia_Foot_Left.glb new file mode 100644 index 0000000..5b5d024 Binary files /dev/null and b/robots/LittleSophia/meshes/Little_Sophia_Foot_Left.glb differ diff --git a/robots/LittleSophia/meshes/Little_Sophia_Foot_Right.glb b/robots/LittleSophia/meshes/Little_Sophia_Foot_Right.glb new file mode 100644 index 0000000..0b959f1 Binary files /dev/null and b/robots/LittleSophia/meshes/Little_Sophia_Foot_Right.glb differ diff --git a/robots/LittleSophia/meshes/Little_Sophia_Torso_.glb b/robots/LittleSophia/meshes/Little_Sophia_Torso_.glb new file mode 100644 index 0000000..18129eb Binary files /dev/null and b/robots/LittleSophia/meshes/Little_Sophia_Torso_.glb differ diff --git a/robots/LittleSophia/urdf/LittleSophia.urdf b/robots/LittleSophia/urdf/LittleSophia.urdf index b7d53f2..adee6ca 100644 --- a/robots/LittleSophia/urdf/LittleSophia.urdf +++ b/robots/LittleSophia/urdf/LittleSophia.urdf @@ -10,7 +10,7 @@ - @@ -161,14 +161,27 @@ - + + - - - + + + + + + + + + + + + + + + @@ -195,19 +208,20 @@ - + - + - + - + @@ -228,19 +242,19 @@ - + - - - + + + - - + - + - + @@ -259,23 +273,22 @@ - - - - - - - + + + + + - + - + - + - + @@ -292,21 +305,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + - + - + @@ -455,7 +513,7 @@ - + transmission_interface/SimpleTransmission @@ -473,7 +531,7 @@ transmission_interface/SimpleTransmission - + PositionJointInterface @@ -488,7 +546,7 @@ transmission_interface/SimpleTransmission - + PositionJointInterface @@ -516,9 +574,24 @@ + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + 4096 + 200 + 3500 + 270 + + + transmission_interface/SimpleTransmission - + PositionJointInterface @@ -533,7 +606,7 @@ transmission_interface/SimpleTransmission - + PositionJointInterface @@ -546,20 +619,6 @@ - - transmission_interface/SimpleTransmission - - PositionJointInterface - - - 1 - PositionJointInterface - 4096 - 200 - 3500 - 270 - - transmission_interface/SimpleTransmission diff --git a/urdf/ExtendedURDFLoader.js b/urdf/ExtendedURDFLoader.js index 9dc2fc3..ac76e57 100644 --- a/urdf/ExtendedURDFLoader.js +++ b/urdf/ExtendedURDFLoader.js @@ -1,7 +1,7 @@ import URDFLoader from './URDFLoader.js'; export default -class ExtendedURDFLoader extends URDFLoader { + class ExtendedURDFLoader extends URDFLoader { constructor(manager) { super(manager); this.transmissionMap = {}; @@ -34,6 +34,28 @@ class ExtendedURDFLoader extends URDFLoader { }); } + parseMimics(xml) { + const joints = xml.querySelectorAll('joint'); + joints.forEach(jointEl => { + const jointName = jointEl.getAttribute('name'); + const mimicEl = jointEl.querySelector('mimic'); + if (mimicEl && jointName) { + const target = mimicEl.getAttribute('joint'); + const multiplier = parseFloat(mimicEl.getAttribute('multiplier') ?? '1.0'); + const offset = parseFloat(mimicEl.getAttribute('offset') ?? '0.0'); + + // Attach mimic info directly to the joint object + if (this.robot?.joints[jointName]) { + this.robot.joints[jointName].mimic = { target, multiplier, offset }; + } + } + }); + + + } + + + async loadFromString(urdfText, options = {}) { const xml = new DOMParser().parseFromString(urdfText, 'application/xml'); const parserError = xml.querySelector('parsererror'); @@ -43,9 +65,10 @@ class ExtendedURDFLoader extends URDFLoader { } this.parseTransmissions(xml); + //this.parseMimics(xml); const robot = this.parse(xml); // ✅ Use inherited parse() directly - +console.log(robot.joints); for (const jointName in robot.joints) { if (this.transmissionMap[jointName]) { robot.joints[jointName].transmission = this.transmissionMap[jointName];