392 lines
15 KiB
Django/Jinja
392 lines
15 KiB
Django/Jinja
{#--------------------------------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 = 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 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 -#}
|
|
|
|
{#----------------------------------------------------------------------------------------------------------#}
|
|
{#---------------------------------------------: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 -#}
|
|
{#-----------------------------------------------------------------------------------------------------------#}
|
|
{#----------------------------------------------: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 - tl/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 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</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) }}
|
|
</model>
|
|
</sdf>
|
|
|
|
<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=2 ts=2 : -->
|