Commit cf91bc0cce38b81af47df588d7e476c96f6cd11b

Authored by Ricardo Rico Uribe
1 parent aa02c97d

added pluggin for ardupilot

aquanaute_description/urdf/aquanaute_description.urdf.xacro
1 <?xml version="1.0"?> 1 <?xml version="1.0"?>
2 -<robot xmlns:xacro="http://ros.org/wiki/xacro"> 2 +<robot xmlns:xacro="http://ros.org/wiki/xacro" name="aquanaute">
3 3
4 <material name="blue"> 4 <material name="blue">
5 <color rgba="0 0 0.8 1"/> 5 <color rgba="0 0 0.8 1"/>
@@ -8,7 +8,7 @@ @@ -8,7 +8,7 @@
8 <color rgba="0.8 0 0 1"/> 8 <color rgba="0.8 0 0 1"/>
9 </material> 9 </material>
10 <material name="green"> 10 <material name="green">
11 - <color rgba="0 0.8 0 1"/> 11 + <color rgba="0 0.8 0 0.5"/>
12 </material> 12 </material>
13 <material name="cyan"> 13 <material name="cyan">
14 <color rgba="0 0.6 0.6 1"/> 14 <color rgba="0 0.6 0.6 1"/>
@@ -29,7 +29,7 @@ @@ -29,7 +29,7 @@
29 <color rgba="0 0 0 1"/> 29 <color rgba="0 0 0 1"/>
30 </material> 30 </material>
31 31
32 - <!-- * * * Link Definitions * * * --> 32 + <!-- * * * Link Definitions * * * -->
33 <link name="base_link"> 33 <link name="base_link">
34 <visual> 34 <visual>
35 <origin rpy="0.0 0 0" xyz="0 0 0"/> 35 <origin rpy="0.0 0 0" xyz="0 0 0"/>
@@ -132,13 +132,30 @@ @@ -132,13 +132,30 @@
132 </visual> 132 </visual>
133 </link> 133 </link>
134 134
  135 + <link name="propeller_link">
  136 + <collision>
  137 + <origin xyz="0 0 0" rpy="0 0 0"/>
  138 + <geometry>
  139 + <cylinder radius="0.05" length="0.04"/>
  140 + </geometry>
  141 + </collision>
  142 +
  143 + <visual>
  144 + <origin rpy="0 0 0" xyz="0 0 0"/>
  145 + <geometry>
  146 + <cylinder radius="0.01" length="0.02"/>
  147 + </geometry>
  148 + <material name="green"/>
  149 + </visual>
  150 + </link>
  151 +
135 <link name="sensor_frame_link"> 152 <link name="sensor_frame_link">
136 <inertial> 153 <inertial>
137 <origin xyz="-0.007 0 0.379" rpy="0 0 0"/> 154 <origin xyz="-0.007 0 0.379" rpy="0 0 0"/>
138 <mass value="1.8"/> 155 <mass value="1.8"/>
139 - <inertia ixx="0.308" ixy="0.00" ixz="0.007" iyy="0.297" iyz="0.00" izz="0.022"/> 156 + <inertia ixx="0.308" ixy="0.00" ixz="0.007" iyy="0.297" iyz="0.00" izz="0.022"/>
140 </inertial> 157 </inertial>
141 - 158 +
142 <collision> 159 <collision>
143 <origin xyz="0 0 0" rpy="0 0 0"/> 160 <origin xyz="0 0 0" rpy="0 0 0"/>
144 <geometry> 161 <geometry>
@@ -157,34 +174,44 @@ @@ -157,34 +174,44 @@
157 174
158 <!-- * * * Joint Definitions * * * --> 175 <!-- * * * Joint Definitions * * * -->
159 <joint name="platform_base_joint" type="fixed"> 176 <joint name="platform_base_joint" type="fixed">
160 - <parent link="base_link"/>  
161 - <child link="platform_link"/>  
162 - <origin xyz="0 0 0.35" rpy="0 0 0"/>  
163 - </joint> 177 + <parent link="base_link"/>
  178 + <child link="platform_link"/>
  179 + <origin xyz="0 0 0.35" rpy="0 0 0"/>
  180 + </joint>
164 181
165 <joint name="right_hull_platform_joint" type="fixed"> 182 <joint name="right_hull_platform_joint" type="fixed">
166 - <parent link="platform_link"/>  
167 - <child link="right_hull_link"/>  
168 - <origin xyz="-0.05 -0.7 -0.036" rpy="0 0 0"/>  
169 - </joint> 183 + <parent link="platform_link"/>
  184 + <child link="right_hull_link"/>
  185 + <origin xyz="-0.05 -0.7 -0.036" rpy="0 0 0"/>
  186 + </joint>
170 187
171 <joint name="left_hull_platform_joint" type="fixed"> 188 <joint name="left_hull_platform_joint" type="fixed">
172 - <parent link="platform_link"/>  
173 - <child link="left_hull_link"/>  
174 - <origin xyz="-0.05 0.7 -0.036" rpy="0 0 0"/>  
175 - </joint> 189 + <parent link="platform_link"/>
  190 + <child link="left_hull_link"/>
  191 + <origin xyz="-0.05 0.7 -0.036" rpy="0 0 0"/>
  192 + </joint>
176 193
177 - <joint name="motor_platform_joint" type="fixed">  
178 - <parent link="platform_link"/>  
179 - <child link="motor_link"/>  
180 - <origin xyz="-0.91 0.0 -0.09" rpy="0 0 0"/>  
181 - </joint> 194 + <joint name="motor_platform_joint" type="revolute">
  195 + <parent link="platform_link"/>
  196 + <child link="motor_link"/>
  197 + <origin xyz="-0.91 0.0 -0.09" rpy="0 0 0"/>
  198 + <axis xyz="0 0 1"/>
  199 + <limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/>
  200 + </joint>
  201 +
  202 + <joint name="propeller_motor_joint" type="revolute">
  203 + <parent link="motor_link"/>
  204 + <child link="propeller_link"/>
  205 + <origin xyz="-0.28 0 -0.42" rpy="0 1.57 0"/>
  206 + <axis xyz="0 0 1"/>
  207 + <limit effort="-1" velocity="-1" lower="-1e+12" upper="1e+12"/>
  208 + </joint>
182 209
183 <joint name="sensor_frame_platform_joint" type="fixed"> 210 <joint name="sensor_frame_platform_joint" type="fixed">
184 - <parent link="platform_link"/>  
185 - <child link="sensor_frame_link"/>  
186 - <origin xyz="-0.55 0.0 0.0" rpy="0 0 0"/>  
187 - </joint> 211 + <parent link="platform_link"/>
  212 + <child link="sensor_frame_link"/>
  213 + <origin xyz="-0.55 0.0 0.0" rpy="0 0 0"/>
  214 + </joint>
188 215
189 <!-- * * * GAZEBO Definitions * * * --> 216 <!-- * * * GAZEBO Definitions * * * -->
190 217
@@ -235,5 +262,52 @@ @@ -235,5 +262,52 @@
235 <mu2>10.0</mu2> 262 <mu2>10.0</mu2>
236 <material>Gazebo/White</material> 263 <material>Gazebo/White</material>
237 </gazebo> 264 </gazebo>
238 - 265 +
  266 + <!-- * * * GAZEBO Plugins * * * -->
  267 +
  268 + <plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so">
  269 + <fdm_addr>127.0.0.1</fdm_addr>
  270 + <fdm_port_in>9002</fdm_port_in>
  271 + <fdm_port_out>9003</fdm_port_out>
  272 + <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
  273 + <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
  274 + <!--<imuName></imuName>-->
  275 + <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
  276 + <!--
  277 + incoming control command [0, 1]
  278 + so offset it by 0 to get [0, 1]
  279 + and divide max target by 1.
  280 + offset = 0
  281 + multiplier = 838 max rpm / 1 = 838
  282 + -->
  283 + <control channel="0">
  284 + <multiplier>1</multiplier>
  285 + <offset>-0.5</offset>
  286 + <type>RIC</type>
  287 + <p_gain>1</p_gain>
  288 + <i_gain>0</i_gain>
  289 + <d_gain>0</d_gain>
  290 + <i_max>0</i_max>
  291 + <i_min>0</i_min>
  292 + <cmd_max>1</cmd_max>
  293 + <cmd_min>-1</cmd_min>
  294 + <jointName>aquanaute::motor_platform_joint</jointName>
  295 + </control>
  296 +
  297 + <control channel="1">
  298 + <multiplier>1</multiplier>
  299 + <offset>-0.5</offset>
  300 + <type>THOMAS</type>
  301 + <p_gain>1</p_gain>
  302 + <i_gain>0</i_gain>
  303 + <d_gain>0</d_gain>
  304 + <i_max>0</i_max>
  305 + <i_min>0</i_min>
  306 + <cmd_max>1</cmd_max>
  307 + <cmd_min>-1</cmd_min>
  308 + <jointName>aquanaute::propeller_motor_joint</jointName>
  309 + </control>
  310 +
  311 + </plugin>
  312 +
239 </robot> 313 </robot>
aquanaute_description/urdf/tmp.urdf 0 → 100644
@@ -0,0 +1,281 @@ @@ -0,0 +1,281 @@
  1 +<?xml version="1.0" encoding="utf-8"?>
  2 +<!-- =================================================================================== -->
  3 +<!-- | This document was autogenerated by xacro from aquanaute_description.urdf.xacro | -->
  4 +<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
  5 +<!-- =================================================================================== -->
  6 +<robot name="aquanaute">
  7 + <material name="blue">
  8 + <color rgba="0 0 0.8 1"/>
  9 + </material>
  10 + <material name="red">
  11 + <color rgba="0.8 0 0 1"/>
  12 + </material>
  13 + <material name="green">
  14 + <color rgba="0 0.8 0 0.5"/>
  15 + </material>
  16 + <material name="cyan">
  17 + <color rgba="0 0.6 0.6 1"/>
  18 + </material>
  19 + <material name="yellow">
  20 + <color rgba="0.8 0.8 0 1"/>
  21 + </material>
  22 + <material name="purple">
  23 + <color rgba="0.5 0 0.5 1"/>
  24 + </material>
  25 + <material name="white">
  26 + <color rgba="1 1 1 1"/>
  27 + </material>
  28 + <material name="gray">
  29 + <color rgba="0.35 0.35 0.35 1"/>
  30 + </material>
  31 + <material name="black">
  32 + <color rgba="0 0 0 1"/>
  33 + </material>
  34 + <!-- * * * Link Definitions * * * -->
  35 + <link name="base_link">
  36 + <visual>
  37 + <origin rpy="0.0 0 0" xyz="0 0 0"/>
  38 + <geometry>
  39 + <sphere radius="0.001"/>
  40 + </geometry>
  41 + <material name="blue"/>
  42 + </visual>
  43 + </link>
  44 + <link name="platform_link">
  45 + <inertial>
  46 + <origin rpy="0 0 0" xyz="-0.024 0 -0007"/>
  47 + <mass value="16"/>
  48 + <inertia ixx="4.206" ixy="0.0" ixz="-0.031" iyy="4.588" iyz="0.0" izz="8.785"/>
  49 + </inertial>
  50 + <collision>
  51 + <origin rpy="0 0 0" xyz="0 0 0"/>
  52 + <geometry>
  53 + <mesh filename="package://aquanaute_description/models/meshes/Platform.stl"/>
  54 + </geometry>
  55 + </collision>
  56 + <visual>
  57 + <origin rpy="0.0 0 0" xyz="0 0 0"/>
  58 + <geometry>
  59 + <mesh filename="package://aquanaute_description/models/meshes/Platform.stl"/>
  60 + </geometry>
  61 + <material name="yellow"/>
  62 + </visual>
  63 + </link>
  64 + <link name="right_hull_link">
  65 + <inertial>
  66 + <origin rpy="0 0 0" xyz="-0.124 0 -0.168"/>
  67 + <mass value="10.45"/>
  68 + <inertia ixx="0.244" ixy="3.346" ixz="0.009" iyy="4.974" iyz="-8.274" izz="4.965"/>
  69 + </inertial>
  70 + <collision>
  71 + <origin rpy="0 0 0" xyz="0 0 0"/>
  72 + <geometry>
  73 + <mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>
  74 + </geometry>
  75 + </collision>
  76 + <visual>
  77 + <origin rpy="0.0 0 0" xyz="0 0 0"/>
  78 + <geometry>
  79 + <mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>
  80 + </geometry>
  81 + <material name="red"/>
  82 + </visual>
  83 + </link>
  84 + <link name="left_hull_link">
  85 + <inertial>
  86 + <origin rpy="0 0 0" xyz="-0.124 0 -0.168"/>
  87 + <mass value="10.45"/>
  88 + <inertia ixx="0.244" ixy="3.346" ixz="0.009" iyy="4.974" iyz="-8.274" izz="4.965"/>
  89 + </inertial>
  90 + <collision>
  91 + <origin rpy="0 0 0" xyz="0 0 0"/>
  92 + <geometry>
  93 + <mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>
  94 + </geometry>
  95 + </collision>
  96 + <visual>
  97 + <origin rpy="0.0 0 0" xyz="0 0 0"/>
  98 + <geometry>
  99 + <mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>
  100 + </geometry>
  101 + <material name="red"/>
  102 + </visual>
  103 + </link>
  104 + <link name="motor_link">
  105 + <inertial>
  106 + <origin rpy="0 0 0" xyz="-0.133 0 -0.391"/>
  107 + <mass value="6.6"/>
  108 + <inertia ixx="0.072" ixy="3.604" ixz="-0.011" iyy="0.111" iyz="5.484" izz="0.042"/>
  109 + </inertial>
  110 + <collision>
  111 + <origin rpy="0 0 0" xyz="0 0 0"/>
  112 + <geometry>
  113 + <mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/>
  114 + </geometry>
  115 + </collision>
  116 + <visual>
  117 + <origin rpy="0.0 0 0" xyz="0 0 0"/>
  118 + <geometry>
  119 + <mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/>
  120 + </geometry>
  121 + <material name="purple"/>
  122 + </visual>
  123 + </link>
  124 + <link name="propeller_link">
  125 + <collision>
  126 + <origin rpy="0 0 0" xyz="0 0 0"/>
  127 + <geometry>
  128 + <cylinder length="0.04" radius="0.05"/>
  129 + </geometry>
  130 + </collision>
  131 + <visual>
  132 + <origin rpy="0 0 0" xyz="0 0 0"/>
  133 + <geometry>
  134 + <cylinder length="0.02" radius="0.01"/>
  135 + </geometry>
  136 + <material name="green"/>
  137 + </visual>
  138 + </link>
  139 + <link name="sensor_frame_link">
  140 + <inertial>
  141 + <origin rpy="0 0 0" xyz="-0.007 0 0.379"/>
  142 + <mass value="1.8"/>
  143 + <inertia ixx="0.308" ixy="0.00" ixz="0.007" iyy="0.297" iyz="0.00" izz="0.022"/>
  144 + </inertial>
  145 + <collision>
  146 + <origin rpy="0 0 0" xyz="0 0 0"/>
  147 + <geometry>
  148 + <mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/>
  149 + </geometry>
  150 + </collision>
  151 + <visual>
  152 + <origin rpy="0.0 0 0" xyz="0 0 0"/>
  153 + <geometry>
  154 + <mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/>
  155 + </geometry>
  156 + <material name="cyan"/>
  157 + </visual>
  158 + </link>
  159 + <!-- * * * Joint Definitions * * * -->
  160 + <joint name="platform_base_joint" type="fixed">
  161 + <parent link="base_link"/>
  162 + <child link="platform_link"/>
  163 + <origin rpy="0 0 0" xyz="0 0 0.35"/>
  164 + </joint>
  165 + <joint name="right_hull_platform_joint" type="fixed">
  166 + <parent link="platform_link"/>
  167 + <child link="right_hull_link"/>
  168 + <origin rpy="0 0 0" xyz="-0.05 -0.7 -0.036"/>
  169 + </joint>
  170 + <joint name="left_hull_platform_joint" type="fixed">
  171 + <parent link="platform_link"/>
  172 + <child link="left_hull_link"/>
  173 + <origin rpy="0 0 0" xyz="-0.05 0.7 -0.036"/>
  174 + </joint>
  175 + <joint name="motor_platform_joint" type="revolute">
  176 + <parent link="platform_link"/>
  177 + <child link="motor_link"/>
  178 + <origin rpy="0 0 0" xyz="-0.91 0.0 -0.09"/>
  179 + <axis xyz="0 0 1"/>
  180 + <limit effort="-1" lower="-0.9" upper="0.9" velocity="-1"/>
  181 + </joint>
  182 + <joint name="propeller_motor_joint" type="revolute">
  183 + <parent link="motor_link"/>
  184 + <child link="propeller_link"/>
  185 + <origin rpy="0 1.57 0" xyz="-0.28 0 -0.42"/>
  186 + <axis xyz="0 0 1"/>
  187 + <limit effort="-1" lower="-1e+12" upper="1e+12" velocity="-1"/>
  188 + </joint>
  189 + <joint name="sensor_frame_platform_joint" type="fixed">
  190 + <parent link="platform_link"/>
  191 + <child link="sensor_frame_link"/>
  192 + <origin rpy="0 0 0" xyz="-0.55 0.0 0.0"/>
  193 + </joint>
  194 + <!-- * * * GAZEBO Definitions * * * -->
  195 + <gazebo reference="base_link">
  196 + <kp>100000.0</kp>
  197 + <kd>100000.0</kd>
  198 + <mu1>10.0</mu1>
  199 + <mu2>10.0</mu2>
  200 + <material>Gazebo/Grey</material>
  201 + </gazebo>
  202 + <gazebo reference="platform_link">
  203 + <kp>100000.0</kp>
  204 + <kd>100000.0</kd>
  205 + <mu1>10.0</mu1>
  206 + <mu2>10.0</mu2>
  207 + <material>Gazebo/Grey</material>
  208 + </gazebo>
  209 + <gazebo reference="right_hull_link">
  210 + <kp>100000.0</kp>
  211 + <kd>100000.0</kd>
  212 + <mu1>10.0</mu1>
  213 + <mu2>10.0</mu2>
  214 + <material>Gazebo/Red</material>
  215 + </gazebo>
  216 + <gazebo reference="left_hull_link">
  217 + <kp>100000.0</kp>
  218 + <kd>100000.0</kd>
  219 + <mu1>10.0</mu1>
  220 + <mu2>10.0</mu2>
  221 + <material>Gazebo/Red</material>
  222 + </gazebo>
  223 + <gazebo reference="motor_link">
  224 + <kp>100000.0</kp>
  225 + <kd>100000.0</kd>
  226 + <mu1>10.0</mu1>
  227 + <mu2>10.0</mu2>
  228 + <material>Gazebo/Black</material>
  229 + </gazebo>
  230 + <gazebo reference="sensor_frame_link">
  231 + <kp>100000.0</kp>
  232 + <kd>100000.0</kd>
  233 + <mu1>10.0</mu1>
  234 + <mu2>10.0</mu2>
  235 + <material>Gazebo/White</material>
  236 + </gazebo>
  237 + <!-- * * * GAZEBO Plugins * * * -->
  238 + <plugin filename="libArduPilotPlugin.so" name="ardupilot_plugin">
  239 + <fdm_addr>127.0.0.1</fdm_addr>
  240 + <fdm_port_in>9002</fdm_port_in>
  241 + <fdm_port_out>9003</fdm_port_out>
  242 + <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
  243 + <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
  244 + <!--<imuName></imuName>-->
  245 + <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
  246 + <!--
  247 + incoming control command [0, 1]
  248 + so offset it by 0 to get [0, 1]
  249 + and divide max target by 1.
  250 + offset = 0
  251 + multiplier = 838 max rpm / 1 = 838
  252 + -->
  253 + <control channel="0">
  254 + <multiplier>1</multiplier>
  255 + <offset>-0.5</offset>
  256 + <type>RIC</type>
  257 + <p_gain>1</p_gain>
  258 + <i_gain>0</i_gain>
  259 + <d_gain>0</d_gain>
  260 + <i_max>0</i_max>
  261 + <i_min>0</i_min>
  262 + <cmd_max>1</cmd_max>
  263 + <cmd_min>-1</cmd_min>
  264 + <jointName>aquanaute::motor_platform_joint</jointName>
  265 + </control>
  266 + <control channel="1">
  267 + <multiplier>1</multiplier>
  268 + <offset>-0.5</offset>
  269 + <type>THOMAS</type>
  270 + <p_gain>1</p_gain>
  271 + <i_gain>0</i_gain>
  272 + <d_gain>0</d_gain>
  273 + <i_max>0</i_max>
  274 + <i_min>0</i_min>
  275 + <cmd_max>1</cmd_max>
  276 + <cmd_min>-1</cmd_min>
  277 + <jointName>aquanaute::propeller_motor_joint</jointName>
  278 + </control>
  279 + </plugin>
  280 +</robot>
  281 +