{#--------------------------------SDF generator for Spiri with Tether Gazebo model---------------------------#}
{#------------------------------Retrieved from:https://github.com/RigidWing/sitl_gazebo----------------------#}
{#---------------------------------Maintained by Kitepower BV: info@kitepower.nl-----------------------------#}
{#--------------------------------The parameters bellow are the ones to be tweaked---------------------------#}
{#--------------------------------------------:Tweakable parameters:-----------------------------------------#}
{%- set number_elements = 1 -%}
{%- 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.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.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:-------------------------------------------#}
{#-----------------------------------------------------------------------------------------------------------#}
{%- set spiri_collision_x = 0.21355 -%}
{%- set spiri_collision_y = 0.132 -%}
{%- set spiri_collision_z = 0.04206 -%}
{%- set theta = 1.5707 -%}
{%- set yaw = 0 -%}                         {#- Necessary to spawn magnometer and Spiri in correct orientation -#}
{#----------------------------------------------------------------------------------------------------------#}
{#---------------------------------------------:Computed parameters:----------------------------------------#}
{#----------------------------------------------------------------------------------------------------------#}
{%- set last_elem = number_elements |int - 1 -%}
{%- set full_tether_len = number_elements * tl -%}
{%- set vehicle_spawn_point_x =  full_tether_len  -%}

{#- Check if we are in debug mode -#}
{%- if debug == 1 -%}

{%- set vehicle_spawn_point_z = 4.0 -%}     {#- Used to spawn in desired z location -#}
{%- set pload_spawn_x = 0 -%}
{%- set pload_spawn_y = 0 -%}
{%- set pload_spawn_z = vehicle_spawn_point_z - tl -%}
{%- set pload_joint_x = 0 -%}
{%- set pload_joint_y = 0 -%}
{%- set pload_joint_z = -tl/2 -%}
{%- else -%}

{%- set vehicle_spawn_point_z = 0.0 -%}
{%- set pload_spawn_x = full_tether_len -%}
{%- set pload_spawn_y = 0 -%}
{%- set pload_spawn_z = 0 -%}
{%- set pload_joint_x = 0 -%}
{%- set pload_joint_y = 0 -%}
{%- set pload_joint_z = tl/2 -%}
{%- endif -%}

{%- set tether_pos =  vehicle_spawn_point_z - full_tether_len/2  -%}  {#- Spawns tether vertically -#}
{#----------------------------------------------------------------------------------------------------------#}
{#-----------------------------------------------:MACROS:---------------------------------------------------#}
{#----------------------------------------------------------------------------------------------------------#}

{%- macro payload(payload_m, n, pr, x, y, z, j_x, j_y, j_z) -%}
  <!-- PAYLOAD -->
  <model name='mass'>
    <link name="payload">
      <pose> {{ x }} {{ y }} {{ z }} 0 0 0</pose>
      <!--pose>0 0 0 0 0 0</pose-->
	    <gravity>1</gravity>
	    <inertial>
		    <mass>{{ payload_m }}</mass>
	    </inertial> 
	    <collision name="payload_collision">
		    <pose>0 0 0 0 0 0</pose>
		    <geometry>
			    <sphere>
				   <radius>{{ pr }}</radius>
			    </sphere>
		    </geometry>
	    </collision>
	    <visual name="payload_visual">
		    <pose>0 0 0 0 0 0</pose>
      {{ sphere(pr) }}
	    </visual>
    </link>
	  <!-- Tether to Payload Connection -->
{%- if number_elements == 1 %}
	  <joint name="tether_mass_connection" type="fixed">
{%- else -%}
{%- set k = 'ball' -%}
{#-     <joint name="tether_mass_connection" type="ball"> -#}
  <joint name="tether_mass_connection" type="ball"> 
{% endif %}
		  <parent>payload</parent>
		  <child>link_{{n}}</child>
{#- joint_param(tl, damping, friction, joint_stiffness, spring_reference, k) #}
		  <pose>{{ j_x }} {{ j_y }} {{ j_z }} 0 0 0</pose> 
    <physics>
      <ode></ode> 
      <provide_feedback>true</provide_feedback> 
    </physics> 
	  </joint>
{%- if n is number -%}
    <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>
{%- endif %}
  <!--/model-->
{%- endmacro -%}

{%- macro cylinder(tl, cr_v) -%}
      <geometry>
          <cylinder>
            <length>{{tl}}</length>
            <radius>{{cr_v}}</radius>
          </cylinder>
        </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>
            <radius>{{sr}}</radius>
          </sphere>
        </geometry>
{%- endmacro -%}

{%- macro inertial(m) -%}
{#- A note about the inertial tensor matrix - It should be as below -#}
{#- set izz = m/2*cr**2 -#} 
{#- set ixx = m/12*tl**2 + m/4*cr**2 -#} 
{#- set iyy = m/12*tl**2 + m/4*cr**2 -#} 
{#- but seems to segfault Gazebo when changing the values of the element specs -#}
{%- set izz = 0.01 -%}
{%- set ixx = 0.01 -%}
{%- set iyy = 0.01 -%}
      <inertial>
        <mass>{{m}}</mass>
        <inertia>
          <ixx>{{ixx}}</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>{{iyy}}</iyy>
          <iyz>0</iyz>
          <izz>{{izz}}</izz>
        </inertia>
      </inertial>
{%- endmacro -%}

{%- macro collision(tl, cr) -%}
<collision name="collision">
        <pose>{{tl / 2 - tl / 2}} 0 0 0 0 0</pose> 
        {{ cylinder(tl, cr) }}
        <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>
{%- endmacro -%}

{%- macro element_material(element_color) -%}
      <material>
        <script>
          <name>Gazebo/{{element_color}}</name>
          <uri>file://media/materials/scripts/gazebo.material</uri>
        </script>
      </material>
{%- endmacro -%}

{%- macro joint_material(joint_color) -%}
      <material>
        <script>
          <name>Gazebo/{{joint_color}}</name>
          <uri>file://media/materials/scripts/gazebo.material</uri>
        </script>
      </material>
{%- endmacro -%}

{%- macro element_visual(tl, cr_v, element_color) -%}
{%- set x = tl * 0.5 -%}
      <visual name="element_visual">
        <pose>0 0 0 0 0 0</pose>
        {{ cylinder(tl, cr_v) }}
        {{ element_material(element_color) }}
      </visual>
{%- endmacro -%}

{%- macro sphere_visual(sr, joint_color) -%}
      <visual name="sphere_visual">
        <pose> 0 0 {{ tl / -2 }} 0 0 0 </pose>
        {{ sphere(sr) }}
        {{ joint_material(joint_color) }}
      </visual>
{%- endmacro -%}

{%- macro link(n, x, y, z, theta, m, tl, cr, cr_v, sr, element_color, joint_color) -%}
{% if debug == 1 %}
  <model name="tether">
{%- endif %}
    <link name="link_{{n}}">
      <gravity>true</gravity>
      <pose>{{x}} {{y}} {{z}} 0.0 {{theta}} 0.0</pose>
      {{ inertial(m) }}
{%- if n != 0 and n != "spiri_attch_fixed" %}
      {{ collision(tl, cr) }}
      {{ element_visual(tl, cr_v, element_color) }}
{%- endif -%}
{%- if n != 0 -%}
{{ sphere_visual(sr, joint_color) }}
{%- endif %}
    </link>
{%- endmacro %}

{%- macro joint(k, damping, friction, joint_stiffness, spring_reference, joint_type, number_elements) -%}
{%- if k is number -%}
{%- set link_n = k - 1 -%}
    <joint name="joint_{{k}}" type="{{joint_type}}">
      <child>link_{{k}}</child>
      <parent>link_{{link_n}}</parent>

{{ joint_param(tl, damping, friction, tether_stiffness, spring_reference, k) }}
{%- elif number_elements == 1 -%}
{%- else -%}
{%- set link_n = 1 -%}
    <joint name="joint_{{k}}" type="{{joint_type}}">
      <child>link_{{k}}</child>
      <parent>link_{{link_n}}</parent>

{{ joint_param(tl, damping, friction, tether_stiffness, spring_reference, k) }}

{%- endif %}
{%- endmacro -%}

{%- macro joint_param(tl, damping, friction, joint_stiffness, spring_reference, k) -%}
{%- if k is number -%}
      <pose>0 0 -{{ tl/2 }} 0 0 0 </pose> 
{%- else -%}
      <pose>0 0 {{ tl/2 }} 0 0 0 </pose>
{%- endif %}    
      <axis>
        <xyz>0 1 0</xyz>
        <dynamics>
          <damping>{{damping}}</damping>
          <friction>{{friction}}</friction>
          <joint_stiffness>{{joint_stiffness}}</joint_stiffness>
          <spring_reference>{{spring_reference}}</spring_reference>
        </dynamics>
        <use_parent_model_frame>true</use_parent_model_frame>
      </axis>
      <axis2>
        <xyz>0 0 1</xyz>
        <dynamics>
          <damping>{{damping}}</damping>
          <friction>{{friction}}</friction>
          <joint_stiffness>{{joint_stiffness}}</joint_stiffness>
          <spring_reference>{{spring_reference}}</spring_reference>
        </dynamics>
        <use_parent_model_frame>true</use_parent_model_frame>
      </axis2>
      <physics>
        <ode>
          <cfm_damping>1</cfm_damping>          
        </ode>
{%- if k is not number -%}
        <provide_feedback>true</provide_feedback>
{%- endif -%}
      </physics>
    </joint>
{%- endmacro -%}

{%- macro lift_drag(n, tl, cr) -%}
{%- set area = 2 * cr * tl -%}
    <plugin name="link_{{n}}_drag" filename="libLiftDragPlugin.so">
      <robotNamespace></robotNamespace>
      <a0>0</a0>
      <cla>0</cla>
      <cda>{{cda}}</cda>
      <alpha_stall>0</alpha_stall>
      <cda_stall>{{cda_stall}}</cda_stall>
      <useConstantDragCoefficient>true</useConstantDragCoefficient>
      <cp>{{tl/2}} {{cr}} {{cr}}</cp>
      <area>{{area}}</area>
      <air_density>1.2041</air_density>
      <forward>0 1 0</forward>
      <upward>0 0 1</upward>
      <link_name>link_{{n}}</link_name>
    </plugin>
{%- endmacro -%}

{#- <!-- Creates the final link - Where payload is attached(?) --> -#}
{%- macro final_tether(tl) -%}
    {%- if debug == 1 -%}
    {{ link("spiri_attch", 0, 0, tether_pos, 0, m, tl, cr, cr_v, sr, element_color, joint_color) }}
    {{ lift_drag("spiri_attch", tl, cr) }}
    {{ joint("spiri_attch", damping, friction, joint_stiffness, spring_reference, "universal", number_elements) }}
    {%- else -%}
    {%- set lose_x = tl/2 -%}
    {{ link("spiri_attch", tl/2, vehicle_spawn_point_y, 0, theta, m, tl, cr, cr_v, sr, element_color, joint_color) }}
    {{ lift_drag("spiri_attch", tl, cr) }}
    {{ joint("spiri_attch", damping, friction, joint_stiffness, spring_reference, "universal", number_elements) }}
    {%- endif -%}
    {%- if debug == 1 -%}
      </model>
    {%- endif -%}
    {%- endmacro -%}

{#- <!-- Used to change attachment joint of tether and drone --> -#}
{%- macro joint_type(j_type) -%}
  {%- if j_type == 'ball' -%}
  <joint name="tether_to_spiri_joint" type="ball">
    <child>link_spiri_attch</child>
    <parent>spiri::base</parent>
{#- joint_param(tl, damping, friction, joint_stiffness, spring_reference, 1) -#}
    {% if debug == 1 %}
    <pose>0 0 {{ tl/2 }} 0 0 0 </pose>
    {%- else -%}
    <pose>0 0 -{{ tl/2 }} 0 0 0 </pose>
    {% endif %} 
    <physics>
      <ode></ode>
      <provide_feedback>true</provide_feedback>
    </physics>
  </joint>
  {%- else -%}
  <joint name="tether_to_spiri_joint" type="universal">
    <child>link_spiri_attch</child>
    <parent>spiri::base</parent>
{{ joint_param(tl, damping, friction, joint_stiffness, spring_reference, 1) }}
{%- endif -%}
{%- endmacro -%}


{#----------------------------------------------------------------------------------------------------------#}
{#-----------------------------------------------:SDF GENERATION:-------------------------------------------#}
{#----------------------------------------------------------------------------------------------------------#}
<!-- DO NOT EDIT: Generated from spiri_with_tether.sdf.jinja -->
<!-- tl = {{tl}} num. of el. = {{number_elements}} payload sz: {{pr}}-->
<sdf version="1.5">
 <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_mocap</uri>
    <pose>0.0 0.0 {{ vehicle_spawn_point_z }} 0.0 0.0 {{ yaw }}</pose>
    <static>{{ debug }}</static>
  </include>
  {{joint_type('ball')}}
  <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>

{#- ---------- CREATE TETHER LINKS -------- -#}
  {{ final_tether(tl) }}
  {%- for n in range(1, number_elements) -%}
  {%- set x = (tl / 2 + (n*tl)) -%}
  {{ link(n, x, vehicle_spawn_point_y, 0.0, theta, m, tl, cr, cr_v, sr, element_color, joint_color) }}
  {{ lift_drag(n, tl, cr) }}
  {%- endfor -%}
  {%- for k in range(2, number_elements) -%}
  {{ joint(k, damping, friction, tether_stiffness, spring_reference, "universal", number_elements) }}
  {% endfor %}

{#------------- PAYLOAD -----------#}
  {%- if number_elements == 1 -%}
  {%- set n = "spiri_attch" -%}
  {%- 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>

<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=2 ts=2 : -->