Skip to content

Commit

Permalink
use buoyancy plugin for the wamv
Browse files Browse the repository at this point in the history 
remove buoyancy code inside usv dyanmics, model wamv buoyancy as two buoyant cylinders in location of pontoons
  • Loading branch information
@acxz
acxz committed Aug 3, 2022
1 parent 61c8e3e commit cbc9bad
Show file tree
Hide file tree
Showing 4 changed files with 56 additions and 74 deletions.
74 changes: 0 additions & 74 deletions usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,11 +170,6 @@ void UsvDynamicsPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
0, 0, 0, 0, 0, this->paramNdotR;
}

double UsvDynamicsPlugin::CircleSegment(double R, double h)
{
return R*R*acos( (R-h)/R ) - (R-h)*sqrt(2*R*h-h*h);
}

//////////////////////////////////////////////////
void UsvDynamicsPlugin::Update()
{
Expand Down Expand Up @@ -287,75 +282,6 @@ void UsvDynamicsPlugin::Update()
ignition::math::Vector3d(kForceSum(0), kForceSum(1), kForceSum(2)));
this->link->AddRelativeTorque(
ignition::math::Vector3d(kForceSum(3), kForceSum(4), kForceSum(5)));

// Loop over boat grid points
// Grid point location in boat frame - might be able to precalculate these?
tf2::Vector3 bpnt(0, 0, 0);
// Grid point location in world frame
tf2::Vector3 bpntW(0, 0, 0);
// For each hull
for (int ii = 0; ii < 2; ii++)
{
// Grid point in boat frame
bpnt.setY((ii*2.0-1.0)*this->paramBoatWidth/2.0);
// For each length segment
for (int jj = 1; jj <= this->paramLengthN; jj++)
{
bpnt.setX(((jj - 0.5) / (static_cast<float>(this->paramLengthN)) - 0.5) *
this->paramBoatLength);

// Transform from vessel to water/world frame
bpntW = xformV * bpnt;

// Debug
ROS_DEBUG_STREAM_THROTTLE(1.0, "[" << ii << "," << jj <<
"] grid points" << bpnt.x() << "," << bpnt.y() << "," << bpnt.z());
ROS_DEBUG_STREAM_THROTTLE(1.0, "v frame euler " << kEuler);
ROS_DEBUG_STREAM_THROTTLE(1.0, "in water frame" << bpntW.x() << "," <<
bpntW.y() << "," << bpntW.z());

// Vertical location of boat grid point in world frame
const float kDdz = kPose.Pos().Z() + bpntW.z();
ROS_DEBUG_STREAM("Z, pose: " << kPose.Pos().Z() << ", bpnt: "
<< bpntW.z() << ", dd: " << kDdz);

// Find vertical displacement of wave field
// World location of grid point
ignition::math::Vector3d X;
X.X() = kPose.Pos().X() + bpntW.x();
X.Y() = kPose.Pos().Y() + bpntW.y();

// Compute the depth at the grid point.
double simTime = kTimeNow.Double();
// double depth = WavefieldSampler::ComputeDepthDirectly(
// *waveParams, X, simTime);
double depth = 0.0;
if (waveParams)
{
depth = WavefieldSampler::ComputeDepthSimply(*waveParams, X, simTime);
}

// Vertical wave displacement.
double dz = depth + X.Z();

// Total z location of boat grid point relative to water surface
double deltaZ = (this->waterLevel + dz) - kDdz;
deltaZ = std::max(deltaZ, 0.0); // enforce only upward buoy force
deltaZ = std::min(deltaZ, this->paramHullRadius);
// Buoyancy force at grid point
const float kBuoyForce = CircleSegment(this->paramHullRadius, deltaZ) *
this->paramBoatLength/(static_cast<float>(this->paramLengthN)) *
GRAVITY * this->waterDensity;
ROS_DEBUG_STREAM("buoyForce: " << kBuoyForce);

// Apply force at grid point
// From web, Appears that position is in the link frame
// and force is in world frame
this->link->AddForceAtRelativePosition(
ignition::math::Vector3d(0, 0, kBuoyForce),
ignition::math::Vector3d(bpnt.x(), bpnt.y(), bpnt.z()));
}
}
}

GZ_REGISTER_MODEL_PLUGIN(UsvDynamicsPlugin);
52 changes: 52 additions & 0 deletions wamv_gazebo/urdf/buoyancy/wamv_buoyancy_plugin.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="usv_buoyancy_gazebo" params="name length:=4.9">
<!--Gazebo Plugin for simulating WAM-V buoyancy-->
<gazebo>
<plugin name="usv_buoyancy_${name}_hull_left" filename="libbuoyancy_gazebo_plugin.so">
<!-- Wave model -->
<wave_model>ocean_waves</wave_model>
<fluid_density>997.8</fluid_density>
<fluid_level>0</fluid_level>

<linear_drag>0</linear_drag>
<angular_drag>0</angular_drag>

<buoyancy>
<link_name>${namespace}/base_link</link_name>
<!-- pose info from wamv_base.urdf.xacro/${prefix}_float-->
<pose>0 -1.03 0.2 0 1.57 0</pose>
<geometry>
<cylinder>
<radius>0.213</radius>
<length>${length}</length>
</cylinder>
</geometry>
</buoyancy>
</plugin>

<plugin name="usv_buoyancy_${name}_hull_right" filename="libbuoyancy_gazebo_plugin.so">
<!-- Wave model -->
<wave_model>ocean_waves</wave_model>
<fluid_density>997.8</fluid_density>
<fluid_level>0</fluid_level>

<linear_drag>0</linear_drag>
<angular_drag>0</angular_drag>

<buoyancy>
<link_name>${namespace}/base_link</link_name>
<!-- pose info from wamv_base.urdf.xacro/${prefix}_float-->
<pose>0 1.03 0.2 0 1.57 0</pose>
<geometry>
<cylinder>
<radius>0.213</radius>
<length>${length}</length>
</cylinder>
</geometry>
</buoyancy>
</plugin>

</gazebo>
</xacro:macro>
</robot>
2 changes: 2 additions & 0 deletions wamv_gazebo/urdf/macros.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
name="WAM-V">
<!-- Include dynamics macros -->
<xacro:include filename="$(find wamv_gazebo)/urdf/dynamics/wamv_gazebo_dynamics_plugin.xacro" />
<!-- Include buoyancy macros -->
<xacro:include filename="$(find wamv_gazebo)/urdf/buoyancy/wamv_buoyancy_plugin.xacro" />

<!-- Include sensor macros -->
<xacro:include filename="$(find wamv_gazebo)/urdf/components/wamv_camera.xacro" />
Expand Down
2 changes: 2 additions & 0 deletions wamv_gazebo/urdf/wamv_gazebo.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,7 @@
<xacro:include filename="${thruster_layout}"/>
<!-- Attach hydrodynamics plugin -->
<xacro:usv_dynamics_gazebo name="wamv_dynamics_plugin"/>
<!-- Attach buoyancy plugin -->
<xacro:usv_buoyancy_gazebo name="wamv_buoyancy_plugin"/>
</xacro:macro>
</robot>

0 comments on commit cbc9bad

Please sign in to comment.