Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
346 lines (327 sloc) 9.02 KB
<?xml version='1.0'?>
<sdf version='1.5'>
<model name='BlueRov2'>
<pose>0 0 0 0 0 0</pose>
<link name='base_link'>
<inertial>
<mass>11</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<visual name='base_link_visual'>
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>model://BlueRov2/meshes/BlueRov2.stl</uri>
</mesh>
</geometry>
</visual>
<sensor name="imu_sensor" type="imu">
<pose>0 0 0 0 0 0</pose>
<always_on>1</always_on>
<update_rate>1000.0</update_rate>
</sensor>
</link>
<link name='thruster1'>
<pose>0.12 -0.11 0 1.5707963 1.5707963 -0.78539815</pose>
<!--visual name="cil">
<geometry>
<cylinder>
<radius>.03</radius>
<length>1</length>
</cylinder>
</geometry>
</visual-->
<inertial>
<mass>1e-10</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
</link>
<joint name='thruster1_joint' type='fixed'>
<parent>base_link</parent>
<child>thruster1</child>
</joint>
<link name='thruster2'>
<pose>0.12 0.11 0 1.5707963 1.5707963 3.92699075</pose>
<!--visual name="cil1">
<geometry>
<cylinder>
<radius>.03</radius>
<length>1</length>
</cylinder>
</geometry>
</visual-->
<inertial>
<mass>1e-10</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
</link>
<joint name='thruster2_joint' type='fixed'>
<parent>base_link</parent>
<child>thruster2</child>
<axis>
<xyz>0 0 1</xyz>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='thruster3'>
<pose>-0.12 -0.11 0 1.5707963 1.5707963 0.78540075</pose>
<!--visual name="cil2">
<geometry>
<cylinder>
<radius>.03</radius>
<length>1</length>
</cylinder>
</geometry>
</visual-->
<inertial>
<mass>1e-10</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
</link>
<joint name='thruster3_joint' type='fixed'>
<parent>base_link</parent>
<child>thruster3</child>
<axis>
<xyz>0 0 1</xyz>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='thruster4'>
<pose>-0.12 0.11 0 1.5707963 1.5707963 2.35619445</pose>
<!--visual name="cil3">
<geometry>
<cylinder>
<radius>.03</radius>
<length>1</length>
</cylinder>
</geometry>
</visual-->
<inertial>
<mass>1e-10</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
</link>
<joint name='thruster4_joint' type='fixed'>
<parent>base_link</parent>
<child>thruster4</child>
<axis>
<xyz>0 0 -1</xyz>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='thruster5'>
<pose>0 -0.11 0 3.1415926 0 0</pose>
<!--visual name="cil4">
<geometry>
<cylinder>
<radius>.03</radius>
<length>1</length>
</cylinder>
</geometry>
</visual-->
<inertial>
<mass>1e-10</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
</link>
<joint name='thruster5_joint' type='fixed'>
<parent>base_link</parent>
<child>thruster5</child>
<axis>
<xyz>0 -1 0</xyz>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<link name='thruster6'>
<pose>0 0.11 0 3.1415926 0 0</pose>
<!--visual name="cil5">
<geometry>
<cylinder>
<radius>.03</radius>
<length>1</length>
</cylinder>
</geometry>
</visual-->
<inertial>
<mass>1e-10</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
</link>
<joint name='thruster6_joint' type='fixed'>
<parent>base_link</parent>
<child>thruster6</child>
<axis>
<xyz>0 1 0</xyz>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
</joint>
<allow_auto_disable>1</allow_auto_disable>
<plugin name="ArduPilotPlugin" 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>-->
<imuName>BlueRov2::base_link::imu_sensor</imuName>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<!--
incoming control command [0, 1]
so offset it by -0.5 to get [-0.5, 0.5].
multiplier = 102
Positive thrust = 51
Negative thrust = -41
-->
<control channel="0">
<type>EFFORT</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>51</cmd_max>
<cmd_min>-40</cmd_min>
<linkName>BlueRov2::thruster1</linkName>
<multiplier>102</multiplier>
</control>
<control channel="1">
<type>EFFORT</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>51</cmd_max>
<cmd_min>-40</cmd_min>
<linkName>BlueRov2::thruster2</linkName>
<multiplier>102</multiplier>
</control>
<control channel="2">
<type>EFFORT</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>51</cmd_max>
<cmd_min>-40</cmd_min>
<linkName>BlueRov2::thruster3</linkName>
<multiplier>102</multiplier>
</control>
<control channel="3">
<type>EFFORT</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>51</cmd_max>
<cmd_min>-40</cmd_min>
<linkName>BlueRov2::thruster4</linkName>
<multiplier>102</multiplier>
</control>
<control channel="4">
<type>EFFORT</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>51</cmd_max>
<cmd_min>-40</cmd_min>
<linkName>BlueRov2::thruster5</linkName>
<multiplier>102</multiplier>
</control>
<control channel="5">
<type>EFFORT</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>51</cmd_max>
<cmd_min>-40</cmd_min>
<linkName>BlueRov2::thruster6</linkName>
<multiplier>102</multiplier>
</control>
</plugin>
<!-- buoyancy plugin -->
<plugin name="freebuoyancy_gazebo" filename="libfreebuoyancy_gazebo.so">
<descriptionParam>robot_description</descriptionParam>
<fluidTopic>current</fluidTopic>
<alwaysOn>true</alwaysOn>
<surface>0 0 0</surface>
<updateRate>.001</updateRate>
<link name="base_link">
<buoyancy>
<compensation>0.2525</compensation>
<origin xyz= "0.12585 0.111 0.1789"/>
</buoyancy>
<buoyancy>
<compensation>0.2525</compensation>
<origin xyz= "-0.12585 0.111 0.1789"/>
</buoyancy>
<buoyancy>
<compensation>0.2525</compensation>
<origin xyz= "-0.12585 -0.111 0.1789"/>
</buoyancy>
<buoyancy>
<compensation>0.2525</compensation>
<origin xyz= "0.12585 -0.111 0.1789"/>
</buoyancy>
</link>
</plugin>
</model>
</sdf>
You can’t perform that action at this time.