diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index a778ac43..d2016d65 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -148,22 +148,33 @@ struct WaveBody { bool valid{false}; + 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}; + 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; } diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index e313a839..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 = @@ -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 = @@ -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 = @@ -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(); diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index a9b6eb0b..3b822159 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -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( + 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( this->dataPtr->model.Entity(),