feat: more urdf parser fixes + stl parser

49 files changed, 6703 insertions, 250 deletions

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
--- /dev/null
+++ b/assets/3d/T12/meshes/Ankle1.STL
diff --git a/assets/3d/T12/meshes/Ankle2.STL b/assets/3d/T12/meshes/Ankle2.STL
new file mode 100644
index 0000000..0aa9ec5
--- /dev/null
+++ b/assets/3d/T12/meshes/Ankle2.STL
diff --git a/assets/3d/T12/meshes/Ankle3.STL b/assets/3d/T12/meshes/Ankle3.STL
new file mode 100644
index 0000000..0aa9ec5
--- /dev/null
+++ b/assets/3d/T12/meshes/Ankle3.STL
diff --git a/assets/3d/T12/meshes/Ankle4.STL b/assets/3d/T12/meshes/Ankle4.STL
new file mode 100644
index 0000000..0aa9ec5
--- /dev/null
+++ b/assets/3d/T12/meshes/Ankle4.STL
diff --git a/assets/3d/T12/meshes/Ankle5.STL b/assets/3d/T12/meshes/Ankle5.STL
new file mode 100644
index 0000000..0aa9ec5
--- /dev/null
+++ b/assets/3d/T12/meshes/Ankle5.STL
diff --git a/assets/3d/T12/meshes/Ankle6.STL b/assets/3d/T12/meshes/Ankle6.STL
new file mode 100644
index 0000000..0aa9ec5
--- /dev/null
+++ b/assets/3d/T12/meshes/Ankle6.STL
diff --git a/assets/3d/T12/meshes/Body.STL b/assets/3d/T12/meshes/Body.STL
new file mode 100644
index 0000000..3379440
--- /dev/null
+++ b/assets/3d/T12/meshes/Body.STL
diff --git a/assets/3d/T12/meshes/Foot1.STL b/assets/3d/T12/meshes/Foot1.STL
new file mode 100644
index 0000000..67da40c
--- /dev/null
+++ b/assets/3d/T12/meshes/Foot1.STL
diff --git a/assets/3d/T12/meshes/Foot2.STL b/assets/3d/T12/meshes/Foot2.STL
new file mode 100644
index 0000000..67da40c
--- /dev/null
+++ b/assets/3d/T12/meshes/Foot2.STL
diff --git a/assets/3d/T12/meshes/Foot3.STL b/assets/3d/T12/meshes/Foot3.STL
new file mode 100644
index 0000000..67da40c
--- /dev/null
+++ b/assets/3d/T12/meshes/Foot3.STL
diff --git a/assets/3d/T12/meshes/Foot4.STL b/assets/3d/T12/meshes/Foot4.STL
new file mode 100644
index 0000000..67da40c
--- /dev/null
+++ b/assets/3d/T12/meshes/Foot4.STL
diff --git a/assets/3d/T12/meshes/Foot5.STL b/assets/3d/T12/meshes/Foot5.STL
new file mode 100644
index 0000000..67da40c
--- /dev/null
+++ b/assets/3d/T12/meshes/Foot5.STL
diff --git a/assets/3d/T12/meshes/Foot6.STL b/assets/3d/T12/meshes/Foot6.STL
new file mode 100644
index 0000000..67da40c
--- /dev/null
+++ b/assets/3d/T12/meshes/Foot6.STL
diff --git a/assets/3d/T12/meshes/Hip1.STL b/assets/3d/T12/meshes/Hip1.STL
new file mode 100644
index 0000000..9a7df1a
--- /dev/null
+++ b/assets/3d/T12/meshes/Hip1.STL
diff --git a/assets/3d/T12/meshes/Hip2.STL b/assets/3d/T12/meshes/Hip2.STL
new file mode 100644
index 0000000..9a7df1a
--- /dev/null
+++ b/assets/3d/T12/meshes/Hip2.STL
diff --git a/assets/3d/T12/meshes/Hip3.STL b/assets/3d/T12/meshes/Hip3.STL
new file mode 100644
index 0000000..9a7df1a
--- /dev/null
+++ b/assets/3d/T12/meshes/Hip3.STL
diff --git a/assets/3d/T12/meshes/Hip4.STL b/assets/3d/T12/meshes/Hip4.STL
new file mode 100644
index 0000000..9a7df1a
--- /dev/null
+++ b/assets/3d/T12/meshes/Hip4.STL
diff --git a/assets/3d/T12/meshes/Hip5.STL b/assets/3d/T12/meshes/Hip5.STL
new file mode 100644
index 0000000..9a7df1a
--- /dev/null
+++ b/assets/3d/T12/meshes/Hip5.STL
diff --git a/assets/3d/T12/meshes/Hip6.STL b/assets/3d/T12/meshes/Hip6.STL
new file mode 100644
index 0000000..9a7df1a
--- /dev/null
+++ b/assets/3d/T12/meshes/Hip6.STL
diff --git a/assets/3d/T12/meshes/Knee1.STL b/assets/3d/T12/meshes/Knee1.STL
new file mode 100644
index 0000000..9057da7
--- /dev/null
+++ b/assets/3d/T12/meshes/Knee1.STL
diff --git a/assets/3d/T12/meshes/Knee2.STL b/assets/3d/T12/meshes/Knee2.STL
new file mode 100644
index 0000000..9057da7
--- /dev/null
+++ b/assets/3d/T12/meshes/Knee2.STL
diff --git a/assets/3d/T12/meshes/Knee3.STL b/assets/3d/T12/meshes/Knee3.STL
new file mode 100644
index 0000000..9057da7
--- /dev/null
+++ b/assets/3d/T12/meshes/Knee3.STL
diff --git a/assets/3d/T12/meshes/Knee4.STL b/assets/3d/T12/meshes/Knee4.STL
new file mode 100644
index 0000000..9057da7
--- /dev/null
+++ b/assets/3d/T12/meshes/Knee4.STL
diff --git a/assets/3d/T12/meshes/Knee5.STL b/assets/3d/T12/meshes/Knee5.STL
new file mode 100644
index 0000000..9057da7
--- /dev/null
+++ b/assets/3d/T12/meshes/Knee5.STL
diff --git a/assets/3d/T12/meshes/Knee6.STL b/assets/3d/T12/meshes/Knee6.STL
new file mode 100644
index 0000000..9057da7
--- /dev/null
+++ b/assets/3d/T12/meshes/Knee6.STL
diff --git a/assets/3d/T12/meshes/Shin1.STL b/assets/3d/T12/meshes/Shin1.STL
new file mode 100644
index 0000000..e515e76
--- /dev/null
+++ b/assets/3d/T12/meshes/Shin1.STL
diff --git a/assets/3d/T12/meshes/Shin2.STL b/assets/3d/T12/meshes/Shin2.STL
new file mode 100644
index 0000000..e515e76
--- /dev/null
+++ b/assets/3d/T12/meshes/Shin2.STL
diff --git a/assets/3d/T12/meshes/Shin3.STL b/assets/3d/T12/meshes/Shin3.STL
new file mode 100644
index 0000000..e515e76
--- /dev/null
+++ b/assets/3d/T12/meshes/Shin3.STL
diff --git a/assets/3d/T12/meshes/Shin4.STL b/assets/3d/T12/meshes/Shin4.STL
new file mode 100644
index 0000000..e515e76
--- /dev/null
+++ b/assets/3d/T12/meshes/Shin4.STL
diff --git a/assets/3d/T12/meshes/Shin5.STL b/assets/3d/T12/meshes/Shin5.STL
new file mode 100644
index 0000000..e515e76
--- /dev/null
+++ b/assets/3d/T12/meshes/Shin5.STL
diff --git a/assets/3d/T12/meshes/Shin6.STL b/assets/3d/T12/meshes/Shin6.STL
new file mode 100644
index 0000000..e515e76
--- /dev/null
+++ b/assets/3d/T12/meshes/Shin6.STL
diff --git a/assets/3d/T12/meshes/Thigh1.STL b/assets/3d/T12/meshes/Thigh1.STL
new file mode 100644
index 0000000..d134267
--- /dev/null
+++ b/assets/3d/T12/meshes/Thigh1.STL
diff --git a/assets/3d/T12/meshes/Thigh2.STL b/assets/3d/T12/meshes/Thigh2.STL
new file mode 100644
index 0000000..d134267
--- /dev/null
+++ b/assets/3d/T12/meshes/Thigh2.STL
diff --git a/assets/3d/T12/meshes/Thigh3.STL b/assets/3d/T12/meshes/Thigh3.STL
new file mode 100644
index 0000000..d134267
--- /dev/null
+++ b/assets/3d/T12/meshes/Thigh3.STL
diff --git a/assets/3d/T12/meshes/Thigh4.STL b/assets/3d/T12/meshes/Thigh4.STL
new file mode 100644
index 0000000..d134267
--- /dev/null
+++ b/assets/3d/T12/meshes/Thigh4.STL
diff --git a/assets/3d/T12/meshes/Thigh5.STL b/assets/3d/T12/meshes/Thigh5.STL
new file mode 100644
index 0000000..d134267
--- /dev/null
+++ b/assets/3d/T12/meshes/Thigh5.STL
diff --git a/assets/3d/T12/meshes/Thigh6.STL b/assets/3d/T12/meshes/Thigh6.STL
new file mode 100644
index 0000000..d134267
--- /dev/null
+++ b/assets/3d/T12/meshes/Thigh6.STL
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="">