little sophia face now loads through urdf->glb pipeline

URDFEditor.js

@@ -3,11 +3,16 @@ 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';
+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';
+
+const gltfLoader = new GLTFLoader();
+
 
 export class URDFEditor {
-  constructor(canvas, urdfPath = './urdf/sample.urdf') {
+  constructor(canvas) {
     this.canvas = canvas;
-    this.urdfPath = urdfPath;
 
     this.scene = new THREE.Scene();
     this.scene.background = new THREE.Color(0xaaaaaa);
@@ -47,13 +52,52 @@ export class URDFEditor {
     this.jointAngles = {};
 
     this.loader = new ExtendedURDFLoader();
-    this.loader.packages = { '': './urdf/' };
+    this.loader.loadMeshCb = (path, manager, onComplete) => {
+      if (path.endsWith('.glb') || path.endsWith('.gltf')) {
+        const gltfLoader = new GLTFLoader(manager);
+        gltfLoader.load(path, (gltf) => {
+          onComplete(gltf.scene);
+        });
+      } else {
+        // fall back to default behavior (STL, DAE)
+        URDFLoader.prototype.loadMeshCb.call(this.loader, path, manager, onComplete);
+      }
+    };
+
+    this.loader.packages = {
+      'Little_Sophia_Face': '/robots/LittleSophia'
+    };
     this.loader.parseVisual = true;
     this.loader.parseCollision = false;
 
     this.setupScene();
     this.loadURDF();
     this.setupEvents();
+
+
+
+    // Add some lights (optional if using MeshBasicMaterial)
+    const ambient = new THREE.AmbientLight(0x404040);
+    this.scene.add(ambient);
+    const dirLight = new THREE.DirectionalLight(0xffffff, 1);
+    dirLight.position.set(5, 5, 5);
+    this.scene.add(dirLight);
+
+    // this.mesh = null;
+    // gltfLoader.load('meshes/Little_Sophia.glb', (gltf) => {
+    //   const model = gltf.scene;
+    //   model.scale.set(0.001, 0.001, 0.001); // adjust if needed
+    //   model.rotation.y = Math.PI; // radians (180°)
+    //   this.scene.add(model);
+
+    //   // Optional: fit camera to model
+    //   const box = new THREE.Box3().setFromObject(model);
+    //   const center = box.getCenter(new THREE.Vector3());
+    //   const size = box.getSize(new THREE.Vector3()).length();
+    // });
+
+
+    this.animate();
   }
 
   setupScene() {
@@ -71,6 +115,8 @@ export class URDFEditor {
   }
 
   async loadURDF() {
+    this.urdfPath = '/robots/LittleSophia/urdf/LittleSophia.urdf';
+
     const urdfText = await fetch(this.urdfPath).then(res => res.text());
     const robot = await this.loader.loadFromString(urdfText);
 
@@ -81,8 +127,6 @@ export class URDFEditor {
     for (const jointName in this.robot.joints) {
       console.log(jointName, this.robot.joints[jointName].transmission);
     }
-
-    this.animate();
   }
 
 
@@ -109,6 +153,8 @@ export class URDFEditor {
     this.mouse.y = -(event.clientY / this.canvas.clientHeight) * 2 + 1;
 
     this.raycaster.setFromCamera(this.mouse, this.camera);
+
+    if (!this.robot) return;
     const intersects = this.raycaster.intersectObjects(this.robot.children, true);
 
     if (this.isDragging && this.draggedJoint && this.worldAxis) {
@@ -224,6 +270,7 @@ export class URDFEditor {
     this.drawJointOverlay(); // ✅ fixed overlay
     //console.log(this.robot.joints)
     this.renderer.render(this.scene, this.camera);
+    //console.log(this.mesh);
   }
 
 
@@ -302,6 +349,18 @@ function getJointLimits(jointName, robot) {
   return { lower: lowerRad, upper: upperRad };
 }
 
-function getJointTransmission(jointName, robot){
+function getJointTransmission(jointName, robot) {
   return robot.joints[jointName].transmission;
 }
+
+function loadObjWithMtl(objPath, mtlPath, onLoad) {
+  const mtlLoader = new MTLLoader();
+  mtlLoader.load(mtlPath, (materials) => {
+    materials.preload();
+    const objLoader = new OBJLoader();
+    objLoader.setMaterials(materials);
+    objLoader.load(objPath, (object) => {
+      onLoad(object);
+    });
+  });
+}

node_modules/.vite/deps/_metadata.json

@@ -2,30 +2,48 @@
   "hash": "9696e02b",
   "configHash": "e103e9de",
   "lockfileHash": "8d45ad8a",
-  "browserHash": "8e3e062d",
+  "browserHash": "43e9bdf0",
   "optimized": {
     "three": {
       "src": "../../three/build/three.module.js",
       "file": "three.js",
-      "fileHash": "acdec5e5",
+      "fileHash": "b96af8a1",
       "needsInterop": false
     },
     "three/examples/jsm/controls/OrbitControls.js": {
       "src": "../../three/examples/jsm/controls/OrbitControls.js",
       "file": "three_examples_jsm_controls_OrbitControls__js.js",
-      "fileHash": "c1a4f949",
+      "fileHash": "950816ee",
       "needsInterop": false
     },
     "three/examples/jsm/loaders/ColladaLoader.js": {
       "src": "../../three/examples/jsm/loaders/ColladaLoader.js",
       "file": "three_examples_jsm_loaders_ColladaLoader__js.js",
-      "fileHash": "dcb1087e",
+      "fileHash": "05327708",
       "needsInterop": false
     },
     "three/examples/jsm/loaders/STLLoader.js": {
       "src": "../../three/examples/jsm/loaders/STLLoader.js",
       "file": "three_examples_jsm_loaders_STLLoader__js.js",
-      "fileHash": "618ff280",
+      "fileHash": "99c8920d",
+      "needsInterop": false
+    },
+    "three/examples/jsm/loaders/OBJLoader.js": {
+      "src": "../../three/examples/jsm/loaders/OBJLoader.js",
+      "file": "three_examples_jsm_loaders_OBJLoader__js.js",
+      "fileHash": "88fd3969",
+      "needsInterop": false
+    },
+    "three/examples/jsm/loaders/MTLLoader.js": {
+      "src": "../../three/examples/jsm/loaders/MTLLoader.js",
+      "file": "three_examples_jsm_loaders_MTLLoader__js.js",
+      "fileHash": "880a49cf",
+      "needsInterop": false
+    },
+    "three/examples/jsm/loaders/GLTFLoader.js": {
+      "src": "../../three/examples/jsm/loaders/GLTFLoader.js",
+      "file": "three_examples_jsm_loaders_GLTFLoader__js.js",
+      "fileHash": "16bba6e6",
       "needsInterop": false
     }
   },

node_modules/.vite/deps/three_examples_jsm_loaders_MTLLoader__js.js

@@ -0,0 +1,319 @@
+import {
+  Color,
+  ColorManagement,
+  DefaultLoadingManager,
+  FileLoader,
+  FrontSide,
+  Loader,
+  LoaderUtils,
+  MeshPhongMaterial,
+  RepeatWrapping,
+  SRGBColorSpace,
+  TextureLoader,
+  Vector2
+} from "./chunk-5P7KF7VS.js";
+
+// node_modules/three/examples/jsm/loaders/MTLLoader.js
+var MTLLoader = class extends Loader {
+  constructor(manager) {
+    super(manager);
+  }
+  /**
+   * Starts loading from the given URL and passes the loaded MTL asset
+   * to the `onLoad()` callback.
+   *
+   * @param {string} url - The path/URL of the file to be loaded. This can also be a data URI.
+   * @param {function(MaterialCreator)} onLoad - Executed when the loading process has been finished.
+   * @param {onProgressCallback} onProgress - Executed while the loading is in progress.
+   * @param {onErrorCallback} onError - Executed when errors occur.
+   */
+  load(url, onLoad, onProgress, onError) {
+    const scope = this;
+    const path = this.path === "" ? LoaderUtils.extractUrlBase(url) : this.path;
+    const loader = new FileLoader(this.manager);
+    loader.setPath(this.path);
+    loader.setRequestHeader(this.requestHeader);
+    loader.setWithCredentials(this.withCredentials);
+    loader.load(url, function(text) {
+      try {
+        onLoad(scope.parse(text, path));
+      } catch (e) {
+        if (onError) {
+          onError(e);
+        } else {
+          console.error(e);
+        }
+        scope.manager.itemError(url);
+      }
+    }, onProgress, onError);
+  }
+  /**
+   * Sets the material options.
+   *
+   * @param {MTLLoader~MaterialOptions} value - The material options.
+   * @return {MTLLoader} A reference to this loader.
+   */
+  setMaterialOptions(value) {
+    this.materialOptions = value;
+    return this;
+  }
+  /**
+   * Parses the given MTL data and returns the resulting material creator.
+   *
+   * @param {string} text - The raw MTL data as a string.
+   * @param {string} path - The URL base path.
+   * @return {MaterialCreator} The material creator.
+   */
+  parse(text, path) {
+    const lines = text.split("\n");
+    let info = {};
+    const delimiter_pattern = /\s+/;
+    const materialsInfo = {};
+    for (let i = 0; i < lines.length; i++) {
+      let line = lines[i];
+      line = line.trim();
+      if (line.length === 0 || line.charAt(0) === "#") {
+        continue;
+      }
+      const pos = line.indexOf(" ");
+      let key = pos >= 0 ? line.substring(0, pos) : line;
+      key = key.toLowerCase();
+      let value = pos >= 0 ? line.substring(pos + 1) : "";
+      value = value.trim();
+      if (key === "newmtl") {
+        info = { name: value };
+        materialsInfo[value] = info;
+      } else {
+        if (key === "ka" || key === "kd" || key === "ks" || key === "ke") {
+          const ss = value.split(delimiter_pattern, 3);
+          info[key] = [parseFloat(ss[0]), parseFloat(ss[1]), parseFloat(ss[2])];
+        } else {
+          info[key] = value;
+        }
+      }
+    }
+    const materialCreator = new MaterialCreator(this.resourcePath || path, this.materialOptions);
+    materialCreator.setCrossOrigin(this.crossOrigin);
+    materialCreator.setManager(this.manager);
+    materialCreator.setMaterials(materialsInfo);
+    return materialCreator;
+  }
+};
+var MaterialCreator = class {
+  constructor(baseUrl = "", options = {}) {
+    this.baseUrl = baseUrl;
+    this.options = options;
+    this.materialsInfo = {};
+    this.materials = {};
+    this.materialsArray = [];
+    this.nameLookup = {};
+    this.crossOrigin = "anonymous";
+    this.side = this.options.side !== void 0 ? this.options.side : FrontSide;
+    this.wrap = this.options.wrap !== void 0 ? this.options.wrap : RepeatWrapping;
+  }
+  setCrossOrigin(value) {
+    this.crossOrigin = value;
+    return this;
+  }
+  setManager(value) {
+    this.manager = value;
+  }
+  setMaterials(materialsInfo) {
+    this.materialsInfo = this.convert(materialsInfo);
+    this.materials = {};
+    this.materialsArray = [];
+    this.nameLookup = {};
+  }
+  convert(materialsInfo) {
+    if (!this.options) return materialsInfo;
+    const converted = {};
+    for (const mn in materialsInfo) {
+      const mat = materialsInfo[mn];
+      const covmat = {};
+      converted[mn] = covmat;
+      for (const prop in mat) {
+        let save = true;
+        let value = mat[prop];
+        const lprop = prop.toLowerCase();
+        switch (lprop) {
+          case "kd":
+          case "ka":
+          case "ks":
+            if (this.options && this.options.normalizeRGB) {
+              value = [value[0] / 255, value[1] / 255, value[2] / 255];
+            }
+            if (this.options && this.options.ignoreZeroRGBs) {
+              if (value[0] === 0 && value[1] === 0 && value[2] === 0) {
+                save = false;
+              }
+            }
+            break;
+          default:
+            break;
+        }
+        if (save) {
+          covmat[lprop] = value;
+        }
+      }
+    }
+    return converted;
+  }
+  preload() {
+    for (const mn in this.materialsInfo) {
+      this.create(mn);
+    }
+  }
+  getIndex(materialName) {
+    return this.nameLookup[materialName];
+  }
+  getAsArray() {
+    let index = 0;
+    for (const mn in this.materialsInfo) {
+      this.materialsArray[index] = this.create(mn);
+      this.nameLookup[mn] = index;
+      index++;
+    }
+    return this.materialsArray;
+  }
+  create(materialName) {
+    if (this.materials[materialName] === void 0) {
+      this.createMaterial_(materialName);
+    }
+    return this.materials[materialName];
+  }
+  createMaterial_(materialName) {
+    const scope = this;
+    const mat = this.materialsInfo[materialName];
+    const params = {
+      name: materialName,
+      side: this.side
+    };
+    function resolveURL(baseUrl, url) {
+      if (typeof url !== "string" || url === "")
+        return "";
+      if (/^https?:\/\//i.test(url)) return url;
+      return baseUrl + url;
+    }
+    function setMapForType(mapType, value) {
+      if (params[mapType]) return;
+      const texParams = scope.getTextureParams(value, params);
+      const map = scope.loadTexture(resolveURL(scope.baseUrl, texParams.url));
+      map.repeat.copy(texParams.scale);
+      map.offset.copy(texParams.offset);
+      map.wrapS = scope.wrap;
+      map.wrapT = scope.wrap;
+      if (mapType === "map" || mapType === "emissiveMap") {
+        map.colorSpace = SRGBColorSpace;
+      }
+      params[mapType] = map;
+    }
+    for (const prop in mat) {
+      const value = mat[prop];
+      let n;
+      if (value === "") continue;
+      switch (prop.toLowerCase()) {
+        // Ns is material specular exponent
+        case "kd":
+          params.color = ColorManagement.colorSpaceToWorking(new Color().fromArray(value), SRGBColorSpace);
+          break;
+        case "ks":
+          params.specular = ColorManagement.colorSpaceToWorking(new Color().fromArray(value), SRGBColorSpace);
+          break;
+        case "ke":
+          params.emissive = ColorManagement.colorSpaceToWorking(new Color().fromArray(value), SRGBColorSpace);
+          break;
+        case "map_kd":
+          setMapForType("map", value);
+          break;
+        case "map_ks":
+          setMapForType("specularMap", value);
+          break;
+        case "map_ke":
+          setMapForType("emissiveMap", value);
+          break;
+        case "norm":
+          setMapForType("normalMap", value);
+          break;
+        case "map_bump":
+        case "bump":
+          setMapForType("bumpMap", value);
+          break;
+        case "disp":
+          setMapForType("displacementMap", value);
+          break;
+        case "map_d":
+          setMapForType("alphaMap", value);
+          params.transparent = true;
+          break;
+        case "ns":
+          params.shininess = parseFloat(value);
+          break;
+        case "d":
+          n = parseFloat(value);
+          if (n < 1) {
+            params.opacity = n;
+            params.transparent = true;
+          }
+          break;
+        case "tr":
+          n = parseFloat(value);
+          if (this.options && this.options.invertTrProperty) n = 1 - n;
+          if (n > 0) {
+            params.opacity = 1 - n;
+            params.transparent = true;
+          }
+          break;
+        default:
+          break;
+      }
+    }
+    this.materials[materialName] = new MeshPhongMaterial(params);
+    return this.materials[materialName];
+  }
+  getTextureParams(value, matParams) {
+    const texParams = {
+      scale: new Vector2(1, 1),
+      offset: new Vector2(0, 0)
+    };
+    const items = value.split(/\s+/);
+    let pos;
+    pos = items.indexOf("-bm");
+    if (pos >= 0) {
+      matParams.bumpScale = parseFloat(items[pos + 1]);
+      items.splice(pos, 2);
+    }
+    pos = items.indexOf("-mm");
+    if (pos >= 0) {
+      matParams.displacementBias = parseFloat(items[pos + 1]);
+      matParams.displacementScale = parseFloat(items[pos + 2]);
+      items.splice(pos, 3);
+    }
+    pos = items.indexOf("-s");
+    if (pos >= 0) {
+      texParams.scale.set(parseFloat(items[pos + 1]), parseFloat(items[pos + 2]));
+      items.splice(pos, 4);
+    }
+    pos = items.indexOf("-o");
+    if (pos >= 0) {
+      texParams.offset.set(parseFloat(items[pos + 1]), parseFloat(items[pos + 2]));
+      items.splice(pos, 4);
+    }
+    texParams.url = items.join(" ").trim();
+    return texParams;
+  }
+  loadTexture(url, mapping, onLoad, onProgress, onError) {
+    const manager = this.manager !== void 0 ? this.manager : DefaultLoadingManager;
+    let loader = manager.getHandler(url);
+    if (loader === null) {
+      loader = new TextureLoader(manager);
+    }
+    if (loader.setCrossOrigin) loader.setCrossOrigin(this.crossOrigin);
+    const texture = loader.load(url, onLoad, onProgress, onError);
+    if (mapping !== void 0) texture.mapping = mapping;
+    return texture;
+  }
+};
+export {
+  MTLLoader
+};
+//# sourceMappingURL=three_examples_jsm_loaders_MTLLoader__js.js.map

node_modules/.vite/deps/three_examples_jsm_loaders_OBJLoader__js.js

@@ -0,0 +1,539 @@
+import {
+  BufferGeometry,
+  Color,
+  FileLoader,
+  Float32BufferAttribute,
+  Group,
+  LineBasicMaterial,
+  LineSegments,
+  Loader,
+  Material,
+  Mesh,
+  MeshPhongMaterial,
+  Points,
+  PointsMaterial,
+  SRGBColorSpace,
+  Vector3
+} from "./chunk-5P7KF7VS.js";
+
+// node_modules/three/examples/jsm/loaders/OBJLoader.js
+var _object_pattern = /^[og]\s*(.+)?/;
+var _material_library_pattern = /^mtllib /;
+var _material_use_pattern = /^usemtl /;
+var _map_use_pattern = /^usemap /;
+var _face_vertex_data_separator_pattern = /\s+/;
+var _vA = new Vector3();
+var _vB = new Vector3();
+var _vC = new Vector3();
+var _ab = new Vector3();
+var _cb = new Vector3();
+var _color = new Color();
+function ParserState() {
+  const state = {
+    objects: [],
+    object: {},
+    vertices: [],
+    normals: [],
+    colors: [],
+    uvs: [],
+    materials: {},
+    materialLibraries: [],
+    startObject: function(name, fromDeclaration) {
+      if (this.object && this.object.fromDeclaration === false) {
+        this.object.name = name;
+        this.object.fromDeclaration = fromDeclaration !== false;
+        return;
+      }
+      const previousMaterial = this.object && typeof this.object.currentMaterial === "function" ? this.object.currentMaterial() : void 0;
+      if (this.object && typeof this.object._finalize === "function") {
+        this.object._finalize(true);
+      }
+      this.object = {
+        name: name || "",
+        fromDeclaration: fromDeclaration !== false,
+        geometry: {
+          vertices: [],
+          normals: [],
+          colors: [],
+          uvs: [],
+          hasUVIndices: false
+        },
+        materials: [],
+        smooth: true,
+        startMaterial: function(name2, libraries) {
+          const previous = this._finalize(false);
+          if (previous && (previous.inherited || previous.groupCount <= 0)) {
+            this.materials.splice(previous.index, 1);
+          }
+          const material = {
+            index: this.materials.length,
+            name: name2 || "",
+            mtllib: Array.isArray(libraries) && libraries.length > 0 ? libraries[libraries.length - 1] : "",
+            smooth: previous !== void 0 ? previous.smooth : this.smooth,
+            groupStart: previous !== void 0 ? previous.groupEnd : 0,
+            groupEnd: -1,
+            groupCount: -1,
+            inherited: false,
+            clone: function(index) {
+              const cloned = {
+                index: typeof index === "number" ? index : this.index,
+                name: this.name,
+                mtllib: this.mtllib,
+                smooth: this.smooth,
+                groupStart: 0,
+                groupEnd: -1,
+                groupCount: -1,
+                inherited: false
+              };
+              cloned.clone = this.clone.bind(cloned);
+              return cloned;
+            }
+          };
+          this.materials.push(material);
+          return material;
+        },
+        currentMaterial: function() {
+          if (this.materials.length > 0) {
+            return this.materials[this.materials.length - 1];
+          }
+          return void 0;
+        },
+        _finalize: function(end) {
+          const lastMultiMaterial = this.currentMaterial();
+          if (lastMultiMaterial && lastMultiMaterial.groupEnd === -1) {
+            lastMultiMaterial.groupEnd = this.geometry.vertices.length / 3;
+            lastMultiMaterial.groupCount = lastMultiMaterial.groupEnd - lastMultiMaterial.groupStart;
+            lastMultiMaterial.inherited = false;
+          }
+          if (end && this.materials.length > 1) {
+            for (let mi = this.materials.length - 1; mi >= 0; mi--) {
+              if (this.materials[mi].groupCount <= 0) {
+                this.materials.splice(mi, 1);
+              }
+            }
+          }
+          if (end && this.materials.length === 0) {
+            this.materials.push({
+              name: "",
+              smooth: this.smooth
+            });
+          }
+          return lastMultiMaterial;
+        }
+      };
+      if (previousMaterial && previousMaterial.name && typeof previousMaterial.clone === "function") {
+        const declared = previousMaterial.clone(0);
+        declared.inherited = true;
+        this.object.materials.push(declared);
+      }
+      this.objects.push(this.object);
+    },
+    finalize: function() {
+      if (this.object && typeof this.object._finalize === "function") {
+        this.object._finalize(true);
+      }
+    },
+    parseVertexIndex: function(value, len) {
+      const index = parseInt(value, 10);
+      return (index >= 0 ? index - 1 : index + len / 3) * 3;
+    },
+    parseNormalIndex: function(value, len) {
+      const index = parseInt(value, 10);
+      return (index >= 0 ? index - 1 : index + len / 3) * 3;
+    },
+    parseUVIndex: function(value, len) {
+      const index = parseInt(value, 10);
+      return (index >= 0 ? index - 1 : index + len / 2) * 2;
+    },
+    addVertex: function(a, b, c) {
+      const src = this.vertices;
+      const dst = this.object.geometry.vertices;
+      dst.push(src[a + 0], src[a + 1], src[a + 2]);
+      dst.push(src[b + 0], src[b + 1], src[b + 2]);
+      dst.push(src[c + 0], src[c + 1], src[c + 2]);
+    },
+    addVertexPoint: function(a) {
+      const src = this.vertices;
+      const dst = this.object.geometry.vertices;
+      dst.push(src[a + 0], src[a + 1], src[a + 2]);
+    },
+    addVertexLine: function(a) {
+      const src = this.vertices;
+      const dst = this.object.geometry.vertices;
+      dst.push(src[a + 0], src[a + 1], src[a + 2]);
+    },
+    addNormal: function(a, b, c) {
+      const src = this.normals;
+      const dst = this.object.geometry.normals;
+      dst.push(src[a + 0], src[a + 1], src[a + 2]);
+      dst.push(src[b + 0], src[b + 1], src[b + 2]);
+      dst.push(src[c + 0], src[c + 1], src[c + 2]);
+    },
+    addFaceNormal: function(a, b, c) {
+      const src = this.vertices;
+      const dst = this.object.geometry.normals;
+      _vA.fromArray(src, a);
+      _vB.fromArray(src, b);
+      _vC.fromArray(src, c);
+      _cb.subVectors(_vC, _vB);
+      _ab.subVectors(_vA, _vB);
+      _cb.cross(_ab);
+      _cb.normalize();
+      dst.push(_cb.x, _cb.y, _cb.z);
+      dst.push(_cb.x, _cb.y, _cb.z);
+      dst.push(_cb.x, _cb.y, _cb.z);
+    },
+    addColor: function(a, b, c) {
+      const src = this.colors;
+      const dst = this.object.geometry.colors;
+      if (src[a] !== void 0) dst.push(src[a + 0], src[a + 1], src[a + 2]);
+      if (src[b] !== void 0) dst.push(src[b + 0], src[b + 1], src[b + 2]);
+      if (src[c] !== void 0) dst.push(src[c + 0], src[c + 1], src[c + 2]);
+    },
+    addUV: function(a, b, c) {
+      const src = this.uvs;
+      const dst = this.object.geometry.uvs;
+      dst.push(src[a + 0], src[a + 1]);
+      dst.push(src[b + 0], src[b + 1]);
+      dst.push(src[c + 0], src[c + 1]);
+    },
+    addDefaultUV: function() {
+      const dst = this.object.geometry.uvs;
+      dst.push(0, 0);
+      dst.push(0, 0);
+      dst.push(0, 0);
+    },
+    addUVLine: function(a) {
+      const src = this.uvs;
+      const dst = this.object.geometry.uvs;
+      dst.push(src[a + 0], src[a + 1]);
+    },
+    addFace: function(a, b, c, ua, ub, uc, na, nb, nc) {
+      const vLen = this.vertices.length;
+      let ia = this.parseVertexIndex(a, vLen);
+      let ib = this.parseVertexIndex(b, vLen);
+      let ic = this.parseVertexIndex(c, vLen);
+      this.addVertex(ia, ib, ic);
+      this.addColor(ia, ib, ic);
+      if (na !== void 0 && na !== "") {
+        const nLen = this.normals.length;
+        ia = this.parseNormalIndex(na, nLen);
+        ib = this.parseNormalIndex(nb, nLen);
+        ic = this.parseNormalIndex(nc, nLen);
+        this.addNormal(ia, ib, ic);
+      } else {
+        this.addFaceNormal(ia, ib, ic);
+      }
+      if (ua !== void 0 && ua !== "") {
+        const uvLen = this.uvs.length;
+        ia = this.parseUVIndex(ua, uvLen);
+        ib = this.parseUVIndex(ub, uvLen);
+        ic = this.parseUVIndex(uc, uvLen);
+        this.addUV(ia, ib, ic);
+        this.object.geometry.hasUVIndices = true;
+      } else {
+        this.addDefaultUV();
+      }
+    },
+    addPointGeometry: function(vertices) {
+      this.object.geometry.type = "Points";
+      const vLen = this.vertices.length;
+      for (let vi = 0, l = vertices.length; vi < l; vi++) {
+        const index = this.parseVertexIndex(vertices[vi], vLen);
+        this.addVertexPoint(index);
+        this.addColor(index);
+      }
+    },
+    addLineGeometry: function(vertices, uvs) {
+      this.object.geometry.type = "Line";
+      const vLen = this.vertices.length;
+      const uvLen = this.uvs.length;
+      for (let vi = 0, l = vertices.length; vi < l; vi++) {
+        this.addVertexLine(this.parseVertexIndex(vertices[vi], vLen));
+      }
+      for (let uvi = 0, l = uvs.length; uvi < l; uvi++) {
+        this.addUVLine(this.parseUVIndex(uvs[uvi], uvLen));
+      }
+    }
+  };
+  state.startObject("", false);
+  return state;
+}
+var OBJLoader = class extends Loader {
+  /**
+   * Constructs a new OBJ loader.
+   *
+   * @param {LoadingManager} [manager] - The loading manager.
+   */
+  constructor(manager) {
+    super(manager);
+    this.materials = null;
+  }
+  /**
+   * Starts loading from the given URL and passes the loaded OBJ asset
+   * to the `onLoad()` callback.
+   *
+   * @param {string} url - The path/URL of the file to be loaded. This can also be a data URI.
+   * @param {function(Group)} onLoad - Executed when the loading process has been finished.
+   * @param {onProgressCallback} onProgress - Executed while the loading is in progress.
+   * @param {onErrorCallback} onError - Executed when errors occur.
+   */
+  load(url, onLoad, onProgress, onError) {
+    const scope = this;
+    const loader = new FileLoader(this.manager);
+    loader.setPath(this.path);
+    loader.setRequestHeader(this.requestHeader);
+    loader.setWithCredentials(this.withCredentials);
+    loader.load(url, function(text) {
+      try {
+        onLoad(scope.parse(text));
+      } catch (e) {
+        if (onError) {
+          onError(e);
+        } else {
+          console.error(e);
+        }
+        scope.manager.itemError(url);
+      }
+    }, onProgress, onError);
+  }
+  /**
+   * Sets the material creator for this OBJ. This object is loaded via {@link MTLLoader}.
+   *
+   * @param {MaterialCreator} materials - An object that creates the materials for this OBJ.
+   * @return {OBJLoader} A reference to this loader.
+   */
+  setMaterials(materials) {
+    this.materials = materials;
+    return this;
+  }
+  /**
+   * Parses the given OBJ data and returns the resulting group.
+   *
+   * @param {string} text - The raw OBJ data as a string.
+   * @return {Group} The parsed OBJ.
+   */
+  parse(text) {
+    const state = new ParserState();
+    if (text.indexOf("\r\n") !== -1) {
+      text = text.replace(/\r\n/g, "\n");
+    }
+    if (text.indexOf("\\\n") !== -1) {
+      text = text.replace(/\\\n/g, "");
+    }
+    const lines = text.split("\n");
+    let result = [];
+    for (let i = 0, l = lines.length; i < l; i++) {
+      const line = lines[i].trimStart();
+      if (line.length === 0) continue;
+      const lineFirstChar = line.charAt(0);
+      if (lineFirstChar === "#") continue;
+      if (lineFirstChar === "v") {
+        const data = line.split(_face_vertex_data_separator_pattern);
+        switch (data[0]) {
+          case "v":
+            state.vertices.push(
+              parseFloat(data[1]),
+              parseFloat(data[2]),
+              parseFloat(data[3])
+            );
+            if (data.length >= 7) {
+              _color.setRGB(
+                parseFloat(data[4]),
+                parseFloat(data[5]),
+                parseFloat(data[6]),
+                SRGBColorSpace
+              );
+              state.colors.push(_color.r, _color.g, _color.b);
+            } else {
+              state.colors.push(void 0, void 0, void 0);
+            }
+            break;
+          case "vn":
+            state.normals.push(
+              parseFloat(data[1]),
+              parseFloat(data[2]),
+              parseFloat(data[3])
+            );
+            break;
+          case "vt":
+            state.uvs.push(
+              parseFloat(data[1]),
+              parseFloat(data[2])
+            );
+            break;
+        }
+      } else if (lineFirstChar === "f") {
+        const lineData = line.slice(1).trim();
+        const vertexData = lineData.split(_face_vertex_data_separator_pattern);
+        const faceVertices = [];
+        for (let j = 0, jl = vertexData.length; j < jl; j++) {
+          const vertex = vertexData[j];
+          if (vertex.length > 0) {
+            const vertexParts = vertex.split("/");
+            faceVertices.push(vertexParts);
+          }
+        }
+        const v1 = faceVertices[0];
+        for (let j = 1, jl = faceVertices.length - 1; j < jl; j++) {
+          const v2 = faceVertices[j];
+          const v3 = faceVertices[j + 1];
+          state.addFace(
+            v1[0],
+            v2[0],
+            v3[0],
+            v1[1],
+            v2[1],
+            v3[1],
+            v1[2],
+            v2[2],
+            v3[2]
+          );
+        }
+      } else if (lineFirstChar === "l") {
+        const lineParts = line.substring(1).trim().split(" ");
+        let lineVertices = [];
+        const lineUVs = [];
+        if (line.indexOf("/") === -1) {
+          lineVertices = lineParts;
+        } else {
+          for (let li = 0, llen = lineParts.length; li < llen; li++) {
+            const parts = lineParts[li].split("/");
+            if (parts[0] !== "") lineVertices.push(parts[0]);
+            if (parts[1] !== "") lineUVs.push(parts[1]);
+          }
+        }
+        state.addLineGeometry(lineVertices, lineUVs);
+      } else if (lineFirstChar === "p") {
+        const lineData = line.slice(1).trim();
+        const pointData = lineData.split(" ");
+        state.addPointGeometry(pointData);
+      } else if ((result = _object_pattern.exec(line)) !== null) {
+        const name = (" " + result[0].slice(1).trim()).slice(1);
+        state.startObject(name);
+      } else if (_material_use_pattern.test(line)) {
+        state.object.startMaterial(line.substring(7).trim(), state.materialLibraries);
+      } else if (_material_library_pattern.test(line)) {
+        state.materialLibraries.push(line.substring(7).trim());
+      } else if (_map_use_pattern.test(line)) {
+        console.warn('THREE.OBJLoader: Rendering identifier "usemap" not supported. Textures must be defined in MTL files.');
+      } else if (lineFirstChar === "s") {
+        result = line.split(" ");
+        if (result.length > 1) {
+          const value = result[1].trim().toLowerCase();
+          state.object.smooth = value !== "0" && value !== "off";
+        } else {
+          state.object.smooth = true;
+        }
+        const material = state.object.currentMaterial();
+        if (material) material.smooth = state.object.smooth;
+      } else {
+        if (line === "\0") continue;
+        console.warn('THREE.OBJLoader: Unexpected line: "' + line + '"');
+      }
+    }
+    state.finalize();
+    const container = new Group();
+    container.materialLibraries = [].concat(state.materialLibraries);
+    const hasPrimitives = !(state.objects.length === 1 && state.objects[0].geometry.vertices.length === 0);
+    if (hasPrimitives === true) {
+      for (let i = 0, l = state.objects.length; i < l; i++) {
+        const object = state.objects[i];
+        const geometry = object.geometry;
+        const materials = object.materials;
+        const isLine = geometry.type === "Line";
+        const isPoints = geometry.type === "Points";
+        let hasVertexColors = false;
+        if (geometry.vertices.length === 0) continue;
+        const buffergeometry = new BufferGeometry();
+        buffergeometry.setAttribute("position", new Float32BufferAttribute(geometry.vertices, 3));
+        if (geometry.normals.length > 0) {
+          buffergeometry.setAttribute("normal", new Float32BufferAttribute(geometry.normals, 3));
+        }
+        if (geometry.colors.length > 0) {
+          hasVertexColors = true;
+          buffergeometry.setAttribute("color", new Float32BufferAttribute(geometry.colors, 3));
+        }
+        if (geometry.hasUVIndices === true) {
+          buffergeometry.setAttribute("uv", new Float32BufferAttribute(geometry.uvs, 2));
+        }
+        const createdMaterials = [];
+        for (let mi = 0, miLen = materials.length; mi < miLen; mi++) {
+          const sourceMaterial = materials[mi];
+          const materialHash = sourceMaterial.name + "_" + sourceMaterial.smooth + "_" + hasVertexColors;
+          let material = state.materials[materialHash];
+          if (this.materials !== null) {
+            material = this.materials.create(sourceMaterial.name);
+            if (isLine && material && !(material instanceof LineBasicMaterial)) {
+              const materialLine = new LineBasicMaterial();
+              Material.prototype.copy.call(materialLine, material);
+              materialLine.color.copy(material.color);
+              material = materialLine;
+            } else if (isPoints && material && !(material instanceof PointsMaterial)) {
+              const materialPoints = new PointsMaterial({ size: 10, sizeAttenuation: false });
+              Material.prototype.copy.call(materialPoints, material);
+              materialPoints.color.copy(material.color);
+              materialPoints.map = material.map;
+              material = materialPoints;
+            }
+          }
+          if (material === void 0) {
+            if (isLine) {
+              material = new LineBasicMaterial();
+            } else if (isPoints) {
+              material = new PointsMaterial({ size: 1, sizeAttenuation: false });
+            } else {
+              material = new MeshPhongMaterial();
+            }
+            material.name = sourceMaterial.name;
+            material.flatShading = sourceMaterial.smooth ? false : true;
+            material.vertexColors = hasVertexColors;
+            state.materials[materialHash] = material;
+          }
+          createdMaterials.push(material);
+        }
+        let mesh;
+        if (createdMaterials.length > 1) {
+          for (let mi = 0, miLen = materials.length; mi < miLen; mi++) {
+            const sourceMaterial = materials[mi];
+            buffergeometry.addGroup(sourceMaterial.groupStart, sourceMaterial.groupCount, mi);
+          }
+          if (isLine) {
+            mesh = new LineSegments(buffergeometry, createdMaterials);
+          } else if (isPoints) {
+            mesh = new Points(buffergeometry, createdMaterials);
+          } else {
+            mesh = new Mesh(buffergeometry, createdMaterials);
+          }
+        } else {
+          if (isLine) {
+            mesh = new LineSegments(buffergeometry, createdMaterials[0]);
+          } else if (isPoints) {
+            mesh = new Points(buffergeometry, createdMaterials[0]);
+          } else {
+            mesh = new Mesh(buffergeometry, createdMaterials[0]);
+          }
+        }
+        mesh.name = object.name;
+        container.add(mesh);
+      }
+    } else {
+      if (state.vertices.length > 0) {
+        const material = new PointsMaterial({ size: 1, sizeAttenuation: false });
+        const buffergeometry = new BufferGeometry();
+        buffergeometry.setAttribute("position", new Float32BufferAttribute(state.vertices, 3));
+        if (state.colors.length > 0 && state.colors[0] !== void 0) {
+          buffergeometry.setAttribute("color", new Float32BufferAttribute(state.colors, 3));
+          material.vertexColors = true;
+        }
+        const points = new Points(buffergeometry, material);
+        container.add(points);
+      }
+    }
+    return container;
+  }
+};
+export {
+  OBJLoader
+};
+//# sourceMappingURL=three_examples_jsm_loaders_OBJLoader__js.js.map

robots/LittleSophia/urdf/LittleSophia.urdf

@@ -0,0 +1,732 @@
+<robot name="robot">
+  <link name="base_footprint"></link>
+  <joint name="base_joint" type="fixed">
+    <parent link="base_footprint" />
+    <child link="base_link" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+  </joint>
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.1 0.05 0.05" />
+      </geometry>
+      <material name="base_link-material">
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.1 0.05 0.05" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="1" />
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
+    </inertial>
+  </link>
+  <joint name="base_link_to_spine_1" type="revolute">
+  <parent link="base_link"/>
+  <child link="spine_1"/>
+  <origin xyz="0 0 0.0337" rpy="0 0 0"/>
+  <axis xyz="-0.9997 -0.0114 0.0229"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+  <link name="spine_1">
+    <visual>
+      <origin xyz="0 0 0.05" rpy="0 0 0" />
+      <geometry>
+        <box size="0.1 0.05 0.1" />
+      </geometry>
+      <material name="spine_1-material">
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.05" rpy="0 0 0" />
+      <geometry>
+        <box size="0.1 0.05 0.1" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0.05" rpy="0 0 0" />
+      <mass value="1" />
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
+    </inertial>
+  </link>
+  <joint name="spine_1_to_upper_arm_right" type="revolute">
+  <parent link="spine_1"/>
+  <child link="upper_arm_right"/>
+  <origin xyz="0.0624 0 0.0973" rpy="-3.1416 -0.6587 0"/>
+  <axis xyz="-0.9997 -0.0114 0.0229"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+  <link name="upper_arm_right">
+    <visual>
+      <origin xyz="0 0 0.03" rpy="0 0 0" />
+      <geometry>
+        <box size="0.03 0.03 0.06" />
+      </geometry>
+      <material name="upper_arm_right-material">
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.03" rpy="0 0 0" />
+      <geometry>
+        <box size="0.03 0.03 0.06" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0.03" rpy="0 0 0" />
+      <mass value="1" />
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
+    </inertial>
+  </link>
+ <joint name="upper_arm_right_to_lower_arm_right" type="revolute">
+  <parent link="upper_arm_right"/>
+  <child link="lower_arm_right"/>
+  <origin xyz="0.0 0 .065" rpy="0 0 0"/>
+  <axis xyz="-0.9997 -0.0114 0.0229"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+  <link name="lower_arm_right">
+    <visual>
+      <origin xyz="0 0 0.025" rpy="0 0 0" />
+      <geometry>
+        <box size="0.03 0.03 0.05" />
+      </geometry>
+      <material name="lower_arm_right-material">
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.025" rpy="0 0 0" />
+      <geometry>
+        <box size="0.03 0.03 0.05" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0.025" rpy="0 0 0" />
+      <mass value="1" />
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
+    </inertial>
+  </link>
+ <joint name="lower_arm_right_to_hand_right" type="revolute">
+  <parent link="lower_arm_right"/>
+  <child link="hand_right"/>
+  <origin xyz="0 0 0.055" rpy="0 0 0"/>
+  <axis xyz="0 1 0"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+  <link name="hand_right">
+    <visual>
+      <origin xyz="0 0 0.015" rpy="0 0 0" />
+      <geometry>
+        <box size="0.03 0.03 0.03" />
+      </geometry>
+      <material name="hand_right-material">
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.015" rpy="0 0 0" />
+      <geometry>
+        <box size="0.03 0.03 0.03" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0.015" rpy="0 0 0" />
+      <mass value="1" />
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
+    </inertial>
+  </link>
+  
+  <!-- Left Upper Arm -->
+<joint name="spine_1_to_upper_arm_left" type="revolute">
+  <parent link="spine_1"/>
+  <child link="upper_arm_left"/>
+  <origin xyz="-0.0624 0 0.0973" rpy="3.1416 0.6587 0"/>
+  <axis xyz="-1 0 0"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+<link name="upper_arm_left">
+  <visual>
+    <origin xyz="0 0 0.03" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.03 0.03 0.06"/>
+    </geometry>
+    <material name="upper_arm_left-material">
+      <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 0.03" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.03 0.03 0.06"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0 0.03" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
+  </inertial>
+</link>
+
+
+<!-- Left Lower Arm -->
+<joint name="upper_arm_left_to_lower_arm_left" type="revolute">
+  <parent link="upper_arm_left"/>
+  <child link="lower_arm_left"/>
+  <origin xyz="0.0 0 0.065" rpy="0 0 0"/>
+  <axis xyz="-1 0 0"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+<link name="lower_arm_left">
+  <visual>
+    <origin xyz="0 0 0.025" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.03 0.03 0.05"/>
+    </geometry>
+    <material name="lower_arm_left-material">
+      <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 0.025" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.03 0.03 0.05"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0 0.025" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
+  </inertial>
+</link>
+
+
+<!-- Left Hand -->
+<joint name="lower_arm_left_to_hand_left" type="revolute">
+  <parent link="lower_arm_left"/>
+  <child link="hand_left"/>
+  <origin xyz="0 0 0.055" rpy="0 0 0"/>
+  <axis xyz="0 1 0"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+
+<link name="hand_left">
+  <visual>
+    <origin xyz="0 0 0.015" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.03 0.03 0.03"/>
+    </geometry>
+    <material name="hand_left-material">
+      <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 0.015" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.03 0.03 0.03"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0 0.015" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666"/>
+  </inertial>
+</link>
+
+
+
+<joint name="base_link_to_upper_leg_right" type="revolute">
+    <parent link="base_link" />
+    <child link="upper_leg_right" />
+    <origin xyz="0.035 0 -0.035" rpy="0 0 0" />
+    <axis xyz="1 0 0"/>
+    <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>  </joint>
+
+  <link name="upper_leg_right">
+  <visual>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.04 0.07"/>
+    </geometry>
+    <material name="upper_leg_right-material">
+      <color rgba="0.0021 0.0497 0.4851 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.04 0.07"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
+  </inertial>
+</link>
+
+
+  <joint name="upper_leg_right_to_lower_leg_right" type="revolute">
+  <parent link="upper_leg_right"/>
+  <child link="lower_leg_right"/>
+  <origin xyz="0 0 -0.075" rpy="0 0 0"/>
+  <axis xyz="-0.9997 -0.0114 0.0229"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+
+  <link name="lower_leg_right">
+  <visual>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.04 0.07"/>
+    </geometry>
+    <material name="lower_leg_right-material">
+      <color rgba="0.0021 0.0497 0.4851 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.04 0.07"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
+  </inertial>
+</link>
+
+
+  <joint name="lower_leg_right_to_foot_right" type="revolute">
+  <parent link="lower_leg_right"/>
+  <child link="foot_right"/>
+  <origin xyz="0 0 -0.075" rpy="0 0 0"/>
+  <axis xyz="-1 0 0"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+<link name="foot_right">
+  <visual>
+    <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.08 0.02"/>
+    </geometry>
+    <material name="foot_right-material">
+      <color rgba="0.0021 0.0497 0.4851 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.08 0.02"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
+  </inertial>
+</link>
+
+
+
+  
+  <!-- Left Upper Leg -->
+<joint name="base_link_to_upper_leg_left" type="revolute">
+  <parent link="base_link"/>
+  <child link="upper_leg_left"/>
+  <origin xyz="-0.035 0 -0.035" rpy="0 0 0"/>
+  <axis xyz="1 0 0"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+<link name="upper_leg_left">
+  <visual>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.04 0.07"/>
+    </geometry>
+    <material name="upper_leg_left-material">
+      <color rgba="0.0021 0.0497 0.4851 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.04 0.07"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
+  </inertial>
+</link>
+
+
+<!-- Left Lower Leg -->
+<joint name="upper_leg_left_to_lower_leg_left" type="revolute">
+  <parent link="upper_leg_left"/>
+  <child link="lower_leg_left"/>
+  <origin xyz="0 0 -0.075" rpy="0 0 0"/>
+  <axis xyz="-0.9997 -0.0114 0.0229"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+<link name="lower_leg_left">
+  <visual>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.04 0.07"/>
+    </geometry>
+    <material name="lower_leg_left-material">
+      <color rgba="0.0021 0.0497 0.4851 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.04 0.07"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0 -0.035" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
+  </inertial>
+</link>
+
+
+<!-- Left Foot -->
+<joint name="lower_leg_left_to_foot_left" type="revolute">
+  <parent link="lower_leg_left"/>
+  <child link="foot_left"/>
+  <origin xyz="0 0 -0.075" rpy="0 0 0"/>
+  <axis xyz="-1 0 0"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+<link name="foot_left">
+  <visual>
+    <origin xyz="0 0.02 -0.01" rpy="0.5 0 0"/>
+    <geometry>
+      <box size="0.04 0.08 0.02"/>
+    </geometry>
+    <material name="foot_left-material">
+      <color rgba="0.0021 0.0497 0.4851 1"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.04 0.08 0.02"/>
+    </geometry>
+  </collision>
+  <inertial>
+    <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
+    <mass value="1"/>
+    <inertia ixx="0.1667" ixy="0" ixz="0" iyy="0.1667" iyz="0" izz="0.1667"/>
+  </inertial>
+</link>
+
+
+<joint name="spine_1_to_neck" type="revolute">
+  <parent link="spine_1"/>
+  <child link="neck"/>
+  <origin xyz="0 0 0.12" rpy="0 0 0"/>
+  <axis xyz="0 0 1"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+  <link name="neck">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.03 0.03 0.03" />
+      </geometry>
+      <material name="neck-material">
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.03 0.03 0.03" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="1" />
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
+    </inertial>
+  </link>
+
+ <joint name="neck_to_head" type="revolute">
+  <parent link="neck"/>
+  <child link="head"/>
+  <origin xyz="0 0 0.04" rpy="0 0 0"/>
+  <axis xyz="0 0 1"/>
+  <limit effort="1000.0" lower="-1" upper="1" velocity="0.5"/>
+</joint>
+
+  <link name="head">
+    <visual>
+      <origin xyz="0 0 0" rpy="-1.5745 3.149 0" />
+       <geometry>
+      <mesh filename="package://Little_Sophia_Face/meshes/Little_Sophia_Face.glb" scale="0.001 0.001 0.001"/>
+    </geometry>
+      <material name="head-material">
+        <color rgba="0.0021246888847058823 0.04970656597728775 0.48514994004665124 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.06 0.06 0.06" />
+      </geometry>
+    </collision>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="1" />
+      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
+    </inertial>
+  </link>
+
+
+  <transmission name="spine_1_trans">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="base_link_to_spine_1">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="spine_motor_1">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+  <transmission name="upper_arm_left">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="spine_1_to_upper_arm_left">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="spine_motor_1">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+ <transmission name="lower_arm_left">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="upper_arm_left_to_lower_arm_left">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="spine_motor_1">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+ <transmission name="hand_left">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="lower_arm_left_to_hand_left">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="spine_motor_1">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="upper_arm_right">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="spine_1_to_upper_arm_right">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="spine_motor_1">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="lower_arm_right">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="upper_arm_right_to_lower_arm_right">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="spine_motor_1">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="hand_right">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="lower_arm_right_to_hand_right">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="spine_motor_1">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="upper_leg_right">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="base_link_to_upper_leg_right">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="upper_leg_motor_right">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="lower_leg_right">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="upper_leg_right_to_lower_leg_right">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="lower_leg_motor_right">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="foot_right">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="lower_leg_right_to_foot_right">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="foot_motor_right">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="upper_leg_left">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="base_link_to_upper_leg_left">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="upper_leg_motor_left">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="lower_leg_left">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="upper_leg_left_to_lower_leg_left">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="lower_leg_motor_left">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+<transmission name="foot_left">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="lower_leg_left_to_foot_left">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="foot_motor_left">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+<transmission name="neck_base">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="spine_1_to_neck">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="neck_motor_base">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>200</encoderValidMin>
+    <encoderValidMax>3500</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+<transmission name="neck_tip">
+  <type>transmission_interface/SimpleTransmission</type>
+  <joint name="neck_to_head">
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+  </joint>
+  <actuator name="neck_motor_tip">
+    <mechanicalReduction>1</mechanicalReduction>
+    <hardwareInterface>PositionJointInterface</hardwareInterface>
+    <encoderTicks>4096</encoderTicks>
+    <encoderValidMin>500</encoderValidMin>
+    <encoderValidMax>4095</encoderValidMax>
+    <encoderRange>270</encoderRange>
+  </actuator>
+</transmission>
+
+</robot>

urdf/sample.urdf

@@ -421,7 +421,7 @@
 
 <link name="foot_left">
   <visual>
-    <origin xyz="0 0.02 -0.01" rpy="0 0 0"/>
+    <origin xyz="0 0.02 -0.01" rpy="0.5 0 0"/>
     <geometry>
       <box size="0.04 0.08 0.02"/>
     </geometry>