From 40905cbb36a784c7f86bd5966418d263e8143525 Mon Sep 17 00:00:00 2001
From: Jake Wilkinson <realrobotshk@gmail.com>
Date: Sat, 22 Nov 2025 14:15:18 +0800
Subject: [PATCH] all major body parts moving, roll,pitch,yaw for ehad on ctrl
 & alt.

---
 URDFEditor.js                              | 64 ++++++++++--------
 robots/LittleSophia/urdf/LittleSophia.urdf | 75 +++++++++-------------
 2 files changed, 70 insertions(+), 69 deletions(-)

diff --git a/URDFEditor.js b/URDFEditor.js
index da09f9b..8ba14cf 100644
--- a/URDFEditor.js
+++ b/URDFEditor.js
@@ -229,8 +229,19 @@ export class URDFEditor {
     this.lastX = event.clientX;
     this.controls.enabled = false;
 
-    this.draggedJoint = this.findJointAncestor(this.hoveredJoint);
-    if (!this.draggedJoint) return;
+    // 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];
@@ -238,29 +249,19 @@ export class URDFEditor {
 
     const axis = jointData.axis.clone();
     if (axis.lengthSq() === 0) return;
-
     this.worldAxis = axis.normalize();
 
-    // ✅ Use transmission-derived limits if available
     const { lower, upper } = getJointLimits(jointName, this.robot);
-    //console.log(lower, upper);
-
-    const jointTransmission = this.robot.joints[jointName].transmission;
-    const jointMimic = this.robot.joints[jointName].mimic;
-    console.log(this.robot.joints[jointName]);
-
-    // Bail only if neither transmission nor mimic
-    if (!jointTransmission && !jointMimic) return;
-
-    // If transmission exists, use its encoderRange
-    let encoderRange = 180; // sensible default
-    if (jointTransmission) {
-      encoderRange = jointTransmission.encoderRange / jointTransmission.mechanicalReduction;
-    }
+    const 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);
@@ -268,6 +269,7 @@ export class URDFEditor {
   }
 
 
+
   onPointerUp() {
     this.isDragging = false;
     this.controls.enabled = true;
@@ -282,14 +284,23 @@ export class URDFEditor {
     this.worldAxis = null;
   }
 
-  findJointAncestor(object) {
-    while (object && object.parent) {
-      if (object.type === 'URDFJoint') return object;
-      object = object.parent;
+  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 null;
+
+    return current;
   }
 
+
   animate() {
     requestAnimationFrame(() => this.animate());
     this.drawJointOverlay(); // ✅ fixed overlay
@@ -326,8 +337,11 @@ export class URDFEditor {
 
       const rotatedChild = jointObject.children?.[0];
       const childName = rotatedChild?.name || '(unnamed)';
-
-      ctx.fillText(childName, nameX, y);
+      const motorName = jointData.transmission?.actuatorName || '(no motor)';
+      if (motorName == "(no motor)") {
+        continue;
+      }
+      ctx.fillText(motorName, nameX, y);
       const angleStr = `${degrees}°`;
       const angleWidth = ctx.measureText(angleStr).width;
       ctx.fillText(angleStr, angleX - angleWidth, y);
diff --git a/robots/LittleSophia/urdf/LittleSophia.urdf b/robots/LittleSophia/urdf/LittleSophia.urdf
index adee6ca..29eca95 100644
--- a/robots/LittleSophia/urdf/LittleSophia.urdf
+++ b/robots/LittleSophia/urdf/LittleSophia.urdf
@@ -1,11 +1,11 @@
 <robot name="robot">
   <link name="base_footprint"></link>
-  <joint name="base_joint" type="fixed">
+  <!-- <joint name="base_joint" type="fixed">
     <parent link="base_footprint" />
     <child link="base_link" />
     <origin xyz="0 0 0.11" rpy="0 0 0" />
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
-  </joint>
+  </joint> -->
   <link name="base_link">
     <visual>
       <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
@@ -208,7 +208,7 @@
   </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.043" rpy="0 0 0" />
@@ -359,6 +359,7 @@
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
   </joint>
 
+
   <link name="foot_left">
     <visual>
       <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
@@ -439,26 +440,12 @@
   </link>
 
 
-  <transmission name="upper_arm_left">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="base_joint">
-      <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">
+    <actuator name="upper_arm_left">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -467,28 +454,12 @@
       <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">
+    <actuator name="hand_left">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -503,7 +474,7 @@
     <joint name="base_link_to_upper_arm_right">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="spine_motor_1">
+    <actuator name="upper_arm_right">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -534,7 +505,7 @@
     <joint name="hip_right_to_upper_leg_right">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="upper_leg_motor_right">
+    <actuator name="upper_leg_right">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -549,7 +520,7 @@
     <joint name="upper_leg_right_to_lower_leg_right_roll">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="lower_leg_motor_right">
+    <actuator name="lower_leg_right">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -564,7 +535,7 @@
     <joint name="lower_leg_right_to_foot_right">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="foot_motor_right">
+    <actuator name="foot_right">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -594,7 +565,7 @@
     <joint name="hip_left_to_upper_leg_left">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="upper_leg_motor_left">
+    <actuator name="upper_leg_left">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -609,7 +580,23 @@
     <joint name="upper_leg_left_to_lower_leg_left_roll">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="lower_leg_motor_left">
+    <actuator name="lower_leg_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_left">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -625,7 +612,7 @@
     <joint name="base_link_to_neck_yaw">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="neck_motor_yaw">
+    <actuator name="neck_yaw">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -641,7 +628,7 @@
     <joint name="neck_pitch">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="neck_motor_pitch">
+    <actuator name="neck_pitch">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>
@@ -657,7 +644,7 @@
     <joint name="neck_roll">
       <hardwareInterface>PositionJointInterface</hardwareInterface>
     </joint>
-    <actuator name="neck_motor_roll">
+    <actuator name="neck_roll">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
       <encoderTicks>4096</encoderTicks>