converted visualiser import to regular folder
parent
0aa935afc1
commit
4b6a275696
|
|
@ -1 +0,0 @@
|
|||
Subproject commit 40905cbb36a784c7f86bd5966418d263e8143525
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
import * as THREE from 'three';
|
||||
|
||||
export function createRotationSector(axis, lower, upper, encoderRange, radius = 0.1, segments = 32) {
|
||||
const motorMargin = 0.4;
|
||||
const motorLower = -encoderRange/2 * (Math.PI / 180);// -Math.PI/2;//lower - motorMargin;
|
||||
const motorUpper = encoderRange/2 * (Math.PI / 180);//Math.PI/2;//upper + motorMargin;
|
||||
|
||||
const redMaterial = new THREE.MeshBasicMaterial({ color: 0xff0000, transparent: true, opacity: 0.2, side: THREE.DoubleSide, depthTest: false, depthWrite: false });
|
||||
const whiteMaterial = new THREE.MeshBasicMaterial({ color: 0xffffff, transparent: true, opacity: 0.6, side: THREE.DoubleSide, depthTest: false, depthWrite: false });
|
||||
|
||||
function createArc(start, end, material) {
|
||||
const shape = new THREE.Shape();
|
||||
shape.moveTo(0, 0);
|
||||
for (let i = 0; i <= segments; i++) {
|
||||
const angle = start + (end - start) * (i / segments);
|
||||
shape.lineTo(Math.cos(angle) * radius, Math.sin(angle) * radius);
|
||||
}
|
||||
shape.lineTo(0, 0);
|
||||
return new THREE.Mesh(new THREE.ShapeGeometry(shape), material);
|
||||
}
|
||||
|
||||
const redLeft = createArc(motorLower, lower, redMaterial);
|
||||
const whiteArc = createArc(lower, upper, whiteMaterial);
|
||||
const redRight = createArc(upper, motorUpper, redMaterial);
|
||||
|
||||
const up = new THREE.Vector3(0, 0, 1);
|
||||
const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
|
||||
const basis = new THREE.Matrix4().makeRotationFromQuaternion(quaternion);
|
||||
|
||||
redLeft.applyMatrix4(basis);
|
||||
whiteArc.applyMatrix4(basis);
|
||||
redRight.applyMatrix4(basis);
|
||||
|
||||
const group = new THREE.Group();
|
||||
group.add(redLeft, whiteArc, redRight);
|
||||
return group;
|
||||
}
|
||||
|
||||
export function createAngleIndicator(axis, angle, radius = 0.1) {
|
||||
const dir = new THREE.Vector3(Math.cos(angle), Math.sin(angle), 0).multiplyScalar(radius);
|
||||
const geometry = new THREE.BufferGeometry().setFromPoints([new THREE.Vector3(0, 0, 0), dir]);
|
||||
const material = new THREE.LineBasicMaterial({ color: 0xffffff, depthTest: false, depthWrite: false });
|
||||
const line = new THREE.Line(geometry, material);
|
||||
|
||||
const up = new THREE.Vector3(0, 0, 1);
|
||||
const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
|
||||
line.applyQuaternion(quaternion);
|
||||
|
||||
return line;
|
||||
}
|
||||
|
||||
export function createJointLabel(name, angle) {
|
||||
const canvas = document.createElement('canvas');
|
||||
canvas.width = 256;
|
||||
canvas.height = 64;
|
||||
const ctx = canvas.getContext('2d');
|
||||
|
||||
ctx.fillStyle = 'rgba(0,0,0,0.6)';
|
||||
ctx.fillRect(0, 0, canvas.width, canvas.height);
|
||||
|
||||
ctx.font = '20px monospace';
|
||||
ctx.fillStyle = '#ffffff';
|
||||
ctx.fillText(`${name}: ${(angle * 180 / Math.PI).toFixed(1)}°`, 10, 40);
|
||||
|
||||
const texture = new THREE.CanvasTexture(canvas);
|
||||
const material = new THREE.SpriteMaterial({ map: texture, transparent: true });
|
||||
const sprite = new THREE.Sprite(material);
|
||||
sprite.scale.set(0.5, 0.125, 1); // adjust size
|
||||
|
||||
return sprite;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,468 @@
|
|||
import * as THREE from 'three';
|
||||
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
|
||||
//import URDFLoader from './urdf/ExtendedURDFLoader.js';
|
||||
import { createRotationSector, createAngleIndicator, createJointLabel } from './JointVisualiser.js';
|
||||
import ExtendedURDFLoader from './urdf/ExtendedURDFLoader.js';
|
||||
import { OBJLoader } from 'three/examples/jsm/loaders/OBJLoader.js';
|
||||
import { MTLLoader } from 'three/examples/jsm/loaders/MTLLoader.js';
|
||||
import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js';
|
||||
import { ViewerOverlay } from './ViewerOverlay.js';
|
||||
const gltfLoader = new GLTFLoader();
|
||||
|
||||
|
||||
export class URDFEditor {
|
||||
constructor(canvas) {
|
||||
this.canvas = canvas;
|
||||
console.log(canvas);
|
||||
this.scene = new THREE.Scene();
|
||||
this.scene.background = new THREE.Color(0xaaaaaa);
|
||||
|
||||
this.camera = new THREE.PerspectiveCamera(60, canvas.clientWidth / canvas.clientHeight, 0.1, 100);
|
||||
this.camera.position.set(-0.3, 0.5, -0.7);
|
||||
this.camera.lookAt(0, 0, 0);
|
||||
|
||||
this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||||
this.renderer.setSize(canvas.clientWidth, canvas.clientHeight);
|
||||
|
||||
|
||||
|
||||
this.controls = new OrbitControls(this.camera, this.renderer.domElement);
|
||||
this.controls.target.set(0, 0.1, 0);
|
||||
this.controls.update();
|
||||
|
||||
this.raycaster = new THREE.Raycaster();
|
||||
this.mouse = new THREE.Vector2();
|
||||
|
||||
this.hoveredJoint = null;
|
||||
this.draggedJoint = null;
|
||||
this.worldAxis = null;
|
||||
this.lastX = null;
|
||||
this.isDragging = false;
|
||||
this.jointAngles = {};
|
||||
|
||||
this.loader = new ExtendedURDFLoader();
|
||||
this.loader.loadMeshCb = (path, manager, onComplete) => {
|
||||
if (path.endsWith('.glb') || path.endsWith('.gltf')) {
|
||||
const gltfLoader = new GLTFLoader(manager);
|
||||
gltfLoader.load(path, (gltf) => {
|
||||
onComplete(gltf.scene);
|
||||
});
|
||||
} else {
|
||||
// fall back to default behavior (STL, DAE)
|
||||
URDFLoader.prototype.loadMeshCb.call(this.loader, path, manager, onComplete);
|
||||
}
|
||||
};
|
||||
|
||||
this.loader.packages = {
|
||||
'Little_Sophia_Face': '/robots/LittleSophia'
|
||||
};
|
||||
this.loader.parseVisual = true;
|
||||
this.loader.parseCollision = false;
|
||||
|
||||
|
||||
this.setupScene();
|
||||
//this.loadURDF();
|
||||
this.loadURDFFromIndexedDB();
|
||||
this.setupEvents();
|
||||
|
||||
this.overlay = new ViewerOverlay(
|
||||
this.renderer,
|
||||
this.robot,
|
||||
this.jointAngles,
|
||||
this.findObjectByName.bind(this),
|
||||
this.saveURDFToIndexedDB.bind(this),
|
||||
this.loadURDFFromIndexedDB.bind(this)
|
||||
);
|
||||
|
||||
|
||||
// Add some lights (optional if using MeshBasicMaterial)
|
||||
const ambient = new THREE.AmbientLight(0x404040);
|
||||
this.scene.add(ambient);
|
||||
const dirLight = new THREE.DirectionalLight(0xffffff, 1);
|
||||
dirLight.position.set(5, 5, 5);
|
||||
this.scene.add(dirLight);
|
||||
|
||||
// this.mesh = null;
|
||||
// gltfLoader.load('meshes/Little_Sophia.glb', (gltf) => {
|
||||
// const model = gltf.scene;
|
||||
// model.scale.set(0.001, 0.001, 0.001); // adjust if needed
|
||||
// model.rotation.y = Math.PI; // radians (180°)
|
||||
// this.scene.add(model);
|
||||
|
||||
// // Optional: fit camera to model
|
||||
// const box = new THREE.Box3().setFromObject(model);
|
||||
// const center = box.getCenter(new THREE.Vector3());
|
||||
// const size = box.getSize(new THREE.Vector3()).length();
|
||||
// });
|
||||
|
||||
|
||||
this.animate();
|
||||
}
|
||||
|
||||
setupScene() {
|
||||
this.scene.add(new THREE.AmbientLight(0xffffff, 0.6));
|
||||
const light1 = new THREE.DirectionalLight(0xffffff, 0.8);
|
||||
light1.position.set(15, 10, 7);
|
||||
this.scene.add(light1);
|
||||
|
||||
const light2 = new THREE.DirectionalLight(0xffffff, 0.8);
|
||||
light2.position.set(-15, 10, -7);
|
||||
this.scene.add(light2);
|
||||
|
||||
this.scene.add(new THREE.AxesHelper(1));
|
||||
this.scene.add(new THREE.GridHelper(2, 10));
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
async loadURDF() {
|
||||
this.urdfPath = '/robots/LittleSophia/urdf/LittleSophia.urdf';
|
||||
|
||||
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;
|
||||
|
||||
//this.overlay.robot = robot;
|
||||
this.overlay.init(robot);
|
||||
|
||||
}
|
||||
|
||||
async loadURDFFromIndexedDB() {
|
||||
return new Promise((resolve, reject) => {
|
||||
const request = indexedDB.open("URDFEditorDB", 1);
|
||||
|
||||
request.onerror = e => reject(e);
|
||||
|
||||
request.onsuccess = e => {
|
||||
const db = e.target.result;
|
||||
const tx = db.transaction("urdfs", "readonly");
|
||||
const getReq = tx.objectStore("urdfs").get("current");
|
||||
|
||||
getReq.onsuccess = async () => {
|
||||
const urdfText = getReq.result;
|
||||
if (!urdfText) {
|
||||
reject("No URDF stored in IndexedDB");
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
// ✅ feed the text into your loader
|
||||
const robot = await this.loader.loadFromString(urdfText);
|
||||
robot.rotation.x = -Math.PI / 2;
|
||||
this.scene.add(robot);
|
||||
this.robot = robot;
|
||||
this.overlay.init(robot);
|
||||
this.saveURDFToIndexedDB(robot);
|
||||
resolve(robot);
|
||||
} catch (err) {
|
||||
reject(err);
|
||||
}
|
||||
};
|
||||
};
|
||||
});
|
||||
}
|
||||
|
||||
// WRITE TO URDF POSITION IN ROBOT
|
||||
// Find the transmission node for neck_roll
|
||||
// const neckTrans = this.robot.urdfRobotNode.querySelector('transmission[name="neck_roll_trans"]');
|
||||
// if (neckTrans) {
|
||||
// const actuator = neckTrans.querySelector('actuator[name="neck_roll"]');
|
||||
// if (actuator) {
|
||||
// // Example: change encoderValidMin/Max
|
||||
// actuator.querySelector('encoderValidMin').textContent = "100";
|
||||
// actuator.querySelector('encoderValidMax').textContent = "3900";
|
||||
// }
|
||||
// }
|
||||
|
||||
downloadURDF(robot, filename = "robot.urdf") {
|
||||
const serializer = new XMLSerializer();
|
||||
const urdfString = serializer.serializeToString(robot.urdfRobotNode);
|
||||
|
||||
const a = document.createElement("a");
|
||||
a.href = "data:text/xml;charset=utf-8," + encodeURIComponent(urdfString);
|
||||
a.download = filename;
|
||||
a.click();
|
||||
}
|
||||
|
||||
saveURDFToIndexedDB(robot) {
|
||||
const serializer = new XMLSerializer();
|
||||
const urdfString = serializer.serializeToString(robot.urdfRobotNode);
|
||||
|
||||
const request = indexedDB.open("URDFEditorDB", 1);
|
||||
request.onupgradeneeded = e => {
|
||||
const db = e.target.result;
|
||||
db.createObjectStore("urdfs");
|
||||
};
|
||||
request.onsuccess = e => {
|
||||
const db = e.target.result;
|
||||
const tx = db.transaction("urdfs", "readwrite");
|
||||
tx.objectStore("urdfs").put(urdfString, "current");
|
||||
console.log("saved URDF to IndexedDB");
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
listUrdfsFromIndexedDB() {
|
||||
const request = indexedDB.open("URDFEditorDB", 1);
|
||||
|
||||
request.onsuccess = e => {
|
||||
const db = e.target.result;
|
||||
const tx = db.transaction("urdfs", "readonly");
|
||||
const store = tx.objectStore("urdfs");
|
||||
const getAllReq = store.getAll();
|
||||
|
||||
getAllReq.onsuccess = () => {
|
||||
console.log("Stored URDFs:", getAllReq.result);
|
||||
};
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
findObjectByName(root, name) {
|
||||
let result = null;
|
||||
root.traverse(child => {
|
||||
if (child.name === name) {
|
||||
result = child;
|
||||
}
|
||||
});
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
setupEvents() {
|
||||
this.canvas.addEventListener('pointermove', this.onPointerMove.bind(this));
|
||||
this.canvas.addEventListener('pointerdown', this.onPointerDown.bind(this));
|
||||
this.canvas.addEventListener('pointerup', this.onPointerUp.bind(this));
|
||||
}
|
||||
|
||||
onPointerMove(event) {
|
||||
const rect = this.canvas.getBoundingClientRect();
|
||||
|
||||
this.mouse.x = ((event.clientX - rect.left) / rect.width) * 2 - 1;
|
||||
this.mouse.y = -((event.clientY - rect.top) / rect.height) * 2 + 1;
|
||||
|
||||
this.raycaster.setFromCamera(this.mouse, this.camera);
|
||||
|
||||
if (!this.robot) return;
|
||||
const intersects = this.raycaster.intersectObjects(this.robot.children, true);
|
||||
|
||||
if (this.isDragging && this.draggedJoint && this.worldAxis) {
|
||||
const deltaX = event.clientX - this.lastX;
|
||||
this.lastX = event.clientX;
|
||||
const angleDelta = deltaX * 0.005;
|
||||
|
||||
let jointName = this.draggedJoint.name;
|
||||
const { lower, upper } = getJointLimits(jointName, this.robot);
|
||||
|
||||
const currentAngle = this.jointAngles[jointName] ?? 0;
|
||||
const proposedAngle = currentAngle + angleDelta;
|
||||
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;
|
||||
if (this.hoveredJoint?.material?.emissive) {
|
||||
this.hoveredJoint.material.emissive.setHex(0x000000);
|
||||
}
|
||||
this.hoveredJoint = target;
|
||||
if (this.hoveredJoint.material?.emissive) {
|
||||
this.hoveredJoint.material.emissive.setHex(0x333333);
|
||||
}
|
||||
} else {
|
||||
if (this.hoveredJoint?.material?.emissive) {
|
||||
this.hoveredJoint.material.emissive.setHex(0x000000);
|
||||
}
|
||||
this.hoveredJoint = null;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
onPointerDown(event) {
|
||||
if (!this.hoveredJoint) return;
|
||||
|
||||
this.isDragging = true;
|
||||
this.lastX = event.clientX;
|
||||
this.controls.enabled = false;
|
||||
|
||||
// Decide how far up the chain to go
|
||||
let levelsUp = 0; // default: climb one level from link → joint
|
||||
if (event.ctrlKey) levelsUp = 1;
|
||||
if (event.altKey) levelsUp = 2;
|
||||
|
||||
this.draggedJoint = this.findJointAncestor(this.hoveredJoint, levelsUp);
|
||||
if (!this.draggedJoint) return;
|
||||
|
||||
// ✅ Skip mimic joints
|
||||
if (this.draggedJoint.type === 'URDFMimicJoint') {
|
||||
this.draggedJoint = findJointAncestor(this.draggedJoint, 1);
|
||||
}
|
||||
|
||||
|
||||
const jointName = this.draggedJoint.name;
|
||||
const jointData = this.robot.joints?.[jointName];
|
||||
if (!(jointData?.axis instanceof THREE.Vector3)) return;
|
||||
|
||||
const axis = jointData.axis.clone();
|
||||
if (axis.lengthSq() === 0) return;
|
||||
this.worldAxis = axis.normalize();
|
||||
|
||||
const { lower, upper } = getJointLimits(jointName, this.robot);
|
||||
const transmission = jointData.transmission;
|
||||
const encoderRange = transmission
|
||||
? transmission.encoderRange / transmission.mechanicalReduction
|
||||
: Math.PI;
|
||||
|
||||
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);
|
||||
this.draggedJoint.userData.gizmo = { sector, indicator };
|
||||
}
|
||||
|
||||
|
||||
|
||||
onPointerUp() {
|
||||
this.isDragging = false;
|
||||
this.controls.enabled = true;
|
||||
|
||||
const gizmo = this.draggedJoint?.userData.gizmo;
|
||||
if (gizmo) {
|
||||
this.draggedJoint.parent.remove(gizmo.sector);
|
||||
this.draggedJoint.parent.remove(gizmo.indicator);
|
||||
}
|
||||
|
||||
this.draggedJoint = null;
|
||||
this.worldAxis = null;
|
||||
}
|
||||
|
||||
findJointAncestor(joint, levelsUp = 0) {
|
||||
let current = joint;
|
||||
let steps = 0;
|
||||
|
||||
while (current && steps <= levelsUp) {
|
||||
current = current.parent;
|
||||
// climb until we find a URDFJoint (not a mimic)
|
||||
while (current && !(current.type === 'URDFJoint')) {
|
||||
current = current.parent;
|
||||
}
|
||||
steps++;
|
||||
}
|
||||
|
||||
return current;
|
||||
}
|
||||
|
||||
|
||||
animate() {
|
||||
requestAnimationFrame(() => this.animate());
|
||||
//console.log(this.robot.joints)
|
||||
this.renderer.render(this.scene, this.camera);
|
||||
this.overlay.draw();
|
||||
//console.log(this.mesh);
|
||||
}
|
||||
}
|
||||
|
||||
function ticksToRadians(ticks, actuator) {
|
||||
const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75
|
||||
const centered = ticks - actuator.encoderTicks / 2; // shift so mid = 0
|
||||
const actuatorDeg = centered / ticksPerDeg;
|
||||
const jointDeg = actuatorDeg / actuator.mechanicalReduction;
|
||||
return jointDeg * (Math.PI / 180); // convert degrees → radians
|
||||
}
|
||||
|
||||
function radiansToTicks(radians, actuator) {
|
||||
const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75
|
||||
const jointDeg = radians * (180 / Math.PI); // radians → degrees
|
||||
const actuatorDeg = jointDeg * actuator.mechanicalReduction; // apply reduction
|
||||
const centered = actuatorDeg * ticksPerDeg; // convert to ticks offset from mid
|
||||
const ticks = centered + actuator.encoderTicks / 2; // shift back to full range
|
||||
return ticks;
|
||||
}
|
||||
|
||||
|
||||
function getJointLimits(jointName, robot) {
|
||||
const transmission = robot.joints[jointName].transmission;
|
||||
|
||||
if (!transmission) {
|
||||
return { lower: -Math.PI, upper: Math.PI };
|
||||
}
|
||||
//console.log(transmission.encoderValidMin, transmission.encoderValidMax);
|
||||
const lowerRad = ticksToRadians(transmission.encoderValidMin, transmission);
|
||||
const upperRad = ticksToRadians(transmission.encoderValidMax, transmission);
|
||||
//console.log(lowerRad, upperRad);
|
||||
return { lower: lowerRad, upper: upperRad };
|
||||
}
|
||||
|
||||
function getJointTransmission(jointName, robot) {
|
||||
return robot.joints[jointName].transmission;
|
||||
}
|
||||
|
||||
function loadObjWithMtl(objPath, mtlPath, onLoad) {
|
||||
const mtlLoader = new MTLLoader();
|
||||
mtlLoader.load(mtlPath, (materials) => {
|
||||
materials.preload();
|
||||
const objLoader = new OBJLoader();
|
||||
objLoader.setMaterials(materials);
|
||||
objLoader.load(objPath, (object) => {
|
||||
onLoad(object);
|
||||
});
|
||||
});
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,296 @@
|
|||
import { Button, Panel, Textbox } from './ui/canvasui.js';
|
||||
|
||||
export class ViewerOverlay {
|
||||
constructor(renderer, robot, jointAngles, findObjectByName, saveURDF, loadURDF) {
|
||||
this.renderer = renderer;
|
||||
this.robot = robot;
|
||||
this.jointAngles = jointAngles;
|
||||
this.findObjectByName = findObjectByName;
|
||||
this.saveURDF = saveURDF; // 🔑 bound to editor
|
||||
this.loadURDF = loadURDF; // 🔑 bound to editor
|
||||
|
||||
|
||||
this.overlayCanvas = document.getElementById('overlay-canvas');
|
||||
this.overlayCtx = this.overlayCanvas.getContext('2d');
|
||||
|
||||
this.uiElements = []; // for motor list items if you wrap them in UIElements
|
||||
this.panels = []; // for modals or floating panels
|
||||
|
||||
|
||||
// this.motorListPanel = this.createMotorListPanel();
|
||||
// this.panels.push(this.motorListPanel);
|
||||
// console.log(this.panels);
|
||||
|
||||
this.showMotorList = true;
|
||||
this.showConfigModal = false;
|
||||
this.configMotor = null;
|
||||
this.configMotorName = null;
|
||||
|
||||
// Handle resizing
|
||||
const resizeOverlay = () => {
|
||||
const { clientWidth, clientHeight } = this.renderer.domElement;
|
||||
this.overlayCanvas.width = clientWidth;
|
||||
this.overlayCanvas.height = clientHeight;
|
||||
};
|
||||
window.addEventListener('resize', resizeOverlay);
|
||||
resizeOverlay();
|
||||
|
||||
this.panels.push(this.createSystemPanel());
|
||||
const handlePointerEvent = (event) => {
|
||||
const rect = this.overlayCanvas.getBoundingClientRect();
|
||||
const x = event.clientX - rect.left;
|
||||
const y = event.clientY - rect.top;
|
||||
|
||||
let consumed = false;
|
||||
|
||||
// Panels get first chance
|
||||
for (const panel of this.panels) {
|
||||
if (panel.contains(x, y)) {
|
||||
panel.handlePointerEvent(event, x, y);
|
||||
consumed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (!consumed) {
|
||||
const evt = new event.constructor(event.type, event);
|
||||
this.renderer.domElement.dispatchEvent(evt);
|
||||
}
|
||||
|
||||
if (['pointermove', 'pointerdown', 'pointerup'].includes(event.type)) {
|
||||
this.draw();
|
||||
}
|
||||
};
|
||||
window.addEventListener('keydown', (event) => {
|
||||
this.panels.forEach(panel => panel.handleKeyEvent(event));
|
||||
this.draw();
|
||||
});
|
||||
|
||||
|
||||
|
||||
|
||||
// Attach handlers
|
||||
['click', 'pointerdown', 'pointerup', 'pointermove', 'wheel'].forEach(type => {
|
||||
this.overlayCanvas.addEventListener(type, handlePointerEvent);
|
||||
});
|
||||
}
|
||||
|
||||
init(robot) {
|
||||
this.robot = robot;
|
||||
this.motorListPanel = this.createMotorListPanel(this.overlayCtx);
|
||||
if (this.motorListPanel) {
|
||||
this.panels.push(this.motorListPanel);
|
||||
}
|
||||
this.draw();
|
||||
}
|
||||
|
||||
draw() {
|
||||
const ctx = this.overlayCtx;
|
||||
ctx.clearRect(0, 0, this.overlayCanvas.width, this.overlayCanvas.height);
|
||||
this.updateMotorLabels();
|
||||
this.panels.forEach(panel => panel.draw(ctx));
|
||||
|
||||
//console.log(this.panels);
|
||||
}
|
||||
|
||||
createSystemPanel() {
|
||||
const w = 160;
|
||||
const h = 80;
|
||||
const x = this.overlayCanvas.width - w - 10; // bottom right
|
||||
const y = this.overlayCanvas.height - h - 10;
|
||||
console.log(this.overlayCanvas.width, this.overlayCanvas.height);
|
||||
|
||||
const panel = new Panel(x, y, w, h, "System");
|
||||
|
||||
// Save button
|
||||
panel.addElement(new Button(x + 20, y + 20, 120, 24, "Save", () => {
|
||||
// delegate to editor’s save
|
||||
this.saveURDF(this.robot);
|
||||
}));
|
||||
|
||||
// Load button
|
||||
panel.addElement(new Button(x + 20, y + 50, 120, 24, "Load", () => {
|
||||
// delegate to editor’s load
|
||||
this.loadURDF();
|
||||
}));
|
||||
|
||||
return panel;
|
||||
}
|
||||
|
||||
|
||||
createMotorListPanel() {
|
||||
if (!this.robot?.joints) return null;
|
||||
this.panels = this.panels.filter(p => !p.title?.startsWith("Config:"));
|
||||
const panel = new Panel(0, 0, 300, this.overlayCanvas.height, "Motors");
|
||||
|
||||
let y = 10;
|
||||
for (const jointName in this.robot.joints) {
|
||||
const jointData = this.robot.joints[jointName];
|
||||
const jointObject = this.findObjectByName(this.robot, jointName);
|
||||
if (!jointData || !jointObject) continue;
|
||||
|
||||
const angle = this.jointAngles?.[jointName] ?? 0;
|
||||
const degrees = (angle * 180 / Math.PI).toFixed(1);
|
||||
|
||||
const motorName = jointData.transmission?.actuatorName || '(no motor)';
|
||||
if (motorName === "(no motor)") continue;
|
||||
|
||||
// Build label string with angle/ticks
|
||||
const t = jointData.transmission;
|
||||
let label = `${motorName} ${degrees}°`;
|
||||
if (t) {
|
||||
const ticks = radiansToTicks(angle, t).toFixed(0);
|
||||
label += ` ${ticks}`;
|
||||
}
|
||||
|
||||
|
||||
// Create button
|
||||
const btn = new Button(10, y + 20, 280, 18, label, () => {
|
||||
this.openMotorConfig(jointName, motorName);
|
||||
});
|
||||
|
||||
btn.jointName = jointName;
|
||||
btn.motorName = motorName;
|
||||
|
||||
|
||||
panel.addElement(btn);
|
||||
y += 22;
|
||||
}
|
||||
|
||||
return panel;
|
||||
}
|
||||
|
||||
updateMotorLabels() {
|
||||
const motorPanel = this.panels.find(p => p.title === "Motors");
|
||||
if (!motorPanel || !this.robot?.joints) return;
|
||||
|
||||
for (const element of motorPanel.elements) {
|
||||
if (!(element instanceof Button) || !element.jointName) continue;
|
||||
|
||||
const jointName = element.jointName;
|
||||
const jointData = this.robot.joints[jointName];
|
||||
if (!jointData) continue;
|
||||
|
||||
const angle = this.jointAngles?.[jointName] ?? 0;
|
||||
const degrees = (angle * 180 / Math.PI).toFixed(1);
|
||||
|
||||
const motorName = jointData.transmission?.actuatorName || '(no motor)';
|
||||
if (motorName === "(no motor)") continue;
|
||||
|
||||
let label = `${motorName} ${degrees}°`;
|
||||
const t = jointData.transmission;
|
||||
if (t) {
|
||||
const ticks = radiansToTicks(angle, t).toFixed(0);
|
||||
label += ` ${ticks}`;
|
||||
}
|
||||
|
||||
element.label = label; // 🔄 update text
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
openMotorConfig(jointName, motorName) {
|
||||
|
||||
const motor = this.robot.joints[jointName];
|
||||
|
||||
// Remove any existing config panel
|
||||
this.panels = this.panels.filter(p => !p.title?.startsWith("Config:"));
|
||||
|
||||
const panel = new Panel(400, 50, 400, 250, `Config: ${motorName}`);
|
||||
|
||||
const motorIdBox = new Textbox(panel.x + 100, panel.y + 45, 80, 24, motor.transmission.motorID || '');
|
||||
panel.addElement(motorIdBox);
|
||||
|
||||
panel.addElement(new Button(panel.x + 20, panel.y + 150, 80, 24, 'Save', () => {
|
||||
const idValue = parseInt(motorIdBox.value, 10) || 0;
|
||||
motor.motorId = idValue;
|
||||
motor.transmission.motorID = idValue;
|
||||
this.addOrUpdateActuatorElement(motorName, "motorID", idValue);
|
||||
this.panels = this.panels.filter(p => p !== panel); // close panel
|
||||
this.draw();
|
||||
}));
|
||||
|
||||
panel.addElement(new Button(panel.x + 120, panel.y + 150, 80, 24, 'Close', () => {
|
||||
this.panels = this.panels.filter(p => p !== panel);
|
||||
this.draw();
|
||||
}));
|
||||
|
||||
this.panels.push(panel);
|
||||
this.draw();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Ensure an element exists under a given actuator and set its text.
|
||||
* @param {string} transmissionName - The transmission name
|
||||
* @param {string} actuatorName - The actuator name
|
||||
* @param {string} elementName - The child element to add/update (e.g. "motorID")
|
||||
* @param {string|number} value - The value to set
|
||||
*/
|
||||
addOrUpdateActuatorElement(actuatorName, elementName, value) {
|
||||
console.log("Updating URDF:", actuatorName, elementName, value);
|
||||
const urdfNode = this.robot?.urdfRobotNode;
|
||||
if (!urdfNode) return;
|
||||
|
||||
// find actuator anywhere in the URDF
|
||||
const actuatorNode = urdfNode.querySelector(`actuator[name="${actuatorName}"]`);
|
||||
if (!actuatorNode) return;
|
||||
|
||||
// find or create the child element
|
||||
let childNode = actuatorNode.querySelector(elementName);
|
||||
if (!childNode) {
|
||||
childNode = urdfNode.ownerDocument.createElement(elementName);
|
||||
actuatorNode.appendChild(childNode);
|
||||
}
|
||||
childNode.textContent = value.toString();
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
onClick(event) {
|
||||
console.log("CLICK");
|
||||
const rect = this.overlayCanvas.getBoundingClientRect();
|
||||
const x = event.clientX - rect.left;
|
||||
const y = event.clientY - rect.top;
|
||||
|
||||
for (const hb of this.motorHitboxes) {
|
||||
if (x >= hb.x && x <= hb.x + hb.w &&
|
||||
y >= hb.y && y <= hb.y + hb.h) {
|
||||
this.openMotorConfig(hb.jointName, hb.motorName);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// openMotorConfig(jointName, motorName) {
|
||||
// this.showConfigModal = true;
|
||||
// this.configMotorName = motorName;
|
||||
// this.configMotor = this.robot.joints[jointName];
|
||||
// this.draw();
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
function ticksToRadians(ticks, actuator) {
|
||||
const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75
|
||||
const centered = ticks - actuator.encoderTicks / 2; // shift so mid = 0
|
||||
const actuatorDeg = centered / ticksPerDeg;
|
||||
const jointDeg = actuatorDeg / actuator.mechanicalReduction;
|
||||
return jointDeg * (Math.PI / 180); // convert degrees → radians
|
||||
}
|
||||
|
||||
function radiansToTicks(radians, actuator) {
|
||||
const ticksPerDeg = actuator.encoderTicks / actuator.encoderRange; // ≈ 22.75
|
||||
const jointDeg = radians * (180 / Math.PI); // radians → degrees
|
||||
const actuatorDeg = jointDeg * actuator.mechanicalReduction; // apply reduction
|
||||
const centered = actuatorDeg * ticksPerDeg; // convert to ticks offset from mid
|
||||
const ticks = centered + actuator.encoderTicks / 2; // shift back to full range
|
||||
return ticks;
|
||||
}
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
|
||||
<head>
|
||||
<meta charset="UTF-8" />
|
||||
<title>URDF Robot Viewer</title>
|
||||
<style>
|
||||
body {
|
||||
margin: 0;
|
||||
overflow: hidden;
|
||||
background-color: #111;
|
||||
}
|
||||
|
||||
#status {
|
||||
position: absolute;
|
||||
top: 10px;
|
||||
left: 10px;
|
||||
color: white;
|
||||
background: rgba(0, 0, 0, 0.5);
|
||||
padding: 6px 10px;
|
||||
border-radius: 4px;
|
||||
font-family: sans-serif;
|
||||
font-size: 14px;
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
|
||||
<body>
|
||||
<div id="status">Loading URDF...</div>
|
||||
<div style="position: relative; width: 100vw; height: 100vh;">
|
||||
<canvas id="urdfCanvas" style="width: 100%; height: 100%; display: block;"></canvas>
|
||||
<canvas id="overlay-canvas" style="position: absolute; top: 0; left: 0; pointer-events: none;"></canvas>
|
||||
</div>
|
||||
|
||||
|
||||
|
||||
<!-- Your logic -->
|
||||
<script type="module" src="script.js"></script>
|
||||
|
||||
</body>
|
||||
|
||||
</html>
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
{
|
||||
"name": "ros_robot_visualiser",
|
||||
"version": "1.0.0",
|
||||
"description": "",
|
||||
"main": "script.js",
|
||||
"scripts": {
|
||||
"test": "echo \"Error: no test specified\" && exit 1"
|
||||
},
|
||||
"keywords": [],
|
||||
"author": "",
|
||||
"license": "ISC",
|
||||
"type": "commonjs",
|
||||
"dependencies": {
|
||||
"three": "^0.181.0"
|
||||
},
|
||||
"devDependencies": {
|
||||
"vite": "^7.2.2"
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
import xml.etree.ElementTree as ET
|
||||
import math
|
||||
|
||||
def round_axis(x, y, z):
|
||||
# Normalize vector
|
||||
mag = math.sqrt(x**2 + y**2 + z**2)
|
||||
if mag == 0:
|
||||
return (0, 0, 0)
|
||||
x, y, z = x/mag, y/mag, z/mag
|
||||
|
||||
# Compare to cardinal directions
|
||||
directions = {
|
||||
(1, 0, 0): 'X+',
|
||||
(-1, 0, 0): 'X-',
|
||||
(0, 1, 0): 'Y+',
|
||||
(0, -1, 0): 'Y-',
|
||||
(0, 0, 1): 'Z+',
|
||||
(0, 0, -1): 'Z-'
|
||||
}
|
||||
|
||||
best = max(directions.keys(), key=lambda d: x*d[0] + y*d[1] + z*d[2])
|
||||
return best
|
||||
|
||||
def process_urdf(file_path, output_path):
|
||||
tree = ET.parse(file_path)
|
||||
root = tree.getroot()
|
||||
|
||||
for joint in root.findall('joint'):
|
||||
axis_elem = joint.find('axis')
|
||||
if axis_elem is not None:
|
||||
axis_str = axis_elem.attrib.get('xyz', '')
|
||||
try:
|
||||
x, y, z = map(float, axis_str.strip().split())
|
||||
rx, ry, rz = round_axis(x, y, z)
|
||||
axis_elem.set('xyz', f"{rx} {ry} {rz}")
|
||||
except:
|
||||
continue
|
||||
|
||||
tree.write(output_path)
|
||||
print(f"Rounded axes written to {output_path}")
|
||||
|
||||
# Example usage
|
||||
process_urdf("./urdf/sample.urdf", "./urdf/output.urdf")
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
import { URDFEditor } from './URDFEditor.js';
|
||||
|
||||
const canvas = document.getElementById('urdfCanvas');
|
||||
const editor = new URDFEditor(canvas, './urdf/sample.urdf');
|
||||
|
|
@ -0,0 +1,356 @@
|
|||
import * as THREE from 'three';
|
||||
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
|
||||
import URDFLoader from './urdf/URDFLoader.js';
|
||||
|
||||
// Setup scene, camera, renderer
|
||||
const canvas = document.getElementById('urdf-canvas');
|
||||
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||||
renderer.setSize(window.innerWidth, window.innerHeight);
|
||||
|
||||
const scene = new THREE.Scene();
|
||||
scene.background = new THREE.Color(0xaaaaaa);
|
||||
|
||||
const camera = new THREE.PerspectiveCamera(45, window.innerWidth / window.innerHeight, 0.1, 100);
|
||||
camera.position.set(2, 2, 2);
|
||||
camera.lookAt(0, 0, 0);
|
||||
|
||||
// Lights
|
||||
scene.add(new THREE.AmbientLight(0xffffff, 0.6));
|
||||
const directional = new THREE.DirectionalLight(0xffffff, 0.8);
|
||||
directional.position.set(15, 10, 7);
|
||||
const directional2 = new THREE.DirectionalLight(0xffffff, 0.8);
|
||||
directional2.position.set(-15, 10, -7);
|
||||
scene.add(directional2);
|
||||
|
||||
// Controls
|
||||
const controls = new OrbitControls(camera, renderer.domElement);
|
||||
controls.target.set(0, 0.5, 0);
|
||||
controls.update();
|
||||
|
||||
// Helpers
|
||||
scene.add(new THREE.AxesHelper(1));
|
||||
const gridHelper = new THREE.GridHelper(2, 10);
|
||||
//gridHelper.rotation.x = Math.PI / 2;
|
||||
gridHelper.position.z = 0;
|
||||
scene.add(gridHelper);
|
||||
|
||||
// Raycasting
|
||||
const raycaster = new THREE.Raycaster();
|
||||
const mouse = new THREE.Vector2();
|
||||
let hoveredJoint = null;
|
||||
let isDragging = false;
|
||||
let lastX = null;
|
||||
let worldAxis = null;
|
||||
|
||||
let draggedJoint = null;
|
||||
let arc = null;
|
||||
const jointAngles = {}; // key: jointName, value: current angle in radians
|
||||
|
||||
|
||||
|
||||
|
||||
function findJointAncestor(object) {
|
||||
while (object && object.parent) {
|
||||
if (object.type === 'URDFJoint') return object;
|
||||
object = object.parent;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
function createRotationArc(axis, lower, upper, radius = 0.1, segments = 32) {
|
||||
const points = [];
|
||||
console.log(axis);
|
||||
console.log(axis.clone());
|
||||
// ✅ Ensure axis is normalized
|
||||
const normalizedAxis = axis.clone().normalize();
|
||||
|
||||
// ✅ Find a perpendicular vector
|
||||
let perp = new THREE.Vector3(0, 1, 0);
|
||||
if (Math.abs(normalizedAxis.dot(perp)) > 0.99) {
|
||||
perp = new THREE.Vector3(1, 0, 0); // fallback if axis is nearly parallel to Y
|
||||
}
|
||||
const tangent = perp.cross(normalizedAxis).normalize();
|
||||
|
||||
for (let i = 0; i <= segments; i++) {
|
||||
const angle = lower + (upper - lower) * (i / segments);
|
||||
const point = tangent.clone().applyAxisAngle(normalizedAxis, angle).multiplyScalar(radius);
|
||||
points.push(point);
|
||||
}
|
||||
|
||||
const geometry = new THREE.BufferGeometry().setFromPoints(points);
|
||||
const material = new THREE.LineBasicMaterial({ color: 0xffff00 });
|
||||
return new THREE.Line(geometry, material);
|
||||
}
|
||||
|
||||
function createRotationSector(axis, lower, upper, radius = 0.1, segments = 64) {
|
||||
const motorMargin = 0.4; // hardcoded extra range beyond joint limits
|
||||
const motorLower = lower - motorMargin;
|
||||
const motorUpper = upper + motorMargin;
|
||||
|
||||
const redMaterial = new THREE.MeshBasicMaterial({
|
||||
color: 0xff0000,
|
||||
transparent: true,
|
||||
opacity: 0.2,
|
||||
side: THREE.DoubleSide,
|
||||
depthTest: false,
|
||||
depthWrite: false
|
||||
});
|
||||
|
||||
const whiteMaterial = new THREE.MeshBasicMaterial({
|
||||
color: 0xffffff,
|
||||
transparent: true,
|
||||
opacity: 0.6,
|
||||
side: THREE.DoubleSide,
|
||||
depthTest: false,
|
||||
depthWrite: false
|
||||
});
|
||||
|
||||
function createArc(start, end, material) {
|
||||
const shape = new THREE.Shape();
|
||||
shape.moveTo(0, 0);
|
||||
for (let i = 0; i <= segments; i++) {
|
||||
const angle = start + (end - start) * (i / segments);
|
||||
shape.lineTo(Math.cos(angle) * radius, Math.sin(angle) * radius);
|
||||
}
|
||||
shape.lineTo(0, 0);
|
||||
const geometry = new THREE.ShapeGeometry(shape);
|
||||
return new THREE.Mesh(geometry, material);
|
||||
}
|
||||
|
||||
const redLeft = createArc(motorLower, lower, redMaterial);
|
||||
const whiteArc = createArc(lower, upper, whiteMaterial);
|
||||
const redRight = createArc(upper, motorUpper, redMaterial);
|
||||
|
||||
// Orient all arcs to match the axis
|
||||
const basis = new THREE.Matrix4();
|
||||
const up = new THREE.Vector3(0, 0, 1);
|
||||
const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
|
||||
basis.makeRotationFromQuaternion(quaternion);
|
||||
|
||||
redLeft.applyMatrix4(basis);
|
||||
whiteArc.applyMatrix4(basis);
|
||||
redRight.applyMatrix4(basis);
|
||||
|
||||
const group = new THREE.Group();
|
||||
group.add(redLeft);
|
||||
group.add(whiteArc);
|
||||
group.add(redRight);
|
||||
|
||||
return group;
|
||||
}
|
||||
|
||||
|
||||
function createAngleIndicator(axis, angle, radius = 0.1) {
|
||||
const dir = new THREE.Vector3(Math.cos(angle), Math.sin(angle), 0).multiplyScalar(radius);
|
||||
|
||||
const geometry = new THREE.BufferGeometry().setFromPoints([
|
||||
new THREE.Vector3(0, 0, 0),
|
||||
dir,
|
||||
]);
|
||||
|
||||
const material = new THREE.LineBasicMaterial({
|
||||
color: 0xffffff, // white
|
||||
linewidth: 2, // note: linewidth only works in some renderers
|
||||
depthTest: false, // ✅ always on top
|
||||
depthWrite: false
|
||||
});
|
||||
|
||||
const line = new THREE.Line(geometry, material);
|
||||
|
||||
// Rotate into axis plane
|
||||
const up = new THREE.Vector3(0, 0, 1);
|
||||
const quaternion = new THREE.Quaternion().setFromUnitVectors(up, axis.clone().normalize());
|
||||
line.applyQuaternion(quaternion);
|
||||
|
||||
return line;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Load URDF
|
||||
const loader = new URDFLoader();
|
||||
loader.packages = { '': './urdf/' };
|
||||
|
||||
function findObjectByName(root, name) {
|
||||
let result = null;
|
||||
root.traverse(child => {
|
||||
if (child.name === name) result = child;
|
||||
});
|
||||
return result;
|
||||
}
|
||||
|
||||
loader.load('./urdf/sample.urdf', robot => {
|
||||
//scene.rotation.x = -Math.PI / 2;
|
||||
robot.rotation.x = -Math.PI / 2;
|
||||
scene.add(robot);
|
||||
document.getElementById('status').textContent = 'URDF Loaded';
|
||||
|
||||
loader.parseVisual = true;
|
||||
loader.parseCollision = false;
|
||||
|
||||
// Add AxesHelpers to all meshes
|
||||
robot.traverse(obj => {
|
||||
if (obj.isMesh) obj.add(new THREE.AxesHelper(0.05));
|
||||
});
|
||||
|
||||
// Setup pointer events
|
||||
canvas.addEventListener('pointermove', event => {
|
||||
mouse.x = (event.clientX / canvas.clientWidth) * 2 - 1;
|
||||
mouse.y = -(event.clientY / canvas.clientHeight) * 2 + 1;
|
||||
|
||||
raycaster.setFromCamera(mouse, camera);
|
||||
const intersects = raycaster.intersectObjects(robot.children, true);
|
||||
|
||||
if (isDragging && draggedJoint && worldAxis) {
|
||||
const deltaX = event.clientX - lastX;
|
||||
lastX = event.clientX;
|
||||
const angleDelta = deltaX * 0.005;
|
||||
|
||||
const jointName = draggedJoint.name;
|
||||
const limits = robot.joints[jointName]?.limit;
|
||||
const lower = limits?.lower ?? -Math.PI;
|
||||
const upper = limits?.upper ?? Math.PI;
|
||||
|
||||
const currentAngle = jointAngles[jointName] ?? 0;
|
||||
const proposedAngle = currentAngle + angleDelta;
|
||||
|
||||
// ✅ Clamp to limits
|
||||
const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle));
|
||||
const actualDelta = clampedAngle - currentAngle;
|
||||
|
||||
// ✅ Apply rotation
|
||||
draggedJoint.rotateOnAxis(worldAxis, actualDelta);
|
||||
jointAngles[jointName] = clampedAngle;
|
||||
|
||||
|
||||
// Remove old indicator
|
||||
if (draggedJoint.userData.gizmo?.indicator) {
|
||||
draggedJoint.parent.remove(draggedJoint.userData.gizmo.indicator);
|
||||
}
|
||||
|
||||
// Add updated indicator
|
||||
const newIndicator = createAngleIndicator(worldAxis, jointAngles[jointName]);
|
||||
newIndicator.position.copy(draggedJoint.position);
|
||||
draggedJoint.parent.add(newIndicator);
|
||||
draggedJoint.userData.gizmo.indicator = newIndicator;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (intersects.length > 0) {
|
||||
const target = intersects[0].object;
|
||||
|
||||
// Reset previous highlight
|
||||
if (
|
||||
hoveredJoint &&
|
||||
hoveredJoint.material &&
|
||||
'emissive' in hoveredJoint.material &&
|
||||
typeof hoveredJoint.material.emissive.setHex === 'function'
|
||||
) {
|
||||
hoveredJoint.material.emissive.setHex(0x000000);
|
||||
}
|
||||
|
||||
hoveredJoint = target;
|
||||
|
||||
// Highlight new joint
|
||||
if (
|
||||
hoveredJoint.material &&
|
||||
'emissive' in hoveredJoint.material &&
|
||||
typeof hoveredJoint.material.emissive.setHex === 'function'
|
||||
) {
|
||||
hoveredJoint.material.emissive.setHex(0x333333);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (
|
||||
hoveredJoint &&
|
||||
hoveredJoint.material &&
|
||||
'emissive' in hoveredJoint.material &&
|
||||
typeof hoveredJoint.material.emissive.setHex === 'function'
|
||||
) {
|
||||
hoveredJoint.material.emissive.setHex(0x000000);
|
||||
}
|
||||
hoveredJoint = null;
|
||||
}
|
||||
|
||||
});
|
||||
|
||||
canvas.addEventListener('pointerdown', event => {
|
||||
if (!hoveredJoint) return;
|
||||
|
||||
isDragging = true;
|
||||
lastX = event.clientX;
|
||||
controls.enabled = false;
|
||||
|
||||
draggedJoint = findJointAncestor(hoveredJoint);
|
||||
|
||||
|
||||
if (!draggedJoint) return;
|
||||
|
||||
const jointName = draggedJoint.name;
|
||||
const jointData = robot.joints?.[jointName];
|
||||
|
||||
// ✅ Check if axis is a valid THREE.Vector3
|
||||
if (!(jointData?.axis instanceof THREE.Vector3)) {
|
||||
console.warn(`Invalid axis for joint "${jointName}":`, jointData?.axis);
|
||||
return;
|
||||
}
|
||||
|
||||
const urdfAxis = jointData.axis.clone();
|
||||
if (urdfAxis.lengthSq() === 0) {
|
||||
console.warn(`Zero-length axis for joint "${jointName}"`);
|
||||
return;
|
||||
}
|
||||
|
||||
worldAxis = urdfAxis.normalize();
|
||||
const limits = robot.joints[jointName]?.limit;
|
||||
const lower = limits?.lower ?? -Math.PI;
|
||||
const upper = limits?.upper ?? Math.PI;
|
||||
|
||||
const sector = createRotationSector(worldAxis, lower, upper);
|
||||
const indicator = createAngleIndicator(worldAxis, jointAngles[jointName] ?? 0);
|
||||
|
||||
sector.position.copy(draggedJoint.position);
|
||||
indicator.position.copy(draggedJoint.position);
|
||||
|
||||
draggedJoint.parent.add(sector);
|
||||
draggedJoint.parent.add(indicator);
|
||||
|
||||
// Store for cleanup
|
||||
draggedJoint.userData.gizmo = { sector, indicator };
|
||||
|
||||
|
||||
|
||||
|
||||
console.log(`Dragging joint "${jointName}" on axis`, worldAxis.toArray());
|
||||
});
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
canvas.addEventListener('pointerup', () => {
|
||||
isDragging = false;
|
||||
controls.enabled = true;
|
||||
const gizmo = draggedJoint?.userData.gizmo;
|
||||
if (gizmo) {
|
||||
draggedJoint.parent.remove(gizmo.sector);
|
||||
draggedJoint.parent.remove(gizmo.indicator);
|
||||
}
|
||||
|
||||
|
||||
draggedJoint = null;
|
||||
worldAxis = null;
|
||||
});
|
||||
|
||||
|
||||
animate();
|
||||
});
|
||||
|
||||
function animate() {
|
||||
requestAnimationFrame(animate);
|
||||
renderer.render(scene, camera);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,171 @@
|
|||
|
||||
|
||||
|
||||
class UIElement {
|
||||
constructor(x, y, w, h) {
|
||||
this.x = x; this.y = y;
|
||||
this.w = w; this.h = h;
|
||||
this.hovered = false;
|
||||
this.active = false;
|
||||
|
||||
}
|
||||
|
||||
contains(px, py) {
|
||||
return px >= this.x && px <= this.x + this.w &&
|
||||
py >= this.y && py <= this.y + this.h;
|
||||
}
|
||||
|
||||
handlePointerEvent(event, px, py) {
|
||||
switch (event.type) {
|
||||
case 'pointermove':
|
||||
this.onPointerMove(px, py);
|
||||
break;
|
||||
case 'pointerdown':
|
||||
this.onPointerDown(px, py);
|
||||
break;
|
||||
case 'pointerup':
|
||||
this.onPointerUp(px, py);
|
||||
break;
|
||||
case 'click':
|
||||
// optional: treat click as pointerup for convenience
|
||||
if (this.contains(px, py)) {
|
||||
this.onClick?.(); // if subclass defines onClick
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
onPointerMove(px, py) {
|
||||
this.hovered = this.contains(px, py);
|
||||
//console.log(this);
|
||||
}
|
||||
|
||||
onPointerDown(px, py) {
|
||||
if (this.contains(px, py)) {
|
||||
this.active = true;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
onPointerUp(px, py) {
|
||||
if (this.active && this.contains(px, py)) {
|
||||
this.active = false;
|
||||
return true;
|
||||
}
|
||||
this.active = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
draw(ctx) { /* override in subclasses */ }
|
||||
}
|
||||
|
||||
export class Panel extends UIElement {
|
||||
constructor(x, y, w, h, title = '') {
|
||||
super(x, y, w, h);
|
||||
this.title = title;
|
||||
this.elements = [];
|
||||
}
|
||||
|
||||
addElement(el) { this.elements.push(el); }
|
||||
|
||||
draw(ctx) {
|
||||
ctx.fillStyle = 'rgba(0,0,0,0.5)';
|
||||
ctx.fillRect(this.x, this.y, this.w, this.h);
|
||||
ctx.strokeStyle = '#fff';
|
||||
ctx.strokeRect(this.x, this.y, this.w, this.h);
|
||||
|
||||
if (this.title) {
|
||||
ctx.fillStyle = '#fff';
|
||||
ctx.font = '16px monospace';
|
||||
ctx.fillText(this.title, this.x + 10, this.y + 20);
|
||||
}
|
||||
|
||||
this.elements.forEach(el => el.draw(ctx));
|
||||
}
|
||||
|
||||
handlePointerEvent(event, px, py) {
|
||||
super.handlePointerEvent(event, px, py); // panel itself
|
||||
this.elements.forEach(el => el.handlePointerEvent(event, px, py));
|
||||
}
|
||||
|
||||
|
||||
handleKeyEvent(event) {
|
||||
this.elements.forEach(el => {
|
||||
if (el.onKeyDown) el.onKeyDown(event);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
export class Button extends UIElement {
|
||||
constructor(x, y, w, h, label, onClick) {
|
||||
super(x, y, w, h);
|
||||
this.label = label;
|
||||
this.onClick = onClick;
|
||||
}
|
||||
|
||||
draw(ctx) {
|
||||
// ctx.fillStyle = this.active ? '#225433' :
|
||||
// this.hovered ? '#666' : '#333';
|
||||
// ctx.fillRect(this.x, this.y, this.w, this.h);
|
||||
|
||||
ctx.fillStyle = '#fff';
|
||||
if (this.hovered) {
|
||||
ctx.fillStyle = '#666';
|
||||
|
||||
}
|
||||
ctx.font = '14px monospace';
|
||||
ctx.fillText(this.label, this.x + 10, this.y + this.h - 8);
|
||||
}
|
||||
|
||||
onPointerMove(px, py) {
|
||||
this.hovered = this.contains(px, py);
|
||||
//console.log(this);
|
||||
}
|
||||
|
||||
onPointerUp(px, py) {
|
||||
const clicked = super.onPointerUp(px, py);
|
||||
if (clicked && this.onClick) this.onClick();
|
||||
return clicked;
|
||||
}
|
||||
}
|
||||
|
||||
export class Textbox extends UIElement {
|
||||
constructor(x, y, w, h, initialValue = '') {
|
||||
super(x, y, w, h);
|
||||
this.value = initialValue;
|
||||
this.focused = false;
|
||||
}
|
||||
|
||||
draw(ctx) {
|
||||
ctx.fillStyle = '#000';
|
||||
ctx.fillRect(this.x, this.y, this.w, this.h);
|
||||
|
||||
ctx.strokeStyle = this.focused ? '#0f0' : '#aaa';
|
||||
ctx.strokeRect(this.x, this.y, this.w, this.h);
|
||||
|
||||
ctx.fillStyle = '#fff';
|
||||
ctx.font = '14px monospace';
|
||||
ctx.fillText(this.value, this.x + 5, this.y + this.h - 8);
|
||||
}
|
||||
|
||||
onPointerUp(px, py) {
|
||||
const clicked = super.onPointerUp(px, py);
|
||||
this.focused = clicked;
|
||||
return clicked;
|
||||
}
|
||||
|
||||
onKeyDown(event) {
|
||||
if (!this.focused) return;
|
||||
if (event.key >= '0' && event.key <= '9') {
|
||||
this.value += event.key;
|
||||
} else if (event.key === 'Backspace') {
|
||||
this.value = this.value.slice(0, -1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
import URDFLoader from './URDFLoader.js';
|
||||
|
||||
export default
|
||||
class ExtendedURDFLoader extends URDFLoader {
|
||||
constructor(manager) {
|
||||
super(manager);
|
||||
this.transmissionMap = {};
|
||||
}
|
||||
|
||||
parseTransmissions(xml) {
|
||||
const transmissions = xml.querySelectorAll('transmission');
|
||||
this.transmissionMap = {};
|
||||
|
||||
transmissions.forEach(trans => {
|
||||
const jointName = trans.querySelector('joint')?.getAttribute('name');
|
||||
const actuator = trans.querySelector('actuator');
|
||||
const reduction = parseFloat(actuator?.querySelector('mechanicalReduction')?.textContent ?? '1');
|
||||
const actuatorName = actuator?.getAttribute('name') ?? null;
|
||||
const encoderTicks = parseInt(actuator?.querySelector('encoderTicks')?.textContent ?? '4096');
|
||||
const encoderValidMin = parseInt(actuator?.querySelector('encoderValidMin')?.textContent ?? '0');
|
||||
const encoderValidMax = parseInt(actuator?.querySelector('encoderValidMax')?.textContent ?? '4095');
|
||||
const encoderRange = parseInt(actuator?.querySelector('encoderRange')?.textContent ?? '180');
|
||||
const motorID = parseInt(actuator?.querySelector('motorID')?.textContent ?? '47');
|
||||
|
||||
if (jointName) {
|
||||
this.transmissionMap[jointName] = {
|
||||
mechanicalReduction: reduction,
|
||||
actuatorName,
|
||||
encoderTicks,
|
||||
encoderValidMin,
|
||||
encoderValidMax,
|
||||
encoderRange,
|
||||
motorID
|
||||
};
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
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');
|
||||
if (parserError) {
|
||||
console.error('❌ XML parser error:', parserError.textContent);
|
||||
throw new Error('Invalid URDF XML: ' + parserError.textContent);
|
||||
}
|
||||
|
||||
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];
|
||||
}
|
||||
}
|
||||
|
||||
return robot;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,469 @@
|
|||
import { Euler, Object3D, Vector3, Quaternion, Matrix4 } from 'three';
|
||||
|
||||
const _tempAxis = new Vector3();
|
||||
const _tempEuler = new Euler();
|
||||
const _tempTransform = new Matrix4();
|
||||
const _tempOrigTransform = new Matrix4();
|
||||
const _tempQuat = new Quaternion();
|
||||
const _tempScale = new Vector3(1.0, 1.0, 1.0);
|
||||
const _tempPosition = new Vector3();
|
||||
|
||||
class URDFBase extends Object3D {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.urdfNode = null;
|
||||
this.urdfName = '';
|
||||
|
||||
}
|
||||
|
||||
copy(source, recursive) {
|
||||
|
||||
super.copy(source, recursive);
|
||||
|
||||
this.urdfNode = source.urdfNode;
|
||||
this.urdfName = source.urdfName;
|
||||
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFCollider extends URDFBase {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.isURDFCollider = true;
|
||||
this.type = 'URDFCollider';
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFVisual extends URDFBase {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.isURDFVisual = true;
|
||||
this.type = 'URDFVisual';
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFLink extends URDFBase {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.isURDFLink = true;
|
||||
this.type = 'URDFLink';
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFJoint extends URDFBase {
|
||||
|
||||
get jointType() {
|
||||
|
||||
return this._jointType;
|
||||
|
||||
}
|
||||
|
||||
set jointType(v) {
|
||||
|
||||
if (this.jointType === v) return;
|
||||
this._jointType = v;
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
switch (v) {
|
||||
|
||||
case 'fixed':
|
||||
this.jointValue = [];
|
||||
break;
|
||||
|
||||
case 'continuous':
|
||||
case 'revolute':
|
||||
case 'prismatic':
|
||||
this.jointValue = new Array(1).fill(0);
|
||||
break;
|
||||
|
||||
case 'planar':
|
||||
// Planar joints are, 3dof: position XY and rotation Z.
|
||||
this.jointValue = new Array(3).fill(0);
|
||||
this.axis = new Vector3(0, 0, 1);
|
||||
break;
|
||||
|
||||
case 'floating':
|
||||
this.jointValue = new Array(6).fill(0);
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
get angle() {
|
||||
|
||||
return this.jointValue[0];
|
||||
|
||||
}
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
|
||||
this.isURDFJoint = true;
|
||||
this.type = 'URDFJoint';
|
||||
|
||||
this.jointValue = null;
|
||||
this.jointType = 'fixed';
|
||||
this.axis = new Vector3(1, 0, 0);
|
||||
this.limit = { lower: 0, upper: 0 };
|
||||
this.ignoreLimits = false;
|
||||
|
||||
this.origPosition = null;
|
||||
this.origQuaternion = null;
|
||||
|
||||
this.mimicJoints = [];
|
||||
|
||||
}
|
||||
|
||||
/* Overrides */
|
||||
copy(source, recursive) {
|
||||
|
||||
super.copy(source, recursive);
|
||||
|
||||
this.jointType = source.jointType;
|
||||
this.axis = source.axis.clone();
|
||||
this.limit.lower = source.limit.lower;
|
||||
this.limit.upper = source.limit.upper;
|
||||
this.ignoreLimits = false;
|
||||
|
||||
this.jointValue = [...source.jointValue];
|
||||
|
||||
this.origPosition = source.origPosition ? source.origPosition.clone() : null;
|
||||
this.origQuaternion = source.origQuaternion ? source.origQuaternion.clone() : null;
|
||||
|
||||
this.mimicJoints = [...source.mimicJoints];
|
||||
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
/* Public Functions */
|
||||
/**
|
||||
* @param {...number|null} values The joint value components to set, optionally null for no-op
|
||||
* @returns {boolean} Whether the invocation of this function resulted in an actual change to the joint value
|
||||
*/
|
||||
setJointValue(...values) {
|
||||
|
||||
// Parse all incoming values into numbers except null, which we treat as a no-op for that value component.
|
||||
values = values.map(v => v === null ? null : parseFloat(v));
|
||||
|
||||
if (!this.origPosition || !this.origQuaternion) {
|
||||
|
||||
this.origPosition = this.position.clone();
|
||||
this.origQuaternion = this.quaternion.clone();
|
||||
|
||||
}
|
||||
|
||||
let didUpdate = false;
|
||||
|
||||
this.mimicJoints.forEach(joint => {
|
||||
|
||||
didUpdate = joint.updateFromMimickedJoint(...values) || didUpdate;
|
||||
|
||||
});
|
||||
|
||||
switch (this.jointType) {
|
||||
|
||||
case 'fixed': {
|
||||
|
||||
return didUpdate;
|
||||
|
||||
}
|
||||
case 'continuous':
|
||||
case 'revolute': {
|
||||
|
||||
let angle = values[0];
|
||||
if (angle == null) return didUpdate;
|
||||
if (angle === this.jointValue[0]) return didUpdate;
|
||||
|
||||
if (!this.ignoreLimits && this.jointType === 'revolute') {
|
||||
|
||||
angle = Math.min(this.limit.upper, angle);
|
||||
angle = Math.max(this.limit.lower, angle);
|
||||
|
||||
}
|
||||
|
||||
this.quaternion
|
||||
.setFromAxisAngle(this.axis, angle)
|
||||
.premultiply(this.origQuaternion);
|
||||
|
||||
if (this.jointValue[0] !== angle) {
|
||||
|
||||
this.jointValue[0] = angle;
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
|
||||
return didUpdate;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
case 'prismatic': {
|
||||
|
||||
let pos = values[0];
|
||||
if (pos == null) return didUpdate;
|
||||
if (pos === this.jointValue[0]) return didUpdate;
|
||||
|
||||
if (!this.ignoreLimits) {
|
||||
|
||||
pos = Math.min(this.limit.upper, pos);
|
||||
pos = Math.max(this.limit.lower, pos);
|
||||
|
||||
}
|
||||
|
||||
this.position.copy(this.origPosition);
|
||||
_tempAxis.copy(this.axis).applyEuler(this.rotation);
|
||||
this.position.addScaledVector(_tempAxis, pos);
|
||||
|
||||
if (this.jointValue[0] !== pos) {
|
||||
|
||||
this.jointValue[0] = pos;
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
|
||||
return didUpdate;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
case 'floating': {
|
||||
|
||||
// no-op if all values are identical to existing value or are null
|
||||
if (this.jointValue.every((value, index) => values[index] === value || values[index] === null)) return didUpdate;
|
||||
// Floating joints have six degrees of freedom: X, Y, Z, R, P, Y.
|
||||
this.jointValue[0] = values[0] !== null ? values[0] : this.jointValue[0];
|
||||
this.jointValue[1] = values[1] !== null ? values[1] : this.jointValue[1];
|
||||
this.jointValue[2] = values[2] !== null ? values[2] : this.jointValue[2];
|
||||
this.jointValue[3] = values[3] !== null ? values[3] : this.jointValue[3];
|
||||
this.jointValue[4] = values[4] !== null ? values[4] : this.jointValue[4];
|
||||
this.jointValue[5] = values[5] !== null ? values[5] : this.jointValue[5];
|
||||
|
||||
// Compose transform of joint origin and transform due to joint values
|
||||
_tempOrigTransform.compose(this.origPosition, this.origQuaternion, _tempScale);
|
||||
_tempQuat.setFromEuler(
|
||||
_tempEuler.set(
|
||||
this.jointValue[3],
|
||||
this.jointValue[4],
|
||||
this.jointValue[5],
|
||||
'XYZ',
|
||||
),
|
||||
);
|
||||
_tempPosition.set(this.jointValue[0], this.jointValue[1], this.jointValue[2]);
|
||||
_tempTransform.compose(_tempPosition, _tempQuat, _tempScale);
|
||||
|
||||
// Calcualte new transform
|
||||
_tempOrigTransform.premultiply(_tempTransform);
|
||||
this.position.setFromMatrixPosition(_tempOrigTransform);
|
||||
this.rotation.setFromRotationMatrix(_tempOrigTransform);
|
||||
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
case 'planar': {
|
||||
|
||||
// no-op if all values are identical to existing value or are null
|
||||
if (this.jointValue.every((value, index) => values[index] === value || values[index] === null)) return didUpdate;
|
||||
|
||||
this.jointValue[0] = values[0] !== null ? values[0] : this.jointValue[0];
|
||||
this.jointValue[1] = values[1] !== null ? values[1] : this.jointValue[1];
|
||||
this.jointValue[2] = values[2] !== null ? values[2] : this.jointValue[2];
|
||||
|
||||
// Compose transform of joint origin and transform due to joint values
|
||||
_tempOrigTransform.compose(this.origPosition, this.origQuaternion, _tempScale);
|
||||
_tempQuat.setFromAxisAngle(this.axis, this.jointValue[2]);
|
||||
_tempPosition.set(this.jointValue[0], this.jointValue[1], 0.0);
|
||||
_tempTransform.compose(_tempPosition, _tempQuat, _tempScale);
|
||||
|
||||
// Calculate new transform
|
||||
_tempOrigTransform.premultiply(_tempTransform);
|
||||
this.position.setFromMatrixPosition(_tempOrigTransform);
|
||||
this.rotation.setFromRotationMatrix(_tempOrigTransform);
|
||||
|
||||
this.matrixWorldNeedsUpdate = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return didUpdate;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFMimicJoint extends URDFJoint {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.type = 'URDFMimicJoint';
|
||||
this.mimicJoint = null;
|
||||
this.offset = 0;
|
||||
this.multiplier = 1;
|
||||
|
||||
}
|
||||
|
||||
updateFromMimickedJoint(...values) {
|
||||
|
||||
const modifiedValues = values.map(x => x * this.multiplier + this.offset);
|
||||
return super.setJointValue(...modifiedValues);
|
||||
|
||||
}
|
||||
|
||||
/* Overrides */
|
||||
copy(source, recursive) {
|
||||
|
||||
super.copy(source, recursive);
|
||||
|
||||
this.mimicJoint = source.mimicJoint;
|
||||
this.offset = source.offset;
|
||||
this.multiplier = source.multiplier;
|
||||
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
class URDFRobot extends URDFLink {
|
||||
|
||||
constructor(...args) {
|
||||
|
||||
super(...args);
|
||||
this.isURDFRobot = true;
|
||||
this.urdfNode = null;
|
||||
|
||||
this.urdfRobotNode = null;
|
||||
this.robotName = null;
|
||||
|
||||
this.links = null;
|
||||
this.joints = null;
|
||||
this.colliders = null;
|
||||
this.visual = null;
|
||||
this.frames = null;
|
||||
|
||||
}
|
||||
|
||||
copy(source, recursive) {
|
||||
|
||||
super.copy(source, recursive);
|
||||
|
||||
this.urdfRobotNode = source.urdfRobotNode;
|
||||
this.robotName = source.robotName;
|
||||
|
||||
this.links = {};
|
||||
this.joints = {};
|
||||
this.colliders = {};
|
||||
this.visual = {};
|
||||
|
||||
this.traverse(c => {
|
||||
|
||||
if (c.isURDFJoint && c.urdfName in source.joints) {
|
||||
|
||||
this.joints[c.urdfName] = c;
|
||||
|
||||
}
|
||||
|
||||
if (c.isURDFLink && c.urdfName in source.links) {
|
||||
|
||||
this.links[c.urdfName] = c;
|
||||
|
||||
}
|
||||
|
||||
if (c.isURDFCollider && c.urdfName in source.colliders) {
|
||||
|
||||
this.colliders[c.urdfName] = c;
|
||||
|
||||
}
|
||||
|
||||
if (c.isURDFVisual && c.urdfName in source.visual) {
|
||||
|
||||
this.visual[c.urdfName] = c;
|
||||
|
||||
}
|
||||
|
||||
});
|
||||
|
||||
// Repair mimic joint references once we've re-accumulated all our joint data
|
||||
for (const joint in this.joints) {
|
||||
this.joints[joint].mimicJoints = this.joints[joint].mimicJoints.map((mimicJoint) => this.joints[mimicJoint.name]);
|
||||
}
|
||||
|
||||
this.frames = {
|
||||
...this.colliders,
|
||||
...this.visual,
|
||||
...this.links,
|
||||
...this.joints,
|
||||
};
|
||||
|
||||
return this;
|
||||
|
||||
}
|
||||
|
||||
getFrame(name) {
|
||||
|
||||
return this.frames[name];
|
||||
|
||||
}
|
||||
|
||||
setJointValue(jointName, ...angle) {
|
||||
|
||||
const joint = this.joints[jointName];
|
||||
if (joint) {
|
||||
|
||||
return joint.setJointValue(...angle);
|
||||
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
setJointValues(values) {
|
||||
|
||||
let didChange = false;
|
||||
for (const name in values) {
|
||||
|
||||
const value = values[name];
|
||||
if (Array.isArray(value)) {
|
||||
|
||||
didChange = this.setJointValue(name, ...value) || didChange;
|
||||
|
||||
} else {
|
||||
|
||||
didChange = this.setJointValue(name, value) || didChange;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return didChange;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
export { URDFRobot, URDFLink, URDFJoint, URDFMimicJoint, URDFVisual, URDFCollider };
|
||||
|
|
@ -0,0 +1,330 @@
|
|||
import { Raycaster, Vector3, Plane, Vector2 } from 'three';
|
||||
|
||||
// Find the nearest parent that is a joint
|
||||
function isJoint(j) {
|
||||
|
||||
return j.isURDFJoint && j.jointType !== 'fixed';
|
||||
|
||||
};
|
||||
|
||||
function findNearestJoint(child) {
|
||||
|
||||
let curr = child;
|
||||
while (curr) {
|
||||
|
||||
if (isJoint(curr)) {
|
||||
|
||||
return curr;
|
||||
|
||||
}
|
||||
|
||||
curr = curr.parent;
|
||||
|
||||
}
|
||||
|
||||
return curr;
|
||||
|
||||
};
|
||||
|
||||
const prevHitPoint = new Vector3();
|
||||
const newHitPoint = new Vector3();
|
||||
const pivotPoint = new Vector3();
|
||||
const tempVector = new Vector3();
|
||||
const tempVector2 = new Vector3();
|
||||
const projectedStartPoint = new Vector3();
|
||||
const projectedEndPoint = new Vector3();
|
||||
const plane = new Plane();
|
||||
export class URDFDragControls {
|
||||
|
||||
constructor(scene) {
|
||||
|
||||
this.enabled = true;
|
||||
this.scene = scene;
|
||||
this.raycaster = new Raycaster();
|
||||
this.initialGrabPoint = new Vector3();
|
||||
|
||||
this.hitDistance = -1;
|
||||
this.hovered = null;
|
||||
this.manipulating = null;
|
||||
|
||||
}
|
||||
|
||||
update() {
|
||||
|
||||
const {
|
||||
raycaster,
|
||||
hovered,
|
||||
manipulating,
|
||||
scene,
|
||||
} = this;
|
||||
|
||||
if (manipulating) {
|
||||
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
let hoveredJoint = null;
|
||||
const intersections = raycaster.intersectObject(scene, true);
|
||||
if (intersections.length !== 0) {
|
||||
|
||||
const hit = intersections[0];
|
||||
this.hitDistance = hit.distance;
|
||||
hoveredJoint = findNearestJoint(hit.object);
|
||||
this.initialGrabPoint.copy(hit.point);
|
||||
|
||||
}
|
||||
|
||||
if (hoveredJoint !== hovered) {
|
||||
|
||||
if (hovered) {
|
||||
|
||||
this.onUnhover(hovered);
|
||||
|
||||
}
|
||||
|
||||
this.hovered = hoveredJoint;
|
||||
|
||||
if (hoveredJoint) {
|
||||
|
||||
this.onHover(hoveredJoint);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
updateJoint(joint, angle) {
|
||||
|
||||
joint.setJointValue(angle);
|
||||
|
||||
}
|
||||
|
||||
onDragStart(joint) {
|
||||
|
||||
}
|
||||
|
||||
onDragEnd(joint) {
|
||||
|
||||
}
|
||||
|
||||
onHover(joint) {
|
||||
|
||||
}
|
||||
|
||||
onUnhover(joint) {
|
||||
|
||||
}
|
||||
|
||||
getRevoluteDelta(joint, startPoint, endPoint) {
|
||||
|
||||
// set up the plane
|
||||
tempVector
|
||||
.copy(joint.axis)
|
||||
.transformDirection(joint.matrixWorld)
|
||||
.normalize();
|
||||
pivotPoint
|
||||
.set(0, 0, 0)
|
||||
.applyMatrix4(joint.matrixWorld);
|
||||
plane
|
||||
.setFromNormalAndCoplanarPoint(tempVector, pivotPoint);
|
||||
|
||||
// project the drag points onto the plane
|
||||
plane.projectPoint(startPoint, projectedStartPoint);
|
||||
plane.projectPoint(endPoint, projectedEndPoint);
|
||||
|
||||
// get the directions relative to the pivot
|
||||
projectedStartPoint.sub(pivotPoint);
|
||||
projectedEndPoint.sub(pivotPoint);
|
||||
|
||||
tempVector.crossVectors(projectedStartPoint, projectedEndPoint);
|
||||
|
||||
const direction = Math.sign(tempVector.dot(plane.normal));
|
||||
return direction * projectedEndPoint.angleTo(projectedStartPoint);
|
||||
|
||||
}
|
||||
|
||||
getPrismaticDelta(joint, startPoint, endPoint) {
|
||||
|
||||
tempVector.subVectors(endPoint, startPoint);
|
||||
plane
|
||||
.normal
|
||||
.copy(joint.axis)
|
||||
.transformDirection(joint.parent.matrixWorld)
|
||||
.normalize();
|
||||
|
||||
return tempVector.dot(plane.normal);
|
||||
|
||||
}
|
||||
|
||||
moveRay(toRay) {
|
||||
|
||||
const { raycaster, hitDistance, manipulating } = this;
|
||||
const { ray } = raycaster;
|
||||
|
||||
if (manipulating) {
|
||||
|
||||
ray.at(hitDistance, prevHitPoint);
|
||||
toRay.at(hitDistance, newHitPoint);
|
||||
|
||||
let delta = 0;
|
||||
if (manipulating.jointType === 'revolute' || manipulating.jointType === 'continuous') {
|
||||
|
||||
delta = this.getRevoluteDelta(manipulating, prevHitPoint, newHitPoint);
|
||||
|
||||
} else if (manipulating.jointType === 'prismatic') {
|
||||
|
||||
delta = this.getPrismaticDelta(manipulating, prevHitPoint, newHitPoint);
|
||||
|
||||
}
|
||||
|
||||
if (delta) {
|
||||
|
||||
this.updateJoint(manipulating, manipulating.angle + delta);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
this.raycaster.ray.copy(toRay);
|
||||
this.update();
|
||||
|
||||
}
|
||||
|
||||
setGrabbed(grabbed) {
|
||||
|
||||
const { hovered, manipulating } = this;
|
||||
|
||||
if (grabbed) {
|
||||
|
||||
if (manipulating !== null || hovered === null) {
|
||||
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
this.manipulating = hovered;
|
||||
this.onDragStart(hovered);
|
||||
|
||||
} else {
|
||||
|
||||
if (this.manipulating === null) {
|
||||
return;
|
||||
}
|
||||
|
||||
this.onDragEnd(this.manipulating);
|
||||
this.manipulating = null;
|
||||
this.update();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
export class PointerURDFDragControls extends URDFDragControls {
|
||||
|
||||
constructor(scene, camera, domElement) {
|
||||
|
||||
super(scene);
|
||||
this.camera = camera;
|
||||
this.domElement = domElement;
|
||||
|
||||
const raycaster = new Raycaster();
|
||||
const mouse = new Vector2();
|
||||
|
||||
function updateMouse(e) {
|
||||
|
||||
const rect = domElement.getBoundingClientRect();
|
||||
mouse.x = ((e.clientX - rect.left) / rect.width) * 2 - 1;
|
||||
mouse.y = -((e.clientY - rect.top) / rect.height) * 2 + 1;
|
||||
|
||||
}
|
||||
|
||||
this._mouseDown = e => {
|
||||
|
||||
updateMouse(e);
|
||||
raycaster.setFromCamera(mouse, this.camera);
|
||||
this.moveRay(raycaster.ray);
|
||||
this.setGrabbed(true);
|
||||
|
||||
};
|
||||
|
||||
this._mouseMove = e => {
|
||||
|
||||
updateMouse(e);
|
||||
raycaster.setFromCamera(mouse, this.camera);
|
||||
this.moveRay(raycaster.ray);
|
||||
|
||||
};
|
||||
|
||||
this._mouseUp = e => {
|
||||
|
||||
updateMouse(e);
|
||||
raycaster.setFromCamera(mouse, this.camera);
|
||||
this.moveRay(raycaster.ray);
|
||||
this.setGrabbed(false);
|
||||
|
||||
};
|
||||
|
||||
domElement.addEventListener('mousedown', this._mouseDown);
|
||||
domElement.addEventListener('mousemove', this._mouseMove);
|
||||
domElement.addEventListener('mouseup', this._mouseUp);
|
||||
|
||||
}
|
||||
|
||||
getRevoluteDelta(joint, startPoint, endPoint) {
|
||||
|
||||
const { camera, initialGrabPoint } = this;
|
||||
|
||||
// set up the plane
|
||||
tempVector
|
||||
.copy(joint.axis)
|
||||
.transformDirection(joint.matrixWorld)
|
||||
.normalize();
|
||||
pivotPoint
|
||||
.set(0, 0, 0)
|
||||
.applyMatrix4(joint.matrixWorld);
|
||||
plane
|
||||
.setFromNormalAndCoplanarPoint(tempVector, pivotPoint);
|
||||
|
||||
tempVector
|
||||
.copy(camera.position)
|
||||
.sub(initialGrabPoint)
|
||||
.normalize();
|
||||
|
||||
// if looking into the plane of rotation
|
||||
if (Math.abs(tempVector.dot(plane.normal)) > 0.3) {
|
||||
|
||||
return super.getRevoluteDelta(joint, startPoint, endPoint);
|
||||
|
||||
} else {
|
||||
|
||||
// get the up direction
|
||||
tempVector.set(0, 1, 0).transformDirection(camera.matrixWorld);
|
||||
|
||||
// get points projected onto the plane of rotation
|
||||
plane.projectPoint(startPoint, projectedStartPoint);
|
||||
plane.projectPoint(endPoint, projectedEndPoint);
|
||||
|
||||
tempVector.set(0, 0, -1).transformDirection(camera.matrixWorld);
|
||||
tempVector.cross(plane.normal);
|
||||
tempVector2.subVectors(endPoint, startPoint);
|
||||
|
||||
return tempVector.dot(tempVector2);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
dispose() {
|
||||
|
||||
const { domElement } = this;
|
||||
domElement.removeEventListener('mousedown', this._mouseDown);
|
||||
domElement.removeEventListener('mousemove', this._mouseMove);
|
||||
domElement.removeEventListener('mouseup', this._mouseUp);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,657 @@
|
|||
import * as THREE from 'three';
|
||||
import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
|
||||
import { ColladaLoader } from 'three/examples/jsm/loaders/ColladaLoader.js';
|
||||
import { URDFRobot, URDFJoint, URDFLink, URDFCollider, URDFVisual, URDFMimicJoint } from './URDFClasses.js';
|
||||
|
||||
/*
|
||||
Reference coordinate frames for THREE.js and ROS.
|
||||
Both coordinate systems are right handed so the URDF is instantiated without
|
||||
frame transforms. The resulting model can be rotated to rectify the proper up,
|
||||
right, and forward directions
|
||||
|
||||
THREE.js
|
||||
Y
|
||||
|
|
||||
|
|
||||
.-----X
|
||||
／
|
||||
Z
|
||||
|
||||
ROS URDf
|
||||
Z
|
||||
| X
|
||||
| ／
|
||||
Y-----.
|
||||
|
||||
*/
|
||||
|
||||
const tempQuaternion = new THREE.Quaternion();
|
||||
const tempEuler = new THREE.Euler();
|
||||
|
||||
// take a vector "x y z" and process it into
|
||||
// an array [x, y, z]
|
||||
function processTuple(val) {
|
||||
|
||||
if (!val) return [0, 0, 0];
|
||||
return val.trim().split(/\s+/g).map(num => parseFloat(num));
|
||||
|
||||
}
|
||||
|
||||
// applies a rotation a threejs object in URDF order
|
||||
function applyRotation(obj, rpy, additive = false) {
|
||||
|
||||
// if additive is true the rotation is applied in
|
||||
// addition to the existing rotation
|
||||
if (!additive) obj.rotation.set(0, 0, 0);
|
||||
|
||||
tempEuler.set(rpy[0], rpy[1], rpy[2], 'ZYX');
|
||||
tempQuaternion.setFromEuler(tempEuler);
|
||||
tempQuaternion.multiply(obj.quaternion);
|
||||
obj.quaternion.copy(tempQuaternion);
|
||||
|
||||
}
|
||||
|
||||
/* URDFLoader Class */
|
||||
// Loads and reads a URDF file into a THREEjs Object3D format
|
||||
export default
|
||||
class URDFLoader {
|
||||
|
||||
constructor(manager) {
|
||||
|
||||
this.manager = manager || THREE.DefaultLoadingManager;
|
||||
this.loadMeshCb = this.defaultMeshLoader.bind(this);
|
||||
this.parseVisual = true;
|
||||
this.parseCollision = false;
|
||||
this.packages = '';
|
||||
this.workingPath = '';
|
||||
this.fetchOptions = {};
|
||||
|
||||
}
|
||||
|
||||
/* Public API */
|
||||
loadAsync(urdf) {
|
||||
|
||||
return new Promise((resolve, reject) => {
|
||||
|
||||
this.load(urdf, resolve, null, reject);
|
||||
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
// urdf: The path to the URDF within the package OR absolute
|
||||
// onComplete: Callback that is passed the model once loaded
|
||||
load(urdf, onComplete, onProgress, onError) {
|
||||
|
||||
// Check if a full URI is specified before
|
||||
// prepending the package info
|
||||
const manager = this.manager;
|
||||
const workingPath = THREE.LoaderUtils.extractUrlBase(urdf);
|
||||
const urdfPath = this.manager.resolveURL(urdf);
|
||||
|
||||
manager.itemStart(urdfPath);
|
||||
|
||||
fetch(urdfPath, this.fetchOptions)
|
||||
.then(res => {
|
||||
|
||||
if (res.ok) {
|
||||
|
||||
if (onProgress) {
|
||||
|
||||
onProgress(null);
|
||||
|
||||
}
|
||||
return res.text();
|
||||
|
||||
} else {
|
||||
|
||||
throw new Error(`URDFLoader: Failed to load url '${ urdfPath }' with error code ${ res.status } : ${ res.statusText }.`);
|
||||
|
||||
}
|
||||
|
||||
})
|
||||
.then(data => {
|
||||
|
||||
const model = this.parse(data, this.workingPath || workingPath);
|
||||
onComplete(model);
|
||||
manager.itemEnd(urdfPath);
|
||||
|
||||
})
|
||||
.catch(e => {
|
||||
|
||||
if (onError) {
|
||||
|
||||
onError(e);
|
||||
|
||||
} else {
|
||||
|
||||
console.error('URDFLoader: Error loading file.', e);
|
||||
|
||||
}
|
||||
manager.itemError(urdfPath);
|
||||
manager.itemEnd(urdfPath);
|
||||
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
parse(content, workingPath = this.workingPath) {
|
||||
|
||||
const packages = this.packages;
|
||||
const loadMeshCb = this.loadMeshCb;
|
||||
const parseVisual = this.parseVisual;
|
||||
const parseCollision = this.parseCollision;
|
||||
const manager = this.manager;
|
||||
const linkMap = {};
|
||||
const jointMap = {};
|
||||
const materialMap = {};
|
||||
|
||||
// Resolves the path of mesh files
|
||||
function resolvePath(path) {
|
||||
|
||||
if (!/^package:\/\//.test(path)) {
|
||||
|
||||
return workingPath ? workingPath + path : path;
|
||||
|
||||
}
|
||||
|
||||
// Remove "package://" keyword and split meshPath at the first slash
|
||||
const [targetPkg, relPath] = path.replace(/^package:\/\//, '').split(/\/(.+)/);
|
||||
|
||||
if (typeof packages === 'string') {
|
||||
|
||||
// "pkg" is one single package
|
||||
if (packages.endsWith(targetPkg)) {
|
||||
|
||||
// "pkg" is the target package
|
||||
return packages + '/' + relPath;
|
||||
|
||||
} else {
|
||||
|
||||
// Assume "pkg" is the target package's parent directory
|
||||
return packages + '/' + targetPkg + '/' + relPath;
|
||||
|
||||
}
|
||||
|
||||
} else if (packages instanceof Function) {
|
||||
|
||||
return packages(targetPkg) + '/' + relPath;
|
||||
|
||||
} else if (typeof packages === 'object') {
|
||||
|
||||
// "pkg" is a map of packages
|
||||
if (targetPkg in packages) {
|
||||
|
||||
return packages[targetPkg] + '/' + relPath;
|
||||
|
||||
} else {
|
||||
|
||||
console.error(`URDFLoader : ${ targetPkg } not found in provided package list.`);
|
||||
return null;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Process the URDF text format
|
||||
function processUrdf(data) {
|
||||
|
||||
let children;
|
||||
if (data instanceof Document) {
|
||||
|
||||
children = [ ...data.children ];
|
||||
|
||||
} else if (data instanceof Element) {
|
||||
|
||||
children = [ data ];
|
||||
|
||||
} else {
|
||||
|
||||
const parser = new DOMParser();
|
||||
const urdf = parser.parseFromString(data, 'text/xml');
|
||||
children = [ ...urdf.children ];
|
||||
|
||||
}
|
||||
|
||||
const robotNode = children.filter(c => c.nodeName === 'robot').pop();
|
||||
return processRobot(robotNode);
|
||||
|
||||
}
|
||||
|
||||
// Process the <robot> node
|
||||
function processRobot(robot) {
|
||||
|
||||
const robotNodes = [ ...robot.children ];
|
||||
const links = robotNodes.filter(c => c.nodeName.toLowerCase() === 'link');
|
||||
const joints = robotNodes.filter(c => c.nodeName.toLowerCase() === 'joint');
|
||||
const materials = robotNodes.filter(c => c.nodeName.toLowerCase() === 'material');
|
||||
const obj = new URDFRobot();
|
||||
|
||||
obj.robotName = robot.getAttribute('name');
|
||||
obj.urdfRobotNode = robot;
|
||||
|
||||
// Create the <material> map
|
||||
materials.forEach(m => {
|
||||
|
||||
const name = m.getAttribute('name');
|
||||
materialMap[name] = processMaterial(m);
|
||||
|
||||
});
|
||||
|
||||
// Create the <link> map
|
||||
const visualMap = {};
|
||||
const colliderMap = {};
|
||||
links.forEach(l => {
|
||||
|
||||
const name = l.getAttribute('name');
|
||||
const isRoot = robot.querySelector(`child[link="${ name }"]`) === null;
|
||||
linkMap[name] = processLink(l, visualMap, colliderMap, isRoot ? obj : null);
|
||||
|
||||
});
|
||||
|
||||
// Create the <joint> map
|
||||
joints.forEach(j => {
|
||||
|
||||
const name = j.getAttribute('name');
|
||||
jointMap[name] = processJoint(j);
|
||||
|
||||
});
|
||||
|
||||
obj.joints = jointMap;
|
||||
obj.links = linkMap;
|
||||
obj.colliders = colliderMap;
|
||||
obj.visual = visualMap;
|
||||
|
||||
// Link up mimic joints
|
||||
const jointList = Object.values(jointMap);
|
||||
jointList.forEach(j => {
|
||||
|
||||
if (j instanceof URDFMimicJoint) {
|
||||
|
||||
jointMap[j.mimicJoint].mimicJoints.push(j);
|
||||
|
||||
}
|
||||
|
||||
});
|
||||
|
||||
// Detect infinite loops of mimic joints
|
||||
jointList.forEach(j => {
|
||||
|
||||
const uniqueJoints = new Set();
|
||||
const iterFunction = joint => {
|
||||
|
||||
if (uniqueJoints.has(joint)) {
|
||||
|
||||
throw new Error('URDFLoader: Detected an infinite loop of mimic joints.');
|
||||
|
||||
}
|
||||
|
||||
uniqueJoints.add(joint);
|
||||
joint.mimicJoints.forEach(j => {
|
||||
|
||||
iterFunction(j);
|
||||
|
||||
});
|
||||
|
||||
};
|
||||
|
||||
iterFunction(j);
|
||||
});
|
||||
|
||||
obj.frames = {
|
||||
...colliderMap,
|
||||
...visualMap,
|
||||
...linkMap,
|
||||
...jointMap,
|
||||
};
|
||||
|
||||
return obj;
|
||||
|
||||
}
|
||||
|
||||
// Process joint nodes and parent them
|
||||
function processJoint(joint) {
|
||||
|
||||
const children = [ ...joint.children ];
|
||||
const jointType = joint.getAttribute('type');
|
||||
|
||||
let obj;
|
||||
|
||||
const mimicTag = children.find(n => n.nodeName.toLowerCase() === 'mimic');
|
||||
if (mimicTag) {
|
||||
|
||||
obj = new URDFMimicJoint();
|
||||
obj.mimicJoint = mimicTag.getAttribute('joint');
|
||||
obj.multiplier = parseFloat(mimicTag.getAttribute('multiplier') || 1.0);
|
||||
obj.offset = parseFloat(mimicTag.getAttribute('offset') || 0.0);
|
||||
|
||||
} else {
|
||||
|
||||
obj = new URDFJoint();
|
||||
|
||||
}
|
||||
|
||||
obj.urdfNode = joint;
|
||||
obj.name = joint.getAttribute('name');
|
||||
obj.urdfName = obj.name;
|
||||
obj.jointType = jointType;
|
||||
|
||||
let parent = null;
|
||||
let child = null;
|
||||
let xyz = [0, 0, 0];
|
||||
let rpy = [0, 0, 0];
|
||||
|
||||
// Extract the attributes
|
||||
children.forEach(n => {
|
||||
|
||||
const type = n.nodeName.toLowerCase();
|
||||
if (type === 'origin') {
|
||||
|
||||
xyz = processTuple(n.getAttribute('xyz'));
|
||||
rpy = processTuple(n.getAttribute('rpy'));
|
||||
|
||||
} else if (type === 'child') {
|
||||
|
||||
child = linkMap[n.getAttribute('link')];
|
||||
|
||||
} else if (type === 'parent') {
|
||||
|
||||
parent = linkMap[n.getAttribute('link')];
|
||||
|
||||
} else if (type === 'limit') {
|
||||
|
||||
obj.limit.lower = parseFloat(n.getAttribute('lower') || obj.limit.lower);
|
||||
obj.limit.upper = parseFloat(n.getAttribute('upper') || obj.limit.upper);
|
||||
|
||||
}
|
||||
});
|
||||
|
||||
// Join the links
|
||||
parent.add(obj);
|
||||
obj.add(child);
|
||||
applyRotation(obj, rpy);
|
||||
obj.position.set(xyz[0], xyz[1], xyz[2]);
|
||||
|
||||
// Set up the rotate function
|
||||
const axisNode = children.filter(n => n.nodeName.toLowerCase() === 'axis')[0];
|
||||
|
||||
if (axisNode) {
|
||||
|
||||
const axisXYZ = axisNode.getAttribute('xyz').split(/\s+/g).map(num => parseFloat(num));
|
||||
obj.axis = new THREE.Vector3(axisXYZ[0], axisXYZ[1], axisXYZ[2]);
|
||||
obj.axis.normalize();
|
||||
|
||||
}
|
||||
|
||||
return obj;
|
||||
|
||||
}
|
||||
|
||||
// Process the <link> nodes
|
||||
function processLink(link, visualMap, colliderMap, target = null) {
|
||||
|
||||
if (target === null) {
|
||||
|
||||
target = new URDFLink();
|
||||
|
||||
}
|
||||
|
||||
const children = [ ...link.children ];
|
||||
target.name = link.getAttribute('name');
|
||||
target.urdfName = target.name;
|
||||
target.urdfNode = link;
|
||||
|
||||
if (parseVisual) {
|
||||
|
||||
const visualNodes = children.filter(n => n.nodeName.toLowerCase() === 'visual');
|
||||
visualNodes.forEach(vn => {
|
||||
|
||||
const v = processLinkElement(vn, materialMap);
|
||||
target.add(v);
|
||||
|
||||
if (vn.hasAttribute('name')) {
|
||||
|
||||
const name = vn.getAttribute('name');
|
||||
v.name = name;
|
||||
v.urdfName = name;
|
||||
visualMap[name] = v;
|
||||
|
||||
}
|
||||
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
if (parseCollision) {
|
||||
|
||||
const collisionNodes = children.filter(n => n.nodeName.toLowerCase() === 'collision');
|
||||
collisionNodes.forEach(cn => {
|
||||
|
||||
const c = processLinkElement(cn);
|
||||
target.add(c);
|
||||
|
||||
if (cn.hasAttribute('name')) {
|
||||
|
||||
const name = cn.getAttribute('name');
|
||||
c.name = name;
|
||||
c.urdfName = name;
|
||||
colliderMap[name] = c;
|
||||
|
||||
}
|
||||
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
return target;
|
||||
|
||||
}
|
||||
|
||||
function processMaterial(node) {
|
||||
|
||||
const matNodes = [ ...node.children ];
|
||||
const material = new THREE.MeshPhongMaterial();
|
||||
|
||||
material.name = node.getAttribute('name') || '';
|
||||
matNodes.forEach(n => {
|
||||
|
||||
const type = n.nodeName.toLowerCase();
|
||||
if (type === 'color') {
|
||||
|
||||
const rgba =
|
||||
n
|
||||
.getAttribute('rgba')
|
||||
.split(/\s/g)
|
||||
.map(v => parseFloat(v));
|
||||
|
||||
material.color.setRGB(rgba[0], rgba[1], rgba[2]);
|
||||
material.opacity = rgba[3];
|
||||
material.transparent = rgba[3] < 1;
|
||||
material.depthWrite = !material.transparent;
|
||||
|
||||
} else if (type === 'texture') {
|
||||
|
||||
// The URDF spec does not require that the <texture/> tag include
|
||||
// a filename attribute so skip loading the texture if not provided.
|
||||
const filename = n.getAttribute('filename');
|
||||
if (filename) {
|
||||
|
||||
const loader = new THREE.TextureLoader(manager);
|
||||
const filePath = resolvePath(filename);
|
||||
material.map = loader.load(filePath);
|
||||
material.map.colorSpace = THREE.SRGBColorSpace;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
});
|
||||
|
||||
return material;
|
||||
|
||||
}
|
||||
|
||||
// Process the visual and collision nodes into meshes
|
||||
function processLinkElement(vn, materialMap = {}) {
|
||||
|
||||
const isCollisionNode = vn.nodeName.toLowerCase() === 'collision';
|
||||
const children = [ ...vn.children ];
|
||||
let material = null;
|
||||
|
||||
// get the material first
|
||||
const materialNode = children.filter(n => n.nodeName.toLowerCase() === 'material')[0];
|
||||
if (materialNode) {
|
||||
|
||||
const name = materialNode.getAttribute('name');
|
||||
if (name && name in materialMap) {
|
||||
|
||||
material = materialMap[name];
|
||||
|
||||
} else {
|
||||
|
||||
material = processMaterial(materialNode);
|
||||
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
material = new THREE.MeshPhongMaterial();
|
||||
|
||||
}
|
||||
|
||||
const group = isCollisionNode ? new URDFCollider() : new URDFVisual();
|
||||
group.urdfNode = vn;
|
||||
|
||||
children.forEach(n => {
|
||||
|
||||
const type = n.nodeName.toLowerCase();
|
||||
if (type === 'geometry') {
|
||||
|
||||
const geoType = n.children[0].nodeName.toLowerCase();
|
||||
if (geoType === 'mesh') {
|
||||
|
||||
const filename = n.children[0].getAttribute('filename');
|
||||
const filePath = resolvePath(filename);
|
||||
|
||||
// file path is null if a package directory is not provided.
|
||||
if (filePath !== null) {
|
||||
|
||||
const scaleAttr = n.children[0].getAttribute('scale');
|
||||
if (scaleAttr) {
|
||||
|
||||
const scale = processTuple(scaleAttr);
|
||||
group.scale.set(scale[0], scale[1], scale[2]);
|
||||
|
||||
}
|
||||
|
||||
loadMeshCb(filePath, manager, (obj, err) => {
|
||||
|
||||
if (err) {
|
||||
|
||||
console.error('URDFLoader: Error loading mesh.', err);
|
||||
|
||||
} else if (obj) {
|
||||
|
||||
if (obj instanceof THREE.Mesh) {
|
||||
|
||||
obj.material = material;
|
||||
|
||||
}
|
||||
|
||||
// We don't expect non identity rotations or positions. In the case of
|
||||
// COLLADA files the model might come in with a custom scale for unit
|
||||
// conversion.
|
||||
obj.position.set(0, 0, 0);
|
||||
obj.quaternion.identity();
|
||||
group.add(obj);
|
||||
|
||||
}
|
||||
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
} else if (geoType === 'box') {
|
||||
|
||||
const primitiveModel = new THREE.Mesh();
|
||||
primitiveModel.geometry = new THREE.BoxGeometry(1, 1, 1);
|
||||
primitiveModel.material = material;
|
||||
|
||||
const size = processTuple(n.children[0].getAttribute('size'));
|
||||
primitiveModel.scale.set(size[0], size[1], size[2]);
|
||||
|
||||
group.add(primitiveModel);
|
||||
|
||||
} else if (geoType === 'sphere') {
|
||||
|
||||
const primitiveModel = new THREE.Mesh();
|
||||
primitiveModel.geometry = new THREE.SphereGeometry(1, 30, 30);
|
||||
primitiveModel.material = material;
|
||||
|
||||
const radius = parseFloat(n.children[0].getAttribute('radius')) || 0;
|
||||
primitiveModel.scale.set(radius, radius, radius);
|
||||
|
||||
group.add(primitiveModel);
|
||||
|
||||
} else if (geoType === 'cylinder') {
|
||||
|
||||
const primitiveModel = new THREE.Mesh();
|
||||
primitiveModel.geometry = new THREE.CylinderGeometry(1, 1, 1, 30);
|
||||
primitiveModel.material = material;
|
||||
|
||||
const radius = parseFloat(n.children[0].getAttribute('radius')) || 0;
|
||||
const length = parseFloat(n.children[0].getAttribute('length')) || 0;
|
||||
primitiveModel.scale.set(radius, length, radius);
|
||||
primitiveModel.rotation.set(Math.PI / 2, 0, 0);
|
||||
|
||||
group.add(primitiveModel);
|
||||
|
||||
}
|
||||
|
||||
} else if (type === 'origin') {
|
||||
|
||||
const xyz = processTuple(n.getAttribute('xyz'));
|
||||
const rpy = processTuple(n.getAttribute('rpy'));
|
||||
|
||||
group.position.set(xyz[0], xyz[1], xyz[2]);
|
||||
group.rotation.set(0, 0, 0);
|
||||
applyRotation(group, rpy);
|
||||
|
||||
}
|
||||
|
||||
});
|
||||
|
||||
return group;
|
||||
|
||||
}
|
||||
|
||||
return processUrdf(content);
|
||||
|
||||
}
|
||||
|
||||
// Default mesh loading function
|
||||
defaultMeshLoader(path, manager, done) {
|
||||
|
||||
if (/\.stl$/i.test(path)) {
|
||||
|
||||
const loader = new STLLoader(manager);
|
||||
loader.load(path, geom => {
|
||||
const mesh = new THREE.Mesh(geom, new THREE.MeshPhongMaterial());
|
||||
done(mesh);
|
||||
});
|
||||
|
||||
} else if (/\.dae$/i.test(path)) {
|
||||
|
||||
const loader = new ColladaLoader(manager);
|
||||
loader.load(path, dae => done(dae.scene));
|
||||
|
||||
} else {
|
||||
|
||||
console.warn(`URDFLoader: Could not load model at ${ path }.\nNo loader available`);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,446 @@
|
|||
<robot name="robot">
|
||||
<link name="base_footprint" />
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint" />
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.1 0.05 0.05" />
|
||||
</geometry>
|
||||
<material name="base_link-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.1 0.05 0.05" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_spine_1" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="spine_1" />
|
||||
<origin xyz="0 0 0.033737961440044734" rpy="0 0 0" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="spine_1">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.1 0.05 0.1" />
|
||||
</geometry>
|
||||
<material name="spine_1-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.1 0.05 0.1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_upper_arm_right" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="upper_arm_right" />
|
||||
<origin xyz="0.062354968072652533 0 0.13096467297466818" rpy="-3.141592653589793 -0.6586873842445309 -1.548624773543236e-16" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="upper_arm_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.06" />
|
||||
</geometry>
|
||||
<material name="upper_arm_right-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.06" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_lower_arm_right" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="lower_arm_right" />
|
||||
<origin xyz="0.10009022777221713 0 0.08243944985877427" rpy="-3.141592653589793 -0.6586873842445309 -2.496506045944997e-16" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="lower_arm_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.05" />
|
||||
</geometry>
|
||||
<material name="lower_arm_right-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.05" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_hand_right" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="hand_right" />
|
||||
<origin xyz="0.13276109632093022 0 0.03940068352661779" rpy="-3.141592653589793 -0.6586873842445309 -2.496506045944997e-16" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="hand_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03" />
|
||||
</geometry>
|
||||
<material name="hand_right-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="base_link_to_upper_arm_left" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="upper_arm_left" />
|
||||
<origin xyz="-0.062354968072652533 0 0.13096467297466818" rpy="-3.141592653589793 0.6586873842445309 -1.548624773543236e-16" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
|
||||
</joint>
|
||||
<link name="upper_arm_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry><box size="0.03 0.03 0.06" /></geometry>
|
||||
<material name="upper_arm_left-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry><box size="0.03 0.03 0.06" /></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="base_link_to_lower_arm_left" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="lower_arm_left" />
|
||||
<origin xyz="-0.10009022777221713 0 0.08243944985877427" rpy="-3.141592653589793 0.6586873842445309 -2.496506045944997e-16" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
|
||||
</joint>
|
||||
<link name="lower_arm_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<geometry><box size="0.03 0.03 0.05" /></geometry>
|
||||
<material name="lower_arm_left-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<geometry><box size="0.03 0.03 0.05" /></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="base_link_to_hand_left" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="hand_left" />
|
||||
<origin xyz="-0.13276109632093022 0 0.03940068352661779" rpy="-3.141592653589793 0.6586873842445309 -2.496506045944997e-16" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
|
||||
</joint>
|
||||
|
||||
<link name="hand_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<geometry><box size="0.03 0.03 0.03" /></geometry>
|
||||
<material name="hand_left-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<geometry><box size="0.03 0.03 0.03" /></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.0" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="base_link_to_upper_leg_right" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="upper_leg_right" />
|
||||
<origin xyz="0.027260906368426083 0 -0.0372788919866843" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="upper_leg_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07" />
|
||||
</geometry>
|
||||
<material name="upper_leg_right-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_lower_leg_right" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="lower_leg_right" />
|
||||
<origin xyz="0.027260906368426083 0 -0.11088915657500885" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="lower_leg_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07" />
|
||||
</geometry>
|
||||
<material name="lower_leg_right-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_lower_leg_right1" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="lower_leg_right1" />
|
||||
<origin xyz="0.027260906368426083 -0.004866298635659946 -0.17845456205258786" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="lower_leg_right1">
|
||||
<visual>
|
||||
<origin xyz="0 0.025 0.01" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.04 0.08 0.02" />
|
||||
</geometry>
|
||||
<material name="lower_leg_right1-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0.025 0.01" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.04 0.08 0.02" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0.025 0.01" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_link_to_upper_leg_left" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="upper_leg_left" />
|
||||
<origin xyz="-0.027260906368426083 0 -0.0372788919866843" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
|
||||
</joint>
|
||||
<link name="upper_leg_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry><box size="0.04 0.04 0.07" /></geometry>
|
||||
<material name="upper_leg_left-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry><box size="0.04 0.04 0.07" /></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="base_link_to_lower_leg_left" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="lower_leg_left" />
|
||||
<origin xyz="-0.027260906368426083 0 -0.11088915657500885" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
|
||||
</joint>
|
||||
<link name="lower_leg_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry><box size="0.04 0.04 0.07" /></geometry>
|
||||
<material name="lower_leg_left-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry><box size="0.04 0.04 0.07" /></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="base_link_to_lower_leg_left1" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="lower_leg_left1" />
|
||||
<origin xyz="-0.027260906368426083 -0.004866298635659946 -0.17845456205258786" rpy="3.141592653589793 2.220446049250313e-16 3.141592653589793" />
|
||||
<axis xyz="-1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
|
||||
</joint>
|
||||
<link name="lower_leg_left1">
|
||||
<visual>
|
||||
<origin xyz="0 0.025 0.01" rpy="0 0 0" />
|
||||
<geometry><box size="0.04 0.08 0.02" /></geometry>
|
||||
<material name="lower_leg_left1-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0.025 0.01" rpy="0 0 0" />
|
||||
<geometry><box size="0.04 0.08 0.02" /></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0.025 0.01" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_neck" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="neck" />
|
||||
<origin xyz="0 0 0.13701911638953843" rpy="0 0 0" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="neck">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03" />
|
||||
</geometry>
|
||||
<material name="neck-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_head" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="head" />
|
||||
<origin xyz="0 0 0.1835254369219812" rpy="0 0 0" />
|
||||
<axis xyz="1 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5" /> </joint>
|
||||
<link name="head">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.06 0.06 0.06" />
|
||||
</geometry>
|
||||
<material name="head-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.06 0.06 0.06" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,705 @@
|
|||
<robot name="robot">
|
||||
<link name="base_footprint"></link>
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint" />
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.1 0.05 0.05" />
|
||||
</geometry>
|
||||
<material name="base_link-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.1 0.05 0.05" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_link_to_base_link" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="base_link"/>
|
||||
<origin xyz="0 0 0.0337" rpy="0 0 0"/>
|
||||
<axis xyz="-0.9997 -0.0114 0.0229"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="upper_arm_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.06" />
|
||||
</geometry>
|
||||
<material name="upper_arm_right-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.06" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="upper_arm_right_to_lower_arm_right" type="revolute">
|
||||
<parent link="upper_arm_right"/>
|
||||
<child link="lower_arm_right"/>
|
||||
<origin xyz="0.0 0 .065" rpy="0 0 0"/>
|
||||
<axis xyz="-0.9997 -0.0114 0.0229"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
<link name="lower_arm_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.05" />
|
||||
</geometry>
|
||||
<material name="lower_arm_right-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.05" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="lower_arm_right_to_hand_right" type="revolute">
|
||||
<parent link="lower_arm_right"/>
|
||||
<child link="hand_right"/>
|
||||
<origin xyz="0 0 0.055" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
<link name="hand_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03" />
|
||||
</geometry>
|
||||
<material name="hand_right-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- Left Upper Arm -->
|
||||
<joint name="base_link_to_upper_arm_left" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="upper_arm_left"/>
|
||||
<origin xyz="-0.0624 0 0.0973" rpy="3.1416 0.6587 0"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="upper_arm_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.06"/>
|
||||
</geometry>
|
||||
<material name="upper_arm_left-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.06"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.03" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<!-- Left Lower Arm -->
|
||||
<joint name="upper_arm_left_to_lower_arm_left" type="revolute">
|
||||
<parent link="upper_arm_left"/>
|
||||
<child link="lower_arm_left"/>
|
||||
<origin xyz="0.0 0 0.065" rpy="0 0 0"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="lower_arm_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.05"/>
|
||||
</geometry>
|
||||
<material name="lower_arm_left-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.05"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.025" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<!-- Left Hand -->
|
||||
<joint name="lower_arm_left_to_hand_left" type="revolute">
|
||||
<parent link="lower_arm_left"/>
|
||||
<child link="hand_left"/>
|
||||
<origin xyz="0 0 0.055" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="hand_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03"/>
|
||||
</geometry>
|
||||
<material name="hand_left-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.015" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
|
||||
<joint name="base_link_to_upper_leg_right" type="revolute">
|
||||
<parent link="base_link" />
|
||||
<child link="upper_leg_right" />
|
||||
<origin xyz="0.035 0 -0.035" rpy="0 0 0" />
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/> </joint>
|
||||
|
||||
<link name="upper_leg_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07"/>
|
||||
</geometry>
|
||||
<material name="upper_leg_right-material">
|
||||
<color rgba="0.0021 0.0497 0.4851 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="upper_leg_right_to_lower_leg_right" type="revolute">
|
||||
<parent link="upper_leg_right"/>
|
||||
<child link="lower_leg_right"/>
|
||||
<origin xyz="0 0 -0.075" rpy="0 0 0"/>
|
||||
<axis xyz="-0.9997 -0.0114 0.0229"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="lower_leg_right">
|
||||
<visual>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07"/>
|
||||
</geometry>
|
||||
<material name="lower_leg_right-material">
|
||||
<color rgba="0.0021 0.0497 0.4851 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="lower_leg_right_to_foot_right" type="revolute">
|
||||
<parent link="lower_leg_right"/>
|
||||
<child link="foot_right"/>
|
||||
<origin xyz="0 0 -0.075" rpy="0 0 0"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="foot_right">
|
||||
<visual>
|
||||
<origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.08 0.02"/>
|
||||
</geometry>
|
||||
<material name="foot_right-material">
|
||||
<color rgba="0.0021 0.0497 0.4851 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.08 0.02"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
|
||||
|
||||
<!-- Left Upper Leg -->
|
||||
<joint name="base_link_to_upper_leg_left" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="upper_leg_left"/>
|
||||
<origin xyz="-0.035 0 -0.035" rpy="0 0 0"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="upper_leg_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07"/>
|
||||
</geometry>
|
||||
<material name="upper_leg_left-material">
|
||||
<color rgba="0.0021 0.0497 0.4851 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<!-- Left Lower Leg -->
|
||||
<joint name="upper_leg_left_to_lower_leg_left" type="revolute">
|
||||
<parent link="upper_leg_left"/>
|
||||
<child link="lower_leg_left"/>
|
||||
<origin xyz="0 0 -0.075" rpy="0 0 0"/>
|
||||
<axis xyz="-0.9997 -0.0114 0.0229"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="lower_leg_left">
|
||||
<visual>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07"/>
|
||||
</geometry>
|
||||
<material name="lower_leg_left-material">
|
||||
<color rgba="0.0021 0.0497 0.4851 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.04 0.07"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 -0.035" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<!-- Left Foot -->
|
||||
<joint name="lower_leg_left_to_foot_left" type="revolute">
|
||||
<parent link="lower_leg_left"/>
|
||||
<child link="foot_left"/>
|
||||
<origin xyz="0 0 -0.075" rpy="0 0 0"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="foot_left">
|
||||
<visual>
|
||||
<origin xyz="0 0.02 -0.01" rpy="0.5 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.08 0.02"/>
|
||||
</geometry>
|
||||
<material name="foot_left-material">
|
||||
<color rgba="0.0021 0.0497 0.4851 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.04 0.08 0.02"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="base_link_to_neck" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="neck"/>
|
||||
<origin xyz="0 0 0.12" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="neck">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03" />
|
||||
</geometry>
|
||||
<material name="neck-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.03 0.03 0.03" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="neck_to_head" type="revolute">
|
||||
<parent link="neck"/>
|
||||
<child link="head"/>
|
||||
<origin xyz="0 0 0.04" rpy="0 0 0"/>
|
||||
<axis xyz="-1 0 0"/>
|
||||
<limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.06 0.06 0.06" />
|
||||
</geometry>
|
||||
<material name="head-material">
|
||||
<color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<box size="0.06 0.06 0.06" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<transmission name="base_link_trans">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="base_link_to_base_link">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="spine_motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="upper_arm_left">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="base_link_to_upper_arm_left">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="spine_motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="lower_arm_left">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="upper_arm_left_to_lower_arm_left">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="spine_motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="hand_left">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lower_arm_left_to_hand_left">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="spine_motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="upper_arm_right">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="base_link_to_upper_arm_right">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="spine_motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="lower_arm_right">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="upper_arm_right_to_lower_arm_right">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="spine_motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="hand_right">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lower_arm_right_to_hand_right">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="spine_motor_1">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="upper_leg_right">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="base_link_to_upper_leg_right">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="upper_leg_motor_right">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="lower_leg_right">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="upper_leg_right_to_lower_leg_right">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="lower_leg_motor_right">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="foot_right">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lower_leg_right_to_foot_right">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="foot_motor_right">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="upper_leg_left">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="base_link_to_upper_leg_left">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="upper_leg_motor_left">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="lower_leg_left">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="upper_leg_left_to_lower_leg_left">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="lower_leg_motor_left">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="foot_left">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="lower_leg_left_to_foot_left">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="foot_motor_left">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="neck_base">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="base_link_to_neck">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="neck_motor_base">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>200</encoderValidMin>
|
||||
<encoderValidMax>3500</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="neck_tip">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="neck_to_head">
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="neck_motor_tip">
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
<hardwareInterface>PositionJointInterface</hardwareInterface>
|
||||
<encoderTicks>4096</encoderTicks>
|
||||
<encoderValidMin>500</encoderValidMin>
|
||||
<encoderValidMax>4095</encoderValidMax>
|
||||
<encoderRange>270</encoderRange>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
</robot>
|
||||
Loading…
Reference in New Issue