forked from xtdrone/XTDrone
111 lines
3.0 KiB
Plaintext
111 lines
3.0 KiB
Plaintext
<!--
|
|
This is how this dock should look like:
|
|
<%- static_dock = layout.pop %>
|
|
<% layout.each do |row| -%>
|
|
<%= row -%>
|
|
<% end %>
|
|
-->
|
|
|
|
<%- link_counter = 0 -%>
|
|
<%- row_counter = 0 -%>
|
|
<%- x_counter = 0 -%>
|
|
<%- delta_x = 2 -%>
|
|
<%- delta_y = 2 -%>
|
|
<%- i = 0 -%>
|
|
|
|
<%- if static_dock == true -%>
|
|
<static>true</static>
|
|
<%- end -%>
|
|
|
|
<%- layout.each do |row| %>
|
|
<!-- Row <%=row_counter %> -->
|
|
<%- y_counter = 0 -%>
|
|
<%- for j in 0..row.size - 1 -%>
|
|
<%- c = row[j] -%>
|
|
<%- if c == 'X' -%>
|
|
<include>
|
|
<name>dock_block_<%= i %>_<%= j %></name>
|
|
<pose><%= x_counter %> <%= y_counter %> 0.25 0 0 0</pose>
|
|
<%- if static_dock != true -%>
|
|
<uri>model://dock_block_4x4_dynamic</uri>
|
|
<%- else -%>
|
|
<uri>model://dock_block_4x4</uri>
|
|
<%- end -%>
|
|
</include>
|
|
<%- link_counter += 1 -%>
|
|
<%- end -%>
|
|
<%- y_counter += delta_x -%>
|
|
<%- end -%>
|
|
<%- i += 1 -%>
|
|
<%- x_counter += delta_y -%>
|
|
<%- row_counter += 1 -%>
|
|
<%- end %>
|
|
|
|
|
|
<%- if static_dock != true -%>
|
|
|
|
<!-- Add joints across rows -->
|
|
<%- joint_limit = 0.1 -%>
|
|
<%- x_counter = 0 -%>
|
|
<%- delta_x = 2 -%>
|
|
<%- delta_y = 2 -%>
|
|
<%- i = 0 -%>
|
|
<%- layout.each do |row| %>
|
|
<!-- Row <%= i%> -->
|
|
<%- y_counter = delta_y -%>
|
|
<%- for j in 1..row.size - 1 -%>
|
|
<%- c = row[j-1] -%>
|
|
<%- d = row[j] -%>
|
|
<%- if (c == 'X') and (d == 'X') -%>
|
|
<joint name="<%= i%>_<%= j-1%>_to_<%= i%>_<%= j%>" type="revolute">
|
|
<pose> 0 <%= delta_y/2.0 %> 0 0 0 0</pose>
|
|
<parent>dock_block_<%= i %>_<%= j %>::link</parent>
|
|
<child>dock_block_<%= i %>_<%= j-1 %>::link</child>
|
|
<axis>
|
|
<limit>
|
|
<lower>-<%= joint_limit %></lower>
|
|
<upper><%= joint_limit %></upper>
|
|
</limit>
|
|
<xyz>1 0 0</xyz>
|
|
</axis>
|
|
</joint>
|
|
<%- end -%>
|
|
<%- y_counter += delta_x -%>
|
|
<%- end -%>
|
|
<%- i += 1 -%>
|
|
<%- x_counter += delta_y -%>
|
|
<%- end %>
|
|
|
|
|
|
<!-- Add joints across columns -->
|
|
<%- y_counter = 0 -%>
|
|
<%- delta_x = 2 -%>
|
|
<%- delta_y = 2 -%>
|
|
<%- j = 0 -%>
|
|
<%- for j in 0..layout[0].length - 1 %>
|
|
<!-- Column <%= j%> -->
|
|
<%- x_counter = delta_x -%>
|
|
<%- for i in 1..layout.length - 1 -%>
|
|
<%- c = layout[i-1][j] -%>
|
|
<%- d = layout[i][j] -%>
|
|
<%- if (c == 'X') and (d == 'X') -%>
|
|
<joint name="<%= i%>_<%= j%>_to_<%= i-1%>_<%= j%>" type="revolute">
|
|
<pose> <%= delta_x/2.0 %> 0 0 0 0 0</pose>
|
|
<parent>dock_block_<%= i %>_<%= j %>::link</parent>
|
|
<child>dock_block_<%= i-1 %>_<%= j %>::link</child>
|
|
<axis>
|
|
<limit>
|
|
<lower>-<%= joint_limit %></lower>
|
|
<upper><%= joint_limit %></upper>
|
|
</limit>
|
|
<xyz>0 1 0</xyz>
|
|
</axis>
|
|
</joint>
|
|
<%- end -%>
|
|
<%- x_counter += delta_x -%>
|
|
<%- end -%>
|
|
<%- j += 1 -%>
|
|
<%- y_counter += delta_y -%>
|
|
<%- end %>
|
|
<%- end %>
|