uuv_simulator
uuv_simulator copied to clipboard
Published
20 hours ago •
uuvsimulator
uuvsimulator
Simulation with multiple slender bodies
Hi,
I am trying to use UUV Sim to simulate some passive robots (without any thruster) in underwater environments. But I am getting several errors.
My robots have slender body properties. Normally, they have shapes similar to a torpedo, but for simplification, I model them as boxes in urdf files. All of their parameters (mass, volume, etc.) are calculated according to their actual shape. I am using strip theory(slender body approximation) & method of equivalent ellipsoid to determine the 3D added mass coefficients. The damping coefficients are disregarded.
I am posting 2 example robot configurations here.
Robot1: namespace: robot1 maximumRadius: 0.025 agentLength: 0.265 agentMass: 0.518532 agentVolume: 0.000503964 centerOfGravity: 0.126807 centerOfBuoyancy: 0.128401 inertiaXX: 0.000247974 inertiaYY: 0.00372173 inertiaZZ: 0.00372173 addedMass11: 0.028014 addedMass22: 0.520733 addedMass26: 0.00143208 addedMass55: 4.61259e-07
Robot2: namespace: robot2 maximumRadius: 0.0284 agentLength: 0.38 agentMass: 0.337542 agentVolume: 0.00029536 centerOfGravity: 0.272673 centerOfBuoyancy: 0.288764 inertiaXX: 0.000132158 inertiaYY: 0.0027912 inertiaZZ: 0.0027912 addedMass11: 0.0116095 addedMass22: 0.305304 addedMass26: 0.000560879 addedMass55: -4.22343e-05
I used auv_underwater_world.launch in package uuv_gazebo_worlds for tests. I start the gazebo world, spawn robots beneath the surface and observe their movements.
Test scenario 1: Simulation with single robot It works perfectly when I run the simulation with robot1. However, if I use robot2, it starts normally, but gets unstable after a while and I get one of the following errors each time:
***** Internal Program Error - assertion (!std::isnan(tau.norm())) failed in virtual void gazebo::HMFossen::ApplyHydrodynamicForces(double, const Vector3d&):
/home/user/catkin_ws/src/uuv_simulator/uuv_gazebo_plugins/uuv_gazebo_plugins/src/HydrodynamicModel.cc(417): Hydrodynamic forces vector is nan
Error [Param.cc:452] Unable to set value [inf -0.6 -inf 3.14159 1.51221 3.14159] for key[pose]
Error [Param.cc:452] Unable to set value [inf -0.6 -inf 0 0.772249 0] for key[pose]
***** Internal Program Error - assertion (!std::isnan(linearAccel) && !std::isnan(angularAccel)) failed in virtual void gazebo::UnderwaterObjectPlugin::Update(const gazebo::common::UpdateInfo&):
/home/user/catkin_ws/src/uuv_simulator/uuv_gazebo_plugins/uuv_gazebo_plugins/src/UnderwaterObjectPlugin.cc(210): Linear or angular accelerations are invalid.
Error [Param.cc:452] Unable to set value [-inf inf -inf 3.14159 0.650865 3.14159] for key[pose]
Error [Param.cc:452] Unable to set value [-inf inf -inf -3.1406 0.987751 3.13399] for key[pose]
gzclient: /usr/include/OGRE/OgreAxisAlignedBox.h:252: void Ogre::AxisAlignedBox::setExtents(const Ogre::Vector3&, const Ogre::Vector3&): Assertion `(min.x <= max.x && min.y <= max.y && min.z <= max.z) && "The minimum corner of the box must be less than or equal to maximum corner"' failed.
Aborted (core dumped)
Test scenario 2: Simulation with multiple robots No matter which robot configuration I use, the simulation crashes always with one of the errors above.
The second situation is more important for me, because I must run the simulation with multiple robots in my work.
Thanks in advance!
default.xacro
<?xml version="1.0"?>
<robot name="ea_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" >
<!--
THERE ARE SIMILAR xacro:arg AND xacro:property. BECAUSE, xacro:arg is a local value
but xacro:property is a global value i.e. xacro:property can be used in the included
files whereas the xacro:arg can only be used in the declared file. AGAIN, only xacro:arg
can be set through passing value from outside whereas the xacro:property is only be set inside
the file.
-->
<!-- ===== XACRO ARGUMENTS ========== -->
<!-- TODO: set defaults -->
<xacro:arg name="debug" default="0"/>
<xacro:arg name="namespace" default="ea_robot"/>
<!-- biggest radius - used for bounding box in gazebo.xacro -->
<xacro:arg name="maximumRadius" default= '0' />
<!-- legth of agent -->
<xacro:arg name="agentLength" default= '0' />
<!-- mass of agent -->
<xacro:arg name="agentMass" default= '0' />
<!-- volume of agent -->
<xacro:arg name="agentVolume" default= '0' />
<!-- center of gravity -->
<xacro:arg name="centerOfGravity" default= '0 0 0' />
<!-- center of buoyancy -->
<xacro:arg name="centerOfBuoyancy" default= '0 0 0' />
<!-- Inertia Ixx of agent -->
<xacro:arg name="inertiaXX" default= '0' />
<!-- Inertia Iyy of agent -->
<xacro:arg name="inertiaYY" default= '0' />
<!-- Inertia Izz of agent -->
<xacro:arg name="inertiaZZ" default= '0' />
<!-- Added-mass-matrix coefficients of agent -->
<xacro:arg name="addedMass11" default= '0' />
<xacro:arg name="addedMass22" default= '0' />
<xacro:arg name="addedMass26" default= '0' />
<xacro:arg name="addedMass55" default= '0' />
<!-- fulid density of liquid in the environment -->
<xacro:arg name="fluidDensity" default= '1024.0' />
<!--
============================================================================
============================================================================
-->
<!-- ===== XACRO PROPERTIES ==========
Vehicle's parameters (remember to enter the model parameters below)
-->
<!-- biggest radius - used for bounding box in gazebo.xacro -->
<xacro:property name="maxRadius" value="$(arg maximumRadius)"/>
<!-- length of agent -->
<xacro:property name="length" value="$(arg agentLength)"/>
<!-- mass of ea_robot -->
<xacro:property name="mass" value="$(arg agentMass)"/>
<!-- volume of ea_robot -->
<xacro:property name="volume" value="$(arg agentVolume)"/>
<!-- Center of gravity -->
<xacro:property name="cog" value="$(arg centerOfGravity)"/>
<!-- Center of buoyancy -->
<xacro:property name="cob" value="$(arg centerOfBuoyancy)"/>
<!-- Inertia Ixx of agent -->
<xacro:property name="Ixx" value="$(arg inertiaXX)"/>
<!-- Inertia Iyy of agent -->
<xacro:property name="Iyy" value="$(arg inertiaYY)"/>
<!-- Inertia Izz of agent -->
<xacro:property name="Izz" value="$(arg inertiaZZ)"/>
<!-- Added-mass-matrix coefficients of agent -->
<xacro:property name="amm11" value="$(arg addedMass11)"/>
<xacro:property name="amm22" value="$(arg addedMass22)"/>
<xacro:property name="amm26" value="$(arg addedMass26)"/>
<xacro:property name="amm55" value="$(arg addedMass55)"/>
<!-- Fluid density -->
<xacro:property name="rho" value="$(arg fluidDensity)"/>
<!--
============================================================================
============================================================================
-->
<!-- Include the ROV macro file -->
<xacro:include filename="$(find ea_robot_description)/urdf/base.xacro"/>
<xacro:include filename="$(find ea_robot_description)/urdf/gazebo.xacro"/>
<!-- Create the ea_robot -->
<xacro:ea_robot_base namespace="$(arg namespace)">
<!-- The underwater object plugin is given as an input block parameter to
allow the addition of external models of manipulator units -->
<gazebo>
<plugin name="uuv_plugin" filename="libuuv_underwater_object_ros_plugin.so">
<fluid_density>${rho}</fluid_density>
<flow_velocity_topic>hydrodynamics/current_velocity</flow_velocity_topic>
<debug>$(arg debug)</debug>
<!-- Adding the hydrodynamic and hydrostatic parameters for the vehicle-->
<xacro:ea_robot_hydro_model namespace="$(arg namespace)"/>
<!--
In case other modules are added to the vehicle (such as a manipulator)
that also have link running with the underwater object plugin, they
should also be added in this block. For this, this new module should
have a file similar to gazebo.xacro above with the description of the
parameter necessary for the underwater object plugin to be initialized.
-->
</plugin>
</gazebo>
</xacro:ea_robot_base>
<!-- Joint state publisher plugin -->
<xacro:default_joint_state_publisher namespace="$(arg namespace)" update_rate="10"/>
</robot>
base.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Loading some constants -->
<xacro:include filename="$(find uuv_descriptions)/urdf/common.urdf.xacro"/>
<!-- Loading file with sensor macros -->
<xacro:include filename="$(find uuv_sensor_ros_plugins)/urdf/sensor_snippets.xacro"/>
<!-- Loading the UUV simulator ROS plugin macros -->
<xacro:include filename="$(find uuv_gazebo_ros_plugins)/urdf/snippets.xacro"/>
<!-- Loading vehicle's specific macros -->
<xacro:include filename="$(find ea_robot_description)/urdf/snippets.xacro"/>
<!--
Visual mesh file for the vehicle, usually in DAE (Collada) format. Be sure to store the
mesh with the origin of the mesh on the same position of the center of mass, otherwise
the mesh pose will have to be corrected below in the <visual> block.
Open the meshes for the RexROV vehicle in Blender to see an example on the mesh placement.
-->
<!-- TODO: filepath
<xacro:property name="visual_mesh_file" value="file://$(find ea_robot_description)/meshes/vehicle.dae"/>
-->
<!-- Collision geometry mesh, usually in STL format (it is recommended to keep
this geometry as simple as possible to improve the performance the physics engine
regarding the computation of collision forces) -->
<!-- TODO: filepath
<xacro:property name="collision_mesh_file" value="file://$(find ea_robot_description)/meshes/vehicle.stl"/>
-->
<!-- Vehicle macro -->
<xacro:macro name="ea_robot_base" params="namespace *gazebo">
<link name="dummy">
<inertial>
<mass value="${mass}"/>
<origin xyz="${cog} 0 0" rpy="0 0 0"/>
<inertia ixx="${Ixx}" ixy="0" ixz="0" iyy="${Iyy}" iyz="0" izz="${Izz}"/>
</inertial>
</link>
<link name="${namespace}/base_link">
<visual>
<origin xyz="${length/2} 0 0" rpy="0 0 0"/>
<geometry>
<!-- <mesh filename="${visual_mesh_file}"/> -->
<!-- simplification - bounding box -->
<box size="${length} ${maxRadius} ${maxRadius}"/>
</geometry>
<material name="ea_white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="${length/2} 0 0" rpy="0 0 0"/>
<geometry>
<!-- <mesh filename="${collision_mesh_file}"/> -->
<!-- simplification - bounding box -->
<box size="${length} ${maxRadius} ${maxRadius}"/>
</geometry>
</collision>
<!-- <inertial> -->
<!-- <mass value="${mass}"/> -->
<!-- <origin xyz="${cog} 0 0" rpy="0 0 0"/> -->
<!-- <inertia ixx="${Ixx}" ixy="0" ixz="0" iyy="${Iyy}" iyz="0" izz="${Izz}"/> -->
<!-- </inertial> -->
</link>
<joint name="dummy_joint" type="fixed">
<parent link="${namespace}/base_link"/>
<child link="dummy"/>
</joint>
<gazebo reference="${namespace}/base_link">
<selfCollide>false</selfCollide>
</gazebo>
<!-- Set up hydrodynamic plugin given as input parameter -->
<xacro:insert_block name="gazebo"/>
<!-- Include the sensor modules -->
<!--TODO edit sensors.xacro <xacro:include filename="$(find ea_robot_description)/urdf/sensors.xacro"/> -->
</xacro:macro>
</robot>
gazebo.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="ea_robot_hydro_model" params="namespace">
<!-- List of hydrodynamic models this robot's links -->
<link name="${namespace}/base_link">
<!-- This flag will make the link neutrally buoyant -->
<neutrally_buoyant>0</neutrally_buoyant>
<!-- Link's volume -->
<volume>${volume}</volume>
<!-- Link's bounding box, it is used to recalculate the immersed
volume when close to the surface.
This is a workaround the invalid bounding box given by Gazebo-->
<box>
<width>${maxRadius}</width>
<length>${length}</length>
<height>${maxRadius}</height>
</box>
<!-- Center of buoyancy := relative to center of gravity-->
<center_of_buoyancy>${cob-cog}</center_of_buoyancy>
<!--
Choose one of the hydrodynamic models below, all are based on
Fossen's equation of motion for underwater vehicles
Reference:
[1] Fossen, Thor I. Handbook of marine craft hydrodynamics and motion
control. John Wiley & Sons, 2011.
-->
<!-- 1) Fossen's equation of motion -->
<hydrodynamic_model>
<type>fossen</type>
<added_mass>
${amm11} 0 0 0 0 0
0 ${amm22} 0 0 0 ${amm26}
0 0 ${amm22} 0 -${amm26} 0
0 0 0 0 0 0
0 0 -${amm26} 0 ${amm55} 0
0 ${amm26} 0 0 0 ${amm55}
</added_mass>
<!--
The linear damping coefficients can be provided as a diagonal (6 elements)
or a full matrix (36 coefficients), like the added-mass coefficients above
-->
<!--
<linear_damping>
0 0 0 0 0 0
</linear_damping>
-->
<!--
The linear damping coefficients proportional to the forward speed
can be provided as a diagonal (6 elements) or a full matrix (36 coefficients),
like the added-mass coefficients above.
This matrix is mostly useful for slender bodies (e.g. AUVs with torpedo shape)
-->
<!-- <linear_damping_forward_speed>
0 0 0 0 0 0
</linear_damping_forward_speed> -->
<!--
The quadratic damping coefficients can be provided as a diagonal (6 elements)
or a full matrix (36 coefficients), like the added-mass coefficients above
-->
<!--
<quadratic_damping>
0 0 0 0 0 0
</quadratic_damping>
-->
</hydrodynamic_model>
</link>
</xacro:macro>
</robot>
