From f9315c948355d255f57aa2128b5e534a9ddd5bd9 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Tue, 19 Sep 2023 16:00:23 -0700 Subject: [PATCH 1/5] added wave_body total forces Signed-off-by: Michael Anderson --- buoy_gazebo/src/LatentData/LatentData/LatentData.hpp | 6 ++++++ .../src/LatentData/LatentData/LatentDataPublisher.cpp | 8 ++++++++ .../src/WaveBodyInteractions/WaveBodyInteractions.cpp | 3 +++ 3 files changed, 17 insertions(+) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index a778ac43..6ece0763 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -150,20 +150,26 @@ struct WaveBody 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->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; } diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index e313a839..e17138bd 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -306,6 +306,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 = @@ -318,6 +321,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 = @@ -330,6 +336,8 @@ 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; diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index a9b6eb0b..0d4659e0 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -364,10 +364,13 @@ void WaveBodyInteractions::PreUpdate( latent_data.wave_body.valid = true; 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( this->dataPtr->model.Entity(), From 3d61b4361279ca6992845dbd712c07f17f7dc6f5 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 20 Sep 2023 10:33:30 -0700 Subject: [PATCH 2/5] added buoy pose and twist to latent data Signed-off-by: Michael Anderson --- .../src/LatentData/LatentData/LatentData.hpp | 3 ++ .../LatentData/LatentDataPublisher.cpp | 32 +++++++++++++++++-- .../WaveBodyInteractions.cpp | 5 +++ 3 files changed, 37 insertions(+), 3 deletions(-) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 6ece0763..b60fdf7b 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -148,6 +148,9 @@ struct WaveBody { bool valid{false}; + gz::math::Pose3 pose; + gz::math::Pose3 twist; + 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}; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index e17138bd..666a0c56 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -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 = @@ -308,7 +336,7 @@ void LatentDataPublisher::PostUpdate( 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 = @@ -342,8 +370,6 @@ void LatentDataPublisher::PostUpdate( 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(); diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index 0d4659e0..d16d813d 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -362,6 +362,11 @@ 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( + 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); From c06cd64c53edaf940138a393b5689502e4da7614 Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 20 Sep 2023 10:36:58 -0700 Subject: [PATCH 3/5] added pose/twist to operator== for latent data Signed-off-by: Michael Anderson --- buoy_gazebo/src/LatentData/LatentData/LatentData.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index b60fdf7b..0ab8afab 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -164,6 +164,8 @@ struct WaveBody 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; From af4b4b1f19b68a79c2ab97a9e881b6724928bbda Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 20 Sep 2023 11:44:54 -0700 Subject: [PATCH 4/5] init pose/twist Signed-off-by: Michael Anderson --- buoy_gazebo/src/LatentData/LatentData/LatentData.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 0ab8afab..d2016d65 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -148,8 +148,8 @@ struct WaveBody { bool valid{false}; - gz::math::Pose3 pose; - gz::math::Pose3 twist; + gz::math::Pose3 pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + gz::math::Pose3 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}; From 0ab976d996eb58f40f61940da6693a1ed686468c Mon Sep 17 00:00:00 2001 From: Michael Anderson Date: Wed, 20 Sep 2023 11:50:50 -0700 Subject: [PATCH 5/5] linters Signed-off-by: Michael Anderson --- buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index d16d813d..3b822159 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -365,8 +365,8 @@ void WaveBodyInteractions::PreUpdate( latent_data.wave_body.pose = w_Pose_p; latent_data.wave_body.twist = gz::math::Pose3( - w_xdot_p.X(), w_xdot_p.Y(), w_xdot_p.Z(), - w_omega_p.X(), w_omega_p.Y(), w_omega_p.Z()); + 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);