From 02cade04408230ef52e8fe34a85be66bcf76dd50 Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <sebcrozet@dimforge.com>
Date: Sun, 26 May 2024 18:18:38 +0200
Subject: feat: more urdf parser fixes + stl parser

---
 Cargo.toml                                  |    2 +-
 assets/3d/T12/README.md                     |    3 +
 assets/3d/T12/meshes/Ankle1.STL             |  Bin 0 -> 37484 bytes
 assets/3d/T12/meshes/Ankle2.STL             |  Bin 0 -> 37484 bytes
 assets/3d/T12/meshes/Ankle3.STL             |  Bin 0 -> 37484 bytes
 assets/3d/T12/meshes/Ankle4.STL             |  Bin 0 -> 37484 bytes
 assets/3d/T12/meshes/Ankle5.STL             |  Bin 0 -> 37484 bytes
 assets/3d/T12/meshes/Ankle6.STL             |  Bin 0 -> 37484 bytes
 assets/3d/T12/meshes/Body.STL               |  Bin 0 -> 30284 bytes
 assets/3d/T12/meshes/Foot1.STL              |  Bin 0 -> 41684 bytes
 assets/3d/T12/meshes/Foot2.STL              |  Bin 0 -> 41684 bytes
 assets/3d/T12/meshes/Foot3.STL              |  Bin 0 -> 41684 bytes
 assets/3d/T12/meshes/Foot4.STL              |  Bin 0 -> 41684 bytes
 assets/3d/T12/meshes/Foot5.STL              |  Bin 0 -> 41684 bytes
 assets/3d/T12/meshes/Foot6.STL              |  Bin 0 -> 41684 bytes
 assets/3d/T12/meshes/Hip1.STL               |  Bin 0 -> 44384 bytes
 assets/3d/T12/meshes/Hip2.STL               |  Bin 0 -> 44384 bytes
 assets/3d/T12/meshes/Hip3.STL               |  Bin 0 -> 44384 bytes
 assets/3d/T12/meshes/Hip4.STL               |  Bin 0 -> 44384 bytes
 assets/3d/T12/meshes/Hip5.STL               |  Bin 0 -> 44384 bytes
 assets/3d/T12/meshes/Hip6.STL               |  Bin 0 -> 44384 bytes
 assets/3d/T12/meshes/Knee1.STL              |  Bin 0 -> 18484 bytes
 assets/3d/T12/meshes/Knee2.STL              |  Bin 0 -> 18484 bytes
 assets/3d/T12/meshes/Knee3.STL              |  Bin 0 -> 18484 bytes
 assets/3d/T12/meshes/Knee4.STL              |  Bin 0 -> 18484 bytes
 assets/3d/T12/meshes/Knee5.STL              |  Bin 0 -> 18484 bytes
 assets/3d/T12/meshes/Knee6.STL              |  Bin 0 -> 18484 bytes
 assets/3d/T12/meshes/Shin1.STL              |  Bin 0 -> 27284 bytes
 assets/3d/T12/meshes/Shin2.STL              |  Bin 0 -> 27284 bytes
 assets/3d/T12/meshes/Shin3.STL              |  Bin 0 -> 27284 bytes
 assets/3d/T12/meshes/Shin4.STL              |  Bin 0 -> 27284 bytes
 assets/3d/T12/meshes/Shin5.STL              |  Bin 0 -> 27284 bytes
 assets/3d/T12/meshes/Shin6.STL              |  Bin 0 -> 27284 bytes
 assets/3d/T12/meshes/Thigh1.STL             |  Bin 0 -> 37584 bytes
 assets/3d/T12/meshes/Thigh2.STL             |  Bin 0 -> 37584 bytes
 assets/3d/T12/meshes/Thigh3.STL             |  Bin 0 -> 37584 bytes
 assets/3d/T12/meshes/Thigh4.STL             |  Bin 0 -> 37584 bytes
 assets/3d/T12/meshes/Thigh5.STL             |  Bin 0 -> 37584 bytes
 assets/3d/T12/meshes/Thigh6.STL             |  Bin 0 -> 37584 bytes
 assets/3d/T12/urdf/T12.URDF                 | 2131 +++++++++++++++++++++++++++
 assets/3d/T12/urdf/T12_flipped.URDF         | 2131 +++++++++++++++++++++++++++
 assets/3d/T12/urdf/T12_wrong_axes.orig.URDF | 2131 +++++++++++++++++++++++++++
 assets/3d/sample.urdf                       |  132 --
 crates/rapier-stl/Cargo.toml                |   10 +
 crates/rapier-stl/src/lib.rs                |   72 +
 crates/rapier-urdf/Cargo.toml               |   14 +-
 crates/rapier-urdf/src/lib.rs               |  304 ++--
 examples3d/Cargo.toml                       |    1 +
 examples3d/urdf3.rs                         |   22 +-
 49 files changed, 6703 insertions(+), 250 deletions(-)
 create mode 100644 assets/3d/T12/README.md
 create mode 100644 assets/3d/T12/meshes/Ankle1.STL
 create mode 100644 assets/3d/T12/meshes/Ankle2.STL
 create mode 100644 assets/3d/T12/meshes/Ankle3.STL
 create mode 100644 assets/3d/T12/meshes/Ankle4.STL
 create mode 100644 assets/3d/T12/meshes/Ankle5.STL
 create mode 100644 assets/3d/T12/meshes/Ankle6.STL
 create mode 100644 assets/3d/T12/meshes/Body.STL
 create mode 100644 assets/3d/T12/meshes/Foot1.STL
 create mode 100644 assets/3d/T12/meshes/Foot2.STL
 create mode 100644 assets/3d/T12/meshes/Foot3.STL
 create mode 100644 assets/3d/T12/meshes/Foot4.STL
 create mode 100644 assets/3d/T12/meshes/Foot5.STL
 create mode 100644 assets/3d/T12/meshes/Foot6.STL
 create mode 100644 assets/3d/T12/meshes/Hip1.STL
 create mode 100644 assets/3d/T12/meshes/Hip2.STL
 create mode 100644 assets/3d/T12/meshes/Hip3.STL
 create mode 100644 assets/3d/T12/meshes/Hip4.STL
 create mode 100644 assets/3d/T12/meshes/Hip5.STL
 create mode 100644 assets/3d/T12/meshes/Hip6.STL
 create mode 100644 assets/3d/T12/meshes/Knee1.STL
 create mode 100644 assets/3d/T12/meshes/Knee2.STL
 create mode 100644 assets/3d/T12/meshes/Knee3.STL
 create mode 100644 assets/3d/T12/meshes/Knee4.STL
 create mode 100644 assets/3d/T12/meshes/Knee5.STL
 create mode 100644 assets/3d/T12/meshes/Knee6.STL
 create mode 100644 assets/3d/T12/meshes/Shin1.STL
 create mode 100644 assets/3d/T12/meshes/Shin2.STL
 create mode 100644 assets/3d/T12/meshes/Shin3.STL
 create mode 100644 assets/3d/T12/meshes/Shin4.STL
 create mode 100644 assets/3d/T12/meshes/Shin5.STL
 create mode 100644 assets/3d/T12/meshes/Shin6.STL
 create mode 100644 assets/3d/T12/meshes/Thigh1.STL
 create mode 100644 assets/3d/T12/meshes/Thigh2.STL
 create mode 100644 assets/3d/T12/meshes/Thigh3.STL
 create mode 100644 assets/3d/T12/meshes/Thigh4.STL
 create mode 100644 assets/3d/T12/meshes/Thigh5.STL
 create mode 100644 assets/3d/T12/meshes/Thigh6.STL
 create mode 100644 assets/3d/T12/urdf/T12.URDF
 create mode 100644 assets/3d/T12/urdf/T12_flipped.URDF
 create mode 100644 assets/3d/T12/urdf/T12_wrong_axes.orig.URDF
 delete mode 100644 assets/3d/sample.urdf
 create mode 100644 crates/rapier-stl/Cargo.toml
 create mode 100644 crates/rapier-stl/src/lib.rs

diff --git a/Cargo.toml b/Cargo.toml
index 36b49f1..f176685 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,6 +1,6 @@
 [workspace]
 members = ["crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d",
-    "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d", "crates/rapier-urdf"]
+    "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d", "crates/rapier-urdf", "crates/rapier-stl"]
 resolver = "2"
 
 [patch.crates-io]
diff --git a/assets/3d/T12/README.md b/assets/3d/T12/README.md
new file mode 100644
index 0000000..011d8e5
--- /dev/null
+++ b/assets/3d/T12/README.md
@@ -0,0 +1,3 @@
+These samples files originate from the repository
+[gkjohnson/urdf-loaders](https://github.com/gkjohnson/urdf-loaders/tree/b67f5de98f6222e2d921ce24f46a6725dad9704e/urdf/T12)
+(Apache 2.0 license).
\ No newline at end of file
diff --git a/assets/3d/T12/meshes/Ankle1.STL b/assets/3d/T12/meshes/Ankle1.STL
new file mode 100644
index 0000000..0aa9ec5
Binary files /dev/null and b/assets/3d/T12/meshes/Ankle1.STL differ
diff --git a/assets/3d/T12/meshes/Ankle2.STL b/assets/3d/T12/meshes/Ankle2.STL
new file mode 100644
index 0000000..0aa9ec5
Binary files /dev/null and b/assets/3d/T12/meshes/Ankle2.STL differ
diff --git a/assets/3d/T12/meshes/Ankle3.STL b/assets/3d/T12/meshes/Ankle3.STL
new file mode 100644
index 0000000..0aa9ec5
Binary files /dev/null and b/assets/3d/T12/meshes/Ankle3.STL differ
diff --git a/assets/3d/T12/meshes/Ankle4.STL b/assets/3d/T12/meshes/Ankle4.STL
new file mode 100644
index 0000000..0aa9ec5
Binary files /dev/null and b/assets/3d/T12/meshes/Ankle4.STL differ
diff --git a/assets/3d/T12/meshes/Ankle5.STL b/assets/3d/T12/meshes/Ankle5.STL
new file mode 100644
index 0000000..0aa9ec5
Binary files /dev/null and b/assets/3d/T12/meshes/Ankle5.STL differ
diff --git a/assets/3d/T12/meshes/Ankle6.STL b/assets/3d/T12/meshes/Ankle6.STL
new file mode 100644
index 0000000..0aa9ec5
Binary files /dev/null and b/assets/3d/T12/meshes/Ankle6.STL differ
diff --git a/assets/3d/T12/meshes/Body.STL b/assets/3d/T12/meshes/Body.STL
new file mode 100644
index 0000000..3379440
Binary files /dev/null and b/assets/3d/T12/meshes/Body.STL differ
diff --git a/assets/3d/T12/meshes/Foot1.STL b/assets/3d/T12/meshes/Foot1.STL
new file mode 100644
index 0000000..67da40c
Binary files /dev/null and b/assets/3d/T12/meshes/Foot1.STL differ
diff --git a/assets/3d/T12/meshes/Foot2.STL b/assets/3d/T12/meshes/Foot2.STL
new file mode 100644
index 0000000..67da40c
Binary files /dev/null and b/assets/3d/T12/meshes/Foot2.STL differ
diff --git a/assets/3d/T12/meshes/Foot3.STL b/assets/3d/T12/meshes/Foot3.STL
new file mode 100644
index 0000000..67da40c
Binary files /dev/null and b/assets/3d/T12/meshes/Foot3.STL differ
diff --git a/assets/3d/T12/meshes/Foot4.STL b/assets/3d/T12/meshes/Foot4.STL
new file mode 100644
index 0000000..67da40c
Binary files /dev/null and b/assets/3d/T12/meshes/Foot4.STL differ
diff --git a/assets/3d/T12/meshes/Foot5.STL b/assets/3d/T12/meshes/Foot5.STL
new file mode 100644
index 0000000..67da40c
Binary files /dev/null and b/assets/3d/T12/meshes/Foot5.STL differ
diff --git a/assets/3d/T12/meshes/Foot6.STL b/assets/3d/T12/meshes/Foot6.STL
new file mode 100644
index 0000000..67da40c
Binary files /dev/null and b/assets/3d/T12/meshes/Foot6.STL differ
diff --git a/assets/3d/T12/meshes/Hip1.STL b/assets/3d/T12/meshes/Hip1.STL
new file mode 100644
index 0000000..9a7df1a
Binary files /dev/null and b/assets/3d/T12/meshes/Hip1.STL differ
diff --git a/assets/3d/T12/meshes/Hip2.STL b/assets/3d/T12/meshes/Hip2.STL
new file mode 100644
index 0000000..9a7df1a
Binary files /dev/null and b/assets/3d/T12/meshes/Hip2.STL differ
diff --git a/assets/3d/T12/meshes/Hip3.STL b/assets/3d/T12/meshes/Hip3.STL
new file mode 100644
index 0000000..9a7df1a
Binary files /dev/null and b/assets/3d/T12/meshes/Hip3.STL differ
diff --git a/assets/3d/T12/meshes/Hip4.STL b/assets/3d/T12/meshes/Hip4.STL
new file mode 100644
index 0000000..9a7df1a
Binary files /dev/null and b/assets/3d/T12/meshes/Hip4.STL differ
diff --git a/assets/3d/T12/meshes/Hip5.STL b/assets/3d/T12/meshes/Hip5.STL
new file mode 100644
index 0000000..9a7df1a
Binary files /dev/null and b/assets/3d/T12/meshes/Hip5.STL differ
diff --git a/assets/3d/T12/meshes/Hip6.STL b/assets/3d/T12/meshes/Hip6.STL
new file mode 100644
index 0000000..9a7df1a
Binary files /dev/null and b/assets/3d/T12/meshes/Hip6.STL differ
diff --git a/assets/3d/T12/meshes/Knee1.STL b/assets/3d/T12/meshes/Knee1.STL
new file mode 100644
index 0000000..9057da7
Binary files /dev/null and b/assets/3d/T12/meshes/Knee1.STL differ
diff --git a/assets/3d/T12/meshes/Knee2.STL b/assets/3d/T12/meshes/Knee2.STL
new file mode 100644
index 0000000..9057da7
Binary files /dev/null and b/assets/3d/T12/meshes/Knee2.STL differ
diff --git a/assets/3d/T12/meshes/Knee3.STL b/assets/3d/T12/meshes/Knee3.STL
new file mode 100644
index 0000000..9057da7
Binary files /dev/null and b/assets/3d/T12/meshes/Knee3.STL differ
diff --git a/assets/3d/T12/meshes/Knee4.STL b/assets/3d/T12/meshes/Knee4.STL
new file mode 100644
index 0000000..9057da7
Binary files /dev/null and b/assets/3d/T12/meshes/Knee4.STL differ
diff --git a/assets/3d/T12/meshes/Knee5.STL b/assets/3d/T12/meshes/Knee5.STL
new file mode 100644
index 0000000..9057da7
Binary files /dev/null and b/assets/3d/T12/meshes/Knee5.STL differ
diff --git a/assets/3d/T12/meshes/Knee6.STL b/assets/3d/T12/meshes/Knee6.STL
new file mode 100644
index 0000000..9057da7
Binary files /dev/null and b/assets/3d/T12/meshes/Knee6.STL differ
diff --git a/assets/3d/T12/meshes/Shin1.STL b/assets/3d/T12/meshes/Shin1.STL
new file mode 100644
index 0000000..e515e76
Binary files /dev/null and b/assets/3d/T12/meshes/Shin1.STL differ
diff --git a/assets/3d/T12/meshes/Shin2.STL b/assets/3d/T12/meshes/Shin2.STL
new file mode 100644
index 0000000..e515e76
Binary files /dev/null and b/assets/3d/T12/meshes/Shin2.STL differ
diff --git a/assets/3d/T12/meshes/Shin3.STL b/assets/3d/T12/meshes/Shin3.STL
new file mode 100644
index 0000000..e515e76
Binary files /dev/null and b/assets/3d/T12/meshes/Shin3.STL differ
diff --git a/assets/3d/T12/meshes/Shin4.STL b/assets/3d/T12/meshes/Shin4.STL
new file mode 100644
index 0000000..e515e76
Binary files /dev/null and b/assets/3d/T12/meshes/Shin4.STL differ
diff --git a/assets/3d/T12/meshes/Shin5.STL b/assets/3d/T12/meshes/Shin5.STL
new file mode 100644
index 0000000..e515e76
Binary files /dev/null and b/assets/3d/T12/meshes/Shin5.STL differ
diff --git a/assets/3d/T12/meshes/Shin6.STL b/assets/3d/T12/meshes/Shin6.STL
new file mode 100644
index 0000000..e515e76
Binary files /dev/null and b/assets/3d/T12/meshes/Shin6.STL differ
diff --git a/assets/3d/T12/meshes/Thigh1.STL b/assets/3d/T12/meshes/Thigh1.STL
new file mode 100644
index 0000000..d134267
Binary files /dev/null and b/assets/3d/T12/meshes/Thigh1.STL differ
diff --git a/assets/3d/T12/meshes/Thigh2.STL b/assets/3d/T12/meshes/Thigh2.STL
new file mode 100644
index 0000000..d134267
Binary files /dev/null and b/assets/3d/T12/meshes/Thigh2.STL differ
diff --git a/assets/3d/T12/meshes/Thigh3.STL b/assets/3d/T12/meshes/Thigh3.STL
new file mode 100644
index 0000000..d134267
Binary files /dev/null and b/assets/3d/T12/meshes/Thigh3.STL differ
diff --git a/assets/3d/T12/meshes/Thigh4.STL b/assets/3d/T12/meshes/Thigh4.STL
new file mode 100644
index 0000000..d134267
Binary files /dev/null and b/assets/3d/T12/meshes/Thigh4.STL differ
diff --git a/assets/3d/T12/meshes/Thigh5.STL b/assets/3d/T12/meshes/Thigh5.STL
new file mode 100644
index 0000000..d134267
Binary files /dev/null and b/assets/3d/T12/meshes/Thigh5.STL differ
diff --git a/assets/3d/T12/meshes/Thigh6.STL b/assets/3d/T12/meshes/Thigh6.STL
new file mode 100644
index 0000000..d134267
Binary files /dev/null and b/assets/3d/T12/meshes/Thigh6.STL differ
diff --git a/assets/3d/T12/urdf/T12.URDF b/assets/3d/T12/urdf/T12.URDF
new file mode 100644
index 0000000..364b0cf
--- /dev/null
+++ b/assets/3d/T12/urdf/T12.URDF
@@ -0,0 +1,2131 @@
+<robot
+  name="T12">
+  <link
+    name="Body">
+    <inertial>
+      <origin
+        xyz="0.015789 0.0084982 0.11668"
+        rpy="0 0 0" />
+      <mass
+        value="266.06" />
+      <inertia
+        ixx="225.36"
+        ixy="0.054583"
+        ixz="0.010358"
+        iyy="210.35"
+        iyz="-0.0001937"
+        izz="424.6" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Body.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Body.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="Hip1">
+    <inertial>
+      <origin
+        xyz="0.12059 0.04703 -0.043975"
+        rpy="0 0 0" />
+      <mass
+        value="10.161" />
+      <inertia
+        ixx="0.69317"
+        ixy="0.0073762"
+        ixz="0.040711"
+        iyy="0.34376"
+        iyz="0.0014897"
+        izz="0.9152" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HY1"
+    type="revolute">
+    <origin
+      xyz="1.7322 0.99572 0.41034"
+      rpy="-4.4608E-16 2.8513E-16 0.5236" />
+    <parent
+      link="Body" />
+    <child
+      link="Hip1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Thigh1">
+    <inertial>
+      <origin
+        xyz="0.37052 1.2567E-08 -0.0016621"
+        rpy="0 0 0" />
+      <mass
+        value="14.238" />
+      <inertia
+        ixx="1.0008"
+        ixy="7.0442E-08"
+        ixz="0.0087687"
+        iyy="2.0253"
+        iyz="-0.00024727"
+        izz="1.3252" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HP1"
+    type="revolute">
+    <origin
+      xyz="0.36808 0 0"
+      rpy="-1.5708 1.2704E-14 -2.2204E-16" />
+    <parent
+      link="Hip1" />
+    <child
+      link="Thigh1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="1.5708"
+      effort="7500"
+      velocity="0" />
+  </joint>
+  <link
+    name="Knee1">
+    <inertial>
+      <origin
+        xyz="0.017762 -0.027385 0.0067752"
+        rpy="0 0 0" />
+      <mass
+        value="5.7913" />
+      <inertia
+        ixx="0.12768"
+        ixy="0.0042111"
+        ixz="-0.00069417"
+        iyy="0.12196"
+        iyz="0.0012334"
+        izz="0.079088" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KP1"
+    type="revolute">
+    <origin
+      xyz="0.8627 0 0"
+      rpy="2.2204E-16 2.6822E-16 1.5708" />
+    <parent
+      link="Thigh1" />
+    <child
+      link="Knee1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Shin1">
+    <inertial>
+      <origin
+        xyz="-1.4482E-09 0.00011789 0.4127"
+        rpy="0 0 0" />
+      <mass
+        value="10.201" />
+      <inertia
+        ixx="0.7541"
+        ixy="-0.00016423"
+        ixz="3.2671E-09"
+        iyy="0.53994"
+        iyz="-0.00028063"
+        izz="0.35892" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KR1"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 -2.7791E-17 8.7217E-15" />
+    <parent
+      link="Knee1" />
+    <child
+      link="Shin1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Ankle1">
+    <inertial>
+      <origin
+        xyz="0.020168 -0.026001 0.0079686"
+        rpy="0 0 0" />
+      <mass
+        value="5.108" />
+      <inertia
+        ixx="0.094931"
+        ixy="0.0040571"
+        ixz="-0.00081815"
+        iyy="0.089357"
+        iyz="0.00089845"
+        izz="0.073015" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AP1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.79818"
+      rpy="-1.5708 -1.3823E-15 -9.3729E-17" />
+    <parent
+      link="Shin1" />
+    <child
+      link="Ankle1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.0944"
+      upper="2.0944"
+      effort="1350"
+      velocity="0" />
+  </joint>
+  <link
+    name="Foot1">
+    <inertial>
+      <origin
+        xyz="-3.0189E-05 0.0041753 0.55175"
+        rpy="0 0 0" />
+      <mass
+        value="27.416" />
+      <inertia
+        ixx="2.0991"
+        ixy="-4.1499E-05"
+        ixz="6.0881E-05"
+        iyy="2.468"
+        iyz="0.023298"
+        izz="0.8501" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AR1"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 4.5989E-16 1.2639E-15" />
+    <parent
+      link="Ankle1" />
+    <child
+      link="Foot1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Hip2">
+    <inertial>
+      <origin
+        xyz="0.11841 0.050794 -0.043975"
+        rpy="0 0 0" />
+      <mass
+        value="10.161" />
+      <inertia
+        ixx="0.69317"
+        ixy="0.0073791"
+        ixz="0.040711"
+        iyy="0.34375"
+        iyz="0.0014908"
+        izz="0.91521" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HY2"
+    type="revolute">
+    <origin
+      xyz="0.0037717 1.998 0.41034"
+      rpy="-4.4608E-16 2.8513E-16 1.5708" />
+    <parent
+      link="Body" />
+    <child
+      link="Hip2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Thigh2">
+    <inertial>
+      <origin
+        xyz="0.36835 1.2571E-08 0.0020978"
+        rpy="0 0 0" />
+      <mass
+        value="14.238" />
+      <inertia
+        ixx="1.0008"
+        ixy="7.0776E-08"
+        ixz="0.0088078"
+        iyy="2.0253"
+        iyz="-0.00024727"
+        izz="1.3252" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HP2"
+    type="revolute">
+    <origin
+      xyz="0.36808 0 0"
+      rpy="-1.5708 1.2704E-14 -2.1472E-16" />
+    <parent
+      link="Hip2" />
+    <child
+      link="Thigh2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="1.5708"
+      effort="7500"
+      velocity="0" />
+  </joint>
+  <link
+    name="Knee2">
+    <inertial>
+      <origin
+        xyz="0.017762 -0.025217 0.010531"
+        rpy="0 0 0" />
+      <mass
+        value="5.7913" />
+      <inertia
+        ixx="0.12768"
+        ixy="0.0042106"
+        ixz="-0.00069509"
+        iyy="0.12197"
+        iyz="0.0012317"
+        izz="0.079085" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KP2"
+    type="revolute">
+    <origin
+      xyz="0.8627 0 0"
+      rpy="2.2937E-16 2.3338E-16 1.5708" />
+    <parent
+      link="Thigh2" />
+    <child
+      link="Knee2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Shin2">
+    <inertial>
+      <origin
+        xyz="-1.3842E-09 0.0038981 0.41085"
+        rpy="0 0 0" />
+      <mass
+        value="10.201" />
+      <inertia
+        ixx="0.75448"
+        ixy="-0.00016423"
+        ixz="3.1853E-09"
+        iyy="0.54028"
+        iyz="3.2414E-05"
+        izz="0.35896" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KR2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 -2.7791E-17 8.7217E-15" />
+    <parent
+      link="Knee2" />
+    <child
+      link="Shin2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Ankle2">
+    <inertial>
+      <origin
+        xyz="0.020168 -0.023832 0.011725"
+        rpy="0 0 0" />
+      <mass
+        value="5.108" />
+      <inertia
+        ixx="0.09493"
+        ixy="0.0040565"
+        ixz="-0.00081912"
+        iyy="0.089358"
+        iyz="0.00089727"
+        izz="0.073013" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AP2"
+    type="revolute">
+    <origin
+      xyz="0 0 0.79818"
+      rpy="-1.5708 -1.4672E-15 -1.2695E-16" />
+    <parent
+      link="Shin2" />
+    <child
+      link="Ankle2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.0944"
+      upper="2.0944"
+      effort="1350"
+      velocity="0" />
+  </joint>
+  <link
+    name="Foot2">
+    <inertial>
+      <origin
+        xyz="-3.0121E-05 0.007929 0.54959"
+        rpy="0 0 0" />
+      <mass
+        value="27.416" />
+      <inertia
+        ixx="2.0992"
+        ixy="-4.1277E-05"
+        ixz="6.0958E-05"
+        iyy="2.468"
+        iyz="0.023238"
+        izz="0.85011" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AR2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 4.2667E-16 1.3488E-15" />
+    <parent
+      link="Ankle2" />
+    <child
+      link="Foot2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Hip3">
+    <inertial>
+      <origin
+        xyz="0.12059 0.054559 -0.043975"
+        rpy="0 0 0" />
+      <mass
+        value="10.161" />
+      <inertia
+        ixx="0.69318"
+        ixy="0.0073858"
+        ixz="0.040711"
+        iyy="0.34376"
+        iyz="0.0014917"
+        izz="0.91522" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HY3"
+    type="revolute">
+    <origin
+      xyz="-1.7284 1.0023 0.41034"
+      rpy="-4.4608E-16 3.1089E-16 2.618" />
+    <parent
+      link="Body" />
+    <child
+      link="Hip3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Thigh3">
+    <inertial>
+      <origin
+        xyz="0.37052 1.2575E-08 0.0058577"
+        rpy="0 0 0" />
+      <mass
+        value="14.238" />
+      <inertia
+        ixx="1.0008"
+        ixy="7.0333E-08"
+        ixz="0.0088473"
+        iyy="2.0253"
+        iyz="-0.00024727"
+        izz="1.3252" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HP3"
+    type="revolute">
+    <origin
+      xyz="0.36808 0 0"
+      rpy="-1.5708 1.2512E-14 5.7201E-30" />
+    <parent
+      link="Hip3" />
+    <child
+      link="Thigh3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="1.5708"
+      effort="7500"
+      velocity="0" />
+  </joint>
+  <link
+    name="Knee3">
+    <inertial>
+      <origin
+        xyz="0.017762 -0.027385 0.014286"
+        rpy="0 0 0" />
+      <mass
+        value="5.7913" />
+      <inertia
+        ixx="0.12768"
+        ixy="0.0042111"
+        ixz="-0.00069602"
+        iyy="0.12197"
+        iyz="0.001229"
+        izz="0.079087" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KP3"
+    type="revolute">
+    <origin
+      xyz="0.8627 0 0"
+      rpy="2.2204E-16 3.2661E-16 1.5708" />
+    <parent
+      link="Thigh3" />
+    <child
+      link="Knee3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Shin3">
+    <inertial>
+      <origin
+        xyz="-1.4539E-09 0.0076634 0.41302"
+        rpy="0 0 0" />
+      <mass
+        value="10.201" />
+      <inertia
+        ixx="0.75448"
+        ixy="-0.00016423"
+        ixz="3.2688E-09"
+        iyy="0.54028"
+        iyz="3.6604E-05"
+        izz="0.35897" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KR3"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 -2.7791E-17 8.7217E-15" />
+    <parent
+      link="Knee3" />
+    <child
+      link="Shin3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Ankle3">
+    <inertial>
+      <origin
+        xyz="0.020167 -0.026001 0.015481"
+        rpy="0 0 0" />
+      <mass
+        value="5.108" />
+      <inertia
+        ixx="0.094934"
+        ixy="0.004057"
+        ixz="-0.00082009"
+        iyy="0.089361"
+        iyz="0.00089501"
+        izz="0.073015" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AP3"
+    type="revolute">
+    <origin
+      xyz="0 0 0.79818"
+      rpy="-1.5708 -1.426E-15 -1.3449E-16" />
+    <parent
+      link="Shin3" />
+    <child
+      link="Ankle3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.0944"
+      upper="2.0944"
+      effort="1350"
+      velocity="0" />
+  </joint>
+  <link
+    name="Foot3">
+    <inertial>
+      <origin
+        xyz="-3.0139E-05 0.011683 0.55175"
+        rpy="0 0 0" />
+      <mass
+        value="27.416" />
+      <inertia
+        ixx="2.0991"
+        ixy="-4.1381E-05"
+        ixz="6.0898E-05"
+        iyy="2.468"
+        iyz="0.023183"
+        izz="0.85012" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AR3"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 4.1913E-16 1.3076E-15" />
+    <parent
+      link="Ankle3" />
+    <child
+      link="Foot3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Hip4">
+    <inertial>
+      <origin
+        xyz="0.12059 0.04703 -0.043975"
+        rpy="0 0 0" />
+      <mass
+        value="10.161" />
+      <inertia
+        ixx="0.69317"
+        ixy="0.0073762"
+        ixz="0.040711"
+        iyy="0.34376"
+        iyz="0.0014897"
+        izz="0.9152" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HY4"
+    type="revolute">
+    <origin
+      xyz="-1.7322 -0.99572 0.41034"
+      rpy="-4.4608E-16 2.8513E-16 -2.618" />
+    <parent
+      link="Body" />
+    <child
+      link="Hip4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Thigh4">
+    <inertial>
+      <origin
+        xyz="0.37052 1.2567E-08 -0.0016621"
+        rpy="0 0 0" />
+      <mass
+        value="14.238" />
+      <inertia
+        ixx="1.0008"
+        ixy="7.0442E-08"
+        ixz="0.0087687"
+        iyy="2.0253"
+        iyz="-0.00024727"
+        izz="1.3252" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HP4"
+    type="revolute">
+    <origin
+      xyz="0.36808 0 0"
+      rpy="-1.5708 1.2704E-14 -2.7756E-16" />
+    <parent
+      link="Hip4" />
+    <child
+      link="Thigh4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="1.5708"
+      effort="7500"
+      velocity="0" />
+  </joint>
+  <link
+    name="Knee4">
+    <inertial>
+      <origin
+        xyz="0.017762 -0.027385 0.0067752"
+        rpy="0 0 0" />
+      <mass
+        value="5.7913" />
+      <inertia
+        ixx="0.12768"
+        ixy="0.0042111"
+        ixz="-0.00069417"
+        iyy="0.12196"
+        iyz="0.0012334"
+        izz="0.079088" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KP4"
+    type="revolute">
+    <origin
+      xyz="0.8627 0 0"
+      rpy="2.2204E-16 2.645E-16 1.5708" />
+    <parent
+      link="Thigh4" />
+    <child
+      link="Knee4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Shin4">
+    <inertial>
+      <origin
+        xyz="-1.4482E-09 0.00011789 0.4127"
+        rpy="0 0 0" />
+      <mass
+        value="10.201" />
+      <inertia
+        ixx="0.7541"
+        ixy="-0.00016423"
+        ixz="3.2671E-09"
+        iyy="0.53994"
+        iyz="-0.00028063"
+        izz="0.35892" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KR4"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 -2.7791E-17 8.7217E-15" />
+    <parent
+      link="Knee4" />
+    <child
+      link="Shin4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Ankle4">
+    <inertial>
+      <origin
+        xyz="0.020168 -0.026001 0.0079686"
+        rpy="0 0 0" />
+      <mass
+        value="5.108" />
+      <inertia
+        ixx="0.094931"
+        ixy="0.0040571"
+        ixz="-0.00081815"
+        iyy="0.089357"
+        iyz="0.00089845"
+        izz="0.073015" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AP4"
+    type="revolute">
+    <origin
+      xyz="0 0 0.79818"
+      rpy="-1.5708 -1.3823E-15 -9.3729E-17" />
+    <parent
+      link="Shin4" />
+    <child
+      link="Ankle4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.0944"
+      upper="2.0944"
+      effort="1350"
+      velocity="0" />
+  </joint>
+  <link
+    name="Foot4">
+    <inertial>
+      <origin
+        xyz="-3.0257E-05 0.0041752 0.55175"
+        rpy="0 0 0" />
+      <mass
+        value="27.416" />
+      <inertia
+        ixx="2.0991"
+        ixy="-4.1743E-05"
+        ixz="6.0625E-05"
+        iyy="2.468"
+        iyz="0.023298"
+        izz="0.8501" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AR4"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 4.5989E-16 1.2639E-15" />
+    <parent
+      link="Ankle4" />
+    <child
+      link="Foot4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Hip5">
+    <inertial>
+      <origin
+        xyz="0.11841 0.050794 -0.043975"
+        rpy="0 0 0" />
+      <mass
+        value="10.161" />
+      <inertia
+        ixx="0.69317"
+        ixy="0.0073791"
+        ixz="0.040711"
+        iyy="0.34375"
+        iyz="0.0014908"
+        izz="0.91521" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HY5"
+    type="revolute">
+    <origin
+      xyz="-0.0037717 -1.998 0.41034"
+      rpy="-4.4608E-16 2.8513E-16 -1.5708" />
+    <parent
+      link="Body" />
+    <child
+      link="Hip5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Thigh5">
+    <inertial>
+      <origin
+        xyz="0.36835 1.2571E-08 0.0020978"
+        rpy="0 0 0" />
+      <mass
+        value="14.238" />
+      <inertia
+        ixx="1.0008"
+        ixy="7.0776E-08"
+        ixz="0.0088078"
+        iyy="2.0253"
+        iyz="-0.00024727"
+        izz="1.3252" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HP5"
+    type="revolute">
+    <origin
+      xyz="0.36808 0 0"
+      rpy="-1.5708 1.2704E-14 -1.1102E-16" />
+    <parent
+      link="Hip5" />
+    <child
+      link="Thigh5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="1.5708"
+      effort="7500"
+      velocity="0" />
+  </joint>
+  <link
+    name="Knee5">
+    <inertial>
+      <origin
+        xyz="0.017762 -0.025217 0.010531"
+        rpy="0 0 0" />
+      <mass
+        value="5.7913" />
+      <inertia
+        ixx="0.12768"
+        ixy="0.0042106"
+        ixz="-0.00069509"
+        iyy="0.12197"
+        iyz="0.0012317"
+        izz="0.079085" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KP5"
+    type="revolute">
+    <origin
+      xyz="0.8627 0 0"
+      rpy="3.3307E-16 2.741E-16 1.5708" />
+    <parent
+      link="Thigh5" />
+    <child
+      link="Knee5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Shin5">
+    <inertial>
+      <origin
+        xyz="-1.3855E-09 0.0038831 0.41053"
+        rpy="0 0 0" />
+      <mass
+        value="10.201" />
+      <inertia
+        ixx="0.75409"
+        ixy="-0.00016423"
+        ixz="3.1801E-09"
+        iyy="0.53993"
+        iyz="-0.00027675"
+        izz="0.35892" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KR5"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 -2.7791E-17 8.7217E-15" />
+    <parent
+      link="Knee5" />
+    <child
+      link="Shin5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Ankle5">
+    <inertial>
+      <origin
+        xyz="0.020168 -0.023832 0.011725"
+        rpy="0 0 0" />
+      <mass
+        value="5.108" />
+      <inertia
+        ixx="0.09493"
+        ixy="0.0040565"
+        ixz="-0.00081912"
+        iyy="0.089358"
+        iyz="0.00089727"
+        izz="0.073013" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AP5"
+    type="revolute">
+    <origin
+      xyz="0 0 0.79818"
+      rpy="-1.5708 -1.4672E-15 -1.2364E-16" />
+    <parent
+      link="Shin5" />
+    <child
+      link="Ankle5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.0944"
+      upper="2.0944"
+      effort="1350"
+      velocity="0" />
+  </joint>
+  <link
+    name="Foot5">
+    <inertial>
+      <origin
+        xyz="-3.0002E-05 0.0079289 0.54959"
+        rpy="0 0 0" />
+      <mass
+        value="27.416" />
+      <inertia
+        ixx="2.0992"
+        ixy="-4.0815E-05"
+        ixz="6.1178E-05"
+        iyy="2.468"
+        iyz="0.023238"
+        izz="0.85011" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AR5"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 4.2998E-16 1.3488E-15" />
+    <parent
+      link="Ankle5" />
+    <child
+      link="Foot5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Hip6">
+    <inertial>
+      <origin
+        xyz="0.12059 0.054559 -0.043975"
+        rpy="0 0 0" />
+      <mass
+        value="10.161" />
+      <inertia
+        ixx="0.69318"
+        ixy="0.0073858"
+        ixz="0.040711"
+        iyy="0.34376"
+        iyz="0.0014917"
+        izz="0.91522" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HY6"
+    type="revolute">
+    <origin
+      xyz="1.7284 -1.0023 0.41034"
+      rpy="-4.4608E-16 2.8513E-16 -0.5236" />
+    <parent
+      link="Body" />
+    <child
+      link="Hip6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Thigh6">
+    <inertial>
+      <origin
+        xyz="0.37052 1.2575E-08 0.0058577"
+        rpy="0 0 0" />
+      <mass
+        value="14.238" />
+      <inertia
+        ixx="1.0008"
+        ixy="7.0333E-08"
+        ixz="0.0088473"
+        iyy="2.0253"
+        iyz="-0.00024727"
+        izz="1.3252" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HP6"
+    type="revolute">
+    <origin
+      xyz="0.36808 0 0"
+      rpy="-1.5708 1.2704E-14 -2.2204E-16" />
+    <parent
+      link="Hip6" />
+    <child
+      link="Thigh6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="1.5708"
+      effort="7500"
+      velocity="0" />
+  </joint>
+  <link
+    name="Knee6">
+    <inertial>
+      <origin
+        xyz="0.017762 -0.027385 0.014286"
+        rpy="0 0 0" />
+      <mass
+        value="5.7913" />
+      <inertia
+        ixx="0.12768"
+        ixy="0.0042111"
+        ixz="-0.00069602"
+        iyy="0.12197"
+        iyz="0.001229"
+        izz="0.079087" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KP6"
+    type="revolute">
+    <origin
+      xyz="0.8627 0 0"
+      rpy="2.7756E-16 2.3418E-16 1.5708" />
+    <parent
+      link="Thigh6" />
+    <child
+      link="Knee6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-1.9199"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Shin6">
+    <inertial>
+      <origin
+        xyz="-1.454E-09 0.0076484 0.4127"
+        rpy="0 0 0" />
+      <mass
+        value="10.201" />
+      <inertia
+        ixx="0.7541"
+        ixy="-0.00016423"
+        ixz="3.2639E-09"
+        iyy="0.53994"
+        iyz="-0.00027259"
+        izz="0.35892" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KR6"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 -2.7791E-17 8.7217E-15" />
+    <parent
+      link="Knee6" />
+    <child
+      link="Shin6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Ankle6">
+    <inertial>
+      <origin
+        xyz="0.020167 -0.026001 0.015481"
+        rpy="0 0 0" />
+      <mass
+        value="5.108" />
+      <inertia
+        ixx="0.094934"
+        ixy="0.004057"
+        ixz="-0.00082009"
+        iyy="0.089361"
+        iyz="0.00089501"
+        izz="0.073015" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AP6"
+    type="revolute">
+    <origin
+      xyz="0 0 0.79818"
+      rpy="-1.5708 -1.4121E-15 -1.1045E-16" />
+    <parent
+      link="Shin6" />
+    <child
+      link="Ankle6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.0944"
+      upper="2.0944"
+      effort="1350"
+      velocity="0" />
+  </joint>
+  <link
+    name="Foot6">
+    <inertial>
+      <origin
+        xyz="-3.0159E-05 0.011683 0.55175"
+        rpy="0 0 0" />
+      <mass
+        value="27.416" />
+      <inertia
+        ixx="2.0991"
+        ixy="-4.1451E-05"
+        ixz="6.0863E-05"
+        iyy="2.468"
+        iyz="0.023183"
+        izz="0.85012" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Foot6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AR6"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 4.4317E-16 1.2937E-15" />
+    <parent
+      link="Ankle6" />
+    <child
+      link="Foot6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+</robot>
diff --git a/assets/3d/T12/urdf/T12_flipped.URDF b/assets/3d/T12/urdf/T12_flipped.URDF
new file mode 100644
index 0000000..fcfe7e8
--- /dev/null
+++ b/assets/3d/T12/urdf/T12_flipped.URDF
@@ -0,0 +1,2131 @@
+<robot
+  name="T12">
+  <link
+    name="Body">
+    <inertial>
+      <origin
+        xyz="0.015789 0.0084982 0.11668"
+        rpy="0 0 0" />
+      <mass
+        value="266.06" />
+      <inertia
+        ixx="225.36"
+        ixy="0.054583"
+        ixz="0.010358"
+        iyy="210.35"
+        iyz="-0.0001937"
+        izz="424.6" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Body.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Body.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="Hip1">
+    <inertial>
+      <origin
+        xyz="0.12059 0.04703 -0.043975"
+        rpy="0 0 0" />
+      <mass
+        value="10.161" />
+      <inertia
+        ixx="0.69317"
+        ixy="0.0073762"
+        ixz="0.040711"
+        iyy="0.34376"
+        iyz="0.0014897"
+        izz="0.9152" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Hip1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HY1"
+    type="revolute">
+    <origin
+      xyz="1.7322 0.99572 0.41034"
+      rpy="-4.4608E-16 2.8513E-16 0.5236" />
+    <parent
+      link="Body" />
+    <child
+      link="Hip1" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Thigh1">
+    <inertial>
+      <origin
+        xyz="0.37052 1.2567E-08 -0.0016621"
+        rpy="0 0 0" />
+      <mass
+        value="14.238" />
+      <inertia
+        ixx="1.0008"
+        ixy="7.0442E-08"
+        ixz="0.0087687"
+        iyy="2.0253"
+        iyz="-0.00024727"
+        izz="1.3252" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Thigh1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="HP1"
+    type="revolute">
+    <origin
+      xyz="0.36808 0 0"
+      rpy="-1.5708 1.2704E-14 -2.2204E-16" />
+    <parent
+      link="Hip1" />
+    <child
+      link="Thigh1" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-1.9199"
+      upper="1.5708"
+      effort="7500"
+      velocity="0" />
+  </joint>
+  <link
+    name="Knee1">
+    <inertial>
+      <origin
+        xyz="0.017762 -0.027385 0.0067752"
+        rpy="0 0 0" />
+      <mass
+        value="5.7913" />
+      <inertia
+        ixx="0.12768"
+        ixy="0.0042111"
+        ixz="-0.00069417"
+        iyy="0.12196"
+        iyz="0.0012334"
+        izz="0.079088" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Knee1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KP1"
+    type="revolute">
+    <origin
+      xyz="0.8627 0 0"
+      rpy="2.2204E-16 2.6822E-16 1.5708" />
+    <parent
+      link="Thigh1" />
+    <child
+      link="Knee1" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-1.9199"
+      upper="3.1416"
+      effort="2700"
+      velocity="0" />
+  </joint>
+  <link
+    name="Shin1">
+    <inertial>
+      <origin
+        xyz="-1.4482E-09 0.00011789 0.4127"
+        rpy="0 0 0" />
+      <mass
+        value="10.201" />
+      <inertia
+        ixx="0.7541"
+        ixy="-0.00016423"
+        ixz="3.2671E-09"
+        iyy="0.53994"
+        iyz="-0.00028063"
+        izz="0.35892" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Shin1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="KR1"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="1.5708 -2.7791E-17 8.7217E-15" />
+    <parent
+      link="Knee1" />
+    <child
+      link="Shin1" />
+    <axis
+      xyz="0 0 -1" />
+    <limit
+      lower="-3.1416"
+      upper="3.1416"
+      effort="900"
+      velocity="0" />
+  </joint>
+  <link
+    name="Ankle1">
+    <inertial>
+      <origin
+        xyz="0.020168 -0.026001 0.0079686"
+        rpy="0 0 0" />
+      <mass
+        value="5.108" />
+      <inertia
+        ixx="0.094931"
+        ixy="0.0040571"
+        ixz="-0.00081815"
+        iyy="0.089357"
+        iyz="0.00089845"
+        izz="0.073015" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.75294 0.75294 0.75294 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="../meshes/Ankle1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="AP1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.79818"
+      rpy="-1.5708 -1.3823E-15 -9.3729E-17" />
+    <parent
+      link="Shin1" />
+    <child
+      link="Ankle1" />
+    <axis
+      xyz="0 0 -1" />