Head pose

reachy_mini_app_example/src/RobotManager.js

@@ -7,6 +7,10 @@ export class RobotManager {
         this.jointMap = {};
         this.statusCallback = statusCallback;
         this.logged = false;
+        this.clonedHead = null;
+        this.originalHead = null;
+        this.baseHeadPosition = new THREE.Vector3();
+        this.baseHeadRotation = new THREE.Euler();
         
         // URL configuration
         this.URDF_URL = new URLSearchParams(location.search).get("urdf")
@@ -65,15 +69,26 @@ export class RobotManager {
                         this.robot = urdfRobot;
                         this.robot.rotation.x = -Math.PI / 2; // Rotate 90 degrees around X axis
                         
-                        // Build joint map
+                        // Build joint map and log robot structure
+                        console.log('Robot structure:');
                         this.robot.traverse((child) => {
                             if (child.isURDFJoint) {
                                 this.jointMap[child.name] = child;
+                                console.log(`  Joint: ${child.name}`, child);
+                            }
+                            if (child.isURDFLink) {
+                                console.log(`  Link: ${child.name}`, child);
+                            }
+                            if (child.name && (child.name.includes('head') || child.name.includes('frame'))) {
+                                console.log(`  Found head/frame object: ${child.name}`, child);
                             }
                         });
                         
                         // Convert robot to blue wireframe after delay
-                        this.applyWireframeMaterial();
+                        //this.applyWireframeMaterial();
+                        
+                        // Clone head after robot is fully loaded
+                        this.cloneHeadForPoseControl();
                         
                         // Clean up the blob URL
                         URL.revokeObjectURL(blobUrl);
@@ -125,8 +140,41 @@ export class RobotManager {
         }
         
         // Update head pose if available
-        if (data.head_pose && this.jointMap['yaw_body']) {
-            if (data.head_pose.yaw !== undefined) {
+        if (data.head_pose) {
+            // Apply head pose transformations to the cloned head (if it exists)
+            if (this.clonedHead) {
+                // Apply transformations directly to the cloned head
+                // Apply pose relative to the base head position
+                if (data.head_pose.x !== undefined) {
+                    const newX = this.baseHeadPosition.x + data.head_pose.x - 0.01;
+                    this.clonedHead.position.x = newX;
+                }
+                if (data.head_pose.y !== undefined) {
+                    const newY = this.baseHeadPosition.y + data.head_pose.y;
+                    this.clonedHead.position.y = newY;
+                }
+                if (data.head_pose.z !== undefined) {
+                    const newZ = this.baseHeadPosition.z + data.head_pose.z + 0.03;
+                    this.clonedHead.position.z = newZ;
+                }
+                
+                // Apply rotations relative to base rotation
+                if (data.head_pose.roll !== undefined) {
+                    const newRoll = this.baseHeadRotation.x - data.head_pose.pitch;
+                    this.clonedHead.rotation.x = newRoll;
+                }
+                if (data.head_pose.pitch !== undefined) {
+                    const newPitch = this.baseHeadRotation.y - data.head_pose.yaw;
+                    this.clonedHead.rotation.y = newPitch;
+                }
+                if (data.head_pose.yaw !== undefined) {
+                    const newYaw = this.baseHeadRotation.z - data.head_pose.roll;
+                    this.clonedHead.rotation.z = newYaw;
+                }
+            }
+            
+            // Also update body yaw joint if it exists (for overall body orientation)
+            if (data.head_pose.yaw !== undefined && this.jointMap['yaw_body']) {
                 this.jointMap['yaw_body'].setJointValue(data.head_pose.yaw);
             }
         }
@@ -150,4 +198,81 @@ export class RobotManager {
     getRobot() {
         return this.robot;
     }
+    
+    cloneHeadForPoseControl() {
+        // Wait a bit for all STL meshes to finish loading
+        setTimeout(() => {
+            this.robot.traverse((child) => {
+                if ((child.name === 'xl_330')) {
+                    this.originalHead = child;
+                    
+                    // Clone the head link for independent control
+                    this.clonedHead = child.clone();
+                    this.clonedHead.name = 'cloned_head';
+                    
+                    // Position the cloned head at the original head's position relative to the robot
+                    const worldPosition = new THREE.Vector3();
+                    const worldRotation = new THREE.Quaternion();
+                    const worldScale = new THREE.Vector3();
+                    child.matrixWorld.decompose(worldPosition, worldRotation, worldScale);
+                    
+                    // Convert world position to robot local coordinates
+                    const robotInverseMatrix = new THREE.Matrix4().copy(this.robot.matrixWorld).invert();
+                    const localPosition = worldPosition.clone().applyMatrix4(robotInverseMatrix);
+                    
+                    // Set position relative to robot, not world
+                    this.clonedHead.position.copy(localPosition);
+                    
+                    // For rotation, we need to account for the robot's rotation
+                    const robotRotation = new THREE.Quaternion().setFromEuler(this.robot.rotation);
+                    const localRotation = worldRotation.clone().premultiply(robotRotation.invert());
+                    this.clonedHead.quaternion.copy(localRotation);
+                    this.clonedHead.scale.copy(worldScale);
+                    
+                    // Store the base position and rotation for relative pose control (in local robot coordinates)
+                    this.baseHeadPosition.copy(localPosition);
+                    this.baseHeadRotation.setFromQuaternion(localRotation);
+                    
+                    // Hide the original head and add the cloned head to the scene
+                    child.visible = false;
+                    this.robot.add(this.clonedHead);
+                    
+                    return; // Exit traverse after finding the head
+                }
+            });
+        }, 3000); // Wait 3 seconds for STL meshes to load
+    }
+    
+    debugHeadMovement() {
+        // Removed debug movement tracking for cleaner performance
+    }
+    
+    stopDebugHeadMovement() {
+        // Removed debug movement tracking for cleaner performance  
+    }
+    
+    // Debug function to manually test head positioning
+    testHeadMovement(x = 0, y = 0, z = 0, roll = 0, pitch = 0, yaw = 0) {
+        if (!this.clonedHead) {
+            console.warn('❌ Cloned head not available for testing');
+            return;
+        }
+        
+        // Simulate head pose data
+        const testData = {
+            head_pose: { x, y, z, roll, pitch, yaw }
+        };
+        
+        this.updateJoints(testData);
+    }
+    
+    // Reset head to base position
+    resetHeadPosition() {
+        if (!this.clonedHead) {
+            return;
+        }
+        
+        this.clonedHead.position.copy(this.baseHeadPosition);
+        this.clonedHead.rotation.setFromEuler(this.baseHeadRotation);
+    }
 }