all limbs added, still need various foot motor and ability to switch between multiple axes on a single joint (head and feet)

URDFEditor.js

@@ -119,14 +119,14 @@ export class URDFEditor {
 
     const urdfText = await fetch(this.urdfPath).then(res => res.text());
     const robot = await this.loader.loadFromString(urdfText);
-
+    //addOriginMarkers(robot);
     robot.rotation.x = -Math.PI / 2;
     this.scene.add(robot);
     this.robot = robot;
 
-    for (const jointName in this.robot.joints) {
+    // for (const jointName in this.robot.joints) {
-      console.log(jointName, this.robot.joints[jointName].transmission);
+    //   console.log(jointName, this.robot.joints[jointName].transmission);
-    }
+    // }
   }
 
 
@@ -162,31 +162,47 @@ export class URDFEditor {
       this.lastX = event.clientX;
       const angleDelta = deltaX * 0.005;
 
-      const jointName = this.draggedJoint.name;
+      let jointName = this.draggedJoint.name;
-
-      // ✅ Use transmission-derived limits if available
       const { lower, upper } = getJointLimits(jointName, this.robot);
 
       const currentAngle = this.jointAngles[jointName] ?? 0;
       const proposedAngle = currentAngle + angleDelta;
-
-      // ✅ Clamp to safe range
       const clampedAngle = Math.max(lower, Math.min(upper, proposedAngle));
       const actualDelta = clampedAngle - currentAngle;
 
+      // ✅ Rotate dragged joint
       this.draggedJoint.rotateOnAxis(this.worldAxis, actualDelta);
       this.jointAngles[jointName] = clampedAngle;
+      //console.log(jointName);
+      // ✅ Apply mimic relationships automatically
+      // ✅ Enforce mimics
+      for (const [name, jointObj] of Object.entries(this.robot.joints)) {
+        if (jointObj.type === 'URDFMimicJoint') {
+          const { mimicJoint, multiplier, offset } = jointObj;
+          if (mimicJoint === jointName) {
+            const mimicAngle = clampedAngle * multiplier + offset;
+            const { lower, upper } = getJointLimits(name, this.robot);
+            const mimicClamped = Math.max(lower, Math.min(upper, mimicAngle));
+            const mimicDelta = mimicClamped - (this.jointAngles[name] ?? 0);
+
+            const mimicNode = this.robot.getObjectByName(name);
+            mimicNode.rotateOnAxis(this.worldAxis, mimicDelta);
+            this.jointAngles[name] = mimicClamped;
+          }
+        }
+      }
 
       // ✅ Update gizmo indicator
       const gizmo = this.draggedJoint.userData.gizmo;
       if (gizmo?.indicator) this.draggedJoint.parent.remove(gizmo.indicator);
-
       const newIndicator = createAngleIndicator(this.worldAxis, clampedAngle);
       newIndicator.position.copy(this.draggedJoint.position);
       this.draggedJoint.parent.add(newIndicator);
       this.draggedJoint.userData.gizmo.indicator = newIndicator;
     }
 
+
+
     // Hover highlighting
     if (intersects.length > 0) {
       const target = intersects[0].object;
@@ -227,16 +243,24 @@ export class URDFEditor {
 
     // ✅ Use transmission-derived limits if available
     const { lower, upper } = getJointLimits(jointName, this.robot);
-    console.log(lower, upper);
+    //console.log(lower, upper);
 
     const jointTransmission = this.robot.joints[jointName].transmission;
-    if (!jointTransmission) return;
+    const jointMimic = this.robot.joints[jointName].mimic;
-    const encoderRange = jointTransmission.encoderRange / jointTransmission.mechanicalReduction;
+    console.log(this.robot.joints[jointName]);
+
+    // Bail only if neither transmission nor mimic
+    if (!jointTransmission && !jointMimic) return;
+
+    // If transmission exists, use its encoderRange
+    let encoderRange = 180; // sensible default
+    if (jointTransmission) {
+      encoderRange = jointTransmission.encoderRange / jointTransmission.mechanicalReduction;
+    }
+
     const sector = createRotationSector(this.worldAxis, lower, upper, encoderRange);
     const indicator = createAngleIndicator(this.worldAxis, this.jointAngles[jointName] ?? 0);
 
-    sector.position.copy(this.draggedJoint.position);
-    indicator.position.copy(this.draggedJoint.position);
 
     this.draggedJoint.parent.add(sector);
     this.draggedJoint.parent.add(indicator);
@@ -369,3 +393,20 @@ function loadObjWithMtl(objPath, mtlPath, onLoad) {
     });
   });
 }
+
+function addOriginMarkers(robot) {
+  console.log(robot.joints);
+  for (const [name, joint] of Object.entries(robot.joints)) {
+    const marker = new THREE.Mesh(
+      new THREE.SphereGeometry(0.01),
+      new THREE.MeshBasicMaterial({ color: 0x00ff00 })
+    );
+    joint.add(marker);
+
+    const axes = new THREE.AxesHelper(0.05);
+    joint.add(axes);
+
+    console.log("Added marker for joint:", name);
+  }
+
+}

robots/LittleSophia/urdf/LittleSophia.urdf

@@ -10,7 +10,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Torso.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Torso_.glb"
           scale="0.001 0.001 0.001" />
       </geometry>
       <material name="base_link-material">
@@ -161,14 +161,27 @@
   </link>
 
 
-  <joint name="upper_leg_right_to_lower_leg_right" type="revolute">
+  <!-- Upper leg to lower leg roll link (pivot at upper gear center) -->
+  <joint name="upper_leg_right_to_lower_leg_right_roll" type="revolute">
     <parent link="upper_leg_right" />
-    <child link="lower_leg_right" />
+    <child link="lower_leg_right_roll_link" />
-    <origin xyz="-0.001 0.0075 -0.0575" rpy="-0.14 0 0" />
+    <origin xyz="0.0 0.007 -0.028" rpy="0 0 0" /> <!-- upper gear center -->
-    <axis xyz="-1 0 0" />
+    <axis xyz="1 0 0" /> <!-- rolling axis -->
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
   </joint>
 
+  <link name="lower_leg_right_roll_link" />
+
+  <!-- Lower leg roll link to actual lower leg (pivot at lower gear center) -->
+  <joint name="lower_leg_roll_to_lower_leg_right" type="revolute">
+    <parent link="lower_leg_right_roll_link" />
+    <child link="lower_leg_right" />
+    <origin xyz="-0.001 -0.0025 -0.029" rpy="-0.35 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <mimic joint="upper_leg_right_to_lower_leg_right_roll" multiplier="1.1" offset="0" />
+  </joint>
+
 
   <link name="lower_leg_right">
     <visual>
@@ -195,19 +208,20 @@
   </link>
 
 
-  <joint name="lower_leg_right_to_foot_right" type="revolute">
+    <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" />
+    <origin xyz="0 0 -0.043" 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" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
       <geometry>
-        <box size="0.04 0.08 0.02" />
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Foot_Right.glb"
+          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="foot_right-material">
         <color rgba="0.0021 0.0497 0.4851 1" />
@@ -228,19 +242,19 @@
 
 
   <!-- Left Upper Leg -->
-  <joint name="base_link_to_upper_leg_left" type="revolute">
+  <joint name="base_link_to_hip_left" type="revolute">
     <parent link="base_link" />
-    <child link="upper_leg_left" />
+    <child link="hip_left" />
-    <origin xyz="-0.035 0 -0.035" rpy="0 0 0" />
+    <origin xyz="-0.024 0.005 -0.023" rpy="0 0 0" />
-    <axis xyz="1 0 0" />
+    <axis xyz="0 0 1" />
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
   </joint>
-
+  <link name="hip_left">
-  <link name="upper_leg_left">
     <visual>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
       <geometry>
-        <box size="0.04 0.04 0.07" />
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Hip_Left.glb"
+          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="upper_leg_left-material">
         <color rgba="0.0021 0.0497 0.4851 1" />
@@ -259,23 +273,22 @@
     </inertial>
   </link>
 
-
+  <joint name="hip_left_to_upper_leg_left" type="revolute">
-  <!-- Left Lower Leg -->
+    <parent link="hip_left" />
-  <joint name="upper_leg_left_to_lower_leg_left" type="revolute">
+    <child link="upper_leg_left" />
-    <parent link="upper_leg_left" />
+    <origin xyz="-0.009 0.0 -0.023" rpy="0 0 0" />
-    <child link="lower_leg_left" />
+    <axis xyz="1 0 0" />
-    <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">
+  <link name="upper_leg_left">
     <visual>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
       <geometry>
-        <box size="0.04 0.04 0.07" />
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Upper_Leg_Left.glb"
+          scale="0.001 0.001 0.001" />
       </geometry>
-      <material name="lower_leg_left-material">
+      <material name="upper_leg_left-material">
         <color rgba="0.0021 0.0497 0.4851 1" />
       </material>
     </visual>
@@ -292,21 +305,66 @@
     </inertial>
   </link>
 
+  <!-- Upper leg to lower leg roll link (pivot at upper gear center) -->
+  <joint name="upper_leg_left_to_lower_leg_left_roll" type="revolute">
+    <parent link="upper_leg_left" />
+    <child link="lower_leg_left_roll_link" />
+    <origin xyz="0.0 0.007 -0.028" rpy="0 0 0" /> <!-- upper gear center -->
+    <axis xyz="1 0 0" /> <!-- rolling axis -->
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+  </joint>
+
+  <link name="lower_leg_left_roll_link" />
+
+  <!-- Lower leg roll link to actual lower leg (pivot at lower gear center) -->
+  <joint name="lower_leg_roll_to_lower_leg_left" type="revolute">
+    <parent link="lower_leg_left_roll_link" />
+    <child link="lower_leg_left" />
+    <origin xyz="-0.001 -0.0025 -0.029" rpy="-0.35 0 0" />
+    <axis xyz="1 0 0" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <mimic joint="upper_leg_left_to_lower_leg_left_roll" multiplier="1.1" offset="0" />
+  </joint>
+
+
+  <link name="lower_leg_left">
+    <visual>
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <geometry>
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Lower_Leg_Left.glb"
+          scale="0.001 0.001 0.001" />
+      </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>
 
-  <!-- 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" />
+    <origin xyz="0 0 -0.043" 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" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
       <geometry>
-        <box size="0.04 0.08 0.02" />
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Foot_Left.glb"
+          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="foot_left-material">
         <color rgba="0.0021 0.0497 0.4851 1" />
@@ -455,7 +513,7 @@
     </actuator>
   </transmission>
 
-  
+
   <transmission name="hip_right">
     <type>transmission_interface/SimpleTransmission</type>
     <joint name="base_link_to_hip_right">
@@ -473,7 +531,7 @@
 
   <transmission name="upper_leg_right">
     <type>transmission_interface/SimpleTransmission</type>
-    <joint name="base_link_to_upper_leg_right">
+    <joint name="hip_right_to_upper_leg_right">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
     <actuator name="upper_leg_motor_right">
@@ -488,7 +546,7 @@
 
   <transmission name="lower_leg_right">
     <type>transmission_interface/SimpleTransmission</type>
-    <joint name="upper_leg_right_to_lower_leg_right">
+    <joint name="upper_leg_right_to_lower_leg_right_roll">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
     <actuator name="lower_leg_motor_right">
@@ -516,9 +574,24 @@
     </actuator>
   </transmission>
 
+  <transmission name="hip_left">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="base_link_to_hip_left">
+      <hardwareInterface>PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="hip_left">
+      <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">
+    <joint name="hip_left_to_upper_leg_left">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
     <actuator name="upper_leg_motor_left">
@@ -533,7 +606,7 @@
 
   <transmission name="lower_leg_left">
     <type>transmission_interface/SimpleTransmission</type>
-    <joint name="upper_leg_left_to_lower_leg_left">
+    <joint name="upper_leg_left_to_lower_leg_left_roll">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
     <actuator name="lower_leg_motor_left">
@@ -546,20 +619,6 @@
     </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>
   <!-- Yaw (pan) transmission -->
   <transmission name="neck_yaw_trans">
     <type>transmission_interface/SimpleTransmission</type>

urdf/ExtendedURDFLoader.js

@@ -1,7 +1,7 @@
 import URDFLoader from './URDFLoader.js';
 
 export default
-class ExtendedURDFLoader extends URDFLoader {
+  class ExtendedURDFLoader extends URDFLoader {
   constructor(manager) {
     super(manager);
     this.transmissionMap = {};
@@ -34,6 +34,28 @@ class ExtendedURDFLoader extends URDFLoader {
     });
   }
 
+  parseMimics(xml) {
+    const joints = xml.querySelectorAll('joint');
+    joints.forEach(jointEl => {
+      const jointName = jointEl.getAttribute('name');
+      const mimicEl = jointEl.querySelector('mimic');
+      if (mimicEl && jointName) {
+        const target = mimicEl.getAttribute('joint');
+        const multiplier = parseFloat(mimicEl.getAttribute('multiplier') ?? '1.0');
+        const offset = parseFloat(mimicEl.getAttribute('offset') ?? '0.0');
+
+        // Attach mimic info directly to the joint object
+        if (this.robot?.joints[jointName]) {
+          this.robot.joints[jointName].mimic = { target, multiplier, offset };
+        }
+      }
+    });
+
+    
+  }
+
+
+
   async loadFromString(urdfText, options = {}) {
     const xml = new DOMParser().parseFromString(urdfText, 'application/xml');
     const parserError = xml.querySelector('parsererror');
@@ -43,9 +65,10 @@ class ExtendedURDFLoader extends URDFLoader {
     }
 
     this.parseTransmissions(xml);
+    //this.parseMimics(xml);
 
     const robot = this.parse(xml); // ✅ Use inherited parse() directly
-
+console.log(robot.joints);
     for (const jointName in robot.joints) {
       if (this.transmissionMap[jointName]) {
         robot.joints[jointName].transmission = this.transmissionMap[jointName];