Commit 4120d9584dbc82f381b076349b053e399e934ce0

Authored by Ricardo Rico Uribe
1 parent cf91bc0c

added imu for plugin - missing fix for error with 2 joints

aquanaute_description/launch/description.launch
@@ -5,7 +5,7 @@ @@ -5,7 +5,7 @@
5 5
6 <!-- send fake joint values --> 6 <!-- send fake joint values -->
7 <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> 7 <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
8 - <param name="joint_state_publisher_gui" value="false"/> 8 + <param name="use_gui" value="false"/>
9 </node> 9 </node>
10 10
11 <!-- Combine joint values --> 11 <!-- Combine joint values -->
aquanaute_description/urdf/aquanaute_description.urdf.xacro
@@ -143,9 +143,9 @@ @@ -143,9 +143,9 @@
143 <visual> 143 <visual>
144 <origin rpy="0 0 0" xyz="0 0 0"/> 144 <origin rpy="0 0 0" xyz="0 0 0"/>
145 <geometry> 145 <geometry>
146 - <cylinder radius="0.01" length="0.02"/> 146 + <cylinder radius="0.04" length="0.05"/>
147 </geometry> 147 </geometry>
148 - <material name="green"/> 148 + <material name="black"/>
149 </visual> 149 </visual>
150 </link> 150 </link>
151 151
@@ -172,6 +172,16 @@ @@ -172,6 +172,16 @@
172 </visual> 172 </visual>
173 </link> 173 </link>
174 174
  175 + <link name="imu_link">
  176 + <visual>
  177 + <origin rpy="0.0 0 0" xyz="0 0 0"/>
  178 + <geometry>
  179 + <cylinder radius="0.04" length="0.05"/>
  180 + </geometry>
  181 + <material name="cyan"/>
  182 + </visual>
  183 + </link>
  184 +
175 <!-- * * * Joint Definitions * * * --> 185 <!-- * * * Joint Definitions * * * -->
176 <joint name="platform_base_joint" type="fixed"> 186 <joint name="platform_base_joint" type="fixed">
177 <parent link="base_link"/> 187 <parent link="base_link"/>
@@ -199,12 +209,12 @@ @@ -199,12 +209,12 @@
199 <limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/> 209 <limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/>
200 </joint> 210 </joint>
201 211
202 - <joint name="propeller_motor_joint" type="revolute"> 212 + <joint name="propeller_motor_joint" type="fixed">
203 <parent link="motor_link"/> 213 <parent link="motor_link"/>
204 <child link="propeller_link"/> 214 <child link="propeller_link"/>
205 - <origin xyz="-0.28 0 -0.42" rpy="0 1.57 0"/> 215 + <origin xyz="-0.5 0 0.42" rpy="0 1.57 0"/>
206 <axis xyz="0 0 1"/> 216 <axis xyz="0 0 1"/>
207 - <limit effort="-1" velocity="-1" lower="-1e+12" upper="1e+12"/> 217 + <limit effort="-1" velocity="-1" lower="-1e+16" upper="1e+16"/>
208 </joint> 218 </joint>
209 219
210 <joint name="sensor_frame_platform_joint" type="fixed"> 220 <joint name="sensor_frame_platform_joint" type="fixed">
@@ -213,101 +223,9 @@ @@ -213,101 +223,9 @@
213 <origin xyz="-0.55 0.0 0.0" rpy="0 0 0"/> 223 <origin xyz="-0.55 0.0 0.0" rpy="0 0 0"/>
214 </joint> 224 </joint>
215 225
216 - <!-- * * * GAZEBO Definitions * * * -->  
217 -  
218 - <gazebo reference="base_link">  
219 - <kp>100000.0</kp>  
220 - <kd>100000.0</kd>  
221 - <mu1>10.0</mu1>  
222 - <mu2>10.0</mu2>  
223 - <material>Gazebo/Grey</material>  
224 - </gazebo>  
225 -  
226 - <gazebo reference="platform_link">  
227 - <kp>100000.0</kp>  
228 - <kd>100000.0</kd>  
229 - <mu1>10.0</mu1>  
230 - <mu2>10.0</mu2>  
231 - <material>Gazebo/Grey</material>  
232 - </gazebo>  
233 -  
234 - <gazebo reference="right_hull_link">  
235 - <kp>100000.0</kp>  
236 - <kd>100000.0</kd>  
237 - <mu1>10.0</mu1>  
238 - <mu2>10.0</mu2>  
239 - <material>Gazebo/Red</material>  
240 - </gazebo>  
241 -  
242 - <gazebo reference="left_hull_link">  
243 - <kp>100000.0</kp>  
244 - <kd>100000.0</kd>  
245 - <mu1>10.0</mu1>  
246 - <mu2>10.0</mu2>  
247 - <material>Gazebo/Red</material>  
248 - </gazebo>  
249 -  
250 - <gazebo reference="motor_link">  
251 - <kp>100000.0</kp>  
252 - <kd>100000.0</kd>  
253 - <mu1>10.0</mu1>  
254 - <mu2>10.0</mu2>  
255 - <material>Gazebo/Black</material>  
256 - </gazebo>  
257 -  
258 - <gazebo reference="sensor_frame_link">  
259 - <kp>100000.0</kp>  
260 - <kd>100000.0</kd>  
261 - <mu1>10.0</mu1>  
262 - <mu2>10.0</mu2>  
263 - <material>Gazebo/White</material>  
264 - </gazebo>  
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 - 226 + <joint name="imu_platform_joint" type="fixed">
  227 + <parent link="platform_link"/>
  228 + <child link="imu_link"/>
  229 + <origin xyz="0 0 0.1" rpy="0 0 0"/>
  230 + </joint>
313 </robot> 231 </robot>
aquanaute_gazebo/launch/test.launch 0 → 100644
@@ -0,0 +1,42 @@ @@ -0,0 +1,42 @@
  1 +<?xml version="1.0"?>
  2 +<launch>
  3 + <!-- Gazebo world to load -->
  4 + <arg name="world" default="$(find aquanaute_gazebo)/worlds/test.world" />
  5 + <!-- If true, run gazebo GUI -->
  6 + <arg name="gui" default="true" />
  7 + <!-- If true, run gazebo in verbose mode -->
  8 + <arg name="verbose" default="true"/>
  9 + <!-- If true, start in paused state -->
  10 + <arg name="paused" default="false"/>
  11 + <!-- Set various other gazebo arguments-->
  12 + <arg name="extra_gazebo_args" default=""/>
  13 + <!-- Start in a default namespace -->
  14 + <arg name="namespace" default="aquanaute"/>
  15 +
  16 + <!-- Initial USV location and attitude-->
  17 + <arg name="x" default="0" />
  18 + <arg name="y" default="0" />
  19 + <arg name="z" default="0.5" />
  20 + <arg name="P" default="0" />
  21 + <arg name="R" default="0" />
  22 + <arg name="Y" default="0" />
  23 +
  24 + <!-- Start Gazebo with the world file -->
  25 + <include file="$(find gazebo_ros)/launch/empty_world.launch">
  26 + <arg name="world_name" value="$(arg world)"/>
  27 + <arg name="verbose" value="$(arg verbose)"/>
  28 + <arg name="paused" value="$(arg paused)"/>
  29 + <arg name="use_sim_time" value="true"/>
  30 + <arg name="gui" value="$(arg gui)" />
  31 + <arg name="enable_ros_network" value="true"/>
  32 + </include>
  33 +
  34 + <!-- Load robot model -->
  35 + <arg name="urdf" default="$(find aquanaute_gazebo)/urdf/aquanaute_gazebo.urdf.xacro"/>
  36 + <param name="$(arg namespace)/robot_description" command="$(find xacro)/xacro '$(arg urdf)'"/>
  37 +
  38 + <!-- Spawn model in Gazebo, script depending on non_competition_mode -->
  39 + <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x) -y $(arg y) -z $(arg z)
  40 + -R $(arg R) -P $(arg P) -Y $(arg Y)
  41 + -urdf -param $(arg namespace)/robot_description -model aquanaute"/>
  42 +</launch>
aquanaute_gazebo/urdf/aquanaute_gazebo.urdf.xacro
1 <?xml version="1.0"?> 1 <?xml version="1.0"?>
2 <!-- Basic WAM-V with gazebo plugins for dynamics --> 2 <!-- Basic WAM-V with gazebo plugins for dynamics -->
3 <robot xmlns:xacro="http://ros.org/wiki/xacro"> 3 <robot xmlns:xacro="http://ros.org/wiki/xacro">
4 - <!-- Include macros for dynamics plugins -->  
5 - <xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" /> 4 + <!-- Include macros for dynamics plugins
  5 + <xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" /> -->
6 <!-- Defines the base vehicle --> 6 <!-- Defines the base vehicle -->
7 <xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/> 7 <xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/>
8 - <!-- Attach hydrodynamics plugin -->  
9 - <xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/> 8 +
  9 + <!-- Defines the sim plugins-->
  10 +
  11 + <gazebo reference="base_link">
  12 + <kp>100000.0</kp>
  13 + <kd>100000.0</kd>
  14 + <mu1>10.0</mu1>
  15 + <mu2>10.0</mu2>
  16 + <material>Gazebo/Grey</material>
  17 + </gazebo>
  18 +
  19 + <gazebo reference="platform_link">
  20 + <kp>100000.0</kp>
  21 + <kd>100000.0</kd>
  22 + <mu1>10.0</mu1>
  23 + <mu2>10.0</mu2>
  24 + <material>Gazebo/Grey</material>
  25 + </gazebo>
  26 +
  27 + <gazebo reference="right_hull_link">
  28 + <kp>100000.0</kp>
  29 + <kd>100000.0</kd>
  30 + <mu1>10.0</mu1>
  31 + <mu2>10.0</mu2>
  32 + <material>Gazebo/Red</material>
  33 + </gazebo>
  34 +
  35 + <gazebo reference="left_hull_link">
  36 + <kp>100000.0</kp>
  37 + <kd>100000.0</kd>
  38 + <mu1>10.0</mu1>
  39 + <mu2>10.0</mu2>
  40 + <material>Gazebo/Red</material>
  41 + </gazebo>
  42 +
  43 + <gazebo reference="motor_link">
  44 + <kp>100000.0</kp>
  45 + <kd>100000.0</kd>
  46 + <mu1>10.0</mu1>
  47 + <mu2>10.0</mu2>
  48 + <material>Gazebo/Black</material>
  49 + </gazebo>
  50 +
  51 + <gazebo reference="propeller_link">
  52 + <kp>100000.0</kp>
  53 + <kd>100000.0</kd>
  54 + <mu1>10.0</mu1>
  55 + <mu2>10.0</mu2>
  56 + <material>Gazebo/Black</material>
  57 + </gazebo>
  58 +
  59 + <gazebo reference="sensor_frame_link">
  60 + <kp>100000.0</kp>
  61 + <kd>100000.0</kd>
  62 + <mu1>10.0</mu1>
  63 + <mu2>10.0</mu2>
  64 + <material>Gazebo/White</material>
  65 + </gazebo>
  66 +
  67 + <gazebo>
  68 + <link name="imu_link">
  69 + <sensor name="imu_sensor" type="imu">
  70 + <pose>0 0 0 3.141593 0 0</pose>
  71 + <always_on>1</always_on>
  72 + <update_rate>1000.0</update_rate>
  73 + </sensor>
  74 + </link>
  75 + </gazebo>
  76 +
  77 + <!-- * * * GAZEBO Plugins * * * -->
  78 + <gazebo>
  79 + <plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so">
  80 + <fdm_addr>127.0.0.1</fdm_addr>
  81 + <fdm_port_in>9002</fdm_port_in>
  82 + <fdm_port_out>9003</fdm_port_out>
  83 + <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
  84 + <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
  85 + <imuName>imu_sensor</imuName>
  86 + <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
  87 + <!--
  88 + incoming control command [0, 1]
  89 + so offset it by 0 to get [0, 1]
  90 + and divide max target by 1.
  91 + offset = 0
  92 + multiplier = 838 max rpm / 1 = 838
  93 + -->
  94 + <control channel="0">
  95 + <multiplier>1</multiplier>
  96 + <offset>-0.5</offset>
  97 + <type>POSITION</type>
  98 + <p_gain>1</p_gain>
  99 + <i_gain>0</i_gain>
  100 + <d_gain>0</d_gain>
  101 + <i_max>0</i_max>
  102 + <i_min>0</i_min>
  103 + <cmd_max>1</cmd_max>
  104 + <cmd_min>-1</cmd_min>
  105 + <jointName>aquanaute::motor_platform_joint</jointName>
  106 + </control>
  107 +
  108 + <control channel="1">
  109 + <multiplier>1</multiplier>
  110 + <offset>-0.5</offset>
  111 + <type>VELOCITY</type>
  112 + <p_gain>1</p_gain>
  113 + <i_gain>0</i_gain>
  114 + <d_gain>0</d_gain>
  115 + <i_max>0</i_max>
  116 + <i_min>0</i_min>
  117 + <cmd_max>1</cmd_max>
  118 + <cmd_min>-1</cmd_min>
  119 + <jointName>aquanaute::propeller_motor_joint</jointName>
  120 + </control>
  121 +
  122 + </plugin>
  123 + </gazebo>
  124 + <!-- Attach hydrodynamics plugin
  125 + <xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/> -->
10 </robot> 126 </robot>
aquanaute_gazebo/worlds/test.world 0 → 100644
@@ -0,0 +1,115 @@ @@ -0,0 +1,115 @@
  1 +<sdf version='1.6'>
  2 + <world name='default'>
  3 + <light name='sun' type='directional'>
  4 + <cast_shadows>1</cast_shadows>
  5 + <pose frame=''>0 0 10 0 -0 0</pose>
  6 + <diffuse>0.8 0.8 0.8 1</diffuse>
  7 + <specular>0.2 0.2 0.2 1</specular>
  8 + <attenuation>
  9 + <range>1000</range>
  10 + <constant>0.9</constant>
  11 + <linear>0.01</linear>
  12 + <quadratic>0.001</quadratic>
  13 + </attenuation>
  14 + <direction>-0.5 0.1 -0.9</direction>
  15 + </light>
  16 + <model name='ground_plane'>
  17 + <static>1</static>
  18 + <link name='link'>
  19 + <collision name='collision'>
  20 + <geometry>
  21 + <plane>
  22 + <normal>0 0 1</normal>
  23 + <size>100 100</size>
  24 + </plane>
  25 + </geometry>
  26 + <surface>
  27 + <contact>
  28 + <collide_bitmask>65535</collide_bitmask>
  29 + <ode/>
  30 + </contact>
  31 + <friction>
  32 + <ode>
  33 + <mu>100</mu>
  34 + <mu2>50</mu2>
  35 + </ode>
  36 + <torsional>
  37 + <ode/>
  38 + </torsional>
  39 + </friction>
  40 + <bounce/>
  41 + </surface>
  42 + <max_contacts>10</max_contacts>
  43 + </collision>
  44 + <visual name='visual'>
  45 + <cast_shadows>0</cast_shadows>
  46 + <geometry>
  47 + <plane>
  48 + <normal>0 0 1</normal>
  49 + <size>100 100</size>
  50 + </plane>
  51 + </geometry>
  52 + <material>
  53 + <script>
  54 + <uri>file://media/materials/scripts/gazebo.material</uri>
  55 + <name>Gazebo/Grey</name>
  56 + </script>
  57 + </material>
  58 + </visual>
  59 + <self_collide>0</self_collide>
  60 + <enable_wind>0</enable_wind>
  61 + <kinematic>0</kinematic>
  62 + </link>
  63 + </model>
  64 + <gravity>0 0 -9.8</gravity>
  65 + <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
  66 + <atmosphere type='adiabatic'/>
  67 + <physics name='default_physics' default='0' type='ode'>
  68 + <max_step_size>0.001</max_step_size>
  69 + <real_time_factor>1</real_time_factor>
  70 + <real_time_update_rate>1000</real_time_update_rate>
  71 + </physics>
  72 + <scene>
  73 + <ambient>0.4 0.4 0.4 1</ambient>
  74 + <background>0.7 0.7 0.7 1</background>
  75 + <shadows>1</shadows>
  76 + </scene>
  77 + <audio>
  78 + <device>default</device>
  79 + </audio>
  80 + <wind/>
  81 + <spherical_coordinates>
  82 + <surface_model>EARTH_WGS84</surface_model>
  83 + <latitude_deg>0</latitude_deg>
  84 + <longitude_deg>0</longitude_deg>
  85 + <elevation>0</elevation>
  86 + <heading_deg>0</heading_deg>
  87 + </spherical_coordinates>
  88 + <state world_name='default'>
  89 + <sim_time>29 253000000</sim_time>
  90 + <real_time>29 457037678</real_time>
  91 + <wall_time>1594385127 732457775</wall_time>
  92 + <iterations>29253</iterations>
  93 + <model name='ground_plane'>
  94 + <pose frame=''>0 0 0 0 -0 0</pose>
  95 + <scale>1 1 1</scale>
  96 + <link name='link'>
  97 + <pose frame=''>0 0 0 0 -0 0</pose>
  98 + <velocity>0 0 0 0 -0 0</velocity>
  99 + <acceleration>0 0 0 0 -0 0</acceleration>
  100 + <wrench>0 0 0 0 -0 0</wrench>
  101 + </link>
  102 + </model>
  103 + <light name='sun'>
  104 + <pose frame=''>0 0 10 0 -0 0</pose>
  105 + </light>
  106 + </state>
  107 + <gui fullscreen='0'>
  108 + <camera name='user_camera'>
  109 + <pose frame=''>5 -5 2 0 0.275643 2.35619</pose>
  110 + <view_controller>orbit</view_controller>
  111 + <projection_type>perspective</projection_type>
  112 + </camera>
  113 + </gui>
  114 + </world>
  115 +</sdf>