diff --git a/web/pose_3d.js b/web/pose_3d.js
index fbbe657..c4ef614 100644
--- a/web/pose_3d.js
+++ b/web/pose_3d.js
@@ -4,6 +4,8 @@
import * as THREE from 'three';
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
import { TransformControls } from 'three/addons/controls/TransformControls.js';
+import { STLLoader } from 'three/addons/loaders/STLLoader.js';
+import URDFLoader from 'three/addons/loaders/URDFLoader.js';
const $ = (id) => document.getElementById(id);
const POLL_MS = 1000;
@@ -81,6 +83,45 @@ const SHOULDER = {
left: new THREE.Vector3(0, 0.20, 0.60),
};
+// ----- URDF loader -----
+// Carica il modello G1 reale a partire dal URDF vendored sotto
+// /static/vendor/g1/. Le mesh STL referenziate nel URDF (path relativi tipo
+// "meshes/pelvis.STL") vengono risolti contro lo stesso prefisso.
+async function loadG1Urdf(scene) {
+ const loader = new URDFLoader();
+ loader.loadMeshCb = function (path, manager, onComplete) {
+ // path arrives like "meshes/pelvis.STL"; resolve to our static path
+ const url = `/static/vendor/g1/${path}`;
+ const stl = new STLLoader(manager);
+ stl.load(
+ url,
+ (geometry) => {
+ const mat = new THREE.MeshStandardMaterial({
+ color: 0x9099a3, metalness: 0.4, roughness: 0.6,
+ transparent: true, opacity: 0.85,
+ });
+ const mesh = new THREE.Mesh(geometry, mat);
+ onComplete(mesh);
+ },
+ undefined,
+ (err) => onComplete(null, err),
+ );
+ };
+ return new Promise((resolve, reject) => {
+ loader.load(
+ '/static/vendor/g1/g1.urdf',
+ (robot) => {
+ // G1 URDF uses Z-up too (Unitree convention) so no rotation needed
+ scene.add(robot);
+ console.log('[pose_3d] G1 URDF caricato:', robot.name || 'g1');
+ resolve(robot);
+ },
+ undefined,
+ (err) => reject(err),
+ );
+ });
+}
+
// ----- scene init -----
function initScene() {
const canvas = $('p3Canvas');
@@ -117,45 +158,12 @@ function initScene() {
frontArrow.scale.set(0.18, 0.045, 1); // più largo ma sottile, "etichetta a terra"
scene.add(frontArrow);
- // Body silhouette: torso + head + legs + shoulders
- const bodyMat = new THREE.MeshStandardMaterial({
- color: 0x4a5560, transparent: true, opacity: 0.6,
+ // Modello G1 reale via URDF (best-effort: se il loader fallisce, niente
+ // body — la tab funziona comunque coi soli polsi/UCS).
+ loadG1Urdf(scene).catch((e) => {
+ console.warn('[pose_3d] URDF G1 non caricato:', e.message);
});
- // Torso: 20cm x 40cm x 50cm, centered at TORSO_POS
- const torso = new THREE.Mesh(new THREE.BoxGeometry(0.20, 0.40, 0.50), bodyMat);
- torso.position.copy(TORSO_POS);
- scene.add(torso);
-
- // Head: sfera 12cm sopra il torso
- const head = new THREE.Mesh(
- new THREE.SphereGeometry(0.09, 24, 16), bodyMat,
- );
- head.position.set(TORSO_POS.x, TORSO_POS.y, TORSO_POS.z + 0.25 + 0.09);
- scene.add(head);
-
- // Hips (rectangle) just below torso
- const hips = new THREE.Mesh(new THREE.BoxGeometry(0.22, 0.36, 0.10), bodyMat);
- hips.position.set(TORSO_POS.x, TORSO_POS.y, TORSO_POS.z - 0.25 - 0.05);
- scene.add(hips);
-
- // Gambe: 2 cilindri verticali, da hip a Z=0
- const legGeom = new THREE.CylinderGeometry(0.06, 0.06, 0.55, 16);
- for (const ly of [-0.10, 0.10]) {
- const leg = new THREE.Mesh(legGeom, bodyMat);
- leg.rotation.x = Math.PI / 2; // asse cilindro su Z
- leg.position.set(TORSO_POS.x, TORSO_POS.y + ly, (TORSO_POS.z - 0.30 - 0.275));
- scene.add(leg);
- }
-
- // Spalle: 2 sferette in corrispondenza degli SHOULDER
- const shoulderGeom = new THREE.SphereGeometry(0.04, 16, 12);
- for (const side of ['right', 'left']) {
- const sh = new THREE.Mesh(shoulderGeom, bodyMat);
- sh.position.copy(SHOULDER[side]);
- scene.add(sh);
- }
-
// Arms
for (const side of ['right', 'left']) {
const wrist = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
diff --git a/web/vendor/g1/g1.urdf b/web/vendor/g1/g1.urdf
new file mode 100644
index 0000000..156a5fd
--- /dev/null
+++ b/web/vendor/g1/g1.urdf
@@ -0,0 +1,1476 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/web/vendor/g1/meshes/head_link.STL b/web/vendor/g1/meshes/head_link.STL
new file mode 100644
index 0000000..2ee5fba
Binary files /dev/null and b/web/vendor/g1/meshes/head_link.STL differ
diff --git a/web/vendor/g1/meshes/left_ankle_pitch_link.STL b/web/vendor/g1/meshes/left_ankle_pitch_link.STL
new file mode 100644
index 0000000..69de849
Binary files /dev/null and b/web/vendor/g1/meshes/left_ankle_pitch_link.STL differ
diff --git a/web/vendor/g1/meshes/left_ankle_roll_link.STL b/web/vendor/g1/meshes/left_ankle_roll_link.STL
new file mode 100644
index 0000000..8864e9f
Binary files /dev/null and b/web/vendor/g1/meshes/left_ankle_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/left_elbow_link.STL b/web/vendor/g1/meshes/left_elbow_link.STL
new file mode 100644
index 0000000..1a96d99
Binary files /dev/null and b/web/vendor/g1/meshes/left_elbow_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hand_index_0_link.STL b/web/vendor/g1/meshes/left_hand_index_0_link.STL
new file mode 100644
index 0000000..8069369
Binary files /dev/null and b/web/vendor/g1/meshes/left_hand_index_0_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hand_index_1_link.STL b/web/vendor/g1/meshes/left_hand_index_1_link.STL
new file mode 100644
index 0000000..89d231d
Binary files /dev/null and b/web/vendor/g1/meshes/left_hand_index_1_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hand_middle_0_link.STL b/web/vendor/g1/meshes/left_hand_middle_0_link.STL
new file mode 100644
index 0000000..8069369
Binary files /dev/null and b/web/vendor/g1/meshes/left_hand_middle_0_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hand_middle_1_link.STL b/web/vendor/g1/meshes/left_hand_middle_1_link.STL
new file mode 100644
index 0000000..89d231d
Binary files /dev/null and b/web/vendor/g1/meshes/left_hand_middle_1_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hand_palm_link.STL b/web/vendor/g1/meshes/left_hand_palm_link.STL
new file mode 100644
index 0000000..7d595ed
Binary files /dev/null and b/web/vendor/g1/meshes/left_hand_palm_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hand_thumb_0_link.STL b/web/vendor/g1/meshes/left_hand_thumb_0_link.STL
new file mode 100644
index 0000000..3028bb4
Binary files /dev/null and b/web/vendor/g1/meshes/left_hand_thumb_0_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hand_thumb_1_link.STL b/web/vendor/g1/meshes/left_hand_thumb_1_link.STL
new file mode 100644
index 0000000..d1c080c
Binary files /dev/null and b/web/vendor/g1/meshes/left_hand_thumb_1_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hand_thumb_2_link.STL b/web/vendor/g1/meshes/left_hand_thumb_2_link.STL
new file mode 100644
index 0000000..8b32e96
Binary files /dev/null and b/web/vendor/g1/meshes/left_hand_thumb_2_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hip_pitch_link.STL b/web/vendor/g1/meshes/left_hip_pitch_link.STL
new file mode 100644
index 0000000..5b751c7
Binary files /dev/null and b/web/vendor/g1/meshes/left_hip_pitch_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hip_roll_link.STL b/web/vendor/g1/meshes/left_hip_roll_link.STL
new file mode 100644
index 0000000..778437f
Binary files /dev/null and b/web/vendor/g1/meshes/left_hip_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/left_hip_yaw_link.STL b/web/vendor/g1/meshes/left_hip_yaw_link.STL
new file mode 100644
index 0000000..383093a
Binary files /dev/null and b/web/vendor/g1/meshes/left_hip_yaw_link.STL differ
diff --git a/web/vendor/g1/meshes/left_knee_link.STL b/web/vendor/g1/meshes/left_knee_link.STL
new file mode 100644
index 0000000..f2e98e5
Binary files /dev/null and b/web/vendor/g1/meshes/left_knee_link.STL differ
diff --git a/web/vendor/g1/meshes/left_rubber_hand.STL b/web/vendor/g1/meshes/left_rubber_hand.STL
new file mode 100644
index 0000000..c44830f
Binary files /dev/null and b/web/vendor/g1/meshes/left_rubber_hand.STL differ
diff --git a/web/vendor/g1/meshes/left_shoulder_pitch_link.STL b/web/vendor/g1/meshes/left_shoulder_pitch_link.STL
new file mode 100644
index 0000000..e698311
Binary files /dev/null and b/web/vendor/g1/meshes/left_shoulder_pitch_link.STL differ
diff --git a/web/vendor/g1/meshes/left_shoulder_roll_link.STL b/web/vendor/g1/meshes/left_shoulder_roll_link.STL
new file mode 100644
index 0000000..80bca84
Binary files /dev/null and b/web/vendor/g1/meshes/left_shoulder_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/left_shoulder_yaw_link.STL b/web/vendor/g1/meshes/left_shoulder_yaw_link.STL
new file mode 100644
index 0000000..281e699
Binary files /dev/null and b/web/vendor/g1/meshes/left_shoulder_yaw_link.STL differ
diff --git a/web/vendor/g1/meshes/left_wrist_pitch_link.STL b/web/vendor/g1/meshes/left_wrist_pitch_link.STL
new file mode 100644
index 0000000..82cc224
Binary files /dev/null and b/web/vendor/g1/meshes/left_wrist_pitch_link.STL differ
diff --git a/web/vendor/g1/meshes/left_wrist_roll_link.STL b/web/vendor/g1/meshes/left_wrist_roll_link.STL
new file mode 100644
index 0000000..f3c263a
Binary files /dev/null and b/web/vendor/g1/meshes/left_wrist_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/left_wrist_roll_rubber_hand.STL b/web/vendor/g1/meshes/left_wrist_roll_rubber_hand.STL
new file mode 100644
index 0000000..8fa435b
Binary files /dev/null and b/web/vendor/g1/meshes/left_wrist_roll_rubber_hand.STL differ
diff --git a/web/vendor/g1/meshes/left_wrist_yaw_link.STL b/web/vendor/g1/meshes/left_wrist_yaw_link.STL
new file mode 100644
index 0000000..31be4fd
Binary files /dev/null and b/web/vendor/g1/meshes/left_wrist_yaw_link.STL differ
diff --git a/web/vendor/g1/meshes/logo_link.STL b/web/vendor/g1/meshes/logo_link.STL
new file mode 100644
index 0000000..e979209
Binary files /dev/null and b/web/vendor/g1/meshes/logo_link.STL differ
diff --git a/web/vendor/g1/meshes/pelvis.STL b/web/vendor/g1/meshes/pelvis.STL
new file mode 100644
index 0000000..691a779
Binary files /dev/null and b/web/vendor/g1/meshes/pelvis.STL differ
diff --git a/web/vendor/g1/meshes/pelvis_contour_link.STL b/web/vendor/g1/meshes/pelvis_contour_link.STL
new file mode 100644
index 0000000..4243433
Binary files /dev/null and b/web/vendor/g1/meshes/pelvis_contour_link.STL differ
diff --git a/web/vendor/g1/meshes/right_ankle_pitch_link.STL b/web/vendor/g1/meshes/right_ankle_pitch_link.STL
new file mode 100644
index 0000000..e77d8a2
Binary files /dev/null and b/web/vendor/g1/meshes/right_ankle_pitch_link.STL differ
diff --git a/web/vendor/g1/meshes/right_ankle_roll_link.STL b/web/vendor/g1/meshes/right_ankle_roll_link.STL
new file mode 100644
index 0000000..d4261dd
Binary files /dev/null and b/web/vendor/g1/meshes/right_ankle_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/right_elbow_link.STL b/web/vendor/g1/meshes/right_elbow_link.STL
new file mode 100644
index 0000000..f259e38
Binary files /dev/null and b/web/vendor/g1/meshes/right_elbow_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hand_index_0_link.STL b/web/vendor/g1/meshes/right_hand_index_0_link.STL
new file mode 100644
index 0000000..f87ad32
Binary files /dev/null and b/web/vendor/g1/meshes/right_hand_index_0_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hand_index_1_link.STL b/web/vendor/g1/meshes/right_hand_index_1_link.STL
new file mode 100644
index 0000000..6dea51a
Binary files /dev/null and b/web/vendor/g1/meshes/right_hand_index_1_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hand_middle_0_link.STL b/web/vendor/g1/meshes/right_hand_middle_0_link.STL
new file mode 100644
index 0000000..f87ad32
Binary files /dev/null and b/web/vendor/g1/meshes/right_hand_middle_0_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hand_middle_1_link.STL b/web/vendor/g1/meshes/right_hand_middle_1_link.STL
new file mode 100644
index 0000000..6dea51a
Binary files /dev/null and b/web/vendor/g1/meshes/right_hand_middle_1_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hand_palm_link.STL b/web/vendor/g1/meshes/right_hand_palm_link.STL
new file mode 100644
index 0000000..5ae00a7
Binary files /dev/null and b/web/vendor/g1/meshes/right_hand_palm_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hand_thumb_0_link.STL b/web/vendor/g1/meshes/right_hand_thumb_0_link.STL
new file mode 100644
index 0000000..1cae7f1
Binary files /dev/null and b/web/vendor/g1/meshes/right_hand_thumb_0_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hand_thumb_1_link.STL b/web/vendor/g1/meshes/right_hand_thumb_1_link.STL
new file mode 100644
index 0000000..c141fbf
Binary files /dev/null and b/web/vendor/g1/meshes/right_hand_thumb_1_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hand_thumb_2_link.STL b/web/vendor/g1/meshes/right_hand_thumb_2_link.STL
new file mode 100644
index 0000000..e942923
Binary files /dev/null and b/web/vendor/g1/meshes/right_hand_thumb_2_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hip_pitch_link.STL b/web/vendor/g1/meshes/right_hip_pitch_link.STL
new file mode 100644
index 0000000..998a0a0
Binary files /dev/null and b/web/vendor/g1/meshes/right_hip_pitch_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hip_roll_link.STL b/web/vendor/g1/meshes/right_hip_roll_link.STL
new file mode 100644
index 0000000..47b2eeb
Binary files /dev/null and b/web/vendor/g1/meshes/right_hip_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/right_hip_yaw_link.STL b/web/vendor/g1/meshes/right_hip_yaw_link.STL
new file mode 100644
index 0000000..3718564
Binary files /dev/null and b/web/vendor/g1/meshes/right_hip_yaw_link.STL differ
diff --git a/web/vendor/g1/meshes/right_knee_link.STL b/web/vendor/g1/meshes/right_knee_link.STL
new file mode 100644
index 0000000..76d21a3
Binary files /dev/null and b/web/vendor/g1/meshes/right_knee_link.STL differ
diff --git a/web/vendor/g1/meshes/right_rubber_hand.STL b/web/vendor/g1/meshes/right_rubber_hand.STL
new file mode 100644
index 0000000..0aacffb
Binary files /dev/null and b/web/vendor/g1/meshes/right_rubber_hand.STL differ
diff --git a/web/vendor/g1/meshes/right_shoulder_pitch_link.STL b/web/vendor/g1/meshes/right_shoulder_pitch_link.STL
new file mode 100644
index 0000000..3f5b4ed
Binary files /dev/null and b/web/vendor/g1/meshes/right_shoulder_pitch_link.STL differ
diff --git a/web/vendor/g1/meshes/right_shoulder_roll_link.STL b/web/vendor/g1/meshes/right_shoulder_roll_link.STL
new file mode 100644
index 0000000..179d617
Binary files /dev/null and b/web/vendor/g1/meshes/right_shoulder_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/right_shoulder_yaw_link.STL b/web/vendor/g1/meshes/right_shoulder_yaw_link.STL
new file mode 100644
index 0000000..2ba6076
Binary files /dev/null and b/web/vendor/g1/meshes/right_shoulder_yaw_link.STL differ
diff --git a/web/vendor/g1/meshes/right_wrist_pitch_link.STL b/web/vendor/g1/meshes/right_wrist_pitch_link.STL
new file mode 100644
index 0000000..da19454
Binary files /dev/null and b/web/vendor/g1/meshes/right_wrist_pitch_link.STL differ
diff --git a/web/vendor/g1/meshes/right_wrist_roll_link.STL b/web/vendor/g1/meshes/right_wrist_roll_link.STL
new file mode 100644
index 0000000..26868d2
Binary files /dev/null and b/web/vendor/g1/meshes/right_wrist_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/right_wrist_roll_rubber_hand.STL b/web/vendor/g1/meshes/right_wrist_roll_rubber_hand.STL
new file mode 100644
index 0000000..c365aa9
Binary files /dev/null and b/web/vendor/g1/meshes/right_wrist_roll_rubber_hand.STL differ
diff --git a/web/vendor/g1/meshes/right_wrist_yaw_link.STL b/web/vendor/g1/meshes/right_wrist_yaw_link.STL
new file mode 100644
index 0000000..d788902
Binary files /dev/null and b/web/vendor/g1/meshes/right_wrist_yaw_link.STL differ
diff --git a/web/vendor/g1/meshes/torso_constraint_L_link.STL b/web/vendor/g1/meshes/torso_constraint_L_link.STL
new file mode 100644
index 0000000..75d82f5
Binary files /dev/null and b/web/vendor/g1/meshes/torso_constraint_L_link.STL differ
diff --git a/web/vendor/g1/meshes/torso_constraint_L_rod_link.STL b/web/vendor/g1/meshes/torso_constraint_L_rod_link.STL
new file mode 100644
index 0000000..6747f3f
Binary files /dev/null and b/web/vendor/g1/meshes/torso_constraint_L_rod_link.STL differ
diff --git a/web/vendor/g1/meshes/torso_constraint_R_link.STL b/web/vendor/g1/meshes/torso_constraint_R_link.STL
new file mode 100644
index 0000000..5cb5958
Binary files /dev/null and b/web/vendor/g1/meshes/torso_constraint_R_link.STL differ
diff --git a/web/vendor/g1/meshes/torso_constraint_R_rod_link.STL b/web/vendor/g1/meshes/torso_constraint_R_rod_link.STL
new file mode 100644
index 0000000..95cf415
Binary files /dev/null and b/web/vendor/g1/meshes/torso_constraint_R_rod_link.STL differ
diff --git a/web/vendor/g1/meshes/torso_link.STL b/web/vendor/g1/meshes/torso_link.STL
new file mode 100644
index 0000000..17745af
Binary files /dev/null and b/web/vendor/g1/meshes/torso_link.STL differ
diff --git a/web/vendor/g1/meshes/torso_link_23dof_rev_1_0.STL b/web/vendor/g1/meshes/torso_link_23dof_rev_1_0.STL
new file mode 100644
index 0000000..079aa81
Binary files /dev/null and b/web/vendor/g1/meshes/torso_link_23dof_rev_1_0.STL differ
diff --git a/web/vendor/g1/meshes/torso_link_rev_1_0.STL b/web/vendor/g1/meshes/torso_link_rev_1_0.STL
new file mode 100644
index 0000000..8a759a7
Binary files /dev/null and b/web/vendor/g1/meshes/torso_link_rev_1_0.STL differ
diff --git a/web/vendor/g1/meshes/waist_constraint_L.STL b/web/vendor/g1/meshes/waist_constraint_L.STL
new file mode 100644
index 0000000..911410f
Binary files /dev/null and b/web/vendor/g1/meshes/waist_constraint_L.STL differ
diff --git a/web/vendor/g1/meshes/waist_constraint_R.STL b/web/vendor/g1/meshes/waist_constraint_R.STL
new file mode 100644
index 0000000..babe794
Binary files /dev/null and b/web/vendor/g1/meshes/waist_constraint_R.STL differ
diff --git a/web/vendor/g1/meshes/waist_roll_link.STL b/web/vendor/g1/meshes/waist_roll_link.STL
new file mode 100644
index 0000000..65831ab
Binary files /dev/null and b/web/vendor/g1/meshes/waist_roll_link.STL differ
diff --git a/web/vendor/g1/meshes/waist_roll_link_rev_1_0.STL b/web/vendor/g1/meshes/waist_roll_link_rev_1_0.STL
new file mode 100644
index 0000000..a64f330
Binary files /dev/null and b/web/vendor/g1/meshes/waist_roll_link_rev_1_0.STL differ
diff --git a/web/vendor/g1/meshes/waist_support_link.STL b/web/vendor/g1/meshes/waist_support_link.STL
new file mode 100644
index 0000000..63660fb
Binary files /dev/null and b/web/vendor/g1/meshes/waist_support_link.STL differ
diff --git a/web/vendor/g1/meshes/waist_yaw_link.STL b/web/vendor/g1/meshes/waist_yaw_link.STL
new file mode 100644
index 0000000..7d36b02
Binary files /dev/null and b/web/vendor/g1/meshes/waist_yaw_link.STL differ
diff --git a/web/vendor/g1/meshes/waist_yaw_link_rev_1_0.STL b/web/vendor/g1/meshes/waist_yaw_link_rev_1_0.STL
new file mode 100644
index 0000000..0fabb63
Binary files /dev/null and b/web/vendor/g1/meshes/waist_yaw_link_rev_1_0.STL differ
diff --git a/web/vendor/three/addons/loaders/URDFClasses.js b/web/vendor/three/addons/loaders/URDFClasses.js
new file mode 100644
index 0000000..ad10e28
--- /dev/null
+++ b/web/vendor/three/addons/loaders/URDFClasses.js
@@ -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 };
diff --git a/web/vendor/three/addons/loaders/URDFLoader.js b/web/vendor/three/addons/loaders/URDFLoader.js
new file mode 100644
index 0000000..d5d8049
--- /dev/null
+++ b/web/vendor/three/addons/loaders/URDFLoader.js
@@ -0,0 +1,661 @@
+import * as THREE from 'three';
+import { STLLoader } from 'three/addons/loaders/STLLoader.js';
+import { URDFRobot, URDFJoint, URDFLink, URDFCollider, URDFVisual, URDFMimicJoint } from './URDFClasses.js';
+
+// ColladaLoader stubbed: G1 URDF uses only STL meshes, so we don't vendor the
+// full ColladaLoader. If a future URDF references .dae files the default
+// loadMeshCb will throw — supply a custom loadMeshCb in that case.
+const ColladaLoader = class { constructor() { throw new Error('ColladaLoader non disponibile (solo STL supportato)'); } };
+
+/*
+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 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 map
+ materials.forEach(m => {
+
+ const name = m.getAttribute('name');
+ materialMap[name] = processMaterial(m);
+
+ });
+
+ // Create the 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 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 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 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`);
+
+ }
+
+ }
+
+};