{#-------------------SDF generator for Tether Gazebo model--------------------#}
{#--------------The parameters bellow are the ones to be tweaked--------------#}
{#--------------------------:Tweakable parameters:----------------------------#}
{%- set number_elements = 1 -%}
{%- set last_elem = number_elements |int - 1 -%}
{%- set cl = 0.17 -%}	  {#- cl: length of the element (meters) -#}
{%- set cr = 0.001 -%}	  {#- cr: radius of the element (meters) -#}
{%- set sr = 0.009 -%}	  {#- sr: element joint radius (sphere) (meters) -#}
{%- set y_el = 0.2 -%}	  {#- y_el: length of the Y elements (meters) -#}
{%- set y_tetha = 18 -%}  {#- y_tetha: angle between the Y elements (degrees)-#}
{%- set m = 0.03 * cl -%} {#- m: mass of the element (kg), given the length -#}
{%- set damping = 0.05 -%}
{%- set friction = 0.0 -%}
{%- set spring_stiffness = 0.01 -%}
{%- set spring_reference = 0.0 -%}
{%- set element_color = 'White' -%}
{%- set joint_color = 'Red' -%}
{#----------------------------------------------------------------------------#}

{%- macro cylinder(cl, cr) -%}
      <geometry>
          <cylinder>
            <length>{{cl}}</length>
            <radius>{{cr}}</radius>
          </cylinder>
        </geometry>
{%- 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 bellow -#}
{#- {%- set izz = m/2*cr**2 -%} -#}
{#- {%- set ixx = m/12*cl**2 + m/4*cr**2 -%} -#}
{#- {%- set iyy = m/12*cl**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(cl, cr) -%}
<collision name="collision">
        <pose>0.1 0 0 0 1.570790 0</pose>
        {{ cylinder(cl, 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(cl, cr, element_color) -%}
{%- set x = cl * 0.5 -%}
      <visual name="element_visual">
        <pose>{{x}} 0 0 0 1.570790 0</pose>
        {{ cylinder(cl, cr) }}
        {{ element_material(element_color) }}
      </visual>
{%- endmacro -%}

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

{%- macro link(n, x, y, z, yaw, m, cl, cr, sr, element_color, joint_color) -%}
    <link name="link_{{n}}">
      <gravity>true</gravity>
      <pose>{{x}} {{y}} {{z}} 0.0 0.0 {{yaw}}</pose>
      {{ inertial(m) }}
      {%- if n != last_elem -%}
      {{ collision(cl, cr) }}
      {{ element_visual(cl, cr, element_color) }}
      {%- endif -%}
      {{ sphere_visual(sr, joint_color) }}
    </link>
{%- endmacro %}

{%- macro joint(k, damping, friction, spring_stiffness, spring_reference) -%}
{%- if k is number -%}
{%- set link_n = k - 1 -%}
    <joint name="joint_{{k}}" type="universal">
      <child>link_{{k}}</child>
      <parent>link_{{link_n}}</parent>
{%- else -%}
{%- set link_n = 0 -%}
    <joint name="joint_{{k}}" type="universal">
      <child>link_{{link_n}}</child>
      <parent>link_{{k}}</parent>
{%- endif %}
      <axis>
        <xyz>0 1 0</xyz>
        <dynamics>
          <damping>{{damping}}</damping>
          <friction>{{friction}}</friction>
          <spring_stiffness>{{spring_stiffness}}</spring_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>
          <spring_stiffness>{{spring_stiffness}}</spring_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>
      </physics>
    </joint>
{%- endmacro -%}

{%- macro lift_drag(n, cl, cr) -%}
{%- set area = 2 * cr * cl -%}
    <plugin name="link_{{n}}_drag" filename="libLiftDragPlugin.so">
      <robotNamespace></robotNamespace>
      <a0>0</a0>
      <cla>0</cla>
      <cda>1.2535816618911175</cda>
      <alpha_stall>0</alpha_stall>
      <cda_stall>1.4326647564469914</cda_stall>
      <cp>0 0 0</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 %}

{%- macro y_element(cl, y_el, y_tetha) %}
{%- set x_l = - cl / 2 -%}
{%- set y_l = 0.0 -%}
{%- set z_l = 0.0 -%}
{%- set yaw_l = - y_tetha / 2 -%}
    {{ link("y_left", x_l, y_l, z_l, yaw_l, m, y_el, cr, sr, element_color, joint_color) }}
    {{ lift_drag("y_left", cl, cr) }}
{%- set x_r = - cl / 2 -%}
{%- set y_r = 0 -%}
{%- set z_r = 0.0 -%}
{%- set yaw_r = y_tetha / 2 %}
    {{ link("y_right", x_r, y_r, z_r, yaw_r, m, y_el, cr, sr, element_color, joint_color) }}
    {{ lift_drag("y_right", cl, cr) }}
    {{ joint("y_left", damping, friction, spring_stiffness, spring_reference) }}
    {{ joint("y_right", damping, friction, spring_stiffness, spring_reference) }}
{%- endmacro -%}

<?xml version="1.0" ?>
<!-- DO NOT EDIT: Generated from tether.sdf.jinja -->
<sdf version="1.5">
  <model name="tether">
    <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
    {% for n in range(number_elements) -%}
    {%- set x = n * cl / 2 + ( n - 1) * cl / 2 -%}
    {%- set y = 0.0 -%}
    {%- set z = 0.0 -%}
    {%- set yaw = 0.0 -%}
    {{ link(n, x, y, z, yaw, m, cl, cr, sr, element_color, joint_color) }}
    {{ lift_drag(n, cl, cr) }}
    {% endfor -%}
    {{ y_element(cl, y_el, y_tetha) }}
    {% for k in range(1, number_elements) -%}
    {{ joint(k, damping, friction, spring_stiffness, spring_reference) }}
    {% endfor -%}
    <joint name="fixed_to_world" type="fixed">
      <child>link_{{last_elem}}</child>
      <parent>world</parent>
    </joint>
  </model>
</sdf>