added imu for plugin - missing fix for error with 2 joints (4120d958) · Commits · U2IS / aquanaute

aquanaute_description/launch/description.launch

+1 −1

Original line number	Original line	Diff line number	Diff line
	@@ -5,7 +5,7 @@

	<!-- send fake joint values -->		<!-- send fake joint values -->
	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">		<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>		</node>

	<!-- Combine joint values -->		<!-- Combine joint values -->

aquanaute_description/urdf/aquanaute_description.urdf.xacro

+20 −102

Original line number	Original line	Diff line number	Diff line
	@@ -143,9 +143,9 @@
	<visual>		<visual>
	<origin rpy="0 0 0" xyz="0 0 0"/>		<origin rpy="0 0 0" xyz="0 0 0"/>
	<geometry>		<geometry>
	<cylinder radius="0.01" length="0.02"/>		<cylinder radius="0.04" length="0.05"/>
	</geometry>		</geometry>
	<material name="green"/>		<material name="black"/>
	</visual>		</visual>
	</link>		</link>

	@@ -172,6 +172,16 @@
	</visual>		</visual>
	</link>		</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 Definitions * * * -->
	<joint name="platform_base_joint" type="fixed">		<joint name="platform_base_joint" type="fixed">
	<parent link="base_link"/>		<parent link="base_link"/>
	@@ -199,12 +209,12 @@
	<limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/>		<limit effort="-1" velocity="-1" lower="-0.9" upper="0.9"/>
	</joint>		</joint>

	<joint name="propeller_motor_joint" type="revolute">		<joint name="propeller_motor_joint" type="fixed">
	<parent link="motor_link"/>		<parent link="motor_link"/>
	<child link="propeller_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"/>		<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>

	<joint name="sensor_frame_platform_joint" type="fixed">		<joint name="sensor_frame_platform_joint" type="fixed">
	@@ -213,101 +223,9 @@
	<origin xyz="-0.55 0.0 0.0" rpy="0 0 0"/>		<origin xyz="-0.55 0.0 0.0" rpy="0 0 0"/>
	</joint>		</joint>

	<!-- * * * GAZEBO Definitions * * * -->		<joint name="imu_platform_joint" type="fixed">
			<parent link="platform_link"/>
	<gazebo reference="base_link">		<child link="imu_link"/>
	<kp>100000.0</kp>		<origin xyz="0 0 0.1" rpy="0 0 0"/>
	<kd>100000.0</kd>		</joint>
	<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>

	</robot>		</robot>

aquanaute_gazebo/launch/test.launch

0 → 100644

+42 −0

Original line number	Original line	Diff line number	Diff line
			<?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

+120 −4

Original line number	Original line	Diff line number	Diff line
	<?xml version="1.0"?>		<?xml version="1.0"?>
	<!-- Basic WAM-V with gazebo plugins for dynamics -->		<!-- Basic WAM-V with gazebo plugins for dynamics -->
	<robot xmlns:xacro="http://ros.org/wiki/xacro">		<robot xmlns:xacro="http://ros.org/wiki/xacro">
	<!-- Include macros for dynamics plugins -->		<!-- Include macros for dynamics plugins
	<xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" />		<xacro:include filename="$(find aquanaute_gazebo)/urdf/macros.xacro" /> -->
	<!-- Defines the base vehicle -->		<!-- Defines the base vehicle -->
	<xacro:include filename="$(find aquanaute_description)/urdf/aquanaute_description.urdf.xacro"/>		<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>		</robot>

aquanaute_gazebo/worlds/test.world

0 → 100644

+115 −0

Original line number	Original line	Diff line number	Diff line
			<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>