diff --git a/.gitignore b/.gitignore
index 665fbe1da049665159323bd85d3e6126770c337e..3f8dca336bc86e53629624549564af2e29ad1ac0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@
 *.pyd
 *.pyo
 build*
+*~
\ No newline at end of file
diff --git a/cmake b/cmake
index 094834088ad9de32f1abdc2d56d29bca2190772c..0adbf0109cf2cb4b19db9b71cce37131c17b1597 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 094834088ad9de32f1abdc2d56d29bca2190772c
+Subproject commit 0adbf0109cf2cb4b19db9b71cce37131c17b1597
diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index 350220fddbf8574e859049a4a06abc008278339a..6d85196adfb942f33a4e733d75d2ba9b90241e8c 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -150,6 +150,15 @@ def loadANYmal(withArm=None):
     return loader().robot
 
 
+class CassieLoader(RobotLoader):
+    path = 'cassie_description'
+    sdf_filename = "cassie_v2.sdf"
+    sdf_subpath = 'robots'
+    srdf_filename = "cassie_v2.srdf"
+    ref_posture = "standing"
+    free_flyer = True
+
+
 class TalosLoader(RobotLoader):
     path = 'talos_data'
     urdf_filename = "talos_reduced.urdf"
@@ -517,6 +526,7 @@ ROBOTS = {
     'anymal': ANYmalLoader,
     'anymal_kinova': ANYmalKinovaLoader,
     'baxter': BaxterLoader,
+    'cassie': CassieLoader,
     'double_pendulum': DoublePendulumLoader,
     'hector': HectorLoader,
     'hyq': HyQLoader,
diff --git a/robots/cassie_description/meshes/achilles-rod.stl b/robots/cassie_description/meshes/achilles-rod.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a62eff52b79b61a6ffd2ac7677ffb00fec49474e
Binary files /dev/null and b/robots/cassie_description/meshes/achilles-rod.stl differ
diff --git a/robots/cassie_description/meshes/foot-crank.stl b/robots/cassie_description/meshes/foot-crank.stl
new file mode 100644
index 0000000000000000000000000000000000000000..adb6be1e9b3ac8bc49fa17a54cc5961d47371872
Binary files /dev/null and b/robots/cassie_description/meshes/foot-crank.stl differ
diff --git a/robots/cassie_description/meshes/foot.stl b/robots/cassie_description/meshes/foot.stl
new file mode 100644
index 0000000000000000000000000000000000000000..4a9c138f011627d5cae00292a47141ef3635e90f
Binary files /dev/null and b/robots/cassie_description/meshes/foot.stl differ
diff --git a/robots/cassie_description/meshes/heel-spring.stl b/robots/cassie_description/meshes/heel-spring.stl
new file mode 100644
index 0000000000000000000000000000000000000000..8ca1e87fcc284553293f57d95afa605fed3bff91
Binary files /dev/null and b/robots/cassie_description/meshes/heel-spring.stl differ
diff --git a/robots/cassie_description/meshes/hip-pitch.stl b/robots/cassie_description/meshes/hip-pitch.stl
new file mode 100644
index 0000000000000000000000000000000000000000..4ed01388d90cc939cb46e0e3b5cee86aed8c7de0
Binary files /dev/null and b/robots/cassie_description/meshes/hip-pitch.stl differ
diff --git a/robots/cassie_description/meshes/hip-roll.stl b/robots/cassie_description/meshes/hip-roll.stl
new file mode 100644
index 0000000000000000000000000000000000000000..d97b2e046ef99f98c87771e8fd1a395557d2f5df
Binary files /dev/null and b/robots/cassie_description/meshes/hip-roll.stl differ
diff --git a/robots/cassie_description/meshes/hip-yaw.stl b/robots/cassie_description/meshes/hip-yaw.stl
new file mode 100644
index 0000000000000000000000000000000000000000..de02fab3c80384c34ba35cff2839f88d6993b26e
Binary files /dev/null and b/robots/cassie_description/meshes/hip-yaw.stl differ
diff --git a/robots/cassie_description/meshes/knee-spring.stl b/robots/cassie_description/meshes/knee-spring.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2ff064b305f6d597d410839e5c815a1df70c95e0
Binary files /dev/null and b/robots/cassie_description/meshes/knee-spring.stl differ
diff --git a/robots/cassie_description/meshes/knee.stl b/robots/cassie_description/meshes/knee.stl
new file mode 100644
index 0000000000000000000000000000000000000000..ef01aa53c3c90e7c75f36475c545cee42e337f47
Binary files /dev/null and b/robots/cassie_description/meshes/knee.stl differ
diff --git a/robots/cassie_description/meshes/pelvis.stl b/robots/cassie_description/meshes/pelvis.stl
new file mode 100644
index 0000000000000000000000000000000000000000..0f5a94d7183a6f1bab39201e1f0fda16a902cb1e
Binary files /dev/null and b/robots/cassie_description/meshes/pelvis.stl differ
diff --git a/robots/cassie_description/meshes/plantar-rod.stl b/robots/cassie_description/meshes/plantar-rod.stl
new file mode 100644
index 0000000000000000000000000000000000000000..1175b40b9281e44ee5a9859f965af9d8cf70ad0c
Binary files /dev/null and b/robots/cassie_description/meshes/plantar-rod.stl differ
diff --git a/robots/cassie_description/meshes/shin.stl b/robots/cassie_description/meshes/shin.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a6a80d67ddef5827a189ae669d57b5e02daa2eae
Binary files /dev/null and b/robots/cassie_description/meshes/shin.stl differ
diff --git a/robots/cassie_description/meshes/tarsus.stl b/robots/cassie_description/meshes/tarsus.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2181ee506480046c44aab6a93e9e9ac13b2fa498
Binary files /dev/null and b/robots/cassie_description/meshes/tarsus.stl differ
diff --git a/robots/cassie_description/robots/cassie.sdf b/robots/cassie_description/robots/cassie.sdf
new file mode 100644
index 0000000000000000000000000000000000000000..68ec25b7d85be0839458b83a350a546d9886b15d
--- /dev/null
+++ b/robots/cassie_description/robots/cassie.sdf
@@ -0,0 +1,1607 @@
+<!-- *
+ * Copyright (c) 2018 Agility Robotics
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ * -->
+
+<sdf version="1.6">
+  <model name="cassie">
+    <self_collide>1</self_collide>
+
+    <link name='pelvis'>
+      <pose>0 0 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0.05066 0.000346 0.02841 0 0 0</pose>
+        <mass>10.33</mass>
+        <inertia>
+          <ixx>0.085821</ixx>
+          <iyy>0.049222</iyy>
+          <izz>0.08626</izz>
+          <ixy>1.276e-05</ixy>
+          <ixz>-0.00016022</ixz>
+          <iyz>-0.000414</iyz>
+        </inertia>
+      </inertial>
+      <visual name="pelvis_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/pelvis.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="pelvis_collision">
+        <pose frame="">0.02 0 0.04 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.14</radius>
+          </sphere>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name='left-hip-roll'>
+      <pose>0.021 0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>-6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-roll_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="left-roll-reference">
+      <pose frame="">0.021 0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose frame="">0.000001 0.000001 -0.027063 0 1.57 0</pose>
+        <mass>0.145608</mass>
+        <inertia>
+          <ixx>0.000073735</ixx>
+          <ixy>1.340232e-073</ixy>
+          <ixz>7.241957e-08</ixz>
+          <iyy>7.357073e-05</iyy>
+          <iyz>-7.096017e-08</iyz>
+          <izz>8.262566e-05</izz>
+        </inertia>
+      </inertial>
+      <visual name="left-roll-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-roll-ip" type="revolute">
+      <child>left-roll-reference</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-roll-op" type="revolute">
+      <child>left-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>-0.2618</lower>
+          <upper>0.3491</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-roll-joint" type="gearbox">
+      <child>left-hip-roll</child>
+      <parent>left-roll-reference</parent>
+      <gearbox_reference_body>pelvis</gearbox_reference_body>
+      <gearbox_ratio>0.04</gearbox_ratio>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='left-hip-yaw'>
+      <pose>-0.049 0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 -1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>-4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>-2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="left-yaw-reference">
+      <pose frame="">-0.049 0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose frame="">0.000001 0.000001 -0.027063 0 1.57 0</pose>
+        <mass>0.145608</mass>
+        <inertia>
+          <ixx>7.368810e-05</ixx>
+          <ixy>1.340232e-073</ixy>
+          <ixz>7.241957e-08</ixz>
+          <iyy>7.357073e-05</iyy>
+          <iyz>-7.096017e-08</iyz>
+          <izz>0.000082672</izz>
+        </inertia>
+      </inertial>
+      <visual name="left-yaw-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-yaw-ip" type="revolute">
+      <child>left-yaw-reference</child>
+      <parent>left-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-yaw-op" type="revolute">
+      <child>left-hip-yaw</child>
+      <parent>left-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-yaw-joint" type="gearbox">
+      <child>left-hip-yaw</child>
+      <parent>left-yaw-reference</parent>
+      <gearbox_reference_body>left-hip-roll</gearbox_reference_body>
+      <gearbox_ratio>0.04</gearbox_ratio>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>0 0 1</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='left-hip-pitch'>
+      <pose>-0.049 0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 -0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>-5.721e-05</ixz>
+          <iyz>9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-hip-pitch_collision">
+        <pose frame="">0.053 0.0 -0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name="left-pitch-reference">
+      <pose frame="">-0.049 0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose frame="">-0.000001 -0.000011 0 1.57 1.57 0</pose>
+        <mass>0.272280</mass>
+            <inertia>
+          <ixx>2.288457e-04</ixx>
+          <ixy>6.854960e-07</ixy>
+          <ixz>-4.163281e-08</ixz>
+          <iyy>0.00023115</iyy>
+          <iyz>-9.248118e-07</iyz>
+          <izz>0.00020476</izz>
+        </inertia>
+      </inertial>
+      <visual name="left-pitch-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-pitch-ip" type="revolute">
+      <child>left-pitch-reference</child>
+      <parent>left-hip-yaw</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-pitch-op" type="revolute">
+      <child>left-hip-pitch</child>
+      <parent>left-hip-yaw</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-pitch-joint" type="gearbox">
+      <child>left-hip-pitch</child>
+      <parent>left-pitch-reference</parent>
+      <gearbox_reference_body>left-hip-yaw</gearbox_reference_body>
+      <gearbox_ratio>0.0625</gearbox_ratio>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='left-achilles-rod'>
+      <pose>-0.049 0.09 0.92 -1.7199 0.91441 3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>-1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-achilles_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-pitch-rod-joint" type="ball">
+      <child>left-achilles-rod</child>
+      <parent>left-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-knee'>
+      <pose>-0.049 0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 -0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>-4.085e-05</ixz>
+          <iyz>-5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-knee_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="left-knee-reference">
+      <pose frame="">-0.049 0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose frame="">-0.000001 -0.000011 0.024425 1.57 1.57 0</pose>
+        <mass>0.272280</mass>
+        <inertia>
+          <ixx>2.288457e-04</ixx>
+          <ixy>6.854960e-07</ixy>
+          <ixz>-4.163281e-08</ixz>
+          <iyy>0.00023115</iyy>
+          <iyz>-9.248118e-07</iyz>
+          <izz>0.00020476</izz>
+        </inertia>
+      </inertial>
+      <visual name="left-knee-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-knee-ip" type="revolute">
+      <parent>left-hip-pitch</parent>
+      <child>left-knee-reference</child>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-knee-op" type="revolute">
+      <child>left-knee</child>
+      <parent>left-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-knee-joint" type="gearbox">
+      <child>left-knee</child>
+      <parent>left-knee-reference</parent>
+      <gearbox_reference_body>left-hip-pitch</gearbox_reference_body>
+      <gearbox_ratio>0.0625</gearbox_ratio>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='left-knee-spring'>
+      <pose>-0.033635 0.1305 0.69882 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.0836 0.0034 0 0 0 0</pose>
+        <mass>0.186</mass>
+        <inertia>
+          <ixx>5.215e-05</ixx>
+          <iyy>0.00041205</iyy>
+          <izz>0.0003669</izz>
+          <ixy>6.87e-06</ixy>
+          <ixz>0</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-knee-spring_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-knee-spring-joint" type="revolute">
+      <child>left-knee-spring</child>
+      <parent>left-knee</parent>
+      <axis>
+        <xyz>0.000796 -1 1e-06</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-shin'>
+      <pose>-0.058383 0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>2.266e-05</ixz>
+          <iyz>-1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-shin_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-knee-shin-joint" type="revolute">
+      <child>left-shin</child>
+      <parent>left-knee</parent>
+      <axis>
+        <xyz>0.000796 -1 1e-06</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-tarsus'>
+      <pose>-0.35166 0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 -0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>-4.987e-05</ixz>
+          <iyz>-4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-tarsus_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-shin-tarsus-joint" type="revolute">
+      <child>left-tarsus</child>
+      <parent>left-shin</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-heel-spring'>
+      <pose>-0.38432 0.12958 0.40749 1.6878 -1.3734 -0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>-6e-07</ixz>
+          <iyz>1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-spring-joint" type="revolute">
+      <child>left-heel-spring</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-foot-crank'>
+      <pose>-0.37174 0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 -0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>-3.9623e-06</ixz>
+          <iyz>-1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name ="left-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-crank-joint" type="revolute">
+      <child>left-foot-crank</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-plantar-rod'>
+      <pose>-0.35937 0.11566 0.28425 1.5708 1.3639 0.1</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>-8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-crank-rod-joint" type="revolute">
+      <child>left-plantar-rod</child>
+      <parent>left-foot-crank</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-foot'>
+      <pose>-0.29886 0.1305 -0.0045361 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 -0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>1.36e-06</ixz>
+          <iyz>-4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-foot_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.005</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+
+    <link name="left-foot-reference">
+      <pose frame="">-0.29886 0.1305 -0.0045361 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose frame="">0.00474 0.02748 -0.00014 0 0 0</pose>
+        <mass>0.001</mass>
+        <inertia>
+          <ixx>9.4e-7</ixx>
+          <ixy>9.4e-9</ixy>
+          <ixz>9.4e-9</ixz>
+          <iyy>9.4e-7</iyy>
+          <iyz>9.4e-9</iyz>
+          <izz>9.4e-6</izz>
+        </inertia>
+      </inertial>
+      <visual name="left-foot-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-foot-ip" type="revolute">
+      <parent>left-tarsus</parent>
+      <child>left-foot-reference</child>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-foot-op" type="revolute">
+      <child>left-foot</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="left-foot-joint" type="gearbox">
+      <child>left-foot</child>
+      <parent>left-foot-reference</parent>
+      <gearbox_reference_body>left-tarsus</gearbox_reference_body>
+      <gearbox_ratio>0.02</gearbox_ratio>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='right-hip-roll'>
+      <pose>0.021 -0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 -0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>-3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 -1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="right-roll-reference">
+      <pose frame="">0.021 -0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose frame="">0.000001 0.000001 -0.027063 0 1.57 0</pose>
+        <mass>0.145608</mass>
+        <inertia>
+          <ixx>0.000073735</ixx>
+          <ixy>1.340232e-073</ixy>
+          <ixz>7.241957e-08</ixz>
+          <iyy>7.357073e-05</iyy>
+          <iyz>-7.096017e-08</iyz>
+          <izz>8.262566e-05</izz>
+        </inertia>
+      </inertial>
+      <visual name="right-roll-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-roll-ip" type="revolute">
+      <child>right-roll-reference</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-roll-op" type="revolute">
+      <child>right-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>-0.3491</lower>
+          <upper>0.2618</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-roll-joint" type="gearbox">
+      <child>right-hip-roll</child>
+      <parent>right-roll-reference</parent>
+      <gearbox_reference_body>pelvis</gearbox_reference_body>
+      <gearbox_ratio>0.04</gearbox_ratio>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='right-hip-yaw'>
+      <pose>-0.049 -0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="right-yaw-reference">
+      <pose frame="">-0.049 -0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose frame="">0.000001 0.000001 -0.027063 0 1.57 0</pose>
+        <mass>0.145608</mass>
+        <inertia>
+          <ixx>7.368810e-05</ixx>
+          <ixy>1.340232e-073</ixy>
+          <ixz>7.241957e-08</ixz>
+          <iyy>7.357073e-05</iyy>
+          <iyz>-7.096017e-08</iyz>
+          <izz>0.000082672</izz>
+        </inertia>
+      </inertial>
+      <visual name="right-yaw-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-yaw-ip" type="revolute">
+      <child>right-yaw-reference</child>
+      <parent>right-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-yaw-op" type="revolute">
+      <child>right-hip-yaw</child>
+      <parent>right-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-yaw-joint" type="gearbox">
+      <child>right-hip-yaw</child>
+      <parent>right-yaw-reference</parent>
+      <gearbox_reference_body>right-hip-roll</gearbox_reference_body>
+      <gearbox_ratio>0.04</gearbox_ratio>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>0 0 1</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='right-hip-pitch'>
+      <pose>-0.049 -0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>5.721e-05</ixz>
+          <iyz>-9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-hip-pitch_collision">
+        <pose frame="">0.053 0.0 0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name="right-pitch-reference">
+      <pose frame="">-0.049 -0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose frame="">-0.000001 -0.000011 0 1.57 1.57 0</pose>
+        <mass>0.272280</mass>
+        <inertia>
+          <ixx>2.288457e-04</ixx>
+          <ixy>6.854960e-07</ixy>
+          <ixz>-4.163281e-08</ixz>
+          <iyy>0.00023115</iyy>
+          <iyz>-9.248118e-07</iyz>
+          <izz>0.00020476</izz>
+        </inertia>
+      </inertial>
+      <visual name="right-pitch-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-pitch-ip" type="revolute">
+      <child>right-pitch-reference</child>
+      <parent>right-hip-yaw</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-pitch-op" type="revolute">
+      <child>right-hip-pitch</child>
+      <parent>right-hip-yaw</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-pitch-joint" type="gearbox">
+      <child>right-hip-pitch</child>
+      <parent>right-pitch-reference</parent>
+      <gearbox_reference_body>right-hip-yaw</gearbox_reference_body>
+      <gearbox_ratio>0.0625</gearbox_ratio>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='right-achilles-rod'>
+      <pose>-0.049 -0.09 0.92 -1.4217 0.91441 -3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-achilles-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-pitch-rod-joint" type="ball">
+      <child>right-achilles-rod</child>
+      <parent>right-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-knee'>
+      <pose>-0.049 -0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>4.085e-05</ixz>
+          <iyz>5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-knee_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="right-knee-reference">
+      <pose frame="">-0.049 -0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose frame="">-0.000001 -0.000011 -0.024425 1.57 1.57 0</pose>
+        <mass>0.272280</mass>
+        <inertia>
+          <ixx>2.288457e-04</ixx>
+          <ixy>6.854960e-07</ixy>
+          <ixz>-4.163281e-08</ixz>
+          <iyy>0.00023115</iyy>
+          <iyz>-9.248118e-07</iyz>
+          <izz>0.00020476</izz>
+        </inertia>
+      </inertial>
+      <visual name="right-knee-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-knee-ip" type="revolute">
+      <parent>right-hip-pitch</parent>
+      <child>right-knee-reference</child>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-knee-op" type="revolute">
+      <child>right-knee</child>
+      <parent>right-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-knee-joint" type="gearbox">
+      <child>right-knee</child>
+      <parent>right-knee-reference</parent>
+      <gearbox_reference_body>right-hip-pitch</gearbox_reference_body>
+      <gearbox_ratio>0.0625</gearbox_ratio>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <link name='right-knee-spring'>
+      <pose>-0.033635 -0.1305 0.69882 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.0836 0.0034 0 0 0 0</pose>
+        <mass>0.186</mass>
+        <inertia>
+          <ixx>5.215e-05</ixx>
+          <iyy>0.00041205</iyy>
+          <izz>0.0003669</izz>
+          <ixy>6.87e-06</ixy>
+          <ixz>0</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-knee-spring_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-knee-spring-joint" type="revolute">
+      <child>right-knee-spring</child>
+      <parent>right-knee</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+
+    <link name='right-shin'>
+      <pose>-0.058383 -0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 -0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>-2.266e-05</ixz>
+          <iyz>1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-shin_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-knee-shin-joint" type="revolute">
+      <child>right-shin</child>
+      <parent>right-knee</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-tarsus'>
+      <pose>-0.35166 -0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>4.987e-05</ixz>
+          <iyz>4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-tarsus_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-shin-tarsus-joint" type="revolute">
+      <child>right-tarsus</child>
+      <parent>right-shin</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-heel-spring'>
+      <pose>-0.38432 -0.12958 0.40749 1.4538 -1.3734 0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>6e-07</ixz>
+          <iyz>-1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-spring-joint" type="revolute">
+      <child>right-heel-spring</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-foot-crank'>
+      <pose>-0.37174 -0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>3.9623e-06</ixz>
+          <iyz>1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-crank-joint" type="revolute">
+      <child>right-foot-crank</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-plantar-rod'>
+      <pose>-0.35937 -0.11566 0.28425 1.5708 1.364 -0.1</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-crank-rod-joint" type="revolute">
+      <child>right-plantar-rod</child>
+      <parent>right-foot-crank</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-foot'>
+      <pose>-0.29886 -0.1305 -0.0045361 1.5708 1.364 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>-1.36e-06</ixz>
+          <iyz>4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+        <contact>
+          <ode>
+            <min_depth>0.005</min_depth>
+          </ode>
+        </contact>
+      </surface>
+      </collision>
+    </link>
+
+    <link name="right-foot-reference">
+      <pose frame="">-0.29886 -0.1305 -0.0045361 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose frame="">0.00474 0.02748 0.00014 0 0 0</pose>
+        <mass>0.001</mass>
+        <inertia>
+          <ixx>9.4e-7</ixx>
+          <ixy>9.4e-9</ixy>
+          <ixz>9.4e-9</ixz>
+          <iyy>9.4e-7</iyy>
+          <iyz>9.4e-9</iyz>
+          <izz>9.4e-6</izz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot-reference_visual">
+        <pose frame="">0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.01</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-foot-ip" type="revolute">
+      <parent>right-tarsus</parent>
+      <child>right-foot-reference</child>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-foot-op" type="revolute">
+      <child>right-foot</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-foot-joint" type="gearbox">
+      <child>right-foot</child>
+      <parent>right-foot-reference</parent>
+      <gearbox_reference_body>right-tarsus</gearbox_reference_body>
+      <gearbox_ratio>0.02</gearbox_ratio>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+      <axis2>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis2>
+    </joint>
+
+    <joint name="right-shin-spring-joint" type="prismatic">
+      <pose>0.13 0.005 0 0 0 0</pose>
+      <parent>right-shin</parent>
+      <child>right-knee-spring</child>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <joint name="right-achilles-spring-joint" type="revolute">
+      <pose>0.11877 -0.01 0 0 0 0</pose>
+      <parent>right-achilles-rod</parent>
+      <child>right-heel-spring</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="right-plantar-foot-joint" type="ball">
+      <pose>0.055 0 -0.008 0 0 0</pose>
+      <parent>right-plantar-rod</parent>
+      <child>right-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-shin-spring-joint" type="prismatic">
+      <pose>0.13 0.005 0 0 0 0</pose>
+      <parent>left-shin</parent>
+      <child>left-knee-spring</child>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <joint name="left-achilles-spring-joint" type="revolute">
+      <pose>0.11877 -0.01 0 0 0 0</pose>
+      <parent>left-achilles-rod</parent>
+      <child>left-heel-spring</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-plantar-foot-joint" type="ball">
+      <pose>0.055 0 0.008 0 0 0</pose>
+      <parent>left-plantar-rod</parent>
+      <child>left-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="static" type="prismatic">
+      <parent>world</parent>
+      <child>pelvis</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>0.2</upper>
+        </limit>
+      </axis>
+    </joint>
+
+    <!-- Attach the plugin to this model -->
+    <plugin filename="libcassie_plugin.so" name="CassiePlugin">
+    </plugin>
+
+</model>
+</sdf>
diff --git a/robots/cassie_description/robots/cassie.world b/robots/cassie_description/robots/cassie.world
new file mode 100644
index 0000000000000000000000000000000000000000..1e76d09863e2f046db701fe0edd239cd9d12be3f
--- /dev/null
+++ b/robots/cassie_description/robots/cassie.world
@@ -0,0 +1,34 @@
+<!-- *
+ * Copyright (c) 2018 Agility Robotics
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ * -->
+
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <physics type="ode">
+      <max_step_size>0.0005</max_step_size>
+      <real_time_update_rate>2000</real_time_update_rate>
+    </physics>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://cassie</uri>
+    </include>
+  </world>
+</sdf>
diff --git a/robots/cassie_description/robots/cassie_reduced.sdf b/robots/cassie_description/robots/cassie_reduced.sdf
new file mode 100644
index 0000000000000000000000000000000000000000..df7528222e699c50072a305d22c18b656bd79c15
--- /dev/null
+++ b/robots/cassie_description/robots/cassie_reduced.sdf
@@ -0,0 +1,1017 @@
+<!-- *
+ * Copyright (c) 2018 Agility Robotics
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ * -->
+
+<sdf version="1.6">
+  <model name="cassie">
+    <self_collide>1</self_collide>
+
+    <link name='pelvis'>
+      <pose>0 0 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0.05066 0.000346 0.02841 0 0 0</pose>
+        <mass>10.33</mass>
+        <inertia>
+          <ixx>0.085821</ixx>
+          <iyy>0.049222</iyy>
+          <izz>0.08626</izz>
+          <ixy>1.276e-05</ixy>
+          <ixz>-0.00016022</ixz>
+          <iyz>-0.000414</iyz>
+        </inertia>
+      </inertial>
+      <visual name="pelvis_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/pelvis.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="pelvis_collision">
+        <pose frame="">0.02 0 0.04 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.14</radius>
+          </sphere>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name='left-hip-roll'>
+      <pose>0.021 0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>-6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-roll_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-roll-op" type="revolute">
+      <child>left-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.2618</lower>
+          <upper>0.3491</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-hip-yaw'>
+      <pose>-0.049 0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 -1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>-4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>-2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-yaw-op" type="revolute">
+      <child>left-hip-yaw</child>
+      <parent>left-hip-roll</parent>
+      <axis>
+        <xyz>0 0 -1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-hip-pitch'>
+      <pose>-0.049 0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 -0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>-5.721e-05</ixz>
+          <iyz>9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-hip-pitch_collision">
+        <pose frame="">0.053 0.0 -0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-pitch-op" type="revolute">
+      <child>left-hip-pitch</child>
+      <parent>left-hip-yaw</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-achilles-rod'>
+      <pose>-0.049 0.09 0.92 -1.7199 0.91441 3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>-1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-achilles_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-pitch-rod-joint" type="ball">
+      <pose> 0 0 0.045 -0.0545114 0.0726228 -0.652947</pose>
+      <parent>left-achilles-rod</parent>
+      <child>left-hip-pitch</child>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-knee'>
+      <pose>-0.049 0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 -0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>-4.085e-05</ixz>
+          <iyz>-5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-knee_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-knee-op" type="revolute">
+      <child>left-knee</child>
+      <parent>left-hip-pitch</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-shin'>
+      <pose>-0.058383 0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>2.266e-05</ixz>
+          <iyz>-1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-shin_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-knee-shin-joint" type="revolute">
+      <child>left-shin</child>
+      <parent>left-knee</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-tarsus'>
+      <pose>-0.35166 0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 -0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>-4.987e-05</ixz>
+          <iyz>-4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-tarsus_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-shin-tarsus-joint" type="revolute">
+      <child>left-tarsus</child>
+      <parent>left-shin</parent>
+      <axis>
+        <xyz> 0 0 1</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-heel-spring'>
+      <pose>-0.38432 0.12958 0.40749 1.6878 -1.3734 -0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>-6e-07</ixz>
+          <iyz>1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-spring-joint" type="revolute">
+      <child>left-heel-spring</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <dynamics>
+          <damping>0</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-foot-crank'>
+      <pose>-0.37174 0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 -0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>-3.9623e-06</ixz>
+          <iyz>-1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name ="left-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-crank-joint" type="revolute">
+      <child>left-foot-crank</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-plantar-rod'>
+      <pose>-0.35937 0.11566 0.28425 1.5708 1.3639 0.1</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>-8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-crank-rod-joint" type="revolute">
+      <child>left-plantar-rod</child>
+      <parent>left-foot-crank</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <link name='left-foot'>
+      <pose>-0.29886 0.1305 -0.0045361 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 -0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>1.36e-06</ixz>
+          <iyz>-4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-foot_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.005</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+
+    <joint name="left-foot-op" type="revolute">
+      <child>left-foot</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-roll'>
+      <pose>0.021 -0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 -0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>-3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 -1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-roll-op" type="revolute">
+      <child>right-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3491</lower>
+          <upper>0.2618</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-yaw'>
+      <pose>-0.049 -0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-yaw-op" type="revolute">
+      <child>right-hip-yaw</child>
+      <parent>right-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-pitch'>
+      <pose>-0.049 -0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>5.721e-05</ixz>
+          <iyz>-9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-hip-pitch_collision">
+        <pose frame="">0.053 0.0 0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-pitch-op" type="revolute">
+      <child>right-hip-pitch</child>
+      <parent>right-hip-yaw</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-achilles-rod'>
+      <pose>-0.049 -0.09 0.92 -1.4217 0.91441 -3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-achilles-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-pitch-rod-joint" type="ball">
+      <pose>0 0 -0.045 0.0557801 -0.0716652 -0.653039</pose>
+      <parent>right-achilles-rod</parent>
+      <child>right-hip-pitch</child>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-knee'>
+      <pose>-0.049 -0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>4.085e-05</ixz>
+          <iyz>5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-knee_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-knee-op" type="revolute">
+      <child>right-knee</child>
+      <parent>right-hip-pitch</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-shin'>
+      <pose>-0.058383 -0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 -0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>-2.266e-05</ixz>
+          <iyz>1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-shin_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-knee-shin-joint" type="revolute">
+      <child>right-shin</child>
+      <parent>right-knee</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-tarsus'>
+      <pose>-0.35166 -0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>4.987e-05</ixz>
+          <iyz>4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-tarsus_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-shin-tarsus-joint" type="revolute">
+      <child>right-tarsus</child>
+      <parent>right-shin</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-heel-spring'>
+      <pose>-0.38432 -0.12958 0.40749 1.4538 -1.3734 0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>6e-07</ixz>
+          <iyz>-1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-spring-joint" type="revolute">
+      <child>right-heel-spring</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-foot-crank'>
+      <pose>-0.37174 -0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>3.9623e-06</ixz>
+          <iyz>1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-crank-joint" type="revolute">
+      <child>right-foot-crank</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-plantar-rod'>
+      <pose>-0.35937 -0.11566 0.28425 1.5708 1.364 -0.1</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-crank-rod-joint" type="revolute">
+      <child>right-plantar-rod</child>
+      <parent>right-foot-crank</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <link name='right-foot'>
+      <pose>-0.29886 -0.1305 -0.0045361 1.5708 1.364 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>-1.36e-06</ixz>
+          <iyz>4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+        <contact>
+          <ode>
+            <min_depth>0.005</min_depth>
+          </ode>
+        </contact>
+      </surface>
+      </collision>
+    </link>
+
+    <joint name="right-foot-op" type="revolute">
+      <child>right-foot</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <joint name="right-achilles-spring-joint" type="revolute">
+      <pose>0.501187 0.00132299 -0.00170135 0.0864423 -0.0521164 -2.68467</pose>
+      <parent>right-heel-spring</parent>
+      <child>right-achilles-rod</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="right-plantar-foot-joint" type="ball">
+      <pose>0.055 0 -0.008 0 0 0</pose>
+      <parent>right-plantar-rod</parent>
+      <child>right-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-achilles-spring-joint" type="revolute">
+      <pose>0.501187 0.00132298 0.00170128 -0.0864415 0.0521197 -2.68468</pose>
+      <parent>left-heel-spring</parent>
+      <child>left-achilles-rod</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-plantar-foot-joint" type="ball">
+      <pose>0.055 0 0.008 0 0 0</pose>
+      <parent>left-plantar-rod</parent>
+      <child>left-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="static" type="prismatic">
+      <parent>world</parent>
+      <child>pelvis</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>0.2</upper>
+        </limit>
+      </axis>
+    </joint>
+
+    <!-- Attach the plugin to this model -->
+    <plugin filename="libcassie_plugin.so" name="CassiePlugin">
+    </plugin>
+
+</model>
+</sdf>
diff --git a/robots/cassie_description/robots/cassie_simple.sdf b/robots/cassie_description/robots/cassie_simple.sdf
new file mode 100644
index 0000000000000000000000000000000000000000..93e5dfff3471890dbff3874b08db8c74f757fcf4
--- /dev/null
+++ b/robots/cassie_description/robots/cassie_simple.sdf
@@ -0,0 +1,1015 @@
+<!-- *
+ * Copyright (c) 2018 Agility Robotics
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ * -->
+
+<sdf version="1.6">
+  <model name="cassie">
+    <self_collide>1</self_collide>
+
+    <link name='pelvis'>
+      <pose>0 0 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0.05066 0.000346 0.02841 0 0 0</pose>
+        <mass>10.33</mass>
+        <inertia>
+          <ixx>0.085821</ixx>
+          <iyy>0.049222</iyy>
+          <izz>0.08626</izz>
+          <ixy>1.276e-05</ixy>
+          <ixz>-0.00016022</ixz>
+          <iyz>-0.000414</iyz>
+        </inertia>
+      </inertial>
+      <visual name="pelvis_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/pelvis.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="pelvis_collision">
+        <pose frame="">0.02 0 0.04 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.14</radius>
+          </sphere>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name='left-hip-roll'>
+      <pose>0.021 0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>-6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-roll_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-roll-op" type="revolute">
+      <child>left-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.2618</lower>
+          <upper>0.3491</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-hip-yaw'>
+      <pose>-0.049 0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 -1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>-4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>-2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-yaw-op" type="revolute">
+      <child>left-hip-yaw</child>
+      <parent>left-hip-roll</parent>
+      <axis>
+        <xyz>0 0 -1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-hip-pitch'>
+      <pose>-0.049 0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 -0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>-5.721e-05</ixz>
+          <iyz>9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-hip-pitch_collision">
+        <pose frame="">0.053 0.0 -0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-pitch-op" type="revolute">
+      <child>left-hip-pitch</child>
+      <parent>left-hip-yaw</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-achilles-rod'>
+      <pose>-0.049 0.09 0.92 -1.7199 0.91441 3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>-1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-achilles_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-pitch-rod-joint" type="ball">
+      <child>left-achilles-rod</child>
+      <parent>left-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-knee'>
+      <pose>-0.049 0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 -0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>-4.085e-05</ixz>
+          <iyz>-5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-knee_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-knee-op" type="revolute">
+      <child>left-knee</child>
+      <parent>left-hip-pitch</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-shin'>
+      <pose>-0.058383 0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>2.266e-05</ixz>
+          <iyz>-1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-shin_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-knee-shin-joint" type="revolute">
+      <child>left-shin</child>
+      <parent>left-knee</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-tarsus'>
+      <pose>-0.35166 0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 -0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>-4.987e-05</ixz>
+          <iyz>-4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-tarsus_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-shin-tarsus-joint" type="revolute">
+      <child>left-tarsus</child>
+      <parent>left-shin</parent>
+      <axis>
+        <xyz> 0 0 1</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-heel-spring'>
+      <pose>-0.38432 0.12958 0.40749 1.6878 -1.3734 -0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>-6e-07</ixz>
+          <iyz>1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-spring-joint" type="revolute">
+      <child>left-heel-spring</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <dynamics>
+          <damping>0</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-foot-crank'>
+      <pose>-0.37174 0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 -0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>-3.9623e-06</ixz>
+          <iyz>-1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name ="left-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-crank-joint" type="revolute">
+      <child>left-foot-crank</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 -1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-plantar-rod'>
+      <pose>-0.35937 0.11566 0.28425 1.5708 1.3639 0.1</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>-8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-crank-rod-joint" type="revolute">
+      <child>left-plantar-rod</child>
+      <parent>left-foot-crank</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <link name='left-foot'>
+      <pose>-0.29886 0.1305 -0.0045361 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 -0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>1.36e-06</ixz>
+          <iyz>-4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-foot_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.005</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+
+    <joint name="left-foot-op" type="revolute">
+      <child>left-foot</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-roll'>
+      <pose>0.021 -0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 -0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>-3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 -1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-roll-op" type="revolute">
+      <child>right-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3491</lower>
+          <upper>0.2618</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-yaw'>
+      <pose>-0.049 -0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-yaw-op" type="revolute">
+      <child>right-hip-yaw</child>
+      <parent>right-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-pitch'>
+      <pose>-0.049 -0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>5.721e-05</ixz>
+          <iyz>-9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-hip-pitch_collision">
+        <pose frame="">0.053 0.0 0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-pitch-op" type="revolute">
+      <child>right-hip-pitch</child>
+      <parent>right-hip-yaw</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-achilles-rod'>
+      <pose>-0.049 -0.09 0.92 -1.4217 0.91441 -3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-achilles-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-pitch-rod-joint" type="ball">
+      <child>right-achilles-rod</child>
+      <parent>right-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-knee'>
+      <pose>-0.049 -0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>4.085e-05</ixz>
+          <iyz>5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-knee_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-knee-op" type="revolute">
+      <child>right-knee</child>
+      <parent>right-hip-pitch</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-shin'>
+      <pose>-0.058383 -0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 -0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>-2.266e-05</ixz>
+          <iyz>1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-shin_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-knee-shin-joint" type="revolute">
+      <child>right-shin</child>
+      <parent>right-knee</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-tarsus'>
+      <pose>-0.35166 -0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>4.987e-05</ixz>
+          <iyz>4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-tarsus_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-shin-tarsus-joint" type="revolute">
+      <child>right-tarsus</child>
+      <parent>right-shin</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-heel-spring'>
+      <pose>-0.38432 -0.12958 0.40749 1.4538 -1.3734 0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>6e-07</ixz>
+          <iyz>-1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-spring-joint" type="revolute">
+      <child>right-heel-spring</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-foot-crank'>
+      <pose>-0.37174 -0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>3.9623e-06</ixz>
+          <iyz>1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-crank-joint" type="revolute">
+      <child>right-foot-crank</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-plantar-rod'>
+      <pose>-0.35937 -0.11566 0.28425 1.5708 1.364 -0.1</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-crank-rod-joint" type="revolute">
+      <child>right-plantar-rod</child>
+      <parent>right-foot-crank</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <link name='right-foot'>
+      <pose>-0.29886 -0.1305 -0.0045361 1.5708 1.364 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>-1.36e-06</ixz>
+          <iyz>4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+        <contact>
+          <ode>
+            <min_depth>0.005</min_depth>
+          </ode>
+        </contact>
+      </surface>
+      </collision>
+    </link>
+
+    <joint name="right-foot-op" type="revolute">
+      <child>right-foot</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <joint name="right-achilles-spring-joint" type="revolute">
+      <pose>0.501187 0.00132299 -0.00170135 0.0864423 -0.0521164 -2.68467</pose>
+      <parent>right-heel-spring</parent>
+      <child>right-achilles-rod</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="right-plantar-foot-joint" type="ball">
+      <pose>0.055 0 -0.008 0 0 0</pose>
+      <parent>right-plantar-rod</parent>
+      <child>right-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-achilles-spring-joint" type="revolute">
+      <pose>0.501187 0.00132298 0.00170128 -0.0864415 0.0521197 -2.68468</pose>
+      <parent>left-heel-spring</parent>
+      <child>left-achilles-rod</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-plantar-foot-joint" type="ball">
+      <pose>0.055 0 0.008 0 0 0</pose>
+      <parent>left-plantar-rod</parent>
+      <child>left-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="static" type="prismatic">
+      <parent>world</parent>
+      <child>pelvis</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>0.2</upper>
+        </limit>
+      </axis>
+    </joint>
+
+    <!-- Attach the plugin to this model -->
+    <plugin filename="libcassie_plugin.so" name="CassiePlugin">
+    </plugin>
+
+</model>
+</sdf>
diff --git a/robots/cassie_description/robots/cassie_v2.sdf b/robots/cassie_description/robots/cassie_v2.sdf
new file mode 100644
index 0000000000000000000000000000000000000000..79d17b70eb2a48c3abbd18322f1330496c75f421
--- /dev/null
+++ b/robots/cassie_description/robots/cassie_v2.sdf
@@ -0,0 +1,1087 @@
+<!-- *
+ * Copyright (c) 2018 Agility Robotics
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ * -->
+
+<sdf version="1.6">
+  <model name="cassie">
+    <self_collide>1</self_collide>
+
+    <link name='pelvis'>
+      <pose>0 0 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0.05066 0.000346 0.02841 0 0 0</pose>
+        <mass>10.33</mass>
+        <inertia>
+          <ixx>0.085821</ixx>
+          <iyy>0.049222</iyy>
+          <izz>0.08626</izz>
+          <ixy>1.276e-05</ixy>
+          <ixz>-0.00016022</ixz>
+          <iyz>-0.000414</iyz>
+        </inertia>
+      </inertial>
+      <visual name="pelvis_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/pelvis.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>
+      </visual>
+      <collision name="pelvis_collision">
+        <pose frame="">0.02 0 0.04 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.14</radius>
+          </sphere>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name='left-hip-roll'>
+      <pose>0.021 0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>-6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-roll_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>
+      </visual>
+    </link>
+
+    <joint name="left-roll-op" type="revolute">
+      <child>left-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.2618</lower>
+          <upper>0.3491</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-hip-yaw'>
+      <pose>-0.049 0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 -1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>-4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>-2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="left-yaw-op" type="revolute">
+      <child>left-hip-yaw</child>
+      <parent>left-hip-roll</parent>
+      <axis>
+        <xyz>0 0 -1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-hip-pitch'>
+      <pose>-0.049 0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 -0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>-5.721e-05</ixz>
+          <iyz>9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+      <collision name="left-hip-pitch_collision">
+        <pose frame="">0.053 0.0 -0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-pitch-op" type="revolute">
+      <child>left-hip-pitch</child>
+      <parent>left-hip-yaw</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-achilles-rod'>
+      <pose>-0.049 0.09 0.92 -1.7199 0.91441 3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>-1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-achilles_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="left-pitch-rod-joint" type="ball">
+      <pose> 0 0 0.045 -0.0545114 0.0726228 -0.652947</pose>
+      <parent>left-achilles-rod</parent>
+      <child>left-hip-pitch</child>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-knee'>
+      <pose>-0.049 0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 -0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>-4.085e-05</ixz>
+          <iyz>-5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-knee_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="left-knee-op" type="revolute">
+      <child>left-knee</child>
+      <parent>left-hip-pitch</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-shin'>
+      <pose>-0.058383 0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>2.266e-05</ixz>
+          <iyz>-1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-shin_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>        
+      </visual>
+      <collision name="left-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-knee-shin-joint" type="revolute">
+      <child>left-shin</child>
+      <parent>left-knee</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-tarsus'>
+      <pose>-0.35166 0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 -0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>-4.987e-05</ixz>
+          <iyz>-4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-tarsus_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>        
+      </visual>
+      <collision name="left-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-shin-tarsus-joint" type="revolute">
+      <child>left-tarsus</child>
+      <parent>left-shin</parent>
+      <axis>
+        <xyz> 0 0 1</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-heel-spring'>
+      <pose>-0.38432 0.12958 0.40749 1.6878 -1.3734 -0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>-6e-07</ixz>
+          <iyz>1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-spring-joint" type="revolute">
+      <child>left-heel-spring</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <dynamics>
+          <damping>0</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-foot-crank'>
+      <pose>-0.37174 0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 -0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>-3.9623e-06</ixz>
+          <iyz>-1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name ="left-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-crank-joint" type="revolute">
+      <child>left-foot-crank</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='left-plantar-rod'>
+      <pose>-0.35937 0.11566 0.28425 1.5708 1.3439 0.0</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>-8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="left-crank-rod-joint" type="revolute">
+      <child>left-plantar-rod</child>
+      <parent>left-foot-crank</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <link name='left-foot'>
+      <pose>-0.29886 0.1305 -0.0045361 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 -0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>1.36e-06</ixz>
+          <iyz>-4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-foot_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+      <collision name="left-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.005</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+
+    <joint name="left-foot-op" type="revolute">
+      <child>left-foot</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-roll'>
+      <pose>0.021 -0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 -0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>-3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 -1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="right-roll-op" type="revolute">
+      <child>right-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3491</lower>
+          <upper>0.2618</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-yaw'>
+      <pose>-0.049 -0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="right-yaw-op" type="revolute">
+      <child>right-hip-yaw</child>
+      <parent>right-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-hip-pitch'>
+      <pose>-0.049 -0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>5.721e-05</ixz>
+          <iyz>-9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+      <collision name="right-hip-pitch_collision">
+        <pose frame="">0.053 0.0 0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-pitch-op" type="revolute">
+      <child>right-hip-pitch</child>
+      <parent>right-hip-yaw</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-achilles-rod'>
+      <pose>-0.049 -0.09 0.92 -1.4217 0.91441 -3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-achilles-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="right-pitch-rod-joint" type="ball">
+      <pose>0 0 -0.045 0.0557801 -0.0716652 -0.653039</pose>
+      <parent>right-achilles-rod</parent>
+      <child>right-hip-pitch</child>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-knee'>
+      <pose>-0.049 -0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>4.085e-05</ixz>
+          <iyz>5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-knee_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="right-knee-op" type="revolute">
+      <child>right-knee</child>
+      <parent>right-hip-pitch</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-shin'>
+      <pose>-0.058383 -0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 -0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>-2.266e-05</ixz>
+          <iyz>1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-shin_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>        
+      </visual>
+      <collision name="right-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-knee-shin-joint" type="revolute">
+      <child>right-shin</child>
+      <parent>right-knee</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-tarsus'>
+      <pose>-0.35166 -0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>4.987e-05</ixz>
+          <iyz>4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-tarsus_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>        
+      </visual>
+      <collision name="right-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-shin-tarsus-joint" type="revolute">
+      <child>right-tarsus</child>
+      <parent>right-shin</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-heel-spring'>
+      <pose>-0.38432 -0.12958 0.40749 1.4538 -1.3734 0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>6e-07</ixz>
+          <iyz>-1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-spring-joint" type="revolute">
+      <child>right-heel-spring</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-foot-crank'>
+      <pose>-0.37174 -0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>3.9623e-06</ixz>
+          <iyz>1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.35 0.35 0.35 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-crank-joint" type="revolute">
+      <child>right-foot-crank</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <link name='right-plantar-rod'>
+      <pose>-0.35937 -0.11566 0.28425 1.5708 1.364 0.0</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+    </link>
+
+    <joint name="right-crank-rod-joint" type="revolute">
+      <child>right-plantar-rod</child>
+      <parent>right-foot-crank</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <link name='right-foot'>
+      <pose>-0.29886 -0.1305 -0.0045361 1.5708 1.364 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>-1.36e-06</ixz>
+          <iyz>4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+            <ambient>0.75 0.75 0.75 1</ambient>
+        </material>        
+      </visual>
+      <collision name="right-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+        <contact>
+          <ode>
+            <min_depth>0.005</min_depth>
+          </ode>
+        </contact>
+      </surface>
+      </collision>
+    </link>
+
+    <joint name="right-foot-op" type="revolute">
+      <child>right-foot</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+      </axis>
+    </joint>
+
+    <joint name="right-achilles-spring-joint" type="revolute">
+      <pose>0.501187 0.00132299 -0.00170135 0.0864423 -0.0521164 -2.68467</pose>
+      <parent>right-heel-spring</parent>
+      <child>right-achilles-rod</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="right-plantar-foot-joint" type="ball">
+      <pose>0.055 0 -0.008 0 0 0</pose>
+      <parent>right-plantar-rod</parent>
+      <child>right-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-achilles-spring-joint" type="revolute">
+      <pose>0.501187 0.00132298 0.00170128 -0.0864415 0.0521197 -2.68468</pose>
+      <parent>left-heel-spring</parent>
+      <child>left-achilles-rod</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-plantar-foot-joint" type="ball">
+      <pose>0.055 0 0.008 0 0 0</pose>
+      <parent>left-plantar-rod</parent>
+      <child>left-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="static" type="prismatic">
+      <parent>world</parent>
+      <child>pelvis</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>0.2</upper>
+        </limit>
+      </axis>
+    </joint>
+
+    <!-- Attach the plugin to this model -->
+    <plugin filename="libcassie_plugin.so" name="CassiePlugin">
+    </plugin>
+
+</model>
+</sdf>
diff --git a/robots/cassie_description/robots/cassie_wo_gearbox_spring.sdf b/robots/cassie_description/robots/cassie_wo_gearbox_spring.sdf
new file mode 100644
index 0000000000000000000000000000000000000000..1d76428073a60730d1b0e4beb8e33cbbde821243
--- /dev/null
+++ b/robots/cassie_description/robots/cassie_wo_gearbox_spring.sdf
@@ -0,0 +1,1037 @@
+<!-- *
+ * Copyright (c) 2018 Agility Robotics
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ * -->
+
+<sdf version="1.6">
+  <model name="cassie">
+    <self_collide>1</self_collide>
+
+    <link name='pelvis'>
+      <pose>0 0 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0.05066 0.000346 0.02841 0 0 0</pose>
+        <mass>10.33</mass>
+        <inertia>
+          <ixx>0.085821</ixx>
+          <iyy>0.049222</iyy>
+          <izz>0.08626</izz>
+          <ixy>1.276e-05</ixy>
+          <ixz>-0.00016022</ixz>
+          <iyz>-0.000414</iyz>
+        </inertia>
+      </inertial>
+      <visual name="pelvis_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/pelvis.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="pelvis_collision">
+        <pose frame="">0.02 0 0.04 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.14</radius>
+          </sphere>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name='left-hip-roll'>
+      <pose>0.021 0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>-6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-roll_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-roll-op" type="revolute">
+      <child>left-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>-0.2618</lower>
+          <upper>0.3491</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-hip-yaw'>
+      <pose>-0.049 0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 -1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>-4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>-2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-yaw-op" type="revolute">
+      <child>left-hip-yaw</child>
+      <parent>left-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-hip-pitch'>
+      <pose>-0.049 0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 -0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>-5.721e-05</ixz>
+          <iyz>9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-hip-pitch_collision">
+        <pose frame="">0.053 0.0 -0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-pitch-op" type="revolute">
+      <child>left-hip-pitch</child>
+      <parent>left-hip-yaw</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-achilles-rod'>
+      <pose>-0.049 0.09 0.92 -1.7199 0.91441 3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>-1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-achilles_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-pitch-rod-joint" type="ball">
+      <child>left-achilles-rod</child>
+      <parent>left-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-knee'>
+      <pose>-0.049 0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 -0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>-4.085e-05</ixz>
+          <iyz>-5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-knee_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-knee-op" type="revolute">
+      <child>left-knee</child>
+      <parent>left-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-shin'>
+      <pose>-0.058383 0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>2.266e-05</ixz>
+          <iyz>-1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-shin_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-knee-shin-joint" type="revolute">
+      <child>left-shin</child>
+      <parent>left-knee</parent>
+      <axis>
+        <xyz>0.000796 -1 1e-06</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-tarsus'>
+      <pose>-0.35166 0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 -0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>-4.987e-05</ixz>
+          <iyz>-4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-tarsus_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="left-shin-tarsus-joint" type="revolute">
+      <child>left-tarsus</child>
+      <parent>left-shin</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-heel-spring'>
+      <pose>-0.38432 0.12958 0.40749 1.6878 -1.3734 -0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>-6e-07</ixz>
+          <iyz>1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-spring-joint" type="revolute">
+      <child>left-heel-spring</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-foot-crank'>
+      <pose>-0.37174 0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 -0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>-3.9623e-06</ixz>
+          <iyz>-1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name ="left-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-tarsus-crank-joint" type="revolute">
+      <child>left-foot-crank</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-plantar-rod'>
+      <pose>-0.35937 0.11566 0.28425 1.5708 1.3639 0.1</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>-8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="left-crank-rod-joint" type="revolute">
+      <child>left-plantar-rod</child>
+      <parent>left-foot-crank</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='left-foot'>
+      <pose>-0.29886 0.1305 -0.0045361 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 -0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>1.36e-06</ixz>
+          <iyz>-4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="left-foot_visual">
+        <geometry>
+          <mesh>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="left-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+          <contact>
+            <ode>
+              <min_depth>0.005</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+
+    <joint name="left-foot-op" type="revolute">
+      <child>left-foot</child>
+      <parent>left-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-hip-roll'>
+      <pose>0.021 -0.135 1.01 0 1.5708 0</pose>
+      <inertial>
+        <pose>-0.01793 -0.0001 -0.04428 0 0 0</pose>
+        <mass>1.82</mass>
+        <inertia>
+          <ixx>0.003431</ixx>
+          <iyy>0.003793</iyy>
+          <izz>0.002135</izz>
+          <ixy>6.65e-07</ixy>
+          <ixz>-0.00084</ixz>
+          <iyz>-3.99e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 -1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-roll-op" type="revolute">
+      <child>right-hip-roll</child>
+      <parent>pelvis</parent>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <lower>-0.3491</lower>
+          <upper>0.2618</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-hip-yaw'>
+      <pose>-0.049 -0.135 1.01 0 0 0</pose>
+      <inertial>
+        <pose>0 1e-05 -0.034277 0 0 0</pose>
+        <mass>1.171</mass>
+        <inertia>
+          <ixx>0.002443</ixx>
+          <iyy>0.002803</iyy>
+          <izz>0.000842</izz>
+          <ixy>4e-08</ixy>
+          <ixz>2.462e-07</ixz>
+          <iyz>2.71e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-yaw_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-yaw-op" type="revolute">
+      <child>right-hip-yaw</child>
+      <parent>right-hip-roll</parent>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>-0.3840</lower>
+          <upper>0.3840</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-hip-pitch'>
+      <pose>-0.049 -0.135 0.92 0 1.5708 -1.57</pose>
+      <inertial>
+        <pose>0.05946 5e-05 0.03581 0 0 0</pose>
+        <mass>5.52</mass>
+        <inertia>
+          <ixx>0.010898</ixx>
+          <iyy>0.029714</iyy>
+          <izz>0.030257</izz>
+          <ixy>-0.0002669</ixy>
+          <ixz>5.721e-05</ixz>
+          <iyz>-9.17e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-hip-pitch_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-hip-pitch_collision">
+        <pose frame="">0.053 0.0 0.0379 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.06</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-pitch-op" type="revolute">
+      <child>right-hip-pitch</child>
+      <parent>right-hip-yaw</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.8727</lower>
+          <upper>1.3963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-achilles-rod'>
+      <pose>-0.049 -0.09 0.92 -1.4217 0.91441 -3.0232</pose>
+      <inertial>
+        <pose>0.24719 0 0 0 0 0</pose>
+        <mass>0.1567</mass>
+        <inertia>
+          <ixx>3.754e-06</ixx>
+          <iyy>0.004487</iyy>
+          <izz>0.004488</izz>
+          <ixy>-3.74e-08</ixy>
+          <ixz>1.61e-08</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-achilles-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-pitch-rod-joint" type="ball">
+      <child>right-achilles-rod</child>
+      <parent>right-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <damping>0.01</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-knee'>
+      <pose>-0.049 -0.1305 0.8 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.023 0.03207 0.002181 0 0 0</pose>
+        <mass>0.7578</mass>
+        <inertia>
+          <ixx>0.001376</ixx>
+          <iyy>0.0010335</iyy>
+          <izz>0.0021637</izz>
+          <ixy>-0.00039744</ixy>
+          <ixz>4.085e-05</ixz>
+          <iyz>5.374e-05</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-knee_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-knee-op" type="revolute">
+      <child>right-knee</child>
+      <parent>right-hip-pitch</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.07694</lower>
+          <upper>0.13963</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-shin'>
+      <pose>-0.058383 -0.1305 0.72357 -1.5708 0.7854 3.1416</pose>
+      <inertial>
+        <pose>0.18338 0.001169 -0.0002123 0 0 0</pose>
+        <mass>0.577</mass>
+        <inertia>
+          <ixx>0.00035939</ixx>
+          <iyy>0.014728</iyy>
+          <izz>0.014707</izz>
+          <ixy>-0.00020981</ixy>
+          <ixz>-2.266e-05</ixz>
+          <iyz>1.2e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-shin_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-shin_collision">
+        <pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.44</length>
+            <radius>0.04</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-knee-shin-joint" type="revolute">
+      <child>right-shin</child>
+      <parent>right-knee</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.34907</lower>
+          <upper>0.34907</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1500</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-tarsus'>
+      <pose>-0.35166 -0.1305 0.402 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.11046 -0.03058 0.00131 0 0 0</pose>
+        <mass>0.782</mass>
+        <inertia>
+          <ixx>0.00039238</ixx>
+          <iyy>0.013595</iyy>
+          <izz>0.013674</izz>
+          <ixy>0.00023651</ixy>
+          <ixz>4.987e-05</ixz>
+          <iyz>4.82e-06</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-tarsus_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-tarsus_collision">
+        <pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
+        <geometry>
+          <cylinder>
+            <length>0.38</length>
+            <radius>0.02</radius>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="right-shin-tarsus-joint" type="revolute">
+      <child>right-tarsus</child>
+      <parent>right-shin</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-0.13963</lower>
+          <upper>1.95477</upper>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-heel-spring'>
+      <pose>-0.38432 -0.12958 0.40749 1.4538 -1.3734 0.18949</pose>
+      <inertial>
+        <pose>0.081 0.0022 0 0 0 0</pose>
+        <mass>0.126</mass>
+        <inertia>
+          <ixx>2.959e-05</ixx>
+          <iyy>0.00022231</iyy>
+          <izz>0.0002007</izz>
+          <ixy>7.15e-06</ixy>
+          <ixz>6e-07</ixz>
+          <iyz>-1e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-heel-spring_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-spring-joint" type="revolute">
+      <child>right-heel-spring</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>1250</spring_stiffness>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-foot-crank'>
+      <pose>-0.37174 -0.10775 0.33784 1.5708 1.3439 0</pose>
+      <inertial>
+        <pose>0.00493 2e-05 0.00215 0 0 0</pose>
+        <mass>0.1261</mass>
+        <inertia>
+          <ixx>2.6941e-05</ixx>
+          <iyy>4.9621e-05</iyy>
+          <izz>6.3362e-05</izz>
+          <ixy>-2.1e-09</ixy>
+          <ixz>3.9623e-06</ixz>
+          <iyz>1.09e-08</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot-crank_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-tarsus-crank-joint" type="revolute">
+      <child>right-foot-crank</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-plantar-rod'>
+      <pose>-0.35937 -0.11566 0.28425 1.5708 1.364 -0.1</pose>
+      <inertial>
+        <pose>0.17792 0 0 0 0 0</pose>
+        <mass>0.1186</mass>
+        <inertia>
+          <ixx>2.779e-06</ixx>
+          <iyy>0.001774</iyy>
+          <izz>0.001775</izz>
+          <ixy>-2.34e-08</ixy>
+          <ixz>8.1e-09</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-plantar-rod_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+
+    <joint name="right-crank-rod-joint" type="revolute">
+      <child>right-plantar-rod</child>
+      <parent>right-foot-crank</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <link name='right-foot'>
+      <pose>-0.29886 -0.1305 -0.0045361 1.5708 1.364 0</pose>
+      <inertial>
+        <pose>0.00474 0.02748 0.00014 0 0 0</pose>
+        <mass>0.1498</mass>
+        <inertia>
+          <ixx>0.00017388</ixx>
+          <iyy>0.00016793</iyy>
+          <izz>0.00033261</izz>
+          <ixy>0.00011814</ixy>
+          <ixz>-1.36e-06</ixz>
+          <iyz>4e-07</iyz>
+        </inertia>
+      </inertial>
+      <visual name="right-foot_visual">
+        <geometry>
+          <mesh>
+            <scale>1 1 -1</scale>
+            <uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="right-foot_collision">
+        <pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
+        <geometry>
+          <cylinder>
+            <length>0.18</length>
+            <radius>0.0168</radius>
+          </cylinder>
+        </geometry>
+        <surface>
+        <contact>
+          <ode>
+            <min_depth>0.005</min_depth>
+          </ode>
+        </contact>
+      </surface>
+      </collision>
+    </link>
+
+    <joint name="right-foot-op" type="revolute">
+      <child>right-foot</child>
+      <parent>right-tarsus</parent>
+      <axis>
+        <xyz>0 -1 0</xyz>
+        <limit>
+          <lower>-2.4435</lower>
+          <upper>-0.6109</upper>
+        </limit>
+        <dynamics>
+          <damping>1</damping>
+        </dynamics>
+        <use_parent_model_frame>1</use_parent_model_frame>
+      </axis>
+    </joint>
+
+    <joint name="right-achilles-spring-joint" type="revolute">
+      <pose>0.11877 -0.01 0 0 0 0</pose>
+      <parent>right-achilles-rod</parent>
+      <child>right-heel-spring</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="right-plantar-foot-joint" type="ball">
+      <pose>0.055 0 -0.008 0 0 0</pose>
+      <parent>right-plantar-rod</parent>
+      <child>right-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-achilles-spring-joint" type="revolute">
+      <pose>0.11877 -0.01 0 0 0 0</pose>
+      <parent>left-achilles-rod</parent>
+      <child>left-heel-spring</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="left-plantar-foot-joint" type="ball">
+      <pose>0.055 0 0.008 0 0 0</pose>
+      <parent>left-plantar-rod</parent>
+      <child>left-foot</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+      </axis>
+    </joint>
+
+    <joint name="static" type="prismatic">
+      <parent>world</parent>
+      <child>pelvis</child>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <lower>0</lower>
+          <upper>0.2</upper>
+        </limit>
+      </axis>
+    </joint>
+
+    <!-- Attach the plugin to this model -->
+    <plugin filename="libcassie_plugin.so" name="CassiePlugin">
+    </plugin>
+
+</model>
+</sdf>
diff --git a/robots/cassie_description/robots/model.config b/robots/cassie_description/robots/model.config
new file mode 100644
index 0000000000000000000000000000000000000000..ee0c870ef44c763fcfabdf18f295d64485781fa9
--- /dev/null
+++ b/robots/cassie_description/robots/model.config
@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Cassie</name>
+  <version>1.0</version>
+  <sdf version="1.5">cassie.sdf</sdf>
+</model>
diff --git a/robots/cassie_description/srdf/cassie_v2.srdf b/robots/cassie_description/srdf/cassie_v2.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..6bfd1518cf3778dabd2f880249886f8d5a8e2c84
--- /dev/null
+++ b/robots/cassie_description/srdf/cassie_v2.srdf
@@ -0,0 +1,55 @@
+<?xml version="1.0" ?>
+<robot name="cassie">
+
+  <group name="whole_body">
+    <joint name="root_joint" />
+    <joint name="left-roll-op" />
+    <joint name="left-yaw-op" />
+    <joint name="left-pitch-op" />
+    <joint name="left-knee-op" />
+    <joint name="left-knee-shin-joint" />
+    <joint name="left-shin-tarsus-joint" />
+    <joint name="left-tarsus-spring-joint" />
+    <joint name="left-achilles-spring-joint" />
+    <joint name="left-tarsus-crank-joint" />
+    <joint name="left-crank-rod-joint1" />
+    <joint name="left-foot-op" />
+    <joint name="right-roll-op" />
+    <joint name="right-yaw-op" />         
+    <joint name="right-pitch-op" />     
+    <joint name="right-knee-op" /> 
+    <joint name="right-knee-shin-joint" />
+    <joint name="right-shin-tarsus-joint" />
+    <joint name="right-tarsus-spring-joint" />
+    <joint name="right-achilles-spring-joint" />
+    <joint name="right-tarsus-crank-joint" />
+    <joint name="right-crank-rod-joint" />
+    <joint name="right-foot-op" />
+  </group>
+
+  <group_state name="standing" group="whole_body">
+    <joint name="root_joint"                 value="-8.84376603e-04  2.01947521e-05  1.03234704e+00  3.76613677e-04 8.54066138e-02 -1.18694658e-05  9.96346109e-01" />
+    <joint name="left-roll-op"               value="-1.04031254e-03" />
+    <joint name="left-yaw-op"                value="1.68090673e-04" />
+    <joint name="left-pitch-op"              value="7.41104603e-01" />
+    <joint name="left-knee-op"               value="-2.71448998e-02" />
+    <joint name="left-knee-shin-joint"       value="-3.90872643e-01" />
+    <joint name="left-shin-tarsus-joint"     value="2.09080249e-01" />
+    <joint name="left-tarsus-spring-joint"   value="4.68025202e-01" />
+    <joint name="left-achilles-spring-joint" value="-6.79067787e-01" />
+    <joint name="left-tarsus-crank-joint"    value="-1.46111523e+00" />
+    <joint name="left-crank-rod-joint"       value="1.46110617e+00" />
+    <joint name="left-foot-op"               value="-1.46114565e+00" />
+    <joint name="right-roll-op"              value="-1.04086943e-03" />
+    <joint name="right-yaw-op"               value="-1.69133207e-04" />         
+    <joint name="right-pitch-op"             value="7.42278082e-01" />     
+    <joint name="right-knee-op"              value="-2.83885130e-02" /> 
+    <joint name="right-knee-shin-joint"      value="-3.91207267e-01" />
+    <joint name="right-shin-tarsus-joint"    value="2.09642093e-01" />
+    <joint name="right-tarsus-spring-joint"  value="4.69313644e-01" />
+    <joint name="right-achilles-spring-joint"value="-6.80943430e-01" />
+    <joint name="right-tarsus-crank-joint"   value="-1.48154922e+00" />
+    <joint name="right-crank-rod-joint"      value="1.48470113e+00" />
+    <joint name="right-foot-op"              value="-1.46130274e+00" />
+  </group_state>
+</robot>
diff --git a/unittest/test_load.py b/unittest/test_load.py
index 23a0ced76bfcd49205104bf454286217a54bb3a6..e76b00cc049e47e0b71d8cc9349224eaf4dee413 100755
--- a/unittest/test_load.py
+++ b/unittest/test_load.py
@@ -43,6 +43,13 @@ class RobotTestCase(unittest.TestCase):
     def test_baxter(self):
         self.check('baxter', 19, 19)
 
+    def test_cassie(self):
+        try:
+            self.check('cassie', 29, 28)
+        except ImportError:
+            import pinocchio
+            self.assertLess(int(pinocchio.__version__.split('.')[0]), 3)
+
     def test_double_pendulum(self):
         self.check('double_pendulum', 2, 2)