Commit 5140bb7f authored by Ricardo Rico Uribe's avatar Ricardo Rico Uribe
Browse files

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

general order and small fixes - missing fix for ardupilot gazebo error where motors turn without control
parent bb89241c
# Aquanaute
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.
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.
---
## Installation
* Clone this repo to a Catkin Workspace
* Clone the VRX repo (hosted at ensta) to the same Workspace (http://git-u2is.ensta.fr/ssh/vrx) and checkout to the branch "aquanaute"
* Clone the VRX repo (hosted at ensta) to the same Workspace (<http://git-u2is.ensta.fr/ssh/vrx>) and change to the branch "aquanaute"
### ardupilot parameters
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).
*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*
---
## Usage
### modify wind and waves
in the ocean.world.xacro (/vrx_gazebo/worlds) you have
```xml
<!--Waves-->
<xacro:include filename="$(find wave_gazebo)/world_models/ocean_waves/model.xacro"/>
<xacro:ocean_waves gain="0.0" period="5" direction_x="-1.0" direction_y="0.0" angle="0.0"/>
<!--Wind-->
<xacro:include filename="$(find vrx_gazebo)/worlds/xacros/usv_wind_plugin.xacro"/>
<xacro:usv_wind_gazebo direction="90" mean_vel="0" var_gain="5" var_time="1">
<wind_objs>
<wind_obj>
<name>wamv</name>
<link_name>base_link</link_name>
<coeff_vector>0.5 0.5 0.33</coeff_vector>
</wind_obj>
</wind_objs>
</xacro:usv_wind_gazebo>
```
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).
in the ocean.world.xacro (/vrx_gazebo/worlds) you have
<!--Waves-->
<xacro:include filename="$(find wave_gazebo)/world_models/ocean_waves/model.xacro"/>
<xacro:ocean_waves gain="0.0" period="5" direction_x="-1.0" direction_y="0.0" angle="0.0"/>
<!--Wind-->
<xacro:include filename="$(find vrx_gazebo)/worlds/xacros/usv_wind_plugin.xacro"/>
<xacro:usv_wind_gazebo direction="90" mean_vel="0" var_gain="5" var_time="1">
<wind_objs>
<wind_obj>
<name>wamv</name>
<link_name>base_link</link_name>
<coeff_vector>0.5 0.5 0.33</coeff_vector>
</wind_obj>
</wind_objs>
</xacro:usv_wind_gazebo>
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).
Even when these values are set to 0, the boat will move from right to left.
---
### run
in the workspace run the command at the beginning (and every time you change a parameter)
```bash
catkin_make
```
#### compilation
in the workspace run the command at the beginning (and every time you change a parameter in the vrx xacros)
catkin_make
#### gazebo simulation
then launch the file you created
```bash
roslaunch aquanaute_gazebo aquanaute.launch testing:=false
```
this parameter can be changed to true to deactivate the plugins from vrx (the world will be changed to an empty world)
roslaunch aquanaute_gazebo aquanaute.launch testing:=false
this parameter can be changed to true to deactivate the plugins from vrx (the world will be changed to an empty world without gravity)
#### ardupilot
on another terminal (inside the ardupilot root) run
```bash
./Tools/autotest/sim_vehicle.py --vehicle=Rover --frame=sailboat-motor --map --console --location=Xlac
```
## Author
Ricardo RICO URIBE intern at U2IS in the summer of 2020
./Tools/autotest/sim_vehicle.py -v APMrover2 -f gazebo-rover --map --console -l 48.71603264538473,2.213283777236939,155,0
## License
---
## Author
Ricardo RICO URIBE intern at U2IS in the summer of 2020
FRAME_CLASS 2
AHRS_ORIENTATIONM 0
GCS_PID_MASK 1
SERVO1_FUNCTION 26
SERVO3_FUNCTION 70
ARMING_CHECK 0
......@@ -212,7 +212,8 @@
<child link="motor_link"/>
<origin xyz="-0.91 0.0 -0.09" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit effort="1" velocity="1" lower="-0.9" upper="0.9"/>
<limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/>
<dynamics damping="0.0000001"/>
</joint>
<joint name="propeller_motor_joint" type="revolute">
......@@ -220,7 +221,8 @@
<child link="propeller_link"/>
<origin xyz="-0.265 0 -0.42" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
<limit effort="1" velocity="1" lower="-1e+16" upper="1e+16"/>
<limit effort="-1" velocity="-1" lower="-1e+16" upper="1e+16"/>
<dynamics damping="0.0000001"/>
</joint>
<joint name="sensor_frame_platform_joint" type="fixed">
......
<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from aquanaute_description.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="aquanaute">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="red">
<color rgba="0.8 0 0 1"/>
</material>
<material name="green">
<color rgba="0 0.8 0 0.5"/>
</material>
<material name="cyan">
<color rgba="0 0.6 0.6 1"/>
</material>
<material name="yellow">
<color rgba="0.8 0.8 0 1"/>
</material>
<material name="purple">
<color rgba="0.5 0 0.5 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<material name="gray">
<color rgba="0.35 0.35 0.35 1"/>
</material>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<!-- * * * Link Definitions * * * -->
<link name="base_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.001"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<link name="platform_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.024 0 -0007"/>
<mass value="16"/>
<inertia ixx="4.206" ixy="0.0" ixz="-0.031" iyy="4.588" iyz="0.0" izz="8.785"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/Platform.stl"/>
</geometry>
</collision>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/Platform.stl"/>
</geometry>
<material name="yellow"/>
</visual>
</link>
<link name="right_hull_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.124 0 -0.168"/>
<mass value="10.45"/>
<inertia ixx="0.244" ixy="3.346" ixz="0.009" iyy="4.974" iyz="-8.274" izz="4.965"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>
</geometry>
</collision>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>
</geometry>
<material name="red"/>
</visual>
</link>
<link name="left_hull_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.124 0 -0.168"/>
<mass value="10.45"/>
<inertia ixx="0.244" ixy="3.346" ixz="0.009" iyy="4.974" iyz="-8.274" izz="4.965"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>
</geometry>
</collision>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/Hull.stl"/>
</geometry>
<material name="red"/>
</visual>
</link>
<link name="motor_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.133 0 -0.391"/>
<mass value="6.6"/>
<inertia ixx="0.072" ixy="3.604" ixz="-0.011" iyy="0.111" iyz="5.484" izz="0.042"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/>
</geometry>
</collision>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/Motor.stl"/>
</geometry>
<material name="purple"/>
</visual>
</link>
<link name="propeller_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.05"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.01"/>
</geometry>
<material name="green"/>
</visual>
</link>
<link name="sensor_frame_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.007 0 0.379"/>
<mass value="1.8"/>
<inertia ixx="0.308" ixy="0.00" ixz="0.007" iyy="0.297" iyz="0.00" izz="0.022"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/>
</geometry>
</collision>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://aquanaute_description/models/meshes/sensor_frame.stl"/>
</geometry>
<material name="cyan"/>
</visual>
</link>
<!-- * * * Joint Definitions * * * -->
<joint name="platform_base_joint" type="fixed">
<parent link="base_link"/>
<child link="platform_link"/>
<origin rpy="0 0 0" xyz="0 0 0.35"/>
</joint>
<joint name="right_hull_platform_joint" type="fixed">
<parent link="platform_link"/>
<child link="right_hull_link"/>
<origin rpy="0 0 0" xyz="-0.05 -0.7 -0.036"/>
</joint>
<joint name="left_hull_platform_joint" type="fixed">
<parent link="platform_link"/>
<child link="left_hull_link"/>
<origin rpy="0 0 0" xyz="-0.05 0.7 -0.036"/>
</joint>
<joint name="motor_platform_joint" type="revolute">
<parent link="platform_link"/>
<child link="motor_link"/>
<origin rpy="0 0 0" xyz="-0.91 0.0 -0.09"/>
<axis xyz="0 0 1"/>
<limit effort="-1" lower="-0.9" upper="0.9" velocity="-1"/>
</joint>
<joint name="propeller_motor_joint" type="revolute">
<parent link="motor_link"/>
<child link="propeller_link"/>
<origin rpy="0 1.57 0" xyz="-0.28 0 -0.42"/>
<axis xyz="0 0 1"/>
<limit effort="-1" lower="-1e+12" upper="1e+12" velocity="-1"/>
</joint>
<joint name="sensor_frame_platform_joint" type="fixed">
<parent link="platform_link"/>
<child link="sensor_frame_link"/>
<origin rpy="0 0 0" xyz="-0.55 0.0 0.0"/>
</joint>
<!-- * * * GAZEBO Definitions * * * -->
<gazebo reference="base_link">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="platform_link">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="right_hull_link">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<material>Gazebo/Red</material>
</gazebo>
<gazebo reference="left_hull_link">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<material>Gazebo/Red</material>
</gazebo>
<gazebo reference="motor_link">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="sensor_frame_link">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<material>Gazebo/White</material>
</gazebo>
<!-- * * * GAZEBO Plugins * * * -->
<plugin filename="libArduPilotPlugin.so" name="ardupilot_plugin">
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<fdm_port_out>9003</fdm_port_out>
<modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
<!--<imuName></imuName>-->
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<control channel="0">
<multiplier>1</multiplier>
<offset>-0.5</offset>
<type>RIC</type>
<p_gain>1</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>1</cmd_max>
<cmd_min>-1</cmd_min>
<jointName>aquanaute::motor_platform_joint</jointName>
</control>
<control channel="1">
<multiplier>1</multiplier>
<offset>-0.5</offset>
<type>THOMAS</type>
<p_gain>1</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>1</cmd_max>
<cmd_min>-1</cmd_min>
<jointName>aquanaute::propeller_motor_joint</jointName>
</control>
</plugin>
</robot>
......@@ -45,18 +45,10 @@
</gazebo>
<gazebo reference="motor_link">
<kp>100000.0</kp>
<kd>100000.0</kd>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="propeller_link">
<kp>1.0</kp>
<kd>1.0</kd>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<material>Gazebo/Blue</material>
</gazebo>
......@@ -87,41 +79,21 @@
<gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
<imuName>imu_sensor</imuName>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<control channel="0">
<multiplier>1</multiplier>
<offset>-0.5</offset>
<type>POSITION</type>
<p_gain>1</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>1</cmd_max>
<cmd_min>-1</cmd_min>
<multiplier>1</multiplier>
<offset>0</offset>
<jointName>aquanaute::motor_platform_joint</jointName>
</control>
<control channel="1">
<control channel="0">
<type>VELOCITY</type>
<offset>-0.5</offset>
<p_gain>1</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>10</cmd_max>
<cmd_min>-10</cmd_min>
<multiplier>1</multiplier>
<offset>0</offset>
<jointName>aquanaute::propeller_motor_joint</jointName>
<multiplier>70</multiplier>
</control>
<!--
-->
</plugin>
</gazebo>
<!-- Attach hydrodynamics plugin -->
......
......@@ -9,9 +9,9 @@
<waterLevel>0</waterLevel>
<waterDensity>997.8</waterDensity>
<!-- Added mass -->
<xDotU>10.0</xDotU>
<yDotV>10.0</yDotV>
<nDotR>25.0</nDotR>
<xDotU>0.0</xDotU>
<yDotV>0.0</yDotV>
<nDotR>0.0</nDotR>
<!-- Linear and quadratic drag -->
<xU>51.3</xU>
<xUU>72.4</xUU>
......@@ -28,7 +28,7 @@
<boatWidth>1.8</boatWidth>
<boatLength>3.0</boatLength>
<!-- Length discretization, AKA, "N" -->
<length_n>10</length_n>
<length_n>2</length_n>
<!-- Wave model -->
<wave_model>ocean_waves</wave_model>
</plugin>
......
......@@ -61,7 +61,7 @@
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<gravity>0 0 0</gravity> <!--0 0 -9.8-->
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment