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

Jump to bottom

Debug flag for WaveBodyInteractions plugin #160

Merged
merged 2 commits into from
Sep 6, 2023
Merged

Debug flag for WaveBodyInteractions plugin #160

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
92ead21
add debug flag to sdf for wavebodyinteractions
andermi Sep 1, 2023
7e35271
linters
andermi Sep 5, 2023
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
3 changes: 2 additions & 1 deletion buoy_description/models/mbari_wec/model.sdf.em
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -265,8 +265,9 @@ if not ignore_piston_mean_pos:
<P0>@(print(f'{P0_l:.00f}', end=''))</P0>
</plugin>

<plugin filename="WaveBodyInteractions" name="buoy_gazebo::WaveBodyInteractions">
<plugin filename="WaveBodyInteractions" name="buoy_gazebo::WaveBodyInteractions">
<LinkName>Buoy</LinkName>
<debug>false</debug>
<WaterplaneOrigin_x>0</WaterplaneOrigin_x> <!-- Waterplane origin relative to link origin -->
<WaterplaneOrigin_y>0</WaterplaneOrigin_y>
<WaterplaneOrigin_z>2.46</WaterplaneOrigin_z>
Expand Down
4 changes: 2 additions & 2 deletions buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -360,7 +360,7 @@ void ElectroHydraulicPTO::PreUpdate(
if (i_try > 0) {
std::stringstream warning;
warning << "Warning: Reduced piston to achieve convergence" << std::endl;
igndbg << warning.str();
gzdbg << warning.str();
}

if (solver_info != 1) {
Expand All @@ -369,7 +369,7 @@ void ElectroHydraulicPTO::PreUpdate(
warning << "Warning: Numericals solver in ElectroHydraulicPTO did not converge" << std::endl;
warning << "solver info: [" << solver_info << "]" << std::endl;
warning << "=================================" << std::endl;
igndbg << warning.str();
gzdbg << warning.str();
}

// Solve Electrical
Expand Down
67 changes: 37 additions & 30 deletions buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,11 @@

namespace buoy_gazebo
{

static bool WBI_DBG_FLAG{false};
#define wbidbg if (WBI_DBG_FLAG) gzdbg


class WaveBodyInteractionsPrivate
{
public:
Expand Down Expand Up @@ -98,24 +103,26 @@ void WaveBodyInteractions::Configure(
{
this->dataPtr->model = gz::sim::Model(_entity);
if (!this->dataPtr->model.Valid(_ecm)) {
ignerr
gzerr
<< "WaveBodyInteractions plugin should be attached to a model entity. "
<< "Failed to initialize." << std::endl;
return;
}

// Get params from SDF.
if (!_sdf->HasElement("LinkName")) {
ignerr << "You musk specify a <LinkName> for the wavebodyinteraction "
gzerr << "You musk specify a <LinkName> for the wavebodyinteraction "
"plugin to act upon"
<< "Failed to initialize." << std::endl;
<< "Failed to initialize." << std::endl;
return;
}
auto linkName = _sdf->Get<std::string>("LinkName");

WBI_DBG_FLAG = _sdf->Get<bool>("debug", false).first;

this->dataPtr->linkEntity = this->dataPtr->model.LinkByName(_ecm, linkName);
if (!_ecm.HasEntity(this->dataPtr->linkEntity)) {
ignerr << "Link name" << linkName << "does not exist";
gzerr << "Link name" << linkName << "does not exist";
return;
}

Expand Down Expand Up @@ -163,7 +170,7 @@ void WaveBodyInteractions::Configure(
ament_index_cpp::get_package_share_directory(package_share_dir) +
base_filenm;
} else {
ignerr << "Only package: URI scheme has been implemented" << std::endl;
gzerr << "Only package: URI scheme has been implemented" << std::endl;
return;
}

Expand All @@ -186,12 +193,12 @@ void WaveBodyInteractions::PreUpdate(
if (_info.iterations == 1) { // First iteration, set timestep size.
double dt = std::chrono::duration<double>(_info.dt).count();
dataPtr->FloatingBody.SetTimestepSize(dt);
gzdbg << " Set Wave Forcing timestep size: dt = " << dt << std::endl;
wbidbg << " Set Wave Forcing timestep size: dt = " << dt << std::endl;
}

// \TODO(anyone): Support rewind
if (_info.dt < std::chrono::steady_clock::duration::zero()) {
ignwarn
gzwarn
<< "Detected jump back in time ["
<< std::chrono::duration_cast<std::chrono::seconds>(_info.dt).count()
<< "s]. System may not work properly." << std::endl;
Expand All @@ -216,7 +223,7 @@ void WaveBodyInteractions::PreUpdate(

gz::sim::Link baseLink(this->dataPtr->linkEntity);

gzdbg << "baseLink.Name = " << baseLink.Name(_ecm).value() << std::endl;
wbidbg << "baseLink.Name = " << baseLink.Name(_ecm).value() << std::endl;

auto w_xddot_b = baseLink.WorldLinearAcceleration(_ecm).value();
auto w_omegadot_b = baseLink.WorldAngularAcceleration(_ecm).value();
Expand All @@ -225,19 +232,19 @@ void WaveBodyInteractions::PreUpdate(

auto w_Pose_b = gz::sim::worldPose(this->dataPtr->linkEntity, _ecm);
auto w_Pose_p = w_Pose_b * this->dataPtr->b_Pose_p;
gzdbg << "w_Pose_b = " << w_Pose_b << std::endl;
gzdbg << "w_Pose_p = " << w_Pose_p << std::endl;
wbidbg << "w_Pose_b = " << w_Pose_b << std::endl;
wbidbg << "w_Pose_p = " << w_Pose_p << std::endl;

// gz::math::Vector3<double> p_xdot = w_Pose_p.Rot().Inverse() * *w_xdot;
gz::math::Vector3<double> w_xdot_p =
w_xdot_b +
w_omega_b.Cross(w_Pose_b.Rot() * this->dataPtr->b_Pose_p.Pos());
gz::math::Vector3<double> w_omega_p =
w_omega_b; // Waterplane Coord sys is parallel to body C.S.
gzdbg << "w_xdot_b = " << w_xdot_b << std::endl;
gzdbg << "w_xdot_p = " << w_xdot_p << std::endl;
gzdbg << "w_omega_b = " << w_omega_b << std::endl;
gzdbg << "w_omega_p = " << w_omega_p << std::endl;
wbidbg << "w_xdot_b = " << w_xdot_b << std::endl;
wbidbg << "w_xdot_p = " << w_xdot_p << std::endl;
wbidbg << "w_omega_b = " << w_omega_b << std::endl;
wbidbg << "w_omega_p = " << w_omega_p << std::endl;

gz::math::Vector3<double> w_xddot_p =
w_xddot_b +
Expand All @@ -246,10 +253,10 @@ void WaveBodyInteractions::PreUpdate(
w_omega_b.Cross(w_Pose_b.Rot() * this->dataPtr->b_Pose_p.Pos()));
gz::math::Vector3<double> w_omegadot_p =
w_omegadot_b; // Waterplane Coord sys is parallel to body C.S.
gzdbg << "w_xddot_b = " << w_xddot_b << std::endl;
gzdbg << "w_xddot_p = " << w_xddot_p << std::endl;
gzdbg << "w_omegadot_b = " << w_omegadot_b << std::endl;
gzdbg << "w_omegadot_p = " << w_omegadot_p << std::endl;
wbidbg << "w_xddot_b = " << w_xddot_b << std::endl;
wbidbg << "w_xddot_p = " << w_xddot_p << std::endl;
wbidbg << "w_omegadot_b = " << w_omegadot_b << std::endl;
wbidbg << "w_omegadot_p = " << w_omegadot_p << std::endl;
gz::math::Vector3<double> b_xddot_p = w_Pose_p.Rot().Inverse() * w_xddot_p;
gz::math::Vector3<double> b_omegadot_p =
w_Pose_p.Rot().Inverse() * w_omegadot_p;
Expand All @@ -260,11 +267,11 @@ void WaveBodyInteractions::PreUpdate(
Eigen::VectorXd x(6);
x << w_Pose_p.X(), w_Pose_p.Y(), w_Pose_p.Z(), w_Pose_p.Roll(),
w_Pose_p.Pitch(), w_Pose_p.Yaw();
gzdbg << "x(6) = " << x.transpose() << std::endl;
wbidbg << "x(6) = " << x.transpose() << std::endl;
Eigen::VectorXd BuoyancyForce(6);
BuoyancyForce = this->dataPtr->FloatingBody.BuoyancyForce(x);
gzdbg << "Buoyancy Force at waterplane = " << BuoyancyForce.transpose()
<< std::endl;
wbidbg << "Buoyancy Force at waterplane = " << BuoyancyForce.transpose()
<< std::endl;

// Compute Buoyancy Force
gz::math::Vector3d w_FBp(BuoyancyForce(0), BuoyancyForce(1),
Expand All @@ -277,7 +284,7 @@ void WaveBodyInteractions::PreUpdate(
// Add contribution due to force offset from origin
w_MBp +=
(w_Pose_b.Rot().RotateVector(this->dataPtr->b_Pose_p.Pos())).Cross(w_FBp);
gzdbg << "Buoyancy: applied moment = " << w_MBp << std::endl;
wbidbg << "Buoyancy: applied moment = " << w_MBp << std::endl;

// Compute Memory part of Radiation Force based on accelerations in the body
// frame, result is in body frame
Expand All @@ -292,14 +299,14 @@ void WaveBodyInteractions::PreUpdate(
// xddot << 0.0, 0.0, 0.0, b_omegadot_p.X(), b_omegadot_p.Y(),
// b_omegadot_p.Z();

gzdbg << "xddot = " << xddot.transpose() << std::endl;
gzdbg << "xddot = " << xddot.transpose() << std::endl;
wbidbg << "xddot = " << xddot.transpose() << std::endl;
wbidbg << "xddot = " << xddot.transpose() << std::endl;
Eigen::VectorXd MemForce(6);
// Note negative sign, FS_Hydrodynamics returns force required to move body in
// prescribed way,
// force of water on body is opposite.
MemForce = -this->dataPtr->FloatingBody.RadiationForce(xddot);
gzdbg << " MemForce at Waterplane = " << MemForce.transpose() << std::endl;
wbidbg << " MemForce at Waterplane = " << MemForce.transpose() << std::endl;
gz::math::Vector3d w_FRp(MemForce(0), MemForce(1), MemForce(2));
// Needs to be adjusted for yaw only becaues of small pitch/roll assumption
gz::math::Vector3d w_MRp(
Expand All @@ -310,22 +317,22 @@ void WaveBodyInteractions::PreUpdate(
// Add contribution due to force offset from origin
w_MRp +=
(w_Pose_b.Rot().RotateVector(this->dataPtr->b_Pose_p.Pos())).Cross(w_FRp);
gzdbg << "Radiation: applied moment = " << w_MRp << std::endl;
wbidbg << "Radiation: applied moment = " << w_MRp << std::endl;

gzdbg << std::endl;
wbidbg << std::endl;
// Compute Wave Exciting Force
Eigen::VectorXd ExtForce(6);
ExtForce = this->dataPtr->FloatingBody.ExcitingForce();
gzdbg << "Exciting Force = " << ExtForce.transpose() << std::endl;
wbidbg << "Exciting Force = " << ExtForce.transpose() << std::endl;
gz::math::Vector3d w_FEp(ExtForce(0), ExtForce(1), ExtForce(2));
// Needs to be adjusted for yaw only
gz::math::Vector3d w_MEp(
1 * (cos(x(5)) * ExtForce(3) - sin(x(5)) * ExtForce(4)),
1 * (sin(x(5)) * ExtForce(3) + cos(x(5)) * ExtForce(4)),
ExtForce(5)); // Needs to be adjusted for yaw only

gzdbg << "Exciting: applied force = " << w_FEp << std::endl;
gzdbg << "Exciting: applied moment = " << w_MEp << std::endl;
wbidbg << "Exciting: applied force = " << w_FEp << std::endl;
wbidbg << "Exciting: applied moment = " << w_MEp << std::endl;

// Add contribution due to force offset from origin
w_MEp +=
Expand Down
Loading