diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 9d10afc4..96effdee 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -34,6 +34,10 @@ cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC wget https://raw.githubusercontent.com/osrf/mbari_wec/main/mbari_wec_all.yaml vcs import --skip-existing < mbari_wec_all.yaml +cd mbari_wec_utils +git checkout andermi/latent_data +cd .. + rosdep init rosdep update rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO diff --git a/buoy_description/models/mbari_wec/model.sdf.em b/buoy_description/models/mbari_wec/model.sdf.em index 12b07c27..862fab1d 100644 --- a/buoy_description/models/mbari_wec/model.sdf.em +++ b/buoy_description/models/mbari_wec/model.sdf.em @@ -265,8 +265,9 @@ if not ignore_piston_mean_pos: @(print(f'{P0_l:.00f}', end='')) - + Buoy + false 0 0 2.46 diff --git a/buoy_description/models/mbari_wec_ros/model.sdf b/buoy_description/models/mbari_wec_ros/model.sdf index eb72fef3..f2825d41 100644 --- a/buoy_description/models/mbari_wec_ros/model.sdf +++ b/buoy_description/models/mbari_wec_ros/model.sdf @@ -64,15 +64,10 @@ / inc_wave_service - -1.0 -1.0 - 0.0 -1.0 - 1.0 -1.0 - -1.0 0.0 0.0 0.0 - 1.0 0.0 - -1.0 1.0 - 0.0 1.0 - 1.0 1.0 + + + diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt b/buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt index f136789e..759aa802 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt +++ b/buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt @@ -3,6 +3,7 @@ gz_add_plugin(ElectroHydraulicPTO ElectroHydraulicPTO.cpp INCLUDE_DIRS .. + ../LatentData EXTRA_ROS_PKGS simple_interp ) diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicLoss.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicLoss.hpp deleted file mode 100644 index 1dffc02d..00000000 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicLoss.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ELECTROHYDRAULICPTO__ELECTROHYDRAULICLOSS_HPP_ -#define ELECTROHYDRAULICPTO__ELECTROHYDRAULICLOSS_HPP_ - - -#include -#include -#include -#include - - -namespace buoy_gazebo -{ - -/// \brief State data for power commands and feedback from sensors for PCRecord message in ROS2 -struct ElectroHydraulicLoss -{ - float hydraulic_motor_loss{0.0F}; - float relief_valve_loss{0.0F}; - float motor_drive_i2r_loss{0.0F}; - float motor_drive_switching_loss{0.0F}; - float motor_drive_friction_loss{0.0F}; - float battery_i2r_loss{0.0F}; - - bool operator==(const ElectroHydraulicLoss & that) const - { - bool equal = fabs(this->hydraulic_motor_loss - that.hydraulic_motor_loss) < 1e-7F; - equal &= fabs(this->relief_valve_loss - that.relief_valve_loss) < 1e-7F; - equal &= fabs(this->motor_drive_i2r_loss - that.motor_drive_i2r_loss) < 1e-7F; - equal &= fabs(this->motor_drive_switching_loss - that.motor_drive_switching_loss) < 1e-7F; - equal &= fabs(this->motor_drive_friction_loss - that.motor_drive_friction_loss) < 1e-7F; - equal &= fabs(this->battery_i2r_loss - that.battery_i2r_loss) < 1e-7F; - return equal; - } -}; - -namespace components -{ -/// \brief State data as component for reporting via ROS2 -using ElectroHydraulicLoss = - gz::sim::components::Component; -GZ_SIM_REGISTER_COMPONENT( - "buoy_gazebo.components.ElectroHydraulicLoss", - ElectroHydraulicLoss) -} // namespace components - -} // namespace buoy_gazebo - -#endif // ELECTROHYDRAULICPTO__ELECTROHYDRAULICLOSS_HPP_ diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 39e1f3b4..fa927ca9 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "ElectroHydraulicPTO.hpp" + #include #include @@ -40,12 +42,12 @@ #include #include -#include "ElectroHydraulicPTO.hpp" #include "ElectroHydraulicSoln.hpp" #include "ElectroHydraulicState.hpp" -#include "ElectroHydraulicLoss.hpp" #include "BatteryState.hpp" +#include + namespace buoy_gazebo { @@ -262,16 +264,15 @@ void ElectroHydraulicPTO::PreUpdate( pto_state = buoy_gazebo::ElectroHydraulicState(pto_state_comp->Data()); } - buoy_gazebo::ElectroHydraulicLoss pto_loss; + buoy_gazebo::LatentData latent_data; if (_ecm.EntityHasComponentType( - this->dataPtr->PrismaticJointEntity, - buoy_gazebo::components::ElectroHydraulicLoss().TypeId())) + this->dataPtr->model.Entity(), + buoy_gazebo::components::LatentData().TypeId())) { - auto pto_loss_comp = - _ecm.Component( - this->dataPtr->PrismaticJointEntity); + auto latent_data_comp = + _ecm.Component(this->dataPtr->model.Entity()); - pto_loss = buoy_gazebo::ElectroHydraulicLoss(pto_loss_comp->Data()); + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); } if (pto_state.scale_command) { @@ -346,7 +347,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) { @@ -355,13 +356,16 @@ 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 const double N = this->dataPtr->x[0U]; double deltaP = this->dataPtr->x[1U]; double VBus = this->dataPtr->x[2U]; + const double eff_m = this->dataPtr->functor.hyd_eff_m.eval(fabs(N)); + const double eff_v = this->dataPtr->functor.hyd_eff_v.eval(fabs(deltaP)); + VBus = std::min(VBus, this->dataPtr->MaxTargetVoltage); double BusPower = this->dataPtr->functor.BusPower; @@ -404,12 +408,35 @@ void ElectroHydraulicPTO::PreUpdate( pto_state.target_a = this->dataPtr->functor.I_Wind.I; - pto_loss.hydraulic_motor_loss += 1.0; - pto_loss.relief_valve_loss += 2.0; - pto_loss.motor_drive_i2r_loss += 3.0; - pto_loss.motor_drive_switching_loss += 4.0; - pto_loss.motor_drive_friction_loss += 5.0; - pto_loss.battery_i2r_loss = I_Batt * I_Batt * this->dataPtr->Ri; + double piston_force = -deltaP * this->dataPtr->PistonArea * buoy_utils::NEWTONS_PER_LB; + + latent_data.electro_hydraulic.valid = true; + latent_data.electro_hydraulic.rpm = N; + latent_data.electro_hydraulic.upper_hydraulic_pressure = + pto_state.upper_hyd_press * buoy_utils::PASCAL_PER_PSI; + latent_data.electro_hydraulic.lower_hydraulic_pressure = + pto_state.lower_hyd_press * buoy_utils::PASCAL_PER_PSI; + latent_data.electro_hydraulic.force = piston_force; + latent_data.electro_hydraulic.supplied_hydraulic_power = + -deltaP * this->dataPtr->functor.Q / buoy_utils::INLB_PER_NM; + latent_data.electro_hydraulic.hydraulic_motor_loss = + this->dataPtr->functor.HydraulicMotorLoss; + latent_data.electro_hydraulic.relief_valve_loss = + this->dataPtr->functor.ReliefValveLoss; + latent_data.electro_hydraulic.shaft_mech_power = + this->dataPtr->functor.ShaftMechPower; + latent_data.electro_hydraulic.motor_drive_i2r_loss = + this->dataPtr->functor.I2RLoss; + latent_data.electro_hydraulic.motor_drive_switching_loss = + this->dataPtr->functor.SwitchingLoss; + latent_data.electro_hydraulic.motor_drive_friction_loss = + this->dataPtr->functor.FrictionLoss; + latent_data.electro_hydraulic.load_dump_power = + I_Load * VBus; + latent_data.electro_hydraulic.battery_i2r_loss = + I_Batt * I_Batt * this->dataPtr->Ri; + latent_data.electro_hydraulic.battery_storage_power = + I_Batt * VBus - latent_data.electro_hydraulic.battery_i2r_loss; _ecm.SetComponentData( this->dataPtr->PrismaticJointEntity, @@ -421,9 +448,9 @@ void ElectroHydraulicPTO::PreUpdate( auto stampMsg = gz::sim::convert(_info.simTime); - _ecm.SetComponentData( - this->dataPtr->PrismaticJointEntity, - pto_loss); + _ecm.SetComponentData( + this->dataPtr->model.Entity(), + latent_data); gz::msgs::Double pistonvel; pistonvel.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); @@ -437,7 +464,6 @@ void ElectroHydraulicPTO::PreUpdate( // Apply force if not in Velocity Mode, in which case a joint velocity is applied elsewhere // (likely by a test Fixture) if (!this->dataPtr->VelMode) { - double piston_force = -deltaP * this->dataPtr->PistonArea * buoy_utils::NEWTONS_PER_LB; // Create new component for this entitiy in ECM (if it doesn't already exist) auto forceComp = _ecm.Component( this->dataPtr->PrismaticJointEntity); diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp index dde5ef4d..1b25f510 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp @@ -110,6 +110,13 @@ struct ElectroHydraulicSoln : Functor mutable double BusPower; double Q; + mutable double ShaftMechPower; + mutable double FrictionLoss; + mutable double SwitchingLoss; + mutable double I2RLoss; + mutable double ReliefValveLoss; + mutable double HydraulicMotorLoss; + private: static const std::vector Peff; // psi static const std::vector Neff; // rpm @@ -170,12 +177,12 @@ struct ElectroHydraulicSoln : Functor const double T_applied = 1.375 * this->I_Wind.TorqueConstantInLbPerAmp * WindCurr; - double ShaftMechPower = T_applied * buoy_utils::NM_PER_INLB * + ShaftMechPower = -T_applied * buoy_utils::NM_PER_INLB * x[0U] * buoy_utils::RPM_TO_RAD_PER_SEC; - BusPower = -ShaftMechPower - ( - MotorDriveFrictionLoss(x[0U]) + - MotorDriveSwitchingLoss(x[2U]) + - MotorDriveISquaredRLoss(WindCurr)); + FrictionLoss = MotorDriveFrictionLoss(x[0U]); + SwitchingLoss = MotorDriveSwitchingLoss(x[2U]); + I2RLoss = MotorDriveISquaredRLoss(WindCurr); + BusPower = ShaftMechPower - (FrictionLoss + SwitchingLoss + I2RLoss); double Q_Relief = 0; if (x[1U] < -PressReliefSetPoint) { // Pressure relief is a one wave valve, // relieves when lower pressure is higher @@ -183,15 +190,19 @@ struct ElectroHydraulicSoln : Functor Q_Relief = (x[1U] + PressReliefSetPoint) * ReliefValveFlowPerPSI * buoy_utils::CubicInchesPerGallon / buoy_utils::SecondsPerMinute; } -// Q_Relief = 0; + ReliefValveLoss = Q_Relief * x[1U] / buoy_utils::INLB_PER_NM; // Result is Watts + double Q_Motor = this->Q - Q_Relief; - double Q = x[0U] * this->HydMotorDisp / buoy_utils::SecondsPerMinute; - double Q_Leak = (1.0 - eff_v) * std::max(fabs(Q_Motor), fabs(Q)) * sgn(x[1]); + double Q_Ideal = x[0U] * this->HydMotorDisp / buoy_utils::SecondsPerMinute; + double Q_Leak = (1.0 - eff_v) * std::max(fabs(Q_Motor), fabs(Q_Ideal)) * sgn(x[1]); double T_Fluid = x[1U] * this->HydMotorDisp / (2.0 * M_PI); double T_Friction = -(1.0 - eff_m) * std::max(fabs(T_applied), fabs(T_Fluid)) * sgn(x[0]); - fvec[0U] = Q_Motor - Q_Leak - Q; + HydraulicMotorLoss = Q_Leak * x[1U] / buoy_utils::INLB_PER_NM - // Result is Watts + T_Friction * x[0U] * 2.0 * M_PI / (buoy_utils::INLB_PER_NM * buoy_utils::SecondsPerMinute); + + fvec[0U] = Q_Motor - Q_Leak - Q_Ideal; fvec[1U] = T_applied + T_Friction + T_Fluid; fvec[2U] = BusPower - (x[2U] - VBattEMF) * x[2U] / this->Ri; diff --git a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp index 97751fd7..db03b76e 100644 --- a/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp +++ b/buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp @@ -334,6 +334,8 @@ void IncWaveHeight::PreUpdate( latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); } + latent_data.inc_wave_heights.valid = this->dataPtr->inc_wave_valid_; + double SimTime = std::chrono::duration(this->dataPtr->current_time_).count(); auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_); // all fixed points from SDF computed at SimTime (relative_time = 0.0) diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index 095a57ee..d2016d65 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -17,6 +17,8 @@ #include +#include + #include #include #include @@ -36,6 +38,7 @@ struct IncWaveHeightPoint double eta{0.0}; // output // ==== orientation ==== + // normal vector of deta/dx, deta/dy (slope of wave) double qx{0.0}, qy{0.0}, qz{0.0}, qw{0.0}; // output bool operator==(const IncWaveHeightPoint & that) const @@ -55,12 +58,14 @@ struct IncWaveHeights { int32_t sec{0}; uint32_t nsec{0U}; + bool valid; std::vector points; bool operator==(const IncWaveHeights & that) const { // shortcut different sizes as not equal bool equal = (this->points.size() == that.points.size()); + equal &= this->valid == that.valid; if (!equal) { return false; } @@ -75,15 +80,140 @@ struct IncWaveHeights } }; +struct AirSpring +{ + bool valid{false}; + double force{0.0}; // Newtons + double T{0.0}; // temp in K + double dQ_dt{0.0}; // heat loss rate + double piston_position{0.0}; // meters + double piston_velocity{0.0}; // m/s + double mass{0.0}; // kg + + bool operator==(const AirSpring & that) const + { + bool equal = (this->valid == that.valid); + equal &= fabs(this->force - that.force) < 1e-7F; + equal &= fabs(this->T - that.T) < 1e-7F; + equal &= fabs(this->dQ_dt - that.dQ_dt) < 1e-7F; + equal &= fabs(this->piston_position - that.piston_position) < 1e-7F; + equal &= fabs(this->piston_velocity - that.piston_velocity) < 1e-7F; + equal &= fabs(this->mass - that.mass) < 1e-7F; + + return equal; + } +}; + +struct ElectroHydraulic +{ + bool valid{false}; + double rpm{0.0}; + double upper_hydraulic_pressure{0.0}; // Pa + double lower_hydraulic_pressure{0.0}; // Pa + double force{0.0}; // Newtons + double supplied_hydraulic_power{0.0}; // Watts + double hydraulic_motor_loss{0.0}; // Watts + double relief_valve_loss{0.0}; // Watts + double shaft_mech_power{0.0}; // Watts + double motor_drive_i2r_loss{0.0}; // Watts + double motor_drive_switching_loss{0.0}; // Watts + double motor_drive_friction_loss{0.0}; // Watts + double load_dump_power{0.0}; // Watts + double battery_i2r_loss{0.0}; // Watts + double battery_storage_power{0.0}; // Watts + + bool operator==(const ElectroHydraulic & that) const + { + bool equal = (this->valid == that.valid); + equal &= fabs(this->rpm - that.rpm) < 1e-7F; + equal &= fabs(this->upper_hydraulic_pressure - that.upper_hydraulic_pressure) < 1e-7F; + equal &= fabs(this->lower_hydraulic_pressure - that.lower_hydraulic_pressure) < 1e-7F; + equal &= fabs(this->force - that.force) < 1e-7F; + equal &= fabs(this->supplied_hydraulic_power - that.supplied_hydraulic_power) < 1e-7F; + equal &= fabs(this->hydraulic_motor_loss - that.hydraulic_motor_loss) < 1e-7F; + equal &= fabs(this->relief_valve_loss - that.relief_valve_loss) < 1e-7F; + equal &= fabs(this->shaft_mech_power - that.shaft_mech_power) < 1e-7F; + equal &= fabs(this->motor_drive_i2r_loss - that.motor_drive_i2r_loss) < 1e-7F; + equal &= fabs(this->motor_drive_switching_loss - that.motor_drive_switching_loss) < 1e-7F; + equal &= fabs(this->motor_drive_friction_loss - that.motor_drive_friction_loss) < 1e-7F; + equal &= fabs(this->load_dump_power - that.load_dump_power) < 1e-7F; + equal &= fabs(this->battery_i2r_loss - that.battery_i2r_loss) < 1e-7F; + equal &= fabs(this->battery_storage_power - that.battery_storage_power) < 1e-7F; + + return equal; + } +}; + +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; + } +}; + /// \brief latent data that is modeled but not directly observed for LatentData message in ROS 2 struct LatentData { IncWaveHeights inc_wave_heights; + AirSpring upper_spring; + AirSpring lower_spring; + ElectroHydraulic electro_hydraulic; + WaveBody wave_body; + + double piston_friction_force_valid; + double piston_friction_force; // Newtons + + bool valid() const + { + /* std::cout << "iwh[" << inc_wave_heights.valid << "] :: " + << "us[" << upper_spring.valid << "] :: " + << "ls[" << lower_spring.valid << "] :: " + << "wb[" << wave_body.valid << "] :: " + << "friction[" << piston_friction_force_valid << "]" << std::endl; */ + return inc_wave_heights.valid && \ + upper_spring.valid && lower_spring.valid && \ + electro_hydraulic.valid && wave_body.valid && \ + piston_friction_force_valid; + } bool operator==(const LatentData & that) const { bool equal = (this->inc_wave_heights == that.inc_wave_heights); - // equal &= fabs(this-> - that.) < 1e-7F; + equal &= (this->upper_spring == that.upper_spring); + equal &= (this->lower_spring == that.lower_spring); + equal &= (this->electro_hydraulic == that.electro_hydraulic); + equal &= (this->wave_body == that.wave_body); + equal &= fabs(this->piston_friction_force - that.piston_friction_force) < 1e-7F; + equal &= (this->piston_friction_force_valid == that.piston_friction_force_valid); return equal; } }; diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index d5263d56..666a0c56 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -221,9 +221,9 @@ void LatentDataPublisher::PostUpdate( _ecm.Component(this->dataPtr->entity_); latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); - this->dataPtr->data_valid_ = true; } else { this->dataPtr->data_valid_ = false; + return; } // low prio data access @@ -250,7 +250,127 @@ void LatentDataPublisher::PostUpdate( this->dataPtr->latent_data_.inc_wave_heights[idx]); } - // TODO(andermi) fill in other stuff + this->dataPtr->latent_data_.upper_spring.force = latent_data.upper_spring.force; + this->dataPtr->latent_data_.upper_spring.temperature = latent_data.upper_spring.T; + this->dataPtr->latent_data_.upper_spring.heat_loss = latent_data.upper_spring.dQ_dt; + this->dataPtr->latent_data_.upper_spring.piston_position = + latent_data.upper_spring.piston_position; + this->dataPtr->latent_data_.upper_spring.piston_velocity = + latent_data.upper_spring.piston_velocity; + this->dataPtr->latent_data_.lower_spring.force = latent_data.lower_spring.force; + this->dataPtr->latent_data_.lower_spring.temperature = latent_data.lower_spring.T; + this->dataPtr->latent_data_.lower_spring.heat_loss = latent_data.lower_spring.dQ_dt; + this->dataPtr->latent_data_.lower_spring.piston_position = + latent_data.lower_spring.piston_position; + this->dataPtr->latent_data_.lower_spring.piston_velocity = + latent_data.lower_spring.piston_velocity; + + this->dataPtr->latent_data_.electro_hydraulic.rpm = + latent_data.electro_hydraulic.rpm; + this->dataPtr->latent_data_.electro_hydraulic.upper_hydraulic_pressure = + latent_data.electro_hydraulic.upper_hydraulic_pressure; + this->dataPtr->latent_data_.electro_hydraulic.lower_hydraulic_pressure = + latent_data.electro_hydraulic.lower_hydraulic_pressure; + this->dataPtr->latent_data_.electro_hydraulic.force = + latent_data.electro_hydraulic.force; + this->dataPtr->latent_data_.electro_hydraulic.supplied_hydraulic_power = + latent_data.electro_hydraulic.supplied_hydraulic_power; + this->dataPtr->latent_data_.electro_hydraulic.hydraulic_motor_loss = + latent_data.electro_hydraulic.hydraulic_motor_loss; + this->dataPtr->latent_data_.electro_hydraulic.relief_valve_loss = + latent_data.electro_hydraulic.relief_valve_loss; + this->dataPtr->latent_data_.electro_hydraulic.shaft_mech_power = + latent_data.electro_hydraulic.shaft_mech_power; + this->dataPtr->latent_data_.electro_hydraulic.motor_drive_i2r_loss = + latent_data.electro_hydraulic.motor_drive_i2r_loss; + this->dataPtr->latent_data_.electro_hydraulic.motor_drive_switching_loss = + latent_data.electro_hydraulic.motor_drive_switching_loss; + this->dataPtr->latent_data_.electro_hydraulic.motor_drive_friction_loss = + latent_data.electro_hydraulic.motor_drive_friction_loss; + this->dataPtr->latent_data_.electro_hydraulic.load_dump_power = + latent_data.electro_hydraulic.load_dump_power; + this->dataPtr->latent_data_.electro_hydraulic.battery_i2r_loss = + latent_data.electro_hydraulic.battery_i2r_loss; + 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 = + latent_data.wave_body.buoyant_force.Y(); + this->dataPtr->latent_data_.wave_body.buoyancy.force.z = + latent_data.wave_body.buoyant_force.Z(); + this->dataPtr->latent_data_.wave_body.buoyancy.torque.x = + latent_data.wave_body.buoyant_moment.X(); + this->dataPtr->latent_data_.wave_body.buoyancy.torque.y = + 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 = + latent_data.wave_body.radiation_force.Y(); + this->dataPtr->latent_data_.wave_body.radiation.force.z = + latent_data.wave_body.radiation_force.Z(); + this->dataPtr->latent_data_.wave_body.radiation.torque.x = + latent_data.wave_body.radiation_moment.X(); + this->dataPtr->latent_data_.wave_body.radiation.torque.y = + 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 = + latent_data.wave_body.exciting_force.Y(); + this->dataPtr->latent_data_.wave_body.excitation.force.z = + latent_data.wave_body.exciting_force.Z(); + this->dataPtr->latent_data_.wave_body.excitation.torque.x = + latent_data.wave_body.exciting_moment.X(); + this->dataPtr->latent_data_.wave_body.excitation.torque.y = + 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; + + this->dataPtr->data_valid_ = latent_data.valid(); data.unlock(); } diff --git a/buoy_gazebo/src/PTOFriction/CMakeLists.txt b/buoy_gazebo/src/PTOFriction/CMakeLists.txt index 356dc639..4eefe1b2 100644 --- a/buoy_gazebo/src/PTOFriction/CMakeLists.txt +++ b/buoy_gazebo/src/PTOFriction/CMakeLists.txt @@ -3,6 +3,7 @@ gz_add_plugin(PTOFriction PTOFriction.cpp INCLUDE_DIRS .. + ../LatentData EXTRA_ROS_PKGS simple_interp -) \ No newline at end of file +) diff --git a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp index 2327b6d0..f63c4064 100644 --- a/buoy_gazebo/src/PTOFriction/PTOFriction.cpp +++ b/buoy_gazebo/src/PTOFriction/PTOFriction.cpp @@ -30,6 +30,8 @@ #include +#include + namespace buoy_gazebo { @@ -48,7 +50,7 @@ class PTOFrictionPrivate /// \brief Piston mean friction force const std::vector meanFriction; // N - /// \brief Construct and approximation of friction model using linear spline + /// \brief Construct and approximation of friction model using linear interpolation simple_interp::Interp1d pto_friction_model; PTOFrictionPrivate() @@ -147,6 +149,24 @@ void PTOFriction::PreUpdate( } else { forceComp->Data()[0] += friction_force; // Add friction to existing forces } + + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->model.Entity(), + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->model.Entity()); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + } + + latent_data.piston_friction_force_valid = true; + latent_data.piston_friction_force = friction_force; + + _ecm.SetComponentData( + this->dataPtr->model.Entity(), + latent_data); } } // namespace buoy_gazebo diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt b/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt index 660d0a42..59d44d93 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/CMakeLists.txt @@ -6,6 +6,7 @@ gz_add_plugin(PolytropicPneumaticSpring PolytropicPneumaticSpring.cpp INCLUDE_DIRS .. + ../LatentData PUBLIC_LINK_LIBS gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} ) diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index d334549e..bdbb5e5f 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "PolytropicPneumaticSpring.hpp" + #include #include @@ -35,8 +37,10 @@ #include #include -#include "PolytropicPneumaticSpring.hpp" #include "SpringState.hpp" +#include + +#include using namespace std::chrono_literals; @@ -238,8 +242,10 @@ void PolytropicPneumaticSpring::computeLawOfCoolingForce(const double & x, const this->dataPtr->T += dT; // TODO(andermi) find Qdot (rate of heat transfer) from h, A, dT (Qdot = h*A*dT) + // Get chamber surface area from CAD... not true cylinder + // Also, since the chambers wrap around, h (heat transfer constant) is not quite water/steel/gas const double radius = 0.045; - const double A = (2.0 * this->dataPtr->config_->piston_area) * radius * x; + const double A = (2.0 * this->dataPtr->config_->piston_area) + 2.0 * GZ_PI * radius * x; const double h = 11.3; // (W/(m^2*K)) -- Water<->Mild Steel<->Gas this->dataPtr->Q_rate = h * A * dT; @@ -270,9 +276,9 @@ void PolytropicPneumaticSpring::computePolytropicForce(const double & x, const d // Retrieved from https://ir.library.oregonstate.edu/downloads/ww72bf399 // heat loss rate for polytropic ideal gas: // dQ/dt = (1 - n/gamma)*(c_p/R)*P*A*dx/dt - // TODO(andermi) A != piston_area... it's the chamber surface area + // TODO(andermi) get chamber surface area from CAD... not a true cylinder const double r = 0.045; - const double A = (2.0 * this->dataPtr->config_->piston_area) * r * x; + const double A = (2.0 * this->dataPtr->config_->piston_area) + 2.0 * GZ_PI * r * x; this->dataPtr->Q_rate = (1.0 - this->dataPtr->n / PolytropicPneumaticSpringConfig::ADIABATIC_INDEX) * cp_R * this->dataPtr->P * A * v; @@ -524,8 +530,8 @@ void PolytropicPneumaticSpring::PreUpdate( const int dt_nano = static_cast(std::chrono::duration_cast(_info.dt).count()); - static const double PASCAL_TO_PSI = 1.450377e-4; // PSI/Pascal - const double pressure_diff = (spring_state.lower_psi - spring_state.upper_psi) / PASCAL_TO_PSI; + const double pressure_diff = + (spring_state.lower_psi - spring_state.upper_psi) * buoy_utils::PASCAL_PER_PSI; if (spring_state.valve_command) { openValve( @@ -559,21 +565,55 @@ void PolytropicPneumaticSpring::PreUpdate( if (this->dataPtr->config_->is_upper) { this->dataPtr->F *= -1.0; + this->dataPtr->Q_rate *= -1.0; } auto stampMsg = gz::sim::convert(_info.simTime); if (this->dataPtr->config_->is_upper) { spring_state.range_finder = x; - spring_state.upper_psi = PASCAL_TO_PSI * this->dataPtr->P; + spring_state.upper_psi = this->dataPtr->P / buoy_utils::PASCAL_PER_PSI; } else { - spring_state.lower_psi = PASCAL_TO_PSI * this->dataPtr->P; + spring_state.lower_psi = this->dataPtr->P / buoy_utils::PASCAL_PER_PSI; } _ecm.SetComponentData( this->dataPtr->config_->jointEntity, spring_state); + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->config_->model.Entity(), + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->config_->model.Entity()); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + } + + if (this->dataPtr->config_->is_upper) { + latent_data.upper_spring.valid = true; + latent_data.upper_spring.force = this->dataPtr->F; + latent_data.upper_spring.T = this->dataPtr->T; + latent_data.upper_spring.dQ_dt = this->dataPtr->Q_rate; + latent_data.upper_spring.piston_position = x; + latent_data.upper_spring.piston_velocity = v; + latent_data.upper_spring.mass = this->dataPtr->mass; + } else { + latent_data.lower_spring.valid = true; + latent_data.lower_spring.force = this->dataPtr->F; + latent_data.lower_spring.T = this->dataPtr->T; + latent_data.lower_spring.dQ_dt = this->dataPtr->Q_rate; + latent_data.lower_spring.piston_position = this->dataPtr->config_->stroke - x; + latent_data.lower_spring.piston_velocity = -v; + latent_data.lower_spring.mass = this->dataPtr->mass; + } + + _ecm.SetComponentData( + this->dataPtr->config_->model.Entity(), + latent_data); + gz::msgs::Double force; force.mutable_header()->mutable_stamp()->CopyFrom(stampMsg); force.set_data(this->dataPtr->F); diff --git a/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt b/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt index 4c03dfc1..d2b9454d 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt +++ b/buoy_gazebo/src/WaveBodyInteractions/CMakeLists.txt @@ -14,6 +14,7 @@ gz_add_plugin(WaveBodyInteractions INCLUDE_DIRS .. ../.. + ../LatentData EXTRA_ROS_PKGS simple_interp ament_index_cpp diff --git a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp index 9ffa22b1..3b822159 100644 --- a/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp +++ b/buoy_gazebo/src/WaveBodyInteractions/WaveBodyInteractions.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include #include @@ -43,15 +43,22 @@ #include #include -#include "IncidentWaves/IncWaveState.hpp" +#include +#include #include #include #include + namespace buoy_gazebo { + +static bool WBI_DBG_FLAG{false}; +#define wbidbg if (WBI_DBG_FLAG) gzdbg + + class WaveBodyInteractionsPrivate { public: @@ -98,7 +105,7 @@ 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; @@ -106,16 +113,18 @@ void WaveBodyInteractions::Configure( // Get params from SDF. if (!_sdf->HasElement("LinkName")) { - ignerr << "You musk specify a for the wavebodyinteraction " + gzerr << "You musk specify a for the wavebodyinteraction " "plugin to act upon" - << "Failed to initialize." << std::endl; + << "Failed to initialize." << std::endl; return; } auto linkName = _sdf->Get("LinkName"); + WBI_DBG_FLAG = _sdf->Get("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; } @@ -163,7 +172,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; } @@ -186,12 +195,12 @@ void WaveBodyInteractions::PreUpdate( if (_info.iterations == 1) { // First iteration, set timestep size. double dt = std::chrono::duration(_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(_info.dt).count() << "s]. System may not work properly." << std::endl; @@ -216,7 +225,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(); @@ -225,8 +234,8 @@ 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 p_xdot = w_Pose_p.Rot().Inverse() * *w_xdot; gz::math::Vector3 w_xdot_p = @@ -234,10 +243,10 @@ void WaveBodyInteractions::PreUpdate( w_omega_b.Cross(w_Pose_b.Rot() * this->dataPtr->b_Pose_p.Pos()); gz::math::Vector3 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 w_xddot_p = w_xddot_b + @@ -246,10 +255,10 @@ void WaveBodyInteractions::PreUpdate( w_omega_b.Cross(w_Pose_b.Rot() * this->dataPtr->b_Pose_p.Pos())); gz::math::Vector3 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 b_xddot_p = w_Pose_p.Rot().Inverse() * w_xddot_p; gz::math::Vector3 b_omegadot_p = w_Pose_p.Rot().Inverse() * w_omegadot_p; @@ -260,11 +269,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), @@ -277,7 +286,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 @@ -292,14 +301,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( @@ -310,13 +319,13 @@ 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( @@ -324,8 +333,8 @@ void WaveBodyInteractions::PreUpdate( 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 += @@ -340,6 +349,37 @@ void WaveBodyInteractions::PreUpdate( _ecm.SetComponentData( this->dataPtr->IncWaveEntity, inc_wave_state); } + + buoy_gazebo::LatentData latent_data; + if (_ecm.EntityHasComponentType( + this->dataPtr->model.Entity(), + buoy_gazebo::components::LatentData().TypeId())) + { + auto latent_data_comp = + _ecm.Component(this->dataPtr->model.Entity()); + + latent_data = buoy_gazebo::LatentData(latent_data_comp->Data()); + } + + 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(), + latent_data); } ////////////////////////////////////////////////// diff --git a/buoy_gazebo/src/buoy_utils/Constants.hpp b/buoy_gazebo/src/buoy_utils/Constants.hpp index 68810e92..49d5e21d 100644 --- a/buoy_gazebo/src/buoy_utils/Constants.hpp +++ b/buoy_gazebo/src/buoy_utils/Constants.hpp @@ -27,6 +27,7 @@ static constexpr double NM_PER_INLB{0.112984829}; static constexpr double INLB_PER_NM{8.851}; static constexpr double INCHES_PER_METER{39.4}; static constexpr double NEWTONS_PER_LB{4.4482}; +static constexpr double PASCAL_PER_PSI = 6894.757; // Pascal/PSI } // namespace buoy_utils