Skip to content

GitLab

Commit 4120d958 authored by Ricardo Rico Uribe's avatar Ricardo Rico Uribe
Browse files

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

parent cf91bc0c
Show whitespace changes
Inline Side-by-side
aquanaute_description/launch/description.launch
View file @ 4120d958
......@@ -5,7 +5,7 @@
<!-- send fake joint values -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="joint_state_publisher_gui" value="false"/>
<param name="use_gui" value="false"/>
</node>
<!-- Combine joint values -->
......
aquanaute_description/urdf/aquanaute_description.urdf.xacro
View file @ 4120d958
......@@ -143,9 +143,9 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius="0.01" length="0.02"/>
<cylinder radius="0.04" length="0.05"/>
</geometry>
<material name="green"/>
<material name="black"/>
</visual>
</link>
......@@ -172,6 +172,16 @@
</visual>
</link>
<link name="imu_link">
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder radius="0.04" length="0.05"/>
</geometry>
<material name="cyan"/>
</visual>
</link>
<!-- * * * Joint Definitions * * * -->
<joint name="platform_base_joint" type="fixed">
<parent link="base_link"/>
......@@ -199,12 +209,12 @@
<limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/>
</joint>
<joint name="propeller_motor_joint" type="revolute">
<joint name="propeller_motor_joint" type="fixed">
<parent link="motor_link"/>
<child link="propeller_link"/>
<origin xyz="-0.28 0 -0.42" rpy="0 1.57 0"/>
<origin xyz="-0.5 0 0.42" rpy="0 1.57 0"/>
<axis xyz="0 0 1"/>
<limit effort="-1" velocity="-1" lower="-1e+12" upper="1e+12"/>
<limit effort="-1" velocity="-1" lower="-1e+16" upper="1e+16"/>
</joint>
<joint name="sensor_frame_platform_joint" type="fixed">
......@@ -213,101 +223,9 @@
<origin xyz="-0.55 0.0 0.0" rpy="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 name="ardupilot_plugin" filename="libArduPilotPlugin.so">
<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>
<joint name="imu_platform_joint" type="fixed">
<parent link="platform_link"/>
<child link="imu_link"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
</joint>
</robot>
aquanaute_gazebo/launch/test.launch 0 → 100644
View file @ 4120d958
<?xml version="1.0"?>
<launch>
<!-- Gazebo world to load -->
<arg name="world" default="$(find aquanaute_gazebo)/worlds/test.world" />
<!-- If true, run gazebo GUI -->
<arg name="gui" default="true" />
<!-- If true, run gazebo in verbose mode -->
<arg name="verbose" default="true"/>
<!-- If true, start in paused state -->
<arg name="paused" default="false"/>
<!-- Set various other gazebo arguments-->
<arg name="extra_gazebo_args" default=""/>
<!-- Start in a default namespace -->
<arg name="namespace" default="aquanaute"/>
<!-- Initial USV location and attitude-->
<arg name="x" default="0" />
<arg name="y" default="0" />
<arg name="z" default="0.5" />
<arg name="P" default="0" />
<arg name="R" default="0" />
<arg name="Y" default="0" />
<!-- Start Gazebo with the world file -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="$(arg gui)" />
<arg name="enable_ros_network" value="true"/>
</include>
<!-- Load robot model -->
<arg name="urdf" default="$(find aquanaute_gazebo)/urdf/aquanaute_gazebo.urdf.xacro"/>
<param name="$(arg namespace)/robot_description" command="$(find xacro)/xacro '$(arg urdf)'"/>
<!-- Spawn model in Gazebo, script depending on non_competition_mode -->
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x) -y $(arg y) -z $(arg z)
-R $(arg R) -P $(arg P) -Y $(arg Y)
-urdf -param $(arg namespace)/robot_description -model aquanaute"/>
</launch>
aquanaute_gazebo/urdf/aquanaute_gazebo.urdf.xacro
View file @ 4120d958
<?xml version="1.0"?>
<!-- Basic WAM-V with gazebo plugins for dynamics -->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Include macros for dynamics plugins -->
<xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" />
<!-- Include macros for dynamics plugins
<xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" /> -->
<!-- Defines the base vehicle -->
<xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/>
<!-- Attach hydrodynamics plugin -->
<xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/>
<!-- Defines the sim plugins-->
<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="propeller_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>
<link name="imu_link">
<sensor name="imu_sensor" type="imu">
<pose>0 0 0 3.141593 0 0</pose>
<always_on>1</always_on>
<update_rate>1000.0</update_rate>
</sensor>
</link>
</gazebo>
<!-- * * * GAZEBO Plugins * * * -->
<gazebo>
<plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so">
<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>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>
<jointName>aquanaute::motor_platform_joint</jointName>
</control>
<control channel="1">
<multiplier>1</multiplier>
<offset>-0.5</offset>
<type>VELOCITY</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>
</gazebo>
<!-- Attach hydrodynamics plugin
<xacro:usv_dynamics_gazebo name="aquanaute_dynamics_plugin"/> -->
</robot>
aquanaute_gazebo/worlds/test.world 0 → 100644
View file @ 4120d958
<sdf version='1.6'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<collide_bitmask>65535</collide_bitmask>
<ode/>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<state world_name='default'>
<sim_time>29 253000000</sim_time>
<real_time>29 457037678</real_time>
<wall_time>1594385127 732457775</wall_time>
<iterations>29253</iterations>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>5 -5 2 0 0.275643 2.35619</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>
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