Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Buoyancy plugin #485

Open
wants to merge 3 commits into
base: gazebo_classic
Choose a base branch
from
Open

Buoyancy plugin #485

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b2661a5
require usv_gazebo_plugins to use c++17
acxz Jul 13, 2022
61c8e3e
require wave_gazebo_plugins to use c++17
acxz Jul 13, 2022
cbc9bad
use buoyancy plugin for the wamv
acxz Jul 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions usv_gazebo_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ catkin_package(
CATKIN_DEPENDS message_runtime gazebo_dev roscpp wave_gazebo_plugins
)

# Plugins require c++11
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
# Plugins require c++17
set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS}")

include_directories( include
${catkin_INCLUDE_DIRS}
Expand Down
74 changes: 0 additions & 74 deletions usv_gazebo_plugins/src/usv_gazebo_dynamics_plugin.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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>
16 changes: 4 additions & 12 deletions wave_gazebo_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,21 +2,13 @@ cmake_minimum_required(VERSION 2.8.3)
project(wave_gazebo_plugins)

###############################################################################
# Compile as C++11, supported in ROS Kinetic and newer

#set(CMAKE_CXX_STANDARD 11)
#set(CMAKE_CXX_STANDARD_REQUIRED ON)

# For this package set as C++14
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX14)
set(CMAKE_CXX_FLAGS "-std=c++14")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "-std=c++0x")
CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
if(COMPILER_SUPPORTS_CXX17)
add_compile_options(-std=c++17)
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler.")
endif()

# Set policy for CMake 3.1+. Use OLD policy to let FindBoost.cmake, dependency
Expand Down