Latent Data on ROS 2

.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

buoy_description/models/mbari_wec/model.sdf.em

@@ -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> 

buoy_description/models/mbari_wec_ros/model.sdf

@@ -64,15 +64,10 @@
       <namespace>/</namespace>
       <node_name>inc_wave_service</node_name>
       <points use_buoy_origin="true">
-        <xy>-1.0 -1.0</xy>
-        <xy>0.0 -1.0</xy>
-        <xy>1.0 -1.0</xy>
-        <xy>-1.0 0.0</xy>
         <xy>0.0 0.0</xy>
-        <xy>1.0 0.0</xy>
-        <xy>-1.0 1.0</xy>
-        <xy>0.0 1.0</xy>
-        <xy>1.0 1.0</xy>
+        <!-- may add multiple xy tags -->
+        <!-- <xy>-1.0 0.0</xy> -->
+        <!-- <xy>1.0 0.0</xy> -->
       </points>
     </plugin>
 

buoy_gazebo/src/ElectroHydraulicPTO/CMakeLists.txt

@@ -3,6 +3,7 @@ gz_add_plugin(ElectroHydraulicPTO
     ElectroHydraulicPTO.cpp
   INCLUDE_DIRS
     ..
+    ../LatentData
   EXTRA_ROS_PKGS
     simple_interp
 )

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 <eigen3/unsupported/Eigen/NonLinearOptimization>
 
 #include <gz/msgs/double.pb.h>
@@ -40,12 +42,12 @@
 #include <gz/plugin/Register.hh>
 #include <gz/transport/Node.hh>
 
-#include "ElectroHydraulicPTO.hpp"
 #include "ElectroHydraulicSoln.hpp"
 #include "ElectroHydraulicState.hpp"
-#include "ElectroHydraulicLoss.hpp"
 #include "BatteryState.hpp"
 
+#include <LatentData/LatentData.hpp>
+
 
 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<buoy_gazebo::components::ElectroHydraulicLoss>(
-      this->dataPtr->PrismaticJointEntity);
+    auto latent_data_comp =
+      _ecm.Component<buoy_gazebo::components::LatentData>(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<buoy_gazebo::components::BatteryState>(
     this->dataPtr->PrismaticJointEntity,
@@ -421,9 +448,9 @@ void ElectroHydraulicPTO::PreUpdate(
 
   auto stampMsg = gz::sim::convert<gz::msgs::Time>(_info.simTime);
 
-  _ecm.SetComponentData<buoy_gazebo::components::ElectroHydraulicLoss>(
-    this->dataPtr->PrismaticJointEntity,
-    pto_loss);
+  _ecm.SetComponentData<buoy_gazebo::components::LatentData>(
+    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<gz::sim::components::JointForceCmd>(
       this->dataPtr->PrismaticJointEntity);

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

@@ -110,6 +110,13 @@ struct ElectroHydraulicSoln : Functor<double>
   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<double> Peff;       // psi
   static const std::vector<double> Neff;       // rpm
@@ -170,28 +177,32 @@ struct ElectroHydraulicSoln : Functor<double>
     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
                                          // than upper (resisting extension)
       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;
 

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<double>(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)