Commit 5140bb7f957433e9a7073649c4cb612031dc5a19

Authored by Ricardo Rico Uribe
1 parent bb89241c

general order and small fixes - missing fix for ardupilot gazebo error where mot…

…ors turn without control
README.md
1 1 # Aquanaute
2 2  
3   -Repository to simulate the Aquanaute boat in maritime conditions such as wind and waves with the plugins created for the VRX and VRX-Challenge in Gazebo and ROS.
  3 +Repository to simulate the Aquanaute boat in maritime conditions such as wind and waves and with autopilot provided by ardupilot, using the plugins created for the VRX and VRX-Challenge in Gazebo and ROS and the plugin ardupilot-gazebo.
  4 +
  5 +---
4 6  
5 7 ## Installation
6 8  
7 9 * Clone this repo to a Catkin Workspace
8 10  
9   -* Clone the VRX repo (hosted at ensta) to the same Workspace (http://git-u2is.ensta.fr/ssh/vrx) and checkout to the branch "aquanaute"
  11 +* Clone the VRX repo (hosted at ensta) to the same Workspace (<http://git-u2is.ensta.fr/ssh/vrx>) and change to the branch "aquanaute"
  12 +
  13 +### ardupilot parameters
  14 +
  15 +in the file "vehiculeinfo.py" (/ardupilot/Tools/autotest/pysim) you need to change the default parameters file for the "gazebo-rover" in line 280 to the aquanaute.parm file provided in /aquanaute/aquanaute_description/ardu_params you can copy and paste the file in the default_params folder located inside (/ardupilot/Tools/autotest).
  16 +*There is an error when you launch ardupilot, the motors in the gazebo simulation will start to turn in a fixed direction and wont respond to commands, this issue has not yet been fixed and it's root cause it's not yet defined, it may be the ardupilot_gazebo plugin or the ardupilot frame*
  17 +
  18 +---
10 19  
11 20 ## Usage
  21 +
12 22 ### modify wind and waves
13   - in the ocean.world.xacro (/vrx_gazebo/worlds) you have
14   - ```xml
15   - <!--Waves-->
16   - <xacro:include filename="$(find wave_gazebo)/world_models/ocean_waves/model.xacro"/>
17   - <xacro:ocean_waves gain="0.0" period="5" direction_x="-1.0" direction_y="0.0" angle="0.0"/>
18   -
19   - <!--Wind-->
20   - <xacro:include filename="$(find vrx_gazebo)/worlds/xacros/usv_wind_plugin.xacro"/>
21   - <xacro:usv_wind_gazebo direction="90" mean_vel="0" var_gain="5" var_time="1">
22   - <wind_objs>
23   - <wind_obj>
24   - <name>wamv</name>
25   - <link_name>base_link</link_name>
26   - <coeff_vector>0.5 0.5 0.33</coeff_vector>
27   - </wind_obj>
28   - </wind_objs>
29   - </xacro:usv_wind_gazebo>
30   - ```
31   - it is configured to have no waves and no wind, also the wind and the waves move in the same direction, to activate wind and waves change the "mean_vel" and "gain" parameters respectively (recomended values are smaller than 1.0).
  23 +
  24 +in the ocean.world.xacro (/vrx_gazebo/worlds) you have
  25 +
  26 + <!--Waves-->
  27 + <xacro:include filename="$(find wave_gazebo)/world_models/ocean_waves/model.xacro"/>
  28 + <xacro:ocean_waves gain="0.0" period="5" direction_x="-1.0" direction_y="0.0" angle="0.0"/>
  29 +
  30 + <!--Wind-->
  31 + <xacro:include filename="$(find vrx_gazebo)/worlds/xacros/usv_wind_plugin.xacro"/>
  32 + <xacro:usv_wind_gazebo direction="90" mean_vel="0" var_gain="5" var_time="1">
  33 + <wind_objs>
  34 + <wind_obj>
  35 + <name>wamv</name>
  36 + <link_name>base_link</link_name>
  37 + <coeff_vector>0.5 0.5 0.33</coeff_vector>
  38 + </wind_obj>
  39 + </wind_objs>
  40 + </xacro:usv_wind_gazebo>
  41 +
  42 +It is configured to have no waves and no wind, also the wind and the waves move in the same direction, to activate wind and waves change the "mean_vel" and "gain" parameters respectively (recomended values are smaller than 1.0).
  43 +Even when these values are set to 0, the boat will move from right to left.
  44 +
  45 +---
32 46  
33 47 ### run
34   -in the workspace run the command at the beginning (and every time you change a parameter)
35 48  
36   - ```bash
37   - catkin_make
38   - ```
  49 +#### compilation
  50 +
  51 +in the workspace run the command at the beginning (and every time you change a parameter in the vrx xacros)
  52 +
  53 + catkin_make
  54 +
  55 +#### gazebo simulation
39 56  
40 57 then launch the file you created
41 58  
42   - ```bash
43   - roslaunch aquanaute_gazebo aquanaute.launch testing:=false
44   - ```
45   - this parameter can be changed to true to deactivate the plugins from vrx (the world will be changed to an empty world)
  59 + roslaunch aquanaute_gazebo aquanaute.launch testing:=false
  60 +
  61 +this parameter can be changed to true to deactivate the plugins from vrx (the world will be changed to an empty world without gravity)
  62 +
  63 +#### ardupilot
46 64  
47 65 on another terminal (inside the ardupilot root) run
48   - ```bash
49   - ./Tools/autotest/sim_vehicle.py --vehicle=Rover --frame=sailboat-motor --map --console --location=Xlac
50   - ```
51 66  
52   -## Author
53   -Ricardo RICO URIBE intern at U2IS in the summer of 2020
  67 + ./Tools/autotest/sim_vehicle.py -v APMrover2 -f gazebo-rover --map --console -l 48.71603264538473,2.213283777236939,155,0
54 68  
55   -## License
  69 +---
56 70  
  71 +## Author
57 72  
  73 +Ricardo RICO URIBE intern at U2IS in the summer of 2020
... ...
aquanaute_description/ardu_params/aquanaute.parm 0 → 100644
... ... @@ -0,0 +1,6 @@
  1 +FRAME_CLASS 2
  2 +AHRS_ORIENTATIONM 0
  3 +GCS_PID_MASK 1
  4 +SERVO1_FUNCTION 26
  5 +SERVO3_FUNCTION 70
  6 +ARMING_CHECK 0
... ...
aquanaute_description/urdf/aquanaute_description.urdf.xacro
... ... @@ -212,7 +212,8 @@
212 212 <child link="motor_link"/>
213 213 <origin xyz="-0.91 0.0 -0.09" rpy="0 0 0"/>
214 214 <axis xyz="0 0 1"/>
215   - <limit effort="1" velocity="1" lower="-0.9" upper="0.9"/>
  215 + <limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/>
  216 + <dynamics damping="0.0000001"/>
216 217 </joint>
217 218  
218 219 <joint name="propeller_motor_joint" type="revolute">
... ... @@ -220,7 +221,8 @@
220 221 <child link="propeller_link"/>
221 222 <origin xyz="-0.265 0 -0.42" rpy="0 0 0"/>
222 223 <axis xyz="1 0 0"/>
223   - <limit effort="1" velocity="1" lower="-1e+16" upper="1e+16"/>
  224 + <limit effort="-1" velocity="-1" lower="-1e+16" upper="1e+16"/>
  225 + <dynamics damping="0.0000001"/>
224 226 </joint>
225 227  
226 228 <joint name="sensor_frame_platform_joint" type="fixed">
... ...
aquanaute_description/urdf/tmp.urdf deleted
... ... @@ -1,281 +0,0 @@
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   -
aquanaute_gazebo/urdf/aquanaute_gazebo.urdf.xacro
... ... @@ -45,18 +45,10 @@
45 45 </gazebo>
46 46  
47 47 <gazebo reference="motor_link">
48   - <kp>100000.0</kp>
49   - <kd>100000.0</kd>
50   - <mu1>10.0</mu1>
51   - <mu2>10.0</mu2>
52 48 <material>Gazebo/Black</material>
53 49 </gazebo>
54 50  
55 51 <gazebo reference="propeller_link">
56   - <kp>1.0</kp>
57   - <kd>1.0</kd>
58   - <mu1>1.0</mu1>
59   - <mu2>1.0</mu2>
60 52 <material>Gazebo/Blue</material>
61 53 </gazebo>
62 54  
... ... @@ -87,41 +79,21 @@
87 79 <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
88 80 <imuName>imu_sensor</imuName>
89 81 <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
90   - <!--
91   - incoming control command [0, 1]
92   - so offset it by 0 to get [0, 1]
93   - and divide max target by 1.
94   - offset = 0
95   - multiplier = 838 max rpm / 1 = 838
96   - -->
  82 +
97 83 <control channel="0">
98   - <multiplier>1</multiplier>
99   - <offset>-0.5</offset>
100 84 <type>POSITION</type>
101   - <p_gain>1</p_gain>
102   - <i_gain>0</i_gain>
103   - <d_gain>0</d_gain>
104   - <i_max>0</i_max>
105   - <i_min>0</i_min>
106   - <cmd_max>1</cmd_max>
107   - <cmd_min>-1</cmd_min>
  85 + <multiplier>1</multiplier>
  86 + <offset>0</offset>
108 87 <jointName>aquanaute::motor_platform_joint</jointName>
109 88 </control>
110   -
111   - <control channel="1">
  89 + <control channel="0">
112 90 <type>VELOCITY</type>
113   - <offset>-0.5</offset>
114   - <p_gain>1</p_gain>
115   - <i_gain>0</i_gain>
116   - <d_gain>0</d_gain>
117   - <i_max>0</i_max>
118   - <i_min>0</i_min>
119   - <cmd_max>10</cmd_max>
120   - <cmd_min>-10</cmd_min>
  91 + <multiplier>1</multiplier>
  92 + <offset>0</offset>
121 93 <jointName>aquanaute::propeller_motor_joint</jointName>
122   - <multiplier>70</multiplier>
123 94 </control>
124   -
  95 +<!--
  96 +-->
125 97 </plugin>
126 98 </gazebo>
127 99 <!-- Attach hydrodynamics plugin -->
... ...
aquanaute_gazebo/urdf/dynamics/aquanaute_gazebo_dynamics_plugin.xacro
... ... @@ -9,9 +9,9 @@
9 9 <waterLevel>0</waterLevel>
10 10 <waterDensity>997.8</waterDensity>
11 11 <!-- Added mass -->
12   - <xDotU>10.0</xDotU>
13   - <yDotV>10.0</yDotV>
14   - <nDotR>25.0</nDotR>
  12 + <xDotU>0.0</xDotU>
  13 + <yDotV>0.0</yDotV>
  14 + <nDotR>0.0</nDotR>
15 15 <!-- Linear and quadratic drag -->
16 16 <xU>51.3</xU>
17 17 <xUU>72.4</xUU>
... ... @@ -28,7 +28,7 @@
28 28 <boatWidth>1.8</boatWidth>
29 29 <boatLength>3.0</boatLength>
30 30 <!-- Length discretization, AKA, "N" -->
31   - <length_n>10</length_n>
  31 + <length_n>2</length_n>
32 32 <!-- Wave model -->
33 33 <wave_model>ocean_waves</wave_model>
34 34 </plugin>
... ...
aquanaute_gazebo/worlds/test.world
... ... @@ -61,7 +61,7 @@
61 61 <kinematic>0</kinematic>
62 62 </link>
63 63 </model>
64   - <gravity>0 0 -9.8</gravity>
  64 + <gravity>0 0 0</gravity> <!--0 0 -9.8-->
65 65 <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
66 66 <atmosphere type='adiabatic'/>
67 67 <physics name='default_physics' default='0' type='ode'>
... ...