joint motion and indicators now match encoder max ticks, limits, and range, as well as mechanical reduction of joint

2025-11-18 11:49:13 +08:00 · 2025-11-18 11:49:13 +08:00 · 06a44e91eb
parent 682999ae01
commit 06a44e91eb
4 changed files with 100 additions and 46 deletions
--- a/JointVisualiser.js
+++ b/JointVisualiser.js
@ -1,9 +1,9 @@
 import * as THREE from 'three';

-export function createRotationSector(axis, lower, upper, radius = 0.1, segments = 64) {
+export function createRotationSector(axis, lower, upper, encoderRange, radius = 0.1, segments = 32) {
  const motorMargin = 0.4;
-  const motorLower = lower - motorMargin;
-  const motorUpper = upper + motorMargin;
+  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 });
--- a/URDFEditor.js
+++ b/URDFEditor.js
@ -1,7 +1,6 @@
-import * as THREE from '/urdfviewer/node_modules/three/build/three.module.js';
-import { OrbitControls } from '/urdfviewer/node_modules/three/examples/jsm/controls/OrbitControls.js';
-
-import URDFLoader from './urdf/ExtendedURDFLoader.js';
+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';

@ -118,19 +117,21 @@ export class URDFEditor {
      const angleDelta = deltaX * 0.005;

      const jointName = this.draggedJoint.name;
-      const limits = this.robot.joints[jointName]?.limit;
-      const lower = limits?.lower ?? -Math.PI;
-      const upper = limits?.upper ?? Math.PI;
+
+      // ✅ 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;

      this.draggedJoint.rotateOnAxis(this.worldAxis, actualDelta);
      this.jointAngles[jointName] = clampedAngle;

-
+      // ✅ Update gizmo indicator
      const gizmo = this.draggedJoint.userData.gizmo;
      if (gizmo?.indicator) this.draggedJoint.parent.remove(gizmo.indicator);

@ -140,17 +141,25 @@ export class URDFEditor {
      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);
+      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);
+      if (this.hoveredJoint.material?.emissive) {
+        this.hoveredJoint.material.emissive.setHex(0x333333);
+      }
    } else {
-      if (this.hoveredJoint?.material?.emissive) this.hoveredJoint.material.emissive.setHex(0x000000);
+      if (this.hoveredJoint?.material?.emissive) {
+        this.hoveredJoint.material.emissive.setHex(0x000000);
+      }
      this.hoveredJoint = null;
    }
  }

+
  onPointerDown(event) {
    if (!this.hoveredJoint) return;

@ -169,11 +178,14 @@ export class URDFEditor {
    if (axis.lengthSq() === 0) return;

    this.worldAxis = axis.normalize();
-    const limits = jointData.limit;
-    const lower = limits?.lower ?? -Math.PI;
-    const upper = limits?.upper ?? Math.PI;

-    const sector = createRotationSector(this.worldAxis, lower, upper);
+    // ✅ 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 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);
@ -184,6 +196,7 @@ export class URDFEditor {
    this.draggedJoint.userData.gizmo = { sector, indicator };
  }

+
  onPointerUp() {
    this.isDragging = false;
    this.controls.enabled = true;
@ -249,22 +262,46 @@ export class URDFEditor {

      const t = jointData.transmission;
      if (t) {
-        const actuatorAngleDeg = (degrees * t.gearRatio) + 4096/2;
-        const ticks = Math.round(actuatorAngleDeg);
+        //const actuatorAngleDeg = (degrees / t.mechanicalReduction) + 4096 / 2;
+        const ticks = radiansToTicks(angle, t).toFixed(0);
        const isValid = ticks >= t.encoderValidMin && ticks <= t.encoderValidMax;

        //ctx.fillStyle = isValid ? '#0f0' : '#f00';
        ctx.fillText(`${ticks}`, nameX + 250, y);
-
-
      }

      y += 18;
    }
  }
+}
+
+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;
+  //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;
+}
--- a/urdf/ExtendedURDFLoader.js
+++ b/urdf/ExtendedURDFLoader.js
@ -16,17 +16,19 @@ class ExtendedURDFLoader extends URDFLoader {
      const actuator = trans.querySelector('actuator');
      const reduction = parseFloat(actuator?.querySelector('mechanicalReduction')?.textContent ?? '1');
      const actuatorName = actuator?.getAttribute('name') ?? null;
-      const encoderMax = parseInt(actuator?.querySelector('encoderTicks')?.textContent ?? '4096');
+      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');

      if (jointName) {
        this.transmissionMap[jointName] = {
-          gearRatio: reduction,
+          mechanicalReduction: reduction,
          actuatorName,
-          encoderMax,
+          encoderTicks,
          encoderValidMin,
-          encoderValidMax
+          encoderValidMax,
+          encoderRange
        };
      }
    });
--- a/urdf/sample.urdf
+++ b/urdf/sample.urdf
@ -512,11 +512,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="spine_motor_1">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -526,11 +527,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="spine_motor_1">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -540,11 +542,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="spine_motor_1">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -554,11 +557,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="spine_motor_1">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -568,11 +572,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="spine_motor_1">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -582,11 +587,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="spine_motor_1">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -596,11 +602,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="spine_motor_1">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -610,11 +617,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="upper_leg_motor_right">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -624,11 +632,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="lower_leg_motor_right">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -638,11 +647,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="foot_motor_right">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -652,11 +662,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="upper_leg_motor_left">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -666,11 +677,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="lower_leg_motor_left">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>

@ -680,11 +692,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="foot_motor_left">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>
 <transmission name="neck_base">
@ -693,11 +706,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="neck_motor_base">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
    <encoderValidMin>200</encoderValidMin>
    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>
 <transmission name="neck_tip">
@ -706,11 +720,12 @@
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="neck_motor_tip">
-    <mechanicalReduction>22.75</mechanicalReduction>
+    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <encoderTicks>4096</encoderTicks>
-    <encoderValidMin>200</encoderValidMin>
-    <encoderValidMax>3500</encoderValidMax>
+    <encoderValidMin>500</encoderValidMin>
+    <encoderValidMax>4095</encoderValidMax>
+    <encoderRange>270</encoderRange>
  </actuator>
 </transmission>