Skip to content
New issue

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

#

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

Already on GitHub? # to your account

Add wave body total power to latent data #165

Merged
merged 7 commits into from
Sep 21, 2023
Merged

Add wave body total power to latent data #165

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
d9510c7
Merge branch 'main' into hamilton_8415/wavebody_latent_data
andermi Sep 19, 2023
f9315c9
added wave_body total forces
andermi Sep 19, 2023
06b5085
Merge branch 'andermi/latent_data' into hamilton_8415/wavebody_latent…
andermi Sep 20, 2023
3d61b43
added buoy pose and twist to latent data
andermi Sep 20, 2023
c06cd64
added pose/twist to operator== for latent data
andermi Sep 20, 2023
af4b4b1
init pose/twist
andermi Sep 20, 2023
0ab976d
linters
andermi Sep 20, 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
11 changes: 11 additions & 0 deletions buoy_gazebo/src/LatentData/LatentData/LatentData.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,22 +148,33 @@ struct WaveBody
{
bool valid{false};

gz::math::Pose3<double> pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
gz::math::Pose3<double> twist{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

gz::math::Vector3d buoyant_force{0.0, 0.0, 0.0};
gz::math::Vector3d buoyant_moment{0.0, 0.0, 0.0};
double buoyancy_total_power{0.0};
gz::math::Vector3d radiation_force{0.0, 0.0, 0.0};
gz::math::Vector3d radiation_moment{0.0, 0.0, 0.0};
double radiation_total_power{0.0};
gz::math::Vector3d exciting_force{0.0, 0.0, 0.0};
gz::math::Vector3d exciting_moment{0.0, 0.0, 0.0};
double excitation_total_power{0.0};

bool operator==(const WaveBody & that) const
{
bool equal = (this->valid == that.valid);
equal &= (this->pose == that.pose);
equal &= (this->twist == that.twist);
equal &= (this->buoyant_force == that.buoyant_force);
equal &= (this->buoyant_moment == that.buoyant_moment);
equal &= fabs(this->buoyancy_total_power - that.buoyancy_total_power) < 1e-7F;
equal &= (this->radiation_force == that.radiation_force);
equal &= (this->radiation_moment == that.radiation_moment);
equal &= fabs(this->radiation_total_power - that.radiation_total_power) < 1e-7F;
equal &= (this->exciting_force == that.exciting_force);
equal &= (this->exciting_moment == that.exciting_moment);
equal &= fabs(this->excitation_total_power - that.excitation_total_power) < 1e-7F;

return equal;
}
Expand Down
38 changes: 36 additions & 2 deletions buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -294,6 +294,34 @@ void LatentDataPublisher::PostUpdate(
this->dataPtr->latent_data_.electro_hydraulic.battery_storage_power =
latent_data.electro_hydraulic.battery_storage_power;

this->dataPtr->latent_data_.wave_body.pose.position.x =
latent_data.wave_body.pose.X();
this->dataPtr->latent_data_.wave_body.pose.position.y =
latent_data.wave_body.pose.Y();
this->dataPtr->latent_data_.wave_body.pose.position.z =
latent_data.wave_body.pose.Z();
this->dataPtr->latent_data_.wave_body.pose.orientation.x =
latent_data.wave_body.pose.Rot().X();
this->dataPtr->latent_data_.wave_body.pose.orientation.y =
latent_data.wave_body.pose.Rot().Y();
this->dataPtr->latent_data_.wave_body.pose.orientation.z =
latent_data.wave_body.pose.Rot().Z();
this->dataPtr->latent_data_.wave_body.pose.orientation.w =
latent_data.wave_body.pose.Rot().W();

this->dataPtr->latent_data_.wave_body.twist.linear.x =
latent_data.wave_body.twist.X();
this->dataPtr->latent_data_.wave_body.twist.linear.y =
latent_data.wave_body.twist.Y();
this->dataPtr->latent_data_.wave_body.twist.linear.z =
latent_data.wave_body.twist.Z();
this->dataPtr->latent_data_.wave_body.twist.angular.x =
latent_data.wave_body.twist.Roll();
this->dataPtr->latent_data_.wave_body.twist.angular.y =
latent_data.wave_body.twist.Pitch();
this->dataPtr->latent_data_.wave_body.twist.angular.z =
latent_data.wave_body.twist.Yaw();

this->dataPtr->latent_data_.wave_body.buoyancy.force.x =
latent_data.wave_body.buoyant_force.X();
this->dataPtr->latent_data_.wave_body.buoyancy.force.y =
Expand All @@ -306,6 +334,9 @@ void LatentDataPublisher::PostUpdate(
latent_data.wave_body.buoyant_moment.Y();
this->dataPtr->latent_data_.wave_body.buoyancy.torque.z =
latent_data.wave_body.buoyant_moment.Z();
this->dataPtr->latent_data_.wave_body.buoyancy_total_power =
latent_data.wave_body.buoyancy_total_power;

this->dataPtr->latent_data_.wave_body.radiation.force.x =
latent_data.wave_body.radiation_force.X();
this->dataPtr->latent_data_.wave_body.radiation.force.y =
Expand All @@ -318,6 +349,9 @@ void LatentDataPublisher::PostUpdate(
latent_data.wave_body.radiation_moment.Y();
this->dataPtr->latent_data_.wave_body.radiation.torque.z =
latent_data.wave_body.radiation_moment.Z();
this->dataPtr->latent_data_.wave_body.radiation_total_power =
latent_data.wave_body.radiation_total_power;

this->dataPtr->latent_data_.wave_body.excitation.force.x =
latent_data.wave_body.exciting_force.X();
this->dataPtr->latent_data_.wave_body.excitation.force.y =
Expand All @@ -330,12 +364,12 @@ void LatentDataPublisher::PostUpdate(
latent_data.wave_body.exciting_moment.Y();
this->dataPtr->latent_data_.wave_body.excitation.torque.z =
latent_data.wave_body.exciting_moment.Z();
this->dataPtr->latent_data_.wave_body.excitation_total_power =
latent_data.wave_body.excitation_total_power;

this->dataPtr->latent_data_.piston_friction_force =
latent_data.piston_friction_force;

// TODO(andermi) fill in other stuff

this->dataPtr->data_valid_ = latent_data.valid();

data.unlock();
Expand Down
8 changes: 8 additions & 0 deletions buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -362,12 +362,20 @@ void WaveBodyInteractions::PreUpdate(
}

latent_data.wave_body.valid = true;
latent_data.wave_body.pose = w_Pose_p;
latent_data.wave_body.twist =
gz::math::Pose3<double>(
w_xdot_p.X(), w_xdot_p.Y(), w_xdot_p.Z(),
w_omega_p.X(), w_omega_p.Y(), w_omega_p.Z());
latent_data.wave_body.buoyant_force = w_FBp;
latent_data.wave_body.buoyant_moment = w_MBp;
latent_data.wave_body.buoyancy_total_power = w_FBp.Dot(w_xdot_p) + w_MBp.Dot(w_omega_p);
latent_data.wave_body.radiation_force = w_FRp;
latent_data.wave_body.radiation_moment = w_MRp;
latent_data.wave_body.radiation_total_power = w_FRp.Dot(w_xdot_p) + w_MRp.Dot(w_omega_p);
latent_data.wave_body.exciting_force = w_FEp;
latent_data.wave_body.exciting_moment = w_MEp;
latent_data.wave_body.excitation_total_power = w_FEp.Dot(w_xdot_p) + w_MEp.Dot(w_omega_p);

_ecm.SetComponentData<buoy_gazebo::components::LatentData>(
this->dataPtr->model.Entity(),
Expand Down