both ways sync working (on most motors)

node_modules/.vite/deps/_metadata.json

@@ -2,48 +2,54 @@
   "hash": "0ef1605c",
   "configHash": "9f008819",
   "lockfileHash": "a7e0c386",
-  "browserHash": "46120af3",
+  "browserHash": "ad1ae0f3",
   "optimized": {
     "three": {
       "src": "../../three/build/three.module.js",
       "file": "three.js",
-      "fileHash": "e74de389",
+      "fileHash": "62948a0b",
       "needsInterop": false
     },
     "three/examples/jsm/controls/OrbitControls.js": {
       "src": "../../three/examples/jsm/controls/OrbitControls.js",
       "file": "three_examples_jsm_controls_OrbitControls__js.js",
-      "fileHash": "b543a1a7",
+      "fileHash": "4cdf108d",
       "needsInterop": false
     },
     "three/examples/jsm/loaders/ColladaLoader.js": {
       "src": "../../three/examples/jsm/loaders/ColladaLoader.js",
       "file": "three_examples_jsm_loaders_ColladaLoader__js.js",
-      "fileHash": "e901d7aa",
+      "fileHash": "674d2c11",
       "needsInterop": false
     },
     "three/examples/jsm/loaders/GLTFLoader.js": {
       "src": "../../three/examples/jsm/loaders/GLTFLoader.js",
       "file": "three_examples_jsm_loaders_GLTFLoader__js.js",
-      "fileHash": "21897848",
+      "fileHash": "767c6c15",
       "needsInterop": false
     },
     "three/examples/jsm/loaders/MTLLoader.js": {
       "src": "../../three/examples/jsm/loaders/MTLLoader.js",
       "file": "three_examples_jsm_loaders_MTLLoader__js.js",
-      "fileHash": "dd3cf025",
+      "fileHash": "c6b19fb3",
       "needsInterop": false
     },
     "three/examples/jsm/loaders/OBJLoader.js": {
       "src": "../../three/examples/jsm/loaders/OBJLoader.js",
       "file": "three_examples_jsm_loaders_OBJLoader__js.js",
-      "fileHash": "2e8bd98d",
+      "fileHash": "7344f204",
       "needsInterop": false
     },
     "three/examples/jsm/loaders/STLLoader.js": {
       "src": "../../three/examples/jsm/loaders/STLLoader.js",
       "file": "three_examples_jsm_loaders_STLLoader__js.js",
-      "fileHash": "82f84286",
+      "fileHash": "544799f5",
+      "needsInterop": false
+    },
+    "three/src/math/MathUtils.js": {
+      "src": "../../three/src/math/MathUtils.js",
+      "file": "three_src_math_MathUtils__js.js",
+      "fileHash": "a7ee4346",
       "needsInterop": false
     }
   },

node_modules/.vite/deps/three_src_math_MathUtils__js.js

@@ -0,0 +1,438 @@
+// node_modules/three/src/utils.js
+var _setConsoleFunction = null;
+function warn(...params) {
+  const message = "THREE." + params.shift();
+  if (_setConsoleFunction) {
+    _setConsoleFunction("warn", message, ...params);
+  } else {
+    console.warn(message, ...params);
+  }
+}
+
+// node_modules/three/src/math/MathUtils.js
+var _lut = ["00", "01", "02", "03", "04", "05", "06", "07", "08", "09", "0a", "0b", "0c", "0d", "0e", "0f", "10", "11", "12", "13", "14", "15", "16", "17", "18", "19", "1a", "1b", "1c", "1d", "1e", "1f", "20", "21", "22", "23", "24", "25", "26", "27", "28", "29", "2a", "2b", "2c", "2d", "2e", "2f", "30", "31", "32", "33", "34", "35", "36", "37", "38", "39", "3a", "3b", "3c", "3d", "3e", "3f", "40", "41", "42", "43", "44", "45", "46", "47", "48", "49", "4a", "4b", "4c", "4d", "4e", "4f", "50", "51", "52", "53", "54", "55", "56", "57", "58", "59", "5a", "5b", "5c", "5d", "5e", "5f", "60", "61", "62", "63", "64", "65", "66", "67", "68", "69", "6a", "6b", "6c", "6d", "6e", "6f", "70", "71", "72", "73", "74", "75", "76", "77", "78", "79", "7a", "7b", "7c", "7d", "7e", "7f", "80", "81", "82", "83", "84", "85", "86", "87", "88", "89", "8a", "8b", "8c", "8d", "8e", "8f", "90", "91", "92", "93", "94", "95", "96", "97", "98", "99", "9a", "9b", "9c", "9d", "9e", "9f", "a0", "a1", "a2", "a3", "a4", "a5", "a6", "a7", "a8", "a9", "aa", "ab", "ac", "ad", "ae", "af", "b0", "b1", "b2", "b3", "b4", "b5", "b6", "b7", "b8", "b9", "ba", "bb", "bc", "bd", "be", "bf", "c0", "c1", "c2", "c3", "c4", "c5", "c6", "c7", "c8", "c9", "ca", "cb", "cc", "cd", "ce", "cf", "d0", "d1", "d2", "d3", "d4", "d5", "d6", "d7", "d8", "d9", "da", "db", "dc", "dd", "de", "df", "e0", "e1", "e2", "e3", "e4", "e5", "e6", "e7", "e8", "e9", "ea", "eb", "ec", "ed", "ee", "ef", "f0", "f1", "f2", "f3", "f4", "f5", "f6", "f7", "f8", "f9", "fa", "fb", "fc", "fd", "fe", "ff"];
+var _seed = 1234567;
+var DEG2RAD = Math.PI / 180;
+var RAD2DEG = 180 / Math.PI;
+function generateUUID() {
+  const d0 = Math.random() * 4294967295 | 0;
+  const d1 = Math.random() * 4294967295 | 0;
+  const d2 = Math.random() * 4294967295 | 0;
+  const d3 = Math.random() * 4294967295 | 0;
+  const uuid = _lut[d0 & 255] + _lut[d0 >> 8 & 255] + _lut[d0 >> 16 & 255] + _lut[d0 >> 24 & 255] + "-" + _lut[d1 & 255] + _lut[d1 >> 8 & 255] + "-" + _lut[d1 >> 16 & 15 | 64] + _lut[d1 >> 24 & 255] + "-" + _lut[d2 & 63 | 128] + _lut[d2 >> 8 & 255] + "-" + _lut[d2 >> 16 & 255] + _lut[d2 >> 24 & 255] + _lut[d3 & 255] + _lut[d3 >> 8 & 255] + _lut[d3 >> 16 & 255] + _lut[d3 >> 24 & 255];
+  return uuid.toLowerCase();
+}
+function clamp(value, min, max) {
+  return Math.max(min, Math.min(max, value));
+}
+function euclideanModulo(n, m) {
+  return (n % m + m) % m;
+}
+function mapLinear(x, a1, a2, b1, b2) {
+  return b1 + (x - a1) * (b2 - b1) / (a2 - a1);
+}
+function inverseLerp(x, y, value) {
+  if (x !== y) {
+    return (value - x) / (y - x);
+  } else {
+    return 0;
+  }
+}
+function lerp(x, y, t) {
+  return (1 - t) * x + t * y;
+}
+function damp(x, y, lambda, dt) {
+  return lerp(x, y, 1 - Math.exp(-lambda * dt));
+}
+function pingpong(x, length = 1) {
+  return length - Math.abs(euclideanModulo(x, length * 2) - length);
+}
+function smoothstep(x, min, max) {
+  if (x <= min) return 0;
+  if (x >= max) return 1;
+  x = (x - min) / (max - min);
+  return x * x * (3 - 2 * x);
+}
+function smootherstep(x, min, max) {
+  if (x <= min) return 0;
+  if (x >= max) return 1;
+  x = (x - min) / (max - min);
+  return x * x * x * (x * (x * 6 - 15) + 10);
+}
+function randInt(low, high) {
+  return low + Math.floor(Math.random() * (high - low + 1));
+}
+function randFloat(low, high) {
+  return low + Math.random() * (high - low);
+}
+function randFloatSpread(range) {
+  return range * (0.5 - Math.random());
+}
+function seededRandom(s) {
+  if (s !== void 0) _seed = s;
+  let t = _seed += 1831565813;
+  t = Math.imul(t ^ t >>> 15, t | 1);
+  t ^= t + Math.imul(t ^ t >>> 7, t | 61);
+  return ((t ^ t >>> 14) >>> 0) / 4294967296;
+}
+function degToRad(degrees) {
+  return degrees * DEG2RAD;
+}
+function radToDeg(radians) {
+  return radians * RAD2DEG;
+}
+function isPowerOfTwo(value) {
+  return (value & value - 1) === 0 && value !== 0;
+}
+function ceilPowerOfTwo(value) {
+  return Math.pow(2, Math.ceil(Math.log(value) / Math.LN2));
+}
+function floorPowerOfTwo(value) {
+  return Math.pow(2, Math.floor(Math.log(value) / Math.LN2));
+}
+function setQuaternionFromProperEuler(q, a, b, c, order) {
+  const cos = Math.cos;
+  const sin = Math.sin;
+  const c2 = cos(b / 2);
+  const s2 = sin(b / 2);
+  const c13 = cos((a + c) / 2);
+  const s13 = sin((a + c) / 2);
+  const c1_3 = cos((a - c) / 2);
+  const s1_3 = sin((a - c) / 2);
+  const c3_1 = cos((c - a) / 2);
+  const s3_1 = sin((c - a) / 2);
+  switch (order) {
+    case "XYX":
+      q.set(c2 * s13, s2 * c1_3, s2 * s1_3, c2 * c13);
+      break;
+    case "YZY":
+      q.set(s2 * s1_3, c2 * s13, s2 * c1_3, c2 * c13);
+      break;
+    case "ZXZ":
+      q.set(s2 * c1_3, s2 * s1_3, c2 * s13, c2 * c13);
+      break;
+    case "XZX":
+      q.set(c2 * s13, s2 * s3_1, s2 * c3_1, c2 * c13);
+      break;
+    case "YXY":
+      q.set(s2 * c3_1, c2 * s13, s2 * s3_1, c2 * c13);
+      break;
+    case "ZYZ":
+      q.set(s2 * s3_1, s2 * c3_1, c2 * s13, c2 * c13);
+      break;
+    default:
+      warn("MathUtils: .setQuaternionFromProperEuler() encountered an unknown order: " + order);
+  }
+}
+function denormalize(value, array) {
+  switch (array.constructor) {
+    case Float32Array:
+      return value;
+    case Uint32Array:
+      return value / 4294967295;
+    case Uint16Array:
+      return value / 65535;
+    case Uint8Array:
+      return value / 255;
+    case Int32Array:
+      return Math.max(value / 2147483647, -1);
+    case Int16Array:
+      return Math.max(value / 32767, -1);
+    case Int8Array:
+      return Math.max(value / 127, -1);
+    default:
+      throw new Error("Invalid component type.");
+  }
+}
+function normalize(value, array) {
+  switch (array.constructor) {
+    case Float32Array:
+      return value;
+    case Uint32Array:
+      return Math.round(value * 4294967295);
+    case Uint16Array:
+      return Math.round(value * 65535);
+    case Uint8Array:
+      return Math.round(value * 255);
+    case Int32Array:
+      return Math.round(value * 2147483647);
+    case Int16Array:
+      return Math.round(value * 32767);
+    case Int8Array:
+      return Math.round(value * 127);
+    default:
+      throw new Error("Invalid component type.");
+  }
+}
+var MathUtils = {
+  DEG2RAD,
+  RAD2DEG,
+  /**
+   * Generate a [UUID](https://en.wikipedia.org/wiki/Universally_unique_identifier)
+   * (universally unique identifier).
+   *
+   * @static
+   * @method
+   * @return {string} The UUID.
+   */
+  generateUUID,
+  /**
+   * Clamps the given value between min and max.
+   *
+   * @static
+   * @method
+   * @param {number} value - The value to clamp.
+   * @param {number} min - The min value.
+   * @param {number} max - The max value.
+   * @return {number} The clamped value.
+   */
+  clamp,
+  /**
+   * Computes the Euclidean modulo of the given parameters that
+   * is `( ( n % m ) + m ) % m`.
+   *
+   * @static
+   * @method
+   * @param {number} n - The first parameter.
+   * @param {number} m - The second parameter.
+   * @return {number} The Euclidean modulo.
+   */
+  euclideanModulo,
+  /**
+   * Performs a linear mapping from range `<a1, a2>` to range `<b1, b2>`
+   * for the given value.
+   *
+   * @static
+   * @method
+   * @param {number} x - The value to be mapped.
+   * @param {number} a1 - Minimum value for range A.
+   * @param {number} a2 - Maximum value for range A.
+   * @param {number} b1 - Minimum value for range B.
+   * @param {number} b2 - Maximum value for range B.
+   * @return {number} The mapped value.
+   */
+  mapLinear,
+  /**
+   * Returns the percentage in the closed interval `[0, 1]` of the given value
+   * between the start and end point.
+   *
+   * @static
+   * @method
+   * @param {number} x - The start point
+   * @param {number} y - The end point.
+   * @param {number} value - A value between start and end.
+   * @return {number} The interpolation factor.
+   */
+  inverseLerp,
+  /**
+   * Returns a value linearly interpolated from two known points based on the given interval -
+   * `t = 0` will return `x` and `t = 1` will return `y`.
+   *
+   * @static
+   * @method
+   * @param {number} x - The start point
+   * @param {number} y - The end point.
+   * @param {number} t - The interpolation factor in the closed interval `[0, 1]`.
+   * @return {number} The interpolated value.
+   */
+  lerp,
+  /**
+   * Smoothly interpolate a number from `x` to `y` in  a spring-like manner using a delta
+   * time to maintain frame rate independent movement. For details, see
+   * [Frame rate independent damping using lerp](http://www.rorydriscoll.com/2016/03/07/frame-rate-independent-damping-using-lerp/).
+   *
+   * @static
+   * @method
+   * @param {number} x - The current point.
+   * @param {number} y - The target point.
+   * @param {number} lambda - A higher lambda value will make the movement more sudden,
+   * and a lower value will make the movement more gradual.
+   * @param {number} dt - Delta time in seconds.
+   * @return {number} The interpolated value.
+   */
+  damp,
+  /**
+   * Returns a value that alternates between `0` and the given `length` parameter.
+   *
+   * @static
+   * @method
+   * @param {number} x - The value to pingpong.
+   * @param {number} [length=1] - The positive value the function will pingpong to.
+   * @return {number} The alternated value.
+   */
+  pingpong,
+  /**
+   * Returns a value in the range `[0,1]` that represents the percentage that `x` has
+   * moved between `min` and `max`, but smoothed or slowed down the closer `x` is to
+   * the `min` and `max`.
+   *
+   * See [Smoothstep](http://en.wikipedia.org/wiki/Smoothstep) for more details.
+   *
+   * @static
+   * @method
+   * @param {number} x - The value to evaluate based on its position between min and max.
+   * @param {number} min - The min value. Any x value below min will be `0`.
+   * @param {number} max - The max value. Any x value above max will be `1`.
+   * @return {number} The alternated value.
+   */
+  smoothstep,
+  /**
+   * A [variation on smoothstep](https://en.wikipedia.org/wiki/Smoothstep#Variations)
+   * that has zero 1st and 2nd order derivatives at x=0 and x=1.
+   *
+   * @static
+   * @method
+   * @param {number} x - The value to evaluate based on its position between min and max.
+   * @param {number} min - The min value. Any x value below min will be `0`.
+   * @param {number} max - The max value. Any x value above max will be `1`.
+   * @return {number} The alternated value.
+   */
+  smootherstep,
+  /**
+   * Returns a random integer from `<low, high>` interval.
+   *
+   * @static
+   * @method
+   * @param {number} low - The lower value boundary.
+   * @param {number} high - The upper value boundary
+   * @return {number} A random integer.
+   */
+  randInt,
+  /**
+   * Returns a random float from `<low, high>` interval.
+   *
+   * @static
+   * @method
+   * @param {number} low - The lower value boundary.
+   * @param {number} high - The upper value boundary
+   * @return {number} A random float.
+   */
+  randFloat,
+  /**
+   * Returns a random integer from `<-range/2, range/2>` interval.
+   *
+   * @static
+   * @method
+   * @param {number} range - Defines the value range.
+   * @return {number} A random float.
+   */
+  randFloatSpread,
+  /**
+   * Returns a deterministic pseudo-random float in the interval `[0, 1]`.
+   *
+   * @static
+   * @method
+   * @param {number} [s] - The integer seed.
+   * @return {number} A random float.
+   */
+  seededRandom,
+  /**
+   * Converts degrees to radians.
+   *
+   * @static
+   * @method
+   * @param {number} degrees - A value in degrees.
+   * @return {number} The converted value in radians.
+   */
+  degToRad,
+  /**
+   * Converts radians to degrees.
+   *
+   * @static
+   * @method
+   * @param {number} radians - A value in radians.
+   * @return {number} The converted value in degrees.
+   */
+  radToDeg,
+  /**
+   * Returns `true` if the given number is a power of two.
+   *
+   * @static
+   * @method
+   * @param {number} value - The value to check.
+   * @return {boolean} Whether the given number is a power of two or not.
+   */
+  isPowerOfTwo,
+  /**
+   * Returns the smallest power of two that is greater than or equal to the given number.
+   *
+   * @static
+   * @method
+   * @param {number} value - The value to find a POT for.
+   * @return {number} The smallest power of two that is greater than or equal to the given number.
+   */
+  ceilPowerOfTwo,
+  /**
+   * Returns the largest power of two that is less than or equal to the given number.
+   *
+   * @static
+   * @method
+   * @param {number} value - The value to find a POT for.
+   * @return {number} The largest power of two that is less than or equal to the given number.
+   */
+  floorPowerOfTwo,
+  /**
+   * Sets the given quaternion from the [Intrinsic Proper Euler Angles](https://en.wikipedia.org/wiki/Euler_angles)
+   * defined by the given angles and order.
+   *
+   * Rotations are applied to the axes in the order specified by order:
+   * rotation by angle `a` is applied first, then by angle `b`, then by angle `c`.
+   *
+   * @static
+   * @method
+   * @param {Quaternion} q - The quaternion to set.
+   * @param {number} a - The rotation applied to the first axis, in radians.
+   * @param {number} b - The rotation applied to the second axis, in radians.
+   * @param {number} c - The rotation applied to the third axis, in radians.
+   * @param {('XYX'|'XZX'|'YXY'|'YZY'|'ZXZ'|'ZYZ')} order - A string specifying the axes order.
+   */
+  setQuaternionFromProperEuler,
+  /**
+   * Normalizes the given value according to the given typed array.
+   *
+   * @static
+   * @method
+   * @param {number} value - The float value in the range `[0,1]` to normalize.
+   * @param {TypedArray} array - The typed array that defines the data type of the value.
+   * @return {number} The normalize value.
+   */
+  normalize,
+  /**
+   * Denormalizes the given value according to the given typed array.
+   *
+   * @static
+   * @method
+   * @param {number} value - The value to denormalize.
+   * @param {TypedArray} array - The typed array that defines the data type of the value.
+   * @return {number} The denormalize (float) value in the range `[0,1]`.
+   */
+  denormalize
+};
+export {
+  DEG2RAD,
+  MathUtils,
+  RAD2DEG,
+  ceilPowerOfTwo,
+  clamp,
+  damp,
+  degToRad,
+  denormalize,
+  euclideanModulo,
+  floorPowerOfTwo,
+  generateUUID,
+  inverseLerp,
+  isPowerOfTwo,
+  lerp,
+  mapLinear,
+  normalize,
+  pingpong,
+  radToDeg,
+  randFloat,
+  randFloatSpread,
+  randInt,
+  seededRandom,
+  setQuaternionFromProperEuler,
+  smootherstep,
+  smoothstep
+};
+//# sourceMappingURL=three_src_math_MathUtils__js.js.map

nodeeditor/NodeEditor.js

@@ -123,7 +123,7 @@ export class NodeEditor {
       this.nodes.push(inputNode);
       this.nodes.push(outputNode);
       this.connections.push({ from: inputNode, to: outputNode });
-      console.log(inputNode);
+      //console.log(inputNode);
     }
     this.draw();
     //this.addVariableNode(50, 120, "Var");

public/robots/LittleSophia/urdf/LittleSophia copy.urdf

@@ -32,7 +32,7 @@
     <parent link="base_link"/>
     <child link="upper_arm_right"/>
     <origin xyz="0.04 0 0.048" rpy="0 0 0"/>
-    <axis xyz="-0.9997 -0.0114 0.0229"/>
+    <axis xyz="1 0 0"/>
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
   <link name="upper_arm_right">
@@ -95,7 +95,7 @@
     <parent link="base_link"/>
     <child link="hip_right"/>
     <origin xyz="0.024 0.005 -0.023" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
+    <axis xyz="0 0 -1"/>
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
   <link name="hip_right">
@@ -124,8 +124,8 @@
   <joint name="hip_right_to_upper_leg_right" type="revolute">
     <parent link="hip_right"/>
     <child link="upper_leg_right"/>
-    <origin xyz="0.009 0.0 -0.023" rpy="0 0 0"/>
+    <origin xyz="0.009 0.0 -0.023" rpy="-0.5 0 0"/>
-    <axis xyz="1 0 0"/>
+    <axis xyz="-1 0 0"/>
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
@@ -158,7 +158,7 @@
     <parent link="upper_leg_right"/>
     <child link="lower_leg_right_roll_link"/>
     <origin xyz="0.0 0.007 -0.028" rpy="0 0 0"/> <!-- upper gear center -->
-    <axis xyz="1 0 0"/> <!-- rolling axis -->
+    <axis xyz="-1 0 0"/> <!-- rolling axis -->
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
@@ -169,7 +169,7 @@
     <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"/>
+    <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>
@@ -203,7 +203,7 @@
     <parent link="lower_leg_right"/>
     <child link="foot_right"/>
     <origin xyz="0 0 -0.043" rpy="0 0 0"/>
-    <axis xyz="-1 0 0"/>
+    <axis xyz="1 0 0"/>
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
@@ -236,9 +236,10 @@
     <parent link="base_link"/>
     <child link="hip_left"/>
     <origin xyz="-0.024 0.005 -0.023" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
+    <axis xyz="0 0 -1"/>
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
+
   <link name="hip_left">
     <visual>
       <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
@@ -265,7 +266,7 @@
   <joint name="hip_left_to_upper_leg_left" type="revolute">
     <parent link="hip_left"/>
     <child link="upper_leg_left"/>
-    <origin xyz="-0.009 0.0 -0.023" rpy="0 0 0"/>
+    <origin xyz="-0.009 0.0 -0.023" rpy="-0.6 0 0"/>
     <axis xyz="1 0 0"/>
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
@@ -386,7 +387,7 @@
     <parent link="neck_yaw_link"/>
     <child link="neck_pitch_link"/>
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="1 0 0"/> <!-- pitch around X -->
+    <axis xyz="-1 0 0"/> <!-- pitch around X -->
     <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
   <link name="neck_pitch_link"/>
@@ -437,7 +438,7 @@
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>23</motorID></actuator>
   </transmission>
   <transmission name="hand_left">
     <type>transmission_interface/SimpleTransmission</type>
@@ -466,7 +467,7 @@
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>25</motorID></actuator>
   </transmission>
 
 
@@ -482,7 +483,7 @@
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>34</motorID></actuator>
   </transmission>
 
   <transmission name="upper_leg_right">
@@ -496,8 +497,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>33</motorID></actuator>
   </transmission>
 
   <transmission name="lower_leg_right">
@@ -511,8 +512,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>32</motorID></actuator>
   </transmission>
 
   <transmission name="foot_right">
@@ -527,7 +528,7 @@
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>30</motorID></actuator>
   </transmission>
 
   <transmission name="hip_left">
@@ -542,7 +543,7 @@
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>39</motorID></actuator>
   </transmission>
 
   <transmission name="upper_leg_left">
@@ -556,8 +557,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>38</motorID></actuator>
   </transmission>
 
   <transmission name="lower_leg_left">
@@ -571,8 +572,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>37</motorID></actuator>
   </transmission>
 
   
@@ -588,7 +589,7 @@
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>35</motorID></actuator>
   </transmission>
 
   <!-- Yaw (pan) transmission -->
@@ -604,7 +605,7 @@
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>27</motorID></actuator>
   </transmission>
 
   <!-- Pitch transmission -->
@@ -620,7 +621,7 @@
       <encoderValidMin>500</encoderValidMin>
       <encoderValidMax>4095</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>41</motorID></actuator>
   </transmission>
 
   <!-- Roll transmission -->
@@ -632,11 +633,11 @@
     <actuator name="neck_roll">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
-      <encoderTicks>4096</encoderTicks>
+      <encoderTicks>4095</encoderTicks>
-      <encoderValidMin>500</encoderValidMin>
+      <encoderValidMin>100</encoderValidMin>
-      <encoderValidMax>4095</encoderValidMax>
+      <encoderValidMax>3900</encoderValidMax>
       <encoderRange>270</encoderRange>
-    </actuator>
+    <motorID>42</motorID></actuator>
   </transmission>
 
 

public/robots/LittleSophia/urdf/LittleSophia.urdf

@@ -1,5 +1,5 @@
 <robot name="robot">
-  <link name="base_footprint"></link>
+  <link name="base_footprint"/>
   <!-- <joint name="base_joint" type="fixed">
     <parent link="base_footprint" />
     <child link="base_link" />
@@ -8,434 +8,420 @@
   </joint> -->
   <link name="base_link">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <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"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="base_link-material">
-        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <box size="0.1 0.05 0.05" />
+        <box size="0.1 0.05 0.05"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0"
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
-        izz="0.16666666666666666" />
     </inertial>
   </link>
   <joint name="base_link_to_upper_arm_right" type="revolute">
-    <parent link="base_link" />
+    <parent link="base_link"/>
-    <child link="upper_arm_right" />
+    <child link="upper_arm_right"/>
-    <origin xyz="0.04 0 0.048" rpy="0 0 0" />
+    <origin xyz="0.04 0 0.048" rpy="0 0 0"/>
-    <axis xyz="-0.9997 -0.0114 0.0229" />
+    <axis xyz="1 0 0"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
   <link name="upper_arm_right">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Arm_Right.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Arm_Right.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="upper_arm_right-material">
-        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 0.03" rpy="0 0 0" />
+      <origin xyz="0 0 0.03" rpy="0 0 0"/>
       <geometry>
-        <box size="0.03 0.03 0.06" />
+        <box size="0.03 0.03 0.06"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 0.03" rpy="0 0 0" />
+      <origin xyz="0 0 0.03" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0"
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
-        izz="0.16666666666666666" />
     </inertial>
   </link>
 
 
   <!-- Left Upper Arm -->
   <joint name="base_link_to_upper_arm_left" type="revolute">
-    <parent link="base_link" />
+    <parent link="base_link"/>
-    <child link="upper_arm_left" />
+    <child link="upper_arm_left"/>
-    <origin xyz="-0.04 0 0.048" rpy="0 0 0" />
+    <origin xyz="-0.04 0 0.048" rpy="0 0 0"/>
-    <axis xyz="-1 0 0" />
+    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
   <link name="upper_arm_left">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Arm_Left.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Arm_Left.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="upper_arm_left-material">
-        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 0.03" rpy="0 0 0" />
+      <origin xyz="0 0 0.03" rpy="0 0 0"/>
       <geometry>
-        <box size="0.03 0.03 0.06" />
+        <box size="0.03 0.03 0.06"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 0.03" rpy="0 0 0" />
+      <origin xyz="0 0 0.03" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0"
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
-        izz="0.16666666666666666" />
     </inertial>
   </link>
 
   <joint name="base_link_to_hip_right" type="revolute">
-    <parent link="base_link" />
+    <parent link="base_link"/>
-    <child link="hip_right" />
+    <child link="hip_right"/>
-    <origin xyz="0.024 0.005 -0.023" rpy="0 0 0" />
+    <origin xyz="0.024 0.005 -0.023" rpy="0 0 0"/>
-    <axis xyz="0 0 1" />
+    <axis xyz="0 0 -1"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
   <link name="hip_right">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Hip_Right.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Hip_Right.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="upper_leg_right-material">
-        <color rgba="0.0021 0.0497 0.4851 1" />
+        <color rgba="0.0021 0.0497 0.4851 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.04 0.07" />
+        <box size="0.04 0.04 0.07"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667" />
+      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
     </inertial>
   </link>
 
   <joint name="hip_right_to_upper_leg_right" type="revolute">
-    <parent link="hip_right" />
+    <parent link="hip_right"/>
-    <child link="upper_leg_right" />
+    <child link="upper_leg_right"/>
-    <origin xyz="0.009 0.0 -0.023" rpy="0 0 0" />
+    <origin xyz="0.009 0.0 -0.023" rpy="-0.5 0 0"/>
-    <axis xyz="1 0 0" />
+    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
   <link name="upper_leg_right">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Upper_Leg_Right.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Upper_Leg_Right.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="upper_leg_right-material">
-        <color rgba="0.0021 0.0497 0.4851 1" />
+        <color rgba="0.0021 0.0497 0.4851 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.04 0.07" />
+        <box size="0.04 0.04 0.07"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667" />
+      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
     </inertial>
   </link>
 
 
   <!-- 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" />
+    <parent link="upper_leg_right"/>
-    <child link="lower_leg_right_roll_link" />
+    <child link="lower_leg_right_roll_link"/>
-    <origin xyz="0.0 0.007 -0.028" rpy="0 0 0" /> <!-- upper gear center -->
+    <origin xyz="0.0 0.007 -0.028" rpy="0 0 0"/> <!-- upper gear center -->
-    <axis xyz="1 0 0" /> <!-- rolling axis -->
+    <axis xyz="-1 0 0"/> <!-- rolling axis -->
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
-  <link name="lower_leg_right_roll_link" />
+  <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" />
+    <parent link="lower_leg_right_roll_link"/>
-    <child link="lower_leg_right" />
+    <child link="lower_leg_right"/>
-    <origin xyz="-0.001 -0.0025 -0.029" rpy="-0.35 0 0" />
+    <origin xyz="-0.001 -0.0025 -0.029" rpy="-0.35 0 0"/>
-    <axis xyz="1 0 0" />
+    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <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" />
+    <mimic joint="upper_leg_right_to_lower_leg_right_roll" multiplier="1.1" offset="0"/>
   </joint>
 
 
   <link name="lower_leg_right">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Lower_Leg_Right.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Lower_Leg_Right.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="lower_leg_right-material">
-        <color rgba="0.0021 0.0497 0.4851 1" />
+        <color rgba="0.0021 0.0497 0.4851 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.04 0.07" />
+        <box size="0.04 0.04 0.07"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667" />
+      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
     </inertial>
   </link>
 
 
   <joint name="lower_leg_right_to_foot_right" type="revolute">
-    <parent link="lower_leg_right" />
+    <parent link="lower_leg_right"/>
-    <child link="foot_right" />
+    <child link="foot_right"/>
-    <origin xyz="0 0 -0.043" rpy="0 0 0" />
+    <origin xyz="0 0 -0.043" rpy="0 0 0"/>
-    <axis xyz="-1 0 0" />
+    <axis xyz="1 0 0"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
   <link name="foot_right">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Foot_Right.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Foot_Right.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="foot_right-material">
-        <color rgba="0.0021 0.0497 0.4851 1" />
+        <color rgba="0.0021 0.0497 0.4851 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0.02 -0.01" rpy="0 0 0" />
+      <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.08 0.02" />
+        <box size="0.04 0.08 0.02"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0.02 -0.01" rpy="0 0 0" />
+      <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667" />
+      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
     </inertial>
   </link>
 
 
   <!-- Left Upper Leg -->
   <joint name="base_link_to_hip_left" type="revolute">
-    <parent link="base_link" />
+    <parent link="base_link"/>
-    <child link="hip_left" />
+    <child link="hip_left"/>
-    <origin xyz="-0.024 0.005 -0.023" rpy="0 0 0" />
+    <origin xyz="-0.024 0.005 -0.023" rpy="0 0 0"/>
-    <axis xyz="0 0 1" />
+    <axis xyz="0 0 -1"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
+
   <link name="hip_left">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Hip_Left.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Hip_Left.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="upper_leg_left-material">
-        <color rgba="0.0021 0.0497 0.4851 1" />
+        <color rgba="0.0021 0.0497 0.4851 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.04 0.07" />
+        <box size="0.04 0.04 0.07"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667" />
+      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
     </inertial>
   </link>
 
   <joint name="hip_left_to_upper_leg_left" type="revolute">
-    <parent link="hip_left" />
+    <parent link="hip_left"/>
-    <child link="upper_leg_left" />
+    <child link="upper_leg_left"/>
-    <origin xyz="-0.009 0.0 -0.023" rpy="0 0 0" />
+    <origin xyz="-0.009 0.0 -0.023" rpy="-0.6 0 0"/>
-    <axis xyz="1 0 0" />
+    <axis xyz="1 0 0"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
   <link name="upper_leg_left">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Upper_Leg_Left.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Upper_Leg_Left.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="upper_leg_left-material">
-        <color rgba="0.0021 0.0497 0.4851 1" />
+        <color rgba="0.0021 0.0497 0.4851 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.04 0.07" />
+        <box size="0.04 0.04 0.07"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667" />
+      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
     </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" />
+    <parent link="upper_leg_left"/>
-    <child link="lower_leg_left_roll_link" />
+    <child link="lower_leg_left_roll_link"/>
-    <origin xyz="0.0 0.007 -0.028" rpy="0 0 0" /> <!-- upper gear center -->
+    <origin xyz="0.0 0.007 -0.028" rpy="0 0 0"/> <!-- upper gear center -->
-    <axis xyz="1 0 0" /> <!-- rolling axis -->
+    <axis xyz="1 0 0"/> <!-- rolling axis -->
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
-  <link name="lower_leg_left_roll_link" />
+  <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" />
+    <parent link="lower_leg_left_roll_link"/>
-    <child link="lower_leg_left" />
+    <child link="lower_leg_left"/>
-    <origin xyz="-0.001 -0.0025 -0.029" rpy="-0.35 0 0" />
+    <origin xyz="-0.001 -0.0025 -0.029" rpy="-0.35 0 0"/>
-    <axis xyz="1 0 0" />
+    <axis xyz="1 0 0"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <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" />
+    <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" />
+      <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"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Lower_Leg_Left.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="lower_leg_right-material">
-        <color rgba="0.0021 0.0497 0.4851 1" />
+        <color rgba="0.0021 0.0497 0.4851 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.04 0.07" />
+        <box size="0.04 0.04 0.07"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 -0.035" rpy="0 0 0" />
+      <origin xyz="0 0 -0.035" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667" />
+      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
     </inertial>
   </link>
 
   <joint name="lower_leg_left_to_foot_left" type="revolute">
-    <parent link="lower_leg_left" />
+    <parent link="lower_leg_left"/>
-    <child link="foot_left" />
+    <child link="foot_left"/>
-    <origin xyz="0 0 -0.043" rpy="0 0 0" />
+    <origin xyz="0 0 -0.043" rpy="0 0 0"/>
-    <axis xyz="-1 0 0" />
+    <axis xyz="-1 0 0"/>
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <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" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Foot_Left.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Foot_Left.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="foot_left-material">
-        <color rgba="0.0021 0.0497 0.4851 1" />
+        <color rgba="0.0021 0.0497 0.4851 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0.02 -0.01" rpy="0 0 0" />
+      <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.08 0.02" />
+        <box size="0.04 0.08 0.02"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0.02 -0.01" rpy="0 0 0" />
+      <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667" />
+      <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
     </inertial>
   </link>
 
 
   <!-- Spine to yaw joint -->
   <joint name="base_link_to_neck_yaw" type="revolute">
-    <parent link="base_link" />
+    <parent link="base_link"/>
-    <child link="neck_yaw_link" />
+    <child link="neck_yaw_link"/>
-    <origin xyz="0 0 0.065" rpy="0 0 0" />
+    <origin xyz="0 0 0.065" rpy="0 0 0"/>
-    <axis xyz="0 0 1" /> <!-- yaw/pan around Z -->
+    <axis xyz="0 0 1"/> <!-- yaw/pan around Z -->
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
-  <link name="neck_yaw_link" />
+  <link name="neck_yaw_link"/>
 
   <!-- Pitch joint -->
   <joint name="neck_pitch" type="revolute">
-    <parent link="neck_yaw_link" />
+    <parent link="neck_yaw_link"/>
-    <child link="neck_pitch_link" />
+    <child link="neck_pitch_link"/>
-    <origin xyz="0 0 0" rpy="0 0 0" />
+    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="1 0 0" /> <!-- pitch around X -->
+    <axis xyz="-1 0 0"/> <!-- pitch around X -->
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
-  <link name="neck_pitch_link" />
+  <link name="neck_pitch_link"/>
 
   <!-- Roll joint -->
   <joint name="neck_roll" type="revolute">
-    <parent link="neck_pitch_link" />
+    <parent link="neck_pitch_link"/>
-    <child link="head" />
+    <child link="head"/>
-    <origin xyz="0 0 0" rpy="0 0 0" />
+    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="0 1 0" /> <!-- roll around Y -->
+    <axis xyz="0 1 0"/> <!-- roll around Y -->
-    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
   </joint>
 
   <!-- Head (includes fused neck geometry) -->
   <link name="head">
     <visual>
-      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0"/>
       <geometry>
-        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Face.glb"
+        <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Face.glb" scale="0.001 0.001 0.001"/>
-          scale="0.001 0.001 0.001" />
       </geometry>
       <material name="head-material">
-        <color rgba="0.002 0.05 0.48 1" />
+        <color rgba="0.002 0.05 0.48 1"/>
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <box size="0.06 0.06 0.06" />
+        <box size="0.06 0.06 0.06"/>
       </geometry>
     </collision>
     <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1" />
+      <mass value="1"/>
-      <inertia ixx="0.16" ixy="0" ixz="0" iyy="0.16" iyz="0" izz="0.16" />
+      <inertia ixx="0.16" ixy="0" ixz="0" iyy="0.16" iyz="0" izz="0.16"/>
     </inertial>
   </link>
 
@@ -451,8 +437,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>23</motorID></actuator>
   </transmission>
   <transmission name="hand_left">
     <type>transmission_interface/SimpleTransmission</type>
@@ -465,7 +451,7 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
     </actuator>
   </transmission>
 
@@ -480,8 +466,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>25</motorID></actuator>
   </transmission>
 
 
@@ -496,8 +482,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>34</motorID></actuator>
   </transmission>
 
   <transmission name="upper_leg_right">
@@ -511,8 +497,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>33</motorID></actuator>
   </transmission>
 
   <transmission name="lower_leg_right">
@@ -526,8 +512,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>32</motorID></actuator>
   </transmission>
 
   <transmission name="foot_right">
@@ -541,8 +527,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>30</motorID></actuator>
   </transmission>
 
   <transmission name="hip_left">
@@ -556,8 +542,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>39</motorID></actuator>
   </transmission>
 
   <transmission name="upper_leg_left">
@@ -571,8 +557,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>38</motorID></actuator>
   </transmission>
 
   <transmission name="lower_leg_left">
@@ -586,8 +572,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>37</motorID></actuator>
   </transmission>
 
   
@@ -602,8 +588,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>35</motorID></actuator>
   </transmission>
 
   <!-- Yaw (pan) transmission -->
@@ -618,8 +604,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>200</encoderValidMin>
       <encoderValidMax>3500</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>27</motorID></actuator>
   </transmission>
 
   <!-- Pitch transmission -->
@@ -634,8 +620,8 @@
       <encoderTicks>4096</encoderTicks>
       <encoderValidMin>500</encoderValidMin>
       <encoderValidMax>4095</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>41</motorID></actuator>
   </transmission>
 
   <!-- Roll transmission -->
@@ -647,11 +633,11 @@
     <actuator name="neck_roll">
       <mechanicalReduction>1</mechanicalReduction>
       <hardwareInterface>PositionJointInterface</hardwareInterface>
-      <encoderTicks>4096</encoderTicks>
+      <encoderTicks>4095</encoderTicks>
-      <encoderValidMin>500</encoderValidMin>
+      <encoderValidMin>100</encoderValidMin>
-      <encoderValidMax>4095</encoderValidMax>
+      <encoderValidMax>3900</encoderValidMax>
-      <encoderRange>270</encoderRange>
+      <encoderRange>300</encoderRange>
-    </actuator>
+    <motorID>42</motorID></actuator>
   </transmission>
 
 

ros_robot_visualiser/URDFEditor.js

@@ -7,13 +7,21 @@ import { OBJLoader } from 'three/examples/jsm/loaders/OBJLoader.js';
 import { MTLLoader } from 'three/examples/jsm/loaders/MTLLoader.js';
 import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js';
 import { ViewerOverlay } from './ViewerOverlay.js';
+import { clamp } from 'three/src/math/MathUtils.js';
 const gltfLoader = new GLTFLoader();
 
+const SyncMode = {
+  None: 0,
+  SimToReal: 1,
+  RealToSim: 2,
+  Calibrate: 3
+};
 
 export class URDFEditor {
-  constructor(canvas) {
+  constructor(canvas, sendMotorPosition, serial) {
     this.canvas = canvas;
-    console.log(canvas);
+    this.sendMotorPosition = sendMotorPosition;
+    this.serial = serial;
     this.scene = new THREE.Scene();
     this.scene.background = new THREE.Color(0xaaaaaa);
 
@@ -33,6 +41,8 @@ export class URDFEditor {
     this.raycaster = new THREE.Raycaster();
     this.mouse = new THREE.Vector2();
 
+    this.currentSyncMode = SyncMode.None;
+
     this.hoveredJoint = null;
     this.draggedJoint = null;
     this.worldAxis = null;
@@ -65,13 +75,15 @@ export class URDFEditor {
     this.loadURDFFromIndexedDB();
     this.setupEvents();
 
+    const editorCallbacks = {}
+
+
     this.overlay = new ViewerOverlay(
       this.renderer,
       this.robot,
       this.jointAngles,
       this.findObjectByName.bind(this),
-      this.saveURDFToIndexedDB.bind(this),
+      this
-      this.loadURDFFromIndexedDB.bind(this)
     );
 
 
@@ -115,8 +127,39 @@ export class URDFEditor {
 
   }
 
+  resetEditor() {
+    if (!this.robot) return;
+    const toRemove = this.scene.children.filter(c => c.type === 'URDFLink' || c.constructor.name === 'URDFRobot');
+    toRemove.forEach(robot => {
+      // detach from scene
+      this.scene.remove(robot);
+
+      // dispose GPU resources
+      robot.traverse(obj => {
+        if (obj.geometry) obj.geometry.dispose();
+        if (obj.material) {
+          if (Array.isArray(obj.material)) obj.material.forEach(m => m.dispose());
+          else obj.material.dispose();
+        }
+      });
+    });
+
+    // Clear references
+    this.robot.urdfRobotNode = null;
+    this.robot.joints = {};
+    this.robot.links = {};
+    this.robot.sceneObject = null;
+    this.robot = null;
+
+    this.overlay.resetEditor();
+  }
 
   async loadURDF() {
+    if (this.robot) {
+      console.log("resetting editor");
+      this.resetEditor();
+    }
+
     this.urdfPath = '/robots/LittleSophia/urdf/LittleSophia.urdf';
 
     const urdfText = await fetch(this.urdfPath).then(res => res.text());
@@ -132,6 +175,11 @@ export class URDFEditor {
   }
 
   async loadURDFFromIndexedDB() {
+    if (this.robot) {
+      console.log("resetting editor");
+      this.resetEditor();
+    }
+
     return new Promise((resolve, reject) => {
       const request = indexedDB.open("URDFEditorDB", 1);
 
@@ -178,16 +226,56 @@ export class URDFEditor {
   //   }
   // }
 
-  downloadURDF(robot, filename = "robot.urdf") {
+  downloadURDF() {
+
     const serializer = new XMLSerializer();
-    const urdfString = serializer.serializeToString(robot.urdfRobotNode);
+    const urdfString = serializer.serializeToString(this.robot.urdfRobotNode);
 
     const a = document.createElement("a");
     a.href = "data:text/xml;charset=utf-8," + encodeURIComponent(urdfString);
-    a.download = filename;
+    a.download = "robot.urdf";
     a.click();
   }
 
+  async uploadURDF() {
+    // 1. Let the user pick a file
+    const input = document.createElement('input');
+    input.type = 'file';
+    input.accept = '.urdf,.xml'; // restrict to URDF/XML files
+
+    if (this.robot) {
+      console.log("resetting editor");
+      this.resetEditor();
+    }
+
+    input.onchange = async (e) => {
+      const file = e.target.files[0];
+      if (!file) return;
+
+      // 2. Reset editor if a robot is already loaded
+      if (this.robot) {
+        console.log("resetting editor");
+        this.resetEditor();
+      }
+
+      // 3. Read the file contents
+      const urdfText = await file.text();
+
+      // 4. Parse and load robot
+      const robot = await this.loader.loadFromString(urdfText);
+      robot.rotation.x = -Math.PI / 2;
+      this.scene.add(robot);
+      this.robot = robot;
+
+      // 5. Initialize overlay
+      this.overlay.init(robot);
+    };
+
+    // trigger the file picker
+    input.click();
+  }
+
+
   saveURDFToIndexedDB(robot) {
     const serializer = new XMLSerializer();
     const urdfString = serializer.serializeToString(robot.urdfRobotNode);
@@ -266,9 +354,10 @@ export class URDFEditor {
       // ✅ Rotate dragged joint
       this.draggedJoint.rotateOnAxis(this.worldAxis, actualDelta);
       this.jointAngles[jointName] = clampedAngle;
-      //console.log(jointName);
+      const jointTransmission = getJointTransmission(jointName, this.robot);
+      this.onRotateMotor(jointTransmission.motorID, radiansToTicks(clampedAngle, jointTransmission).toFixed(0));
+
       // ✅ 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;
@@ -314,6 +403,67 @@ export class URDFEditor {
     }
   }
 
+  setSyncMode(newMode) {
+    console.log("changing from " + this.currentSyncMode + " to " + newMode);
+    this.currentSyncMode = newMode;
+    if (this.currentSyncMode == SyncMode.RealToSim) {
+      this.serial.requestPositionStreaming(true);
+    } else {
+      this.serial.requestPositionStreaming(false);
+
+    }
+  }
+
+
+  setMotorPosition(motorID, positionTicks) {
+    for (const jointName in this.robot.joints) {
+      const joint = this.robot.joints[jointName];
+      const transmission = joint.transmission;
+      if (!transmission) continue;
+
+      if (transmission.motorID === motorID) {
+        // 1) Convert ticks → target joint angle (radians)
+        const targetAngle = ticksToRadians(positionTicks, transmission);
+
+        // 2) Compute delta vs tracked state
+        const currentAngle = this.jointAngles[jointName] ?? 0;
+        const delta = targetAngle - currentAngle;
+
+        // 3) Rotate driven joint
+        joint.rotateOnAxis(joint.axis, delta);
+        this.jointAngles[jointName] = targetAngle;
+
+        // 4) ✅ Apply mimic relationships
+        for (const [name, jointObj] of Object.entries(this.robot.joints)) {
+          if (jointObj.type === 'URDFMimicJoint') {
+            const { mimicJoint, multiplier = 1.0, offset = 0.0 } = jointObj;
+            if (mimicJoint === jointName) {
+              const mimicAngle = targetAngle * multiplier + offset;
+              const mimicCurrent = this.jointAngles[name] ?? 0;
+              const mimicDelta = mimicAngle - mimicCurrent;
+
+              const mimicNode = this.robot.getObjectByName(name);
+              mimicNode.rotateOnAxis(mimicNode.axis, mimicDelta);
+              this.jointAngles[name] = mimicAngle;
+            }
+          }
+        }
+
+        break; // found and updated the matching joint
+      }
+    }
+  }
+
+
+
+
+
+
+  onRotateMotor(motorID, positionTicks) {
+    if (this.currentSyncMode === SyncMode.SimToReal) {
+      this.sendMotorPosition(motorID, positionTicks);
+    }
+  }
 
   onPointerDown(event) {
     if (!this.hoveredJoint) return;
@@ -393,6 +543,57 @@ export class URDFEditor {
     return current;
   }
 
+  applyCalibrationOffsets() {
+    console.log(this.jointAngles);
+    if (!this.robot?.urdfRobotNode || !this.jointAngles) return;
+
+    for (const jointName in this.jointAngles) {
+      const offset = this.jointAngles[jointName]; // radians
+      const jointNode = this.robot.urdfRobotNode.querySelector(`joint[name="${jointName}"]`);
+      if (!jointNode) continue;
+
+      // get or create <origin>
+      let originNode = jointNode.querySelector("origin");
+      let [rx, ry, rz] = originNode?.getAttribute("rpy")?.split(" ").map(parseFloat) || [0, 0, 0];
+
+      // determine axis of rotation
+      const axisNode = jointNode.querySelector("axis");
+      const [ax, ay, az] = axisNode ? axisNode.getAttribute("xyz").split(" ").map(parseFloat) : [0, 0, 1];
+
+      // apply offset along the correct axis
+      if (Math.abs(ax) === 1) rx += offset * ax;
+      else if (Math.abs(ay) === 1) ry += offset * ay;
+      else if (Math.abs(az) === 1) rz += offset * az;
+
+      // update the URDF DOM
+      console.log(jointName, [rx, ry, rz]);
+      this.updateJointOriginRpy(jointName, [rx, ry, rz]);
+    }
+  }
+
+
+  updateJointOriginRpy(jointName, rpyArray) {
+    //console.log("Updating URDF joint rpy:", jointName, rpyArray);
+    const urdfNode = this.robot?.urdfRobotNode;
+    if (!urdfNode) return;
+
+    // find joint anywhere in the URDF
+    const jointNode = urdfNode.querySelector(`joint[name="${jointName}"]`);
+    if (!jointNode) return;
+
+    // find or create the <origin> child
+    let originNode = jointNode.querySelector("origin");
+    if (!originNode) {
+      originNode = urdfNode.ownerDocument.createElement("origin");
+      jointNode.appendChild(originNode);
+    }
+
+    // ensure we have 3 numbers [rx, ry, rz]
+    const [rx, ry, rz] = rpyArray;
+    originNode.setAttribute("rpy", `${rx} ${ry} ${rz}`);
+  }
+
+
 
   animate() {
     requestAnimationFrame(() => this.animate());

ros_robot_visualiser/ViewerOverlay.js

@@ -1,14 +1,19 @@
-import { Button, Panel, Textbox, Label, Checkbox, RadioButton, RadioGroup } from './ui/canvasui.js';
+import { Button, Panel, Textbox, Label, Checkbox, RadioButton, RadioGroup, Slider } from './ui/canvasui.js';
+
+const SyncMode = {
+    None: 0,
+    SimToReal: 1,
+    RealToSim: 2,
+    Calibrate: 3
+};
 
 export class ViewerOverlay {
-    constructor(renderer, robot, jointAngles, findObjectByName, saveURDF, loadURDF) {
+    constructor(renderer, robot, jointAngles, findObjectByName, parent) {
         this.renderer = renderer;
         this.robot = robot;
         this.jointAngles = jointAngles;
         this.findObjectByName = findObjectByName;
-        this.saveURDF = saveURDF;   // 🔑 bound to editor
+        this.parent = parent;
-        this.loadURDF = loadURDF;   // 🔑 bound to editor
-
 
         this.overlayCanvas = document.getElementById('overlay-canvas');
         this.overlayCanvas.addEventListener("wheel", (e) => {
@@ -101,6 +106,16 @@ export class ViewerOverlay {
         this.draw();
     }
 
+    resetEditor() {
+        if (!this.motorListPanel) return;
+
+        // filter out the motorListPanel from panels
+        this.panels = this.panels.filter(p => p !== this.motorListPanel);
+
+        // clear the reference
+        this.motorListPanel = null;
+    }
+
     draw() {
         const ctx = this.overlayCtx;
         ctx.clearRect(0, 0, this.overlayCanvas.width, this.overlayCanvas.height);
@@ -120,13 +135,13 @@ export class ViewerOverlay {
         const group = new RadioGroup();
 
         const rb1 = new RadioButton(panel.x + 20, panel.y + 40, 16, "None", group, true, () => {
-            this.setSyncMode("none");
+            this.parent.setSyncMode(SyncMode.None);
         }, "");
         const rb2 = new RadioButton(panel.x + 20, panel.y + 70, 16, "Sim -> Real", group, false, () => {
-            this.setSyncMode("sim_to_real");
+            this.parent.setSyncMode(SyncMode.SimToReal);
         }, "Real robot will mirror simulated movements");
         const rb3 = new RadioButton(panel.x + 20, panel.y + 100, 16, "Real -> Sim", group, false, () => {
-            this.setSyncMode("real_to_sim");
+            this.parent.setSyncMode(SyncMode.RealToSim);
         }, "Deactivate all motor torque and simulated robot will mirror real robot motion");
 
         group.addButton(rb1);
@@ -137,40 +152,63 @@ export class ViewerOverlay {
         panel.addElement(rb2);
         panel.addElement(rb3);
 
+        const slider = new Slider(
+            panel.x + panel.w * 0.025, panel.y + panel.h * 0.85, panel.w * 0.925, 20,
+            0, 100, 50,
+            (val) => console.log("Slider value:", val)
+        );
+
+        panel.addElement(slider);
 
         return panel;
     }
 
-    setSyncMode(mode) {
-
-        console.log(mode);
-    }
 
     createSystemPanel() {
-        const w = this.overlayCanvas.width * 0.1;
-        const h = this.overlayCanvas.height * 0.05;
         const x = this.overlayCanvas.width * 0.9;   // bottom right
-        const y = this.overlayCanvas.height * 0; console.log(this.overlayCanvas.width, this.overlayCanvas.height);
+        const y = this.overlayCanvas.height * 0;
+        const w = this.overlayCanvas.width * 0.1;
+        const h = this.overlayCanvas.height * 0.2;
+
 
         const panel = new Panel(x, y, w, h, "System");
 
         // Save button
-        panel.addElement(new Button(x, h / 2, 50, 24, "Save", () => {
+        panel.addElement(new Button(x, y + 28, 50, 24, "Save", () => {
             // delegate to editor’s save
-            this.saveURDF(this.robot);
+            this.parent.saveURDFToIndexedDB(this.robot);
         }));
 
         // Load button
-        panel.addElement(new Button(x + w / 2, h / 2, 50, 24, "Load", () => {
+        panel.addElement(new Button(x, y + 28 + 24 * 1, 50, 24, "Load", () => {
             // delegate to editor’s load
-            this.loadURDF();
+
+            this.parent.loadURDFFromIndexedDB();
         }));
 
+        panel.addElement(new Button(x, y + 28 + 24 * 2, 50, 24, "Download", () => {
+            // delegate to editor’s load
+            this.parent.downloadURDF();
+        }));
+
+        panel.addElement(new Button(x, y + 28 + 24 * 3, 50, 24, "Upload", () => {
+            // delegate to editor’s load
+            this.parent.uploadURDF();
+        }));
+
+        panel.addElement(new Button(x, y + 28 + 24 * 5, 50, 24, "Calibrate", () => {
+
+            this.applyCalibrationOffsets();
+        }));
+
+
         return panel;
     }
 
 
+
     createMotorListPanel() {
+        console.log(":-)");
         if (!this.robot?.joints) return null;
         this.panels = this.panels.filter(p => !p.title?.startsWith("Config:"));
         const panel = new Panel(0, 0, 300, this.overlayCanvas.height / 8 * 7, "Motors");

ros_robot_visualiser/ui/canvasui.js

@@ -170,11 +170,11 @@ export class Button extends UIElement {
     }
 
 
-    onPointerUp(px, py) {
+    // onPointerUp(px, py) {
-        const clicked = super.onPointerUp(px, py);
+    //     const clicked = super.onPointerUp(px, py);
-        if (clicked && this.onClick) this.onClick();
+    //     if (clicked && this.onClick) this.onClick();
-        return clicked;
+    //     return clicked;
-    }
+    // }
 }
 
 export class Textbox extends UIElement {
@@ -357,5 +357,73 @@ export class Tooltip {
 }
 
 
+export class Slider extends UIElement {
+    constructor(x, y, w, h, min = 0, max = 100, initialValue = 0, onChange = null) {
+        super(x, y, w, h);
+        this.min = min;
+        this.max = max;
+        this.value = initialValue;
+        this.onChange = onChange;
+        this.dragging = false;
+    }
+
+    draw(ctx) {
+        // track
+        ctx.strokeStyle = '#aaa';
+        ctx.lineWidth = 2;
+        ctx.beginPath();
+        ctx.moveTo(this.x, this.y + this.h / 2);
+        ctx.lineTo(this.x + this.w, this.y + this.h / 2);
+        ctx.stroke();
+
+        // knob position
+        const ratio = (this.value - this.min) / (this.max - this.min);
+        const knobX = this.x + ratio * this.w;
+        const knobY = this.y + this.h / 2;
+
+        // knob
+        ctx.fillStyle = this.hovered || this.dragging ? '#0f0' : '#fff';
+        ctx.beginPath();
+        ctx.arc(knobX, knobY, this.h / 2, 0, Math.PI * 2);
+        ctx.fill();
+        ctx.strokeStyle = '#333';
+        ctx.stroke();
+
+        // optional value text
+        ctx.fillStyle = '#fff';
+        ctx.font = '12px monospace';
+        ctx.fillText(this.value.toFixed(0), this.x + this.w + 10, this.y + this.h / 2 + 4);
+    }
+
+    onPointerDown(px, py) {
+        if (this.contains(px, py)) {
+            this.dragging = true;
+            this.updateValue(px);
+            return true;
+        }
+        return false;
+    }
+
+    onPointerMove(px, py) {
+        super.onPointerMove(px, py);
+        if (this.dragging) {
+            this.updateValue(px);
+        }
+    }
+
+    onPointerUp(px, py) {
+        if (this.dragging) {
+            this.dragging = false;
+            return true;
+        }
+        return false;
+    }
+
+    updateValue(px) {
+        const ratio = Math.min(Math.max((px - this.x) / this.w, 0), 1);
+        this.value = this.min + ratio * (this.max - this.min);
+        if (this.onChange) this.onChange(this.value);
+    }
+}
 
 

script.js

@@ -11,10 +11,11 @@ import { URDFEditor } from './ros_robot_visualiser/URDFEditor.js';
 
 window.onload = () => {
 
-    const urdfCanvas = document.getElementById('urdfCanvas');
-    const visualEditor = new URDFEditor(urdfCanvas);
-
     const serial = new SerialManager();
+    const urdfCanvas = document.getElementById('urdfCanvas');
+    const visualEditor = new URDFEditor(urdfCanvas, sendMotorPosition, serial);
+
+
     const servoMotors = [[], []]; // index 0 = channel 0, index 1 = channel 1
     const statusText = document.getElementById('statusText');
     const disconnectBtn = document.getElementById('disconnect');
@@ -29,52 +30,14 @@ window.onload = () => {
 
     let isInterpolating = false;
     let currentFrame = 0;
-    let dialKeyframes = Array.from({ length: 5 }, () => ({}));
 
-    let selectedDial = null;
+
+
     let draggingKeyframe = null; // { dialIndex, originalFrame }
     let isDragging = false;
 
 
 
-    const dialColors = [
-        '#F50057', // Raspberry
-        '#6200EA', // Deep Violet
-        '#FFB400', // Bright Amber
-        '#2979FF', // Royal Blue
-        '#FF5252', // Coral Red
-        '#00C853', // Vivid Green
-        '#FF80AB', // Bubblegum
-        '#00B8D4', // Sky Cyan
-        '#FF9100', // Neon Orange
-        '#651FFF', // Electric Indigo
-        '#FF4F81', // Vibrant Pink
-        '#AEEA00', // Chartreuse
-        '#FFAB40', // Sunset Orange
-        '#00E5FF', // Aqua
-        '#FF6F00', // Vivid Orange
-        '#64DD17', // Leaf Green
-        '#FFEA00', // Vivid Yellow
-        '#C51162', // Deep Rose
-        '#40C4FF', // Sky Blue
-        '#D500F9', // Vivid Purple
-        '#76FF03', // Lime Green
-        '#FF4081', // Hot Pink
-        '#00B0FF', // Electric Blue
-        '#FF1744', // Crimson
-        '#B388FF', // Soft Violet
-        '#1DE9B6', // Minty Teal
-        '#AA00FF', // Vivid Lavender
-        '#FFB347', // Pastel Orange
-        '#00C1D4', // Bright Cyan
-        '#7C4DFF'  // Neon Purple
-    ];
-
-
-
-
-
-    let dials = [];
     const frameSlider = document.getElementById('frameSlider');
     const frameDisplay = document.getElementById('frameDisplay');
 
@@ -115,11 +78,11 @@ window.onload = () => {
         connectedRobot = robot;
         console.log(connectedRobot);
         let motorIDList = []
-        clearDials();
+
         for (const motor of connectedRobot.motors) {
             curveEditor.addChannel(motor.ID);
             motorIDList.push(motor.ID);
-            addDial(motor.ID, motor.NAME);
+
         }
         if (connectedRobot.motors.length > 0) {
             setSelectedMotor(connectedRobot.motors[0].ID);
@@ -135,24 +98,8 @@ window.onload = () => {
 
 
     function setSelectedMotor(motorID) {
-        //console.log(motorID);
+        console.log(motorID);
         curveEditor.selectMotor(motorID);
-        selectedDial = motorID;
-
-        const dialElements = document.querySelectorAll('.dial');
-
-        dialElements.forEach((el, index) => {
-            el.classList.remove('selected');
-
-            if (dials[index]?.motorID === motorID) {
-                el.classList.add('selected');
-            }
-        });
-
-        //console.log("Selected motor:", motorID);
-        // Any other logic you want to run
-
-
     }
     window.setSelectedMotor = setSelectedMotor;
 
@@ -168,11 +115,9 @@ window.onload = () => {
             let motor = new ServoMotor(0, 10 + i, "SCS009")
             robot.assignMotor(positions[i], motor);
 
-            addDial(motor.ID);
+
         }
-        // addDial(123);
+
-        // Retrieve motor by position
-        //console.log(robot.getMotor('eyelids'));
 
 
         return robot;
@@ -282,83 +227,32 @@ window.onload = () => {
         frameDisplay.textContent = currentFrame;
         //console.log(currentFrame);
 
-        syncDialsWithCurveEditor();
+
         syncMotorsWithTimeline();
 
 
     };
 
-    function clearDials() {
+    function sendMotorPosition(motorID, position) {
-        const dialArea = document.getElementById('dialArea');
+
-        dialArea.innerHTML = ''; // Remove all child elements
+        const now = Date.now();
-        dials = [];
+        if (now - lastSyncTime >= syncIntervalMs) {
+            lastSyncTime = now;
+            // Each payload is 3 bytes: [motorId (uint8), position (uint16 little-endian)]
+            const buffer = new ArrayBuffer(3);
+            const view = new DataView(buffer);
+
+            view.setUint8(0, motorID);
+            view.setUint16(1, position, true); // little-endian
+
+            const payload = new Uint8Array(buffer);
+
+            // Send upward to parent / serial
+            serial.sendSetPositions(payload);
+            //console.log(payload);
         }
-
-    function addDial(motorID, motorName) {
-
-        const index = dials.length;
-
-        // Create dial wrapper
-        const dialWrapper = document.createElement('div');
-        dialWrapper.className = 'dial';
-        dialWrapper.dataset.index = index;
-
-        // Create label
-        const label = document.createElement('label');
-        label.textContent = "MotorID " + motorID;
-
-        const label2 = document.createElement('label2');
-        label2.textContent = motorName;
-
-        // Create dial container
-        const dialDiv = document.createElement('div');
-        dialDiv.id = `dial${index}`;
-
-        // Create value display
-        const valueSpan = document.createElement('span');
-        valueSpan.id = `value${index}`;
-        valueSpan.textContent = '2048';
-
-        // Assemble and append
-        dialWrapper.appendChild(label);
-        dialWrapper.appendChild(label2);
-        dialWrapper.appendChild(dialDiv);
-        dialWrapper.appendChild(valueSpan);
-        document.getElementById('dialArea').appendChild(dialWrapper);
-
-        // Create Nexus dial
-        const dial = new Nexus.Dial(`#dial${index}`, {
-            size: [80, 80],
-            min: 0,
-            max: 4095,
-            value: 4095 / 2
-        });
-
-        dial.motorID = motorID;
-        dial.colorize("accent", dialColors[index]);
-
-        dial.on('change', (v) => {
-            if (isInterpolating) return;
-            const val = Math.round(v);
-            document.getElementById(`value${index}`).textContent = val;
-            //dialKeyframes[index][currentFrame] = val;
-
-            syncMotorsWithTimeline();
-        });
-
-        dials.push(dial);
     }
 
-    function syncDialsWithCurveEditor() {
-        for (let ch = 0; ch < dials.length; ch++) {
-            dials[ch].value = curveEditor.getMotorPositionAtTime(dials[ch].motorID, currentFrame);
-
-        }
-
-    }
-
-
-
     function syncMotorsWithTimeline() {
 
         const now = Date.now();
@@ -367,11 +261,6 @@ window.onload = () => {
 
             const motorPayloads = [];
 
-            for (let ch = 0; ch < dials.length; ch++) {
-                const value = dials[ch].value;
-                motorPayloads.push({ motorId: dials[ch].motorID, position: value });
-            }
-
             const buffer = new ArrayBuffer(motorPayloads.length * 3);
             const view = new DataView(buffer);
 
@@ -388,17 +277,6 @@ window.onload = () => {
         }
     }
 
-
-    document.querySelectorAll('.dial').forEach(el => {
-        el.onclick = () => {
-            const selectedDial = parseInt(el.dataset.index);
-
-            setSelectedMotor(dials[selectedDial].motorID);
-        };
-    });
-
-
-
     // Connect button
     document.getElementById('connect').addEventListener('click', async () => {
         try {
@@ -523,21 +401,25 @@ window.onload = () => {
     });
 
     function handlePositionStreamPacket(data) {
-
+        // Each motor record = 1 byte ID + 2 bytes position
-        //console.log(data);
+        const motorCount = Math.floor(data.length / 3);
-        const motorCount = Math.floor(data.length / 2); // Each motor uses 2 bytes
         let d = [];
+
         for (let i = 0; i < motorCount; i++) {
-            const high = data[i * 2];       // High byte
+            const motorID = data[i * 3];             // 1 byte
-            const low = data[i * 2 + 1];    // Low byte
+            const low = data[i * 3 + 1];         // low byte of position
-            const value = (low << 8) | high; // Combine into uint16_t
+            const high = data[i * 3 + 2];         // high byte of position
-            d.push(value);
+            const positionTicks = (high << 8) | low; // combine into uint16_t
-            if (dials[i]) {
+
-                dials[i].value = value;
+            d.push({ motorID, positionTicks });
+
+            // apply to editor
+            visualEditor.setMotorPosition(motorID, positionTicks);
+
+            // console.log(`Incoming Stream: Motor ${motorID}: Position ${positionTicks}`);
         }
     }
 
-    }
 
     function handleLoadedFile(data) {
         const raw = new Uint8Array(data);
@@ -752,7 +634,7 @@ window.onload = () => {
 
     clearBtn.addEventListener('click', () => {
         currentFrame = 0;
-        dialKeyframes = Array.from({ length: 5 }, () => ({}));
+
     });
 
 
@@ -766,14 +648,7 @@ window.onload = () => {
 
     document.addEventListener('click', (e) => {
         const ignoredTags = ['BUTTON', 'INPUT', 'TEXTAREA', 'CANVAS'];
-        const clickedInsideDial = e.target.closest('.dial');
-        const clickedControl = ignoredTags.includes(e.target.tagName);
 
-        if (!clickedInsideDial && !clickedControl && selectedDial !== null) {
-            selectedDial = null;
-            document.querySelectorAll('.dial').forEach(el => el.classList.remove('selected'));
-
-        }
 
     });
 
@@ -1153,6 +1028,7 @@ window.onload = () => {
     });
 
 
+
     function matchPositions() {
         // if (feebackCheckbox.checked) {
         //     console.log("Running every 100ms");

style.css

@@ -1,27 +1,9 @@
 html, body {
   margin: 0;
   padding: 0;
-  overflow: hidden; /* optional: prevents scrollbars entirely */
+  /* overflow: hidden; optional: prevents scrollbars entirely */
 }
 
-.dial-container {
-  display: flex;
-  justify-content: center;
-  gap: 30px;
-  margin: 20px 0;
-}
-
-.dial {
-  display: flex;
-  flex-direction: column;
-  align-items: center;
-}
-
-.dial.selected {
-  background-color: #ddd;
-  border-radius: 10px;
-  padding: 10px;
-}
 
 
 #fileListWrapper {