From 9284b4b7ba246472ee89f051a3f4cf2cac29bb9f Mon Sep 17 00:00:00 2001
From: cesar <cesar.rodriguez@spirirobotics.com>
Date: Thu, 15 Sep 2022 16:45:30 -0300
Subject: [PATCH] Can now reliably be installed on 18.04 with ROS Melodic

---
 .gitignore                                    |    2 +-
 airframes/18.04/4000_spiri                    |    1 +
 airframes/18.04/4001_spiri_with_tether        |    3 +
 .../spiri_with_tether-gen.sdf                 | 1138 -----------------
 .../spiri_with_tether/spiri_with_tether.sdf   | 1073 +---------------
 .../spiri_with_tether.sdf.jinja               |   91 +-
 px4_setup/px4_bash.sh                         |    9 +
 src/path_follow.cpp                           |   15 +-
 src/wpoint_tracker.py                         |   11 +-
 9 files changed, 157 insertions(+), 2186 deletions(-)
 delete mode 100644 models/spiri_with_tether/spiri_with_tether-gen.sdf
 create mode 100644 px4_setup/px4_bash.sh

diff --git a/.gitignore b/.gitignore
index 3b19e0b..c4132b6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
-config/mocap_*
+config/mocap*
 launch/cortex_bridge.launch
 launch/debug.launch
 launch/klausen_dampen.launch
diff --git a/airframes/18.04/4000_spiri b/airframes/18.04/4000_spiri
index 60c0c1f..00d0c06 100644
--- a/airframes/18.04/4000_spiri
+++ b/airframes/18.04/4000_spiri
@@ -20,6 +20,7 @@ param set-default MPC_Z_VEL_MAX_UP 1.0
 param set EKF2_AID_MASK 1
 param set EKF2_HGT_MODE 0
 param set SYS_FAILURE_EN 0
+param set COM_RCL_EXCEPT 4
 param set NAV_RCL_ACT 0
 param set NAV_DLL_ACT 0
 
diff --git a/airframes/18.04/4001_spiri_with_tether b/airframes/18.04/4001_spiri_with_tether
index 0bb23c7..43badca 100644
--- a/airframes/18.04/4001_spiri_with_tether
+++ b/airframes/18.04/4001_spiri_with_tether
@@ -21,6 +21,9 @@ param set MPC_TKO_SPEED 1.0
 param set EKF2_AID_MASK 1
 param set EKF2_HGT_MODE 0
 param set SYS_FAILURE_EN 0
+param set COM_RCL_EXCEPT 4
+param set NAV_RCL_ACT 0
+param set NAV_DLL_ACT 0
 
 set MIXER quad_x
 set PWM_OUT 1234
diff --git a/models/spiri_with_tether/spiri_with_tether-gen.sdf b/models/spiri_with_tether/spiri_with_tether-gen.sdf
deleted file mode 100644
index 64ebeef..0000000
--- a/models/spiri_with_tether/spiri_with_tether-gen.sdf
+++ /dev/null
@@ -1,1138 +0,0 @@
-<!-- DO NOT EDIT: Generated from spiri_with_tether.sdf.jinja -->
-<!-- tl = 0.3 num. of el. = 10 payload sz: 0.025-->
-<sdf version="1.5">
- <model name="spiri_with_tether">
-  <pose> 0 0 0 0.0 0.0 0.0</pose> 
-  <include>
-    <uri>model://spiri</uri>
-    <pose>0.0 0.0 0.0 0.0 0.0 0</pose>
-    <static>0</static>
-  </include>
-  <joint name="tether_to_spiri_joint" type="ball">
-    <child>link_spiri_attch</child>
-    <parent>spiri::base</parent><pose>0 0 -0.15 0 0 0 </pose>
-     
-    <physics>
-      <ode></ode>
-      <provide_feedback>true</provide_feedback>
-    </physics>
-  </joint>
-  <plugin name="ft_sensor_spiri" filename="libgazebo_ros_ft_sensor.so">
-    <updateRate>1.0</updateRate>
-    <topicName>ft_sensor_topic/spiri</topicName>
-    <jointName>tether_to_spiri_joint</jointName>
-  </plugin>
-    <link name="link_spiri_attch">
-      <gravity>true</gravity>
-      <pose>0.15 0.0 0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-    <plugin name="link_spiri_attch_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_spiri_attch</link_name>
-    </plugin>
-    <joint name="joint_spiri_attch" type="universal">
-      <child>link_spiri_attch</child>
-      <parent>link_1</parent>
-
-<pose>0 0 0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode><provide_feedback>true</provide_feedback></physics>
-    </joint>
-    <link name="link_1">
-      <gravity>true</gravity>
-      <pose>0.44999999999999996 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_1_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_1</link_name>
-    </plugin>
-    <link name="link_2">
-      <gravity>true</gravity>
-      <pose>0.75 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_2_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_2</link_name>
-    </plugin>
-    <link name="link_3">
-      <gravity>true</gravity>
-      <pose>1.0499999999999998 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_3_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_3</link_name>
-    </plugin>
-    <link name="link_4">
-      <gravity>true</gravity>
-      <pose>1.3499999999999999 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_4_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_4</link_name>
-    </plugin>
-    <link name="link_5">
-      <gravity>true</gravity>
-      <pose>1.65 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_5_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_5</link_name>
-    </plugin>
-    <link name="link_6">
-      <gravity>true</gravity>
-      <pose>1.9499999999999997 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_6_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_6</link_name>
-    </plugin>
-    <link name="link_7">
-      <gravity>true</gravity>
-      <pose>2.25 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_7_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_7</link_name>
-    </plugin>
-    <link name="link_8">
-      <gravity>true</gravity>
-      <pose>2.55 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_8_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_8</link_name>
-    </plugin>
-    <link name="link_9">
-      <gravity>true</gravity>
-      <pose>2.8499999999999996 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_9_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_9</link_name>
-    </plugin><joint name="joint_2" type="universal">
-      <child>link_2</child>
-      <parent>link_1</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_3" type="universal">
-      <child>link_3</child>
-      <parent>link_2</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_4" type="universal">
-      <child>link_4</child>
-      <parent>link_3</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_5" type="universal">
-      <child>link_5</child>
-      <parent>link_4</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_6" type="universal">
-      <child>link_6</child>
-      <parent>link_5</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_7" type="universal">
-      <child>link_7</child>
-      <parent>link_6</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_8" type="universal">
-      <child>link_8</child>
-      <parent>link_7</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_9" type="universal">
-      <child>link_9</child>
-      <parent>link_8</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-   <!-- PAYLOAD -->
-  <model name='mass'>
-    <link name="payload">
-      <pose> 3.0 0 0 0 0 0</pose>
-      <!--pose>0 0 0 0 0 0</pose-->
-	    <gravity>1</gravity>
-	    <inertial>
-		    <mass>0.5</mass>
-	    </inertial> 
-	    <collision name="payload_collision">
-		    <pose>0 0 0 0 0 0</pose>
-		    <geometry>
-			    <sphere>
-				   <radius>0.025</radius>
-			    </sphere>
-		    </geometry>
-	    </collision>
-	    <visual name="payload_visual">
-		    <pose>0 0 0 0 0 0</pose>
-      <geometry>
-          <sphere>
-            <radius>0.025</radius>
-          </sphere>
-        </geometry>
-	    </visual>
-    </link>
-	  <!-- Tether to Payload Connection --><joint name="tether_mass_connection" type="ball"> 
-
-		  <parent>payload</parent>
-		  <child>link_9</child>
-		  <pose>0 0 0.15 0 0 0</pose> 
-    <physics>
-      <ode></ode> 
-      <provide_feedback>true</provide_feedback> 
-    </physics> 
-	  </joint><plugin name="ft_sensor_payload" filename="libgazebo_ros_ft_sensor.so">
-      <updateRate>1.0</updateRate>
-      <topicName>ft_sensor_topic/payload</topicName>
-      <jointName>tether_mass_connection</jointName>
-    </plugin>
-  </model>
- </model>
-</sdf>
-
-<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=2 ts=2 : -->
\ No newline at end of file
diff --git a/models/spiri_with_tether/spiri_with_tether.sdf b/models/spiri_with_tether/spiri_with_tether.sdf
index 64ebeef..873ab98 100644
--- a/models/spiri_with_tether/spiri_with_tether.sdf
+++ b/models/spiri_with_tether/spiri_with_tether.sdf
@@ -1,5 +1,5 @@
 <!-- DO NOT EDIT: Generated from spiri_with_tether.sdf.jinja -->
-<!-- tl = 0.3 num. of el. = 10 payload sz: 0.025-->
+<!-- tl = 0.8 num. of el. = 1 payload sz: 0.025-->
 <sdf version="1.5">
  <model name="spiri_with_tether">
   <pose> 0 0 0 0.0 0.0 0.0</pose> 
@@ -10,7 +10,7 @@
   </include>
   <joint name="tether_to_spiri_joint" type="ball">
     <child>link_spiri_attch</child>
-    <parent>spiri::base</parent><pose>0 0 -0.15 0 0 0 </pose>
+    <parent>spiri::base</parent><pose>0 0 -0.4 0 0 0 </pose>
      
     <physics>
       <ode></ode>
@@ -24,9 +24,9 @@
   </plugin>
     <link name="link_spiri_attch">
       <gravity>true</gravity>
-      <pose>0.15 0.0 0 0.0 1.5707 0.0</pose>
+      <pose>0.4 0.0 0 0.0 1.5707 0.0</pose>
       <inertial>
-        <mass>0.0015</mass>
+        <mass>0.004</mass>
         <inertia>
           <ixx>0.01</ixx>
           <ixy>0</ixy>
@@ -40,7 +40,7 @@
         <pose>0.0 0 0 0 0 0</pose> 
         <geometry>
           <cylinder>
-            <length>0.3</length>
+            <length>0.8</length>
             <radius>0.005</radius>
           </cylinder>
         </geometry>
@@ -62,7 +62,7 @@
         <pose>0 0 0 0 0 0</pose>
         <geometry>
           <cylinder>
-            <length>0.3</length>
+            <length>0.8</length>
             <radius>0.005</radius>
           </cylinder>
         </geometry>
@@ -73,7 +73,7 @@
         </script>
       </material>
       </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
+        <pose> 0 0 -0.4 0 0 0 </pose>
         <geometry>
           <sphere>
             <radius>0.01</radius>
@@ -95,1010 +95,21 @@
       <alpha_stall>0</alpha_stall>
       <cda_stall>1.4326647564469914</cda_stall>
       <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
+      <cp>0.4 0.005 0.005</cp>
+      <area>0.008</area>
       <air_density>1.2041</air_density>
       <forward>0 1 0</forward>
       <upward>0 0 1</upward>
       <link_name>link_spiri_attch</link_name>
     </plugin>
-    <joint name="joint_spiri_attch" type="universal">
-      <child>link_spiri_attch</child>
-      <parent>link_1</parent>
-
-<pose>0 0 0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode><provide_feedback>true</provide_feedback></physics>
-    </joint>
-    <link name="link_1">
-      <gravity>true</gravity>
-      <pose>0.44999999999999996 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_1_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_1</link_name>
-    </plugin>
-    <link name="link_2">
-      <gravity>true</gravity>
-      <pose>0.75 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_2_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_2</link_name>
-    </plugin>
-    <link name="link_3">
-      <gravity>true</gravity>
-      <pose>1.0499999999999998 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_3_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_3</link_name>
-    </plugin>
-    <link name="link_4">
-      <gravity>true</gravity>
-      <pose>1.3499999999999999 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_4_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_4</link_name>
-    </plugin>
-    <link name="link_5">
-      <gravity>true</gravity>
-      <pose>1.65 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_5_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_5</link_name>
-    </plugin>
-    <link name="link_6">
-      <gravity>true</gravity>
-      <pose>1.9499999999999997 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_6_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_6</link_name>
-    </plugin>
-    <link name="link_7">
-      <gravity>true</gravity>
-      <pose>2.25 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_7_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_7</link_name>
-    </plugin>
-    <link name="link_8">
-      <gravity>true</gravity>
-      <pose>2.55 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_8_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_8</link_name>
-    </plugin>
-    <link name="link_9">
-      <gravity>true</gravity>
-      <pose>2.8499999999999996 0.0 0.0 0.0 1.5707 0.0</pose>
-      <inertial>
-        <mass>0.0015</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name="collision">
-        <pose>0.0 0 0 0 0 0</pose> 
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-	      <min_depth>0.00005</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-	      <mu>1.0</mu>
-	      <mu2>1.0</mu2>
-            </ode>
-          </friction>
-        </surface>
-      </collision>
-      <visual name="element_visual">
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.3</length>
-            <radius>0.005</radius>
-          </cylinder>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/White</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual><visual name="sphere_visual">
-        <pose> 0 0 -0.15 0 0 0 </pose>
-        <geometry>
-          <sphere>
-            <radius>0.01</radius>
-          </sphere>
-        </geometry>
-        <material>
-        <script>
-          <name>Gazebo/Red</name>
-          <uri>file://media/materials/scripts/gazebo.material</uri>
-        </script>
-      </material>
-      </visual>
-    </link>
-  <plugin name="link_9_drag" filename="libLiftDragPlugin.so">
-      <robotNamespace></robotNamespace>
-      <a0>0</a0>
-      <cla>0</cla>
-      <cda>1.253582</cda>
-      <alpha_stall>0</alpha_stall>
-      <cda_stall>1.4326647564469914</cda_stall>
-      <useConstantDragCoefficient>true</useConstantDragCoefficient>
-      <cp>0.15 0.005 0.005</cp>
-      <area>0.003</area>
-      <air_density>1.2041</air_density>
-      <forward>0 1 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>link_9</link_name>
-    </plugin><joint name="joint_2" type="universal">
-      <child>link_2</child>
-      <parent>link_1</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_3" type="universal">
-      <child>link_3</child>
-      <parent>link_2</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_4" type="universal">
-      <child>link_4</child>
-      <parent>link_3</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_5" type="universal">
-      <child>link_5</child>
-      <parent>link_4</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_6" type="universal">
-      <child>link_6</child>
-      <parent>link_5</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_7" type="universal">
-      <child>link_7</child>
-      <parent>link_6</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_8" type="universal">
-      <child>link_8</child>
-      <parent>link_7</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-  <joint name="joint_9" type="universal">
-      <child>link_9</child>
-      <parent>link_8</parent>
-
-<pose>0 0 -0.15 0 0 0 </pose>    
-      <axis>
-        <xyz>0 1 0</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis>
-      <axis2>
-        <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.05</damping>
-          <friction>0.0</friction>
-          <joint_stiffness>0.05</joint_stiffness>
-          <spring_reference>0.0</spring_reference>
-        </dynamics>
-        <use_parent_model_frame>true</use_parent_model_frame>
-      </axis2>
-      <physics>
-        <ode>
-          <cfm_damping>1</cfm_damping>          
-        </ode></physics>
-    </joint>
-   <!-- PAYLOAD -->
+     <!-- PAYLOAD -->
   <model name='mass'>
     <link name="payload">
-      <pose> 3.0 0 0 0 0 0</pose>
+      <pose> 0.8 0 0 0 0 0</pose>
       <!--pose>0 0 0 0 0 0</pose-->
 	    <gravity>1</gravity>
 	    <inertial>
-		    <mass>0.5</mass>
+		    <mass>0.25</mass>
 	    </inertial> 
 	    <collision name="payload_collision">
 		    <pose>0 0 0 0 0 0</pose>
@@ -1117,22 +128,60 @@
         </geometry>
 	    </visual>
     </link>
-	  <!-- Tether to Payload Connection --><joint name="tether_mass_connection" type="ball"> 
-
+	  <!-- Tether to Payload Connection -->
+	  <joint name="tether_mass_connection" type="fixed">
 		  <parent>payload</parent>
-		  <child>link_9</child>
-		  <pose>0 0 0.15 0 0 0</pose> 
+		  <child>link_spiri_attch</child>
+		  <pose>0 0 0.4 0 0 0</pose> 
     <physics>
       <ode></ode> 
       <provide_feedback>true</provide_feedback> 
     </physics> 
-	  </joint><plugin name="ft_sensor_payload" filename="libgazebo_ros_ft_sensor.so">
-      <updateRate>1.0</updateRate>
-      <topicName>ft_sensor_topic/payload</topicName>
-      <jointName>tether_mass_connection</jointName>
-    </plugin>
-  </model>
+	  </joint>
+  <!--/model--><link name='pload_imu'>
+	      <pose>0.8 0 0 0 0 0</pose>
+	      <inertial>
+		<mass>0.015</mass>
+		<inertia>
+		  <ixx>1e-05</ixx>
+		  <ixy>0</ixy>
+		  <ixz>0</ixz>
+		  <iyy>1e-05</iyy>
+		  <iyz>0</iyz>
+		  <izz>1e-05</izz>
+		</inertia>
+	      </inertial>
+	    </link>
+	    <joint name='pload_imu_joint' type='revolute'>
+	      <child>pload_imu</child>
+	      <parent>payload</parent>
+	      <axis>
+		<xyz>1 0 0</xyz>
+		<limit>
+		  <lower>0</lower>
+		  <upper>0</upper>
+		  <effort>0</effort>
+		  <velocity>0</velocity>
+		</limit>
+		<dynamics>
+		  <spring_reference>0</spring_reference>
+		  <spring_stiffness>0</spring_stiffness>
+		</dynamics>
+		<use_parent_model_frame>0</use_parent_model_frame>
+	      </axis>
+	    </joint>
+      <plugin name='imu_plugin' filename='libgazebo_ros_imu.so'>
+        <robotNamespace/>
+        <alwaysOn>true</alwaysOn>
+        <bodyName>pload_imu</bodyName>
+        <topicName>/pload_imu</topicName>
+        <serviceName>imu_service</serviceName>
+        <gaussianNoise>0.0</gaussianNoise>
+        <updateRate>20.0</updateRate>
+      </plugin>
+    </model>
+  
  </model>
 </sdf>
 
-<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=2 ts=2 : -->
\ No newline at end of file
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=2 ts=2 : -->
diff --git a/models/spiri_with_tether/spiri_with_tether.sdf.jinja b/models/spiri_with_tether/spiri_with_tether.sdf.jinja
index 6714efd..9ffd63c 100644
--- a/models/spiri_with_tether/spiri_with_tether.sdf.jinja
+++ b/models/spiri_with_tether/spiri_with_tether.sdf.jinja
@@ -4,28 +4,29 @@
 {#--------------------------------The parameters bellow are the ones to be tweaked---------------------------#}
 {#--------------------------------------------:Tweakable parameters:-----------------------------------------#}
 {%- set number_elements = 1 -%}
-{%- set tl = 1 -%}	  			                {#- tl: length of the tether element (meters) -#}
-{%- set cr = 0.005 -%}	  			            {#- cr: radius of the tether element (meters) -#}
-{%- set cr_v = 0.005 -%}	  		            {#- cr_v: radius of the tether element visual (meters) -#}
-{%- set sr = 0.01 -%}	  			              {#- sr: element joint radius (sphere) (meters) -#}
-{%- set m = 0.005 * tl -%}			            {#- m: mass of the element (kg), given the length -#}
-{%- set cda = 1.253582 -%}		              {#- cda: the ratio of the drag coefficient before stall. -#}
+{%- set tl = 0.8 -%}                        {#- tl: length of the tether element (meters) -#}
+{%- set cr = 0.005 -%}                      {#- cr: radius of the tether element (meters) -#}
+{%- set cr_v = 0.005 -%}                    {#- cr_v: radius of the tether element visual (meters) -#}
+{%- set sr = 0.01 -%}                       {#- sr: element joint radius (sphere) (meters) -#}
+{%- set m = 0.005 * tl -%}                  {#- m: mass of the element (kg), given the length -#}
+{%- set cda = 1.253582 -%}                  {#- cda: the ratio of the drag coefficient before stall. -#}
 {%- set cda_stall = 1.4326647564469914 -%}  {#- cda_stall: the ratio of the drag coefficient after stall. -#}
-{%- set damping = 0.05 -%}		              {#- Model damping -#}
-{%- set friction = 0.0 -%}			            {#- Friction of the model relative to the world -#}
-{%- set tether_stiffness = 0.05 -%}		      {#- Tether stiffness  -#}
-{%- set joint_stiffness = 0.05 -%}		      {#- Joint stiffness, i.e where tether attaches to drone -#}
-{%- set spring_reference = 0.0 -%}		      {#- Reference where the spring forces are applied -#}
-{%- set element_color = 'White' -%}		      {#- Color of the tether elements -#}
-{%- set joint_color = 'Red' -%}		  	      {#- Color of the tether element joints -#}
-{%- set vehicle_spawn_point_y = 0.0 -%}	    {#- Y coordinate of vehicle spawn point -#}
-{%- set debug = 0 -%}		            	      {#- Used for debugging tether joints -#}
+{%- set damping = 0.0001 -%}                  {#- Model damping -#}
+{%- set friction = 0.0 -%}                  {#- Friction of the model relative to the world -#}
+{%- set tether_stiffness = 0.0005 -%}         {#- Tether stiffness  -#}
+{%- set joint_stiffness = 0.0005 -%}          {#- Joint stiffness, i.e where tether attaches to drone -#}
+{%- set spring_reference = 0.0 -%}          {#- Reference where the spring forces are applied -#}
+{%- set element_color = 'White' -%}         {#- Color of the tether elements -#}
+{%- set joint_color = 'Red' -%}             {#- Color of the tether element joints -#}
+{%- set vehicle_spawn_point_y = 0.0 -%}     {#- Y coordinate of vehicle spawn point -#}
+{%- set debug = 0 -%}                       {#- Used for debugging tether joints -#}
 
 {#----------------------------------------------------------------------------------------------------------#}
 {#---------------------------------------------:Payload parameters:-----------------------------------------#}
 {#----------------------------------------------------------------------------------------------------------#}
-{%- set payload_m = 0.5 -%}                 {#- Payload mass (kg) -#}
-{%- set pr = 0.025 -%}                      {#- Payload radius (meters). Has to be less than FULL tether length -#}
+{%- set payload_m = 0.25 -%}  {#- Payload mass (kg). Used to be 0.5 -#}
+{%- set pr = 0.025 -%}       {#- Payload radius (meters). Has to be less than FULL tether length -#}
+{%- set add_imu = 1 -%}      {#- Add IMU to payload for localization-#}
 {#-----------------------------------------------------------------------------------------------------------#}
 {#----------------------------------------------:Spiri parameters:-------------------------------------------#}
 {#-----------------------------------------------------------------------------------------------------------#}
@@ -62,7 +63,7 @@
 {%- set pload_joint_z = tl/2 -%}
 {%- endif -%}
 
-{%- set tether_pos =  vehicle_spawn_point_z - tl/2  -%}  {#- Spawns tether vertically -#}
+{%- set tether_pos =  vehicle_spawn_point_z - full_tether_len/2  -%}  {#- Spawns tether vertically -#}
 {#----------------------------------------------------------------------------------------------------------#}
 {#-----------------------------------------------:MACROS:---------------------------------------------------#}
 {#----------------------------------------------------------------------------------------------------------#}
@@ -114,7 +115,7 @@
       <jointName>tether_mass_connection</jointName>
     </plugin>
 {%- endif %}
-  </model>
+  <!--/model-->
 {%- endmacro -%}
 
 {%- macro cylinder(tl, cr_v) -%}
@@ -126,6 +127,51 @@
         </geometry>
 {%- endmacro -%}
 
+{%- macro imu_link(x, y, z) -%}
+	    <link name='pload_imu'>
+	      <pose>{{ x }} {{ y }} {{ z }} 0 0 0</pose>
+	      <inertial>
+		<mass>0.015</mass>
+		<inertia>
+		  <ixx>1e-05</ixx>
+		  <ixy>0</ixy>
+		  <ixz>0</ixz>
+		  <iyy>1e-05</iyy>
+		  <iyz>0</iyz>
+		  <izz>1e-05</izz>
+		</inertia>
+	      </inertial>
+	    </link>
+	    <joint name='pload_imu_joint' type='revolute'>
+	      <child>pload_imu</child>
+	      <parent>payload</parent>
+	      <axis>
+		<xyz>1 0 0</xyz>
+		<limit>
+		  <lower>0</lower>
+		  <upper>0</upper>
+		  <effort>0</effort>
+		  <velocity>0</velocity>
+		</limit>
+		<dynamics>
+		  <spring_reference>0</spring_reference>
+		  <spring_stiffness>0</spring_stiffness>
+		</dynamics>
+		<use_parent_model_frame>0</use_parent_model_frame>
+	      </axis>
+	    </joint>
+      <plugin name='imu_plugin' filename='libgazebo_ros_imu.so'>
+        <robotNamespace/>
+        <alwaysOn>true</alwaysOn>
+        <bodyName>pload_imu</bodyName>
+        <topicName>/pload_imu</topicName>
+        <serviceName>imu_service</serviceName>
+        <gaussianNoise>0.0</gaussianNoise>
+        <updateRate>20.0</updateRate>
+      </plugin>
+    </model>
+{%- endmacro -%}
+
 {%- macro sphere(sr) -%}
       <geometry>
           <sphere>
@@ -357,7 +403,7 @@
  <model name="spiri_with_tether">
   <pose> 0 0 0 0.0 0.0 0.0</pose> {# only have to change this to change spawn location #}
   <include>
-    <uri>model://spiri</uri>
+    <uri>model://spiri_mocap</uri>
     <pose>0.0 0.0 {{ vehicle_spawn_point_z }} 0.0 0.0 {{ yaw }}</pose>
     <static>{{ debug }}</static>
   </include>
@@ -385,6 +431,11 @@
   {%- else -%}
   {%- set n = number_elements - 1 -%}
   {% endif %} {{ payload(payload_m,n,pr,pload_spawn_x,pload_spawn_y,pload_spawn_z,pload_joint_x,pload_joint_y,pload_joint_z) }}
+
+{#------------- ADD IMU -----------#}
+  {%- if add_imu == 1 -%}
+  {{ imu_link(pload_spawn_x,pload_spawn_y,pload_spawn_z) }}
+  {% endif %}
  </model>
 </sdf>
 
diff --git a/px4_setup/px4_bash.sh b/px4_setup/px4_bash.sh
new file mode 100644
index 0000000..16e7299
--- /dev/null
+++ b/px4_setup/px4_bash.sh
@@ -0,0 +1,9 @@
+echo "Copying model files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/models/* ~/PX4-Autopilot/Tools/sitl_gazebo/models
+echo "Copying world files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/worlds/* ~/PX4-Autopilot/Tools/sitl_gazebo/worlds
+echo "Copying airframes files to px4 directory..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/airframes/18.04/* ~/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
+echo "Adding airframe names to px4_sitl make targets..."
+cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/CMakeLists.txt ~/PX4-Autopilot/Tools/sitl_gazebo/
+echo "Done!"
diff --git a/src/path_follow.cpp b/src/path_follow.cpp
index 1b922f7..7afb312 100644
--- a/src/path_follow.cpp
+++ b/src/path_follow.cpp
@@ -143,7 +143,7 @@ int main(int argc, char **argv)
 	if (waypoint_cl.call(wpoint_srv))
 	{
 		ROS_INFO("Waypoints received");
-		// populate desired waypoints - this is only for original hover as 
+		// populate desired waypoints
 		buff_pose.pose.position.x = wpoint_srv.response.xd.x;
 		buff_pose.pose.position.y = wpoint_srv.response.xd.y;
 		buff_pose.pose.position.z = wpoint_srv.response.xd.z;
@@ -167,11 +167,14 @@ int main(int argc, char **argv)
 				last_request = ros::Time::now();
 			}
 		}
-		
-		if(current_state.mode == "OFFBOARD" && current_state.armed){
-			ROS_INFO_ONCE("Spiri is taking off");
+		if (waypoint_cl.call(wpoint_srv))
+		{
+			// check if waypoints have changed
+			buff_pose.pose.position.x = wpoint_srv.response.xd.x;
+			buff_pose.pose.position.y = wpoint_srv.response.xd.y;
+			buff_pose.pose.position.z = wpoint_srv.response.xd.z;
+			alt_des = wpoint_srv.response.xd.z;
 		}
-
 		// Check if we want to use oscillation controller
 		if (ros::param::has("/status/use_ctrl")){
 			ros::param::get("/status/use_ctrl", oscillation_damp);
@@ -186,7 +189,7 @@ int main(int argc, char **argv)
 				// Publish position setpoints
 				local_pos_pub.publish(buff_pose);
 			}
-			ROS_INFO("Des Altitude: %.2f", wpoint_srv.response.xd.z);
+			ROS_INFO("Des Altitude: %.2f", alt_des);
 			ROS_INFO("Cur Altitude: %.2f", current_altitude);	
 			ROS_INFO("---------------------------");	
 		} else {
diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py
index 6c7382b..da56e7a 100755
--- a/src/wpoint_tracker.py
+++ b/src/wpoint_tracker.py
@@ -25,19 +25,12 @@ class Main:
 		self.param_exists = False # check in case param does not exist
 
 		while self.param_exists == False:
-			if rospy.has_param('sim/waypoints'):
-				self.wpoints	= rospy.get_param('sim/waypoints') # get waypoints
+			if rospy.has_param('status/waypoints'):
+				self.wpoints	= rospy.get_param('status/waypoints') # get waypoints
 				self.xd.x = self.wpoints['x']
 				self.xd.y = self.wpoints['y']
 				self.xd.z = self.wpoints['z']
 				self.param_exists = True
-			elif rospy.has_param('mocap/waypoints'):
-				self.wpoints	= rospy.get_param('mocap/waypoints') # get waypoints
-				self.xd.x = self.wpoints['x']
-				self.xd.y = self.wpoints['y']
-				self.xd.z = self.wpoints['z']
-				self.param_exists = True
-
 			elif rospy.get_time() - self.tstart >= 5.0:
 				self.xd.x = 0.0
 				self.xd.y = 0.0