Merge pull request #162 from osrf/hamilton8415/latent_data

Adjusting reported Latent Data fields for electrohydraulic PTO

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp

@@ -44,7 +44,6 @@
 
 #include "ElectroHydraulicSoln.hpp"
 #include "ElectroHydraulicState.hpp"
-#include "ElectroHydraulicLoss.hpp"
 #include "BatteryState.hpp"
 
 #include <LatentData/LatentData.hpp>
@@ -265,18 +264,6 @@ void ElectroHydraulicPTO::PreUpdate(
     pto_state = buoy_gazebo::ElectroHydraulicState(pto_state_comp->Data());
   }
 
-  buoy_gazebo::ElectroHydraulicLoss pto_loss;
-  if (_ecm.EntityHasComponentType(
-      this->dataPtr->PrismaticJointEntity,
-      buoy_gazebo::components::ElectroHydraulicLoss().TypeId()))
-  {
-    auto pto_loss_comp =
-      _ecm.Component<buoy_gazebo::components::ElectroHydraulicLoss>(
-      this->dataPtr->PrismaticJointEntity);
-
-    pto_loss = buoy_gazebo::ElectroHydraulicLoss(pto_loss_comp->Data());
-  }
-
   buoy_gazebo::LatentData latent_data;
   if (_ecm.EntityHasComponentType(
       this->dataPtr->model.Entity(),
@@ -421,26 +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 =
-    this->dataPtr->functor.MotorDriveISquaredRLoss(this->dataPtr->functor.I_Wind.I);
-  pto_loss.motor_drive_switching_loss = this->dataPtr->functor.MotorDriveSwitchingLoss(VBus);
-  pto_loss.motor_drive_friction_loss = this->dataPtr->functor.MotorDriveFrictionLoss(N);
-  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.inst_power = VBus * (I_Batt + I_Load);
   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.motor_drive_i2r_loss = pto_loss.motor_drive_i2r_loss;
-  latent_data.electro_hydraulic.motor_drive_switching_loss = pto_loss.motor_drive_switching_loss;
-  latent_data.electro_hydraulic.motor_drive_friction_loss = pto_loss.motor_drive_friction_loss;
-  latent_data.electro_hydraulic.battery_i2r_loss = pto_loss.battery_i2r_loss;
-  latent_data.electro_hydraulic.eff_m = eff_m;
-  latent_data.electro_hydraulic.eff_v = eff_v;
+  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,
@@ -452,10 +448,6 @@ 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);

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/LatentData/LatentData.hpp

@@ -107,27 +107,38 @@ struct AirSpring
 struct ElectroHydraulic
 {
   bool valid{false};
-  double inst_power{0.0};  // Watts
   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_friction_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 eff_m{0.0};  // mechanical efficiency
-  double eff_v{0.0};  // volumetric efficiency
+  double battery_storage_power{0.0};  // Watts
 
   bool operator==(const ElectroHydraulic & that) const
   {
     bool equal = (this->valid == that.valid);
-    equal &= fabs(this->inst_power - that.inst_power) < 1e-7F;
+    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_friction_loss - that.motor_drive_friction_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->eff_m - that.eff_m) < 1e-7F;
-    equal &= fabs(this->eff_v - that.eff_v) < 1e-7F;
+    equal &= fabs(this->battery_storage_power - that.battery_storage_power) < 1e-7F;
 
     return equal;
   }

buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp

@@ -265,24 +265,34 @@ void LatentDataPublisher::PostUpdate(
   this->dataPtr->latent_data_.lower_spring.piston_velocity =
     latent_data.lower_spring.piston_velocity;
 
-  this->dataPtr->latent_data_.electro_hydraulic.inst_power =
-    latent_data.electro_hydraulic.inst_power;
   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.eff_m =
-    latent_data.electro_hydraulic.eff_m;
-  this->dataPtr->latent_data_.electro_hydraulic.eff_v =
-    latent_data.electro_hydraulic.eff_v;
+  this->dataPtr->latent_data_.electro_hydraulic.battery_storage_power =
+    latent_data.electro_hydraulic.battery_storage_power;
 
   this->dataPtr->latent_data_.wave_body.buoyancy.force.x =
     latent_data.wave_body.buoyant_force.X();