Merge pull request #169 from osrf/hamilton8415/ehpto_switchlossfix

Hamilton8415/ehpto switchlossfix

.gitattributes

@@ -1,4 +1,5 @@
 *.em -linguist-detectable
 *.tst -linguist-detectable
+*.exp -linguist-detectable
 *.1 -linguist-detectable
 *.3 -linguist-detectable

buoy_description/models/mbari_wec/model.sdf.em

@@ -221,7 +221,7 @@ if not ignore_piston_mean_pos:
       <is_upper>true</is_upper>
       <initial_piston_position>@(initial_piston_position)</initial_piston_position>
       <!-- measure of valve opening cross-section and duration (meter-seconds) -->
-      <valve_absement>7.77e-6</valve_absement>
+      <valve_absement>1.3e-5</valve_absement>
       <pump_absement>5.5e-8</pump_absement>
       <pump_pressure>1.7e+6</pump_pressure>
       <stroke>2.03</stroke>
@@ -246,7 +246,7 @@ if not ignore_piston_mean_pos:
       <chamber>lower_polytropic</chamber>
       <is_upper>false</is_upper>
       <!-- measure of valve opening cross-section and duration (meter-seconds) -->
-      <valve_absement>7.77e-6</valve_absement>
+      <valve_absement>1.3e-5</valve_absement>
       <pump_absement>5.5e-8</pump_absement>
       <pump_pressure>1.7e+6</pump_pressure>
       <stroke>2.03</stroke>

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp

@@ -326,8 +326,7 @@ void ElectroHydraulicPTO::PreUpdate(
       this->dataPtr->functor.HydMotorDisp;
 
     double WindCurr = this->dataPtr->functor.I_Wind(this->dataPtr->x[0U]);
-    // 1.375 fudge factor required to match experiments, not yet sure why.
-    const double T_applied = 1.375 * this->dataPtr->functor.I_Wind.TorqueConstantInLbPerAmp *
+    const double T_applied = 1.00 * this->dataPtr->functor.I_Wind.TorqueConstantInLbPerAmp *
       WindCurr;
     this->dataPtr->x[1] = -T_applied / (this->dataPtr->functor.HydMotorDisp / (2 * M_PI));
 
@@ -362,6 +361,7 @@ void ElectroHydraulicPTO::PreUpdate(
   // Solve Electrical
   const double N = this->dataPtr->x[0U];
   double deltaP = this->dataPtr->x[1U];
+//  std::cout << this->dataPtr->functor.Q << "   " << N << "   " << deltaP << std::endl;
   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));
@@ -423,14 +423,14 @@ void ElectroHydraulicPTO::PreUpdate(
     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_emf_power =
+    this->dataPtr->functor.MotorEMFPower;
   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;
+    this->dataPtr->functor.ElectricMotorFrictionLoss;
   latent_data.electro_hydraulic.load_dump_power =
     I_Load * VBus;
   latent_data.electro_hydraulic.battery_i2r_loss =

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicSoln.hpp

@@ -89,15 +89,15 @@ struct ElectroHydraulicSoln : Functor<double>
   static constexpr double HydMotorDisp{0.30};  // Default to Parker F11-5  0.30in^3/rev
 
   // Friction Loss Model constants
-  static constexpr double tau_c{.1};  // N-m
-  static constexpr double k_v{.05 / 1000.0};  // N-m/RPM
-  static constexpr double k_th{19.0};
+  static constexpr double tau_c{.1596};  // N-m
+  static constexpr double k_v{.12897 / 1000.0};  // N-m/RPM
+  static constexpr double k_th{100.0};
 
   // Switching Loss Model constants
-  static constexpr double k_switch{0.05};             // W/Volt
+  static constexpr double k_switch{5.34e-4};             // W/Volt
 
   // Winding Resitance Loss Model constants
-  static constexpr double R_w{0.8};             // Ohms
+  static constexpr double R_w{0.1833};             // Ohms
 
   double VBattEMF;       // Battery internal EMF voltage
   double Ri;       // Battery internal resistance
@@ -110,8 +110,8 @@ struct ElectroHydraulicSoln : Functor<double>
   mutable double BusPower;
   double Q;
 
-  mutable double ShaftMechPower;
-  mutable double FrictionLoss;
+  mutable double MotorEMFPower;  // Power applied to hydraulic motor.
+  mutable double ElectricMotorFrictionLoss;
   mutable double SwitchingLoss;
   mutable double I2RLoss;
   mutable double ReliefValveLoss;
@@ -132,22 +132,26 @@ struct ElectroHydraulicSoln : Functor<double>
   {
   }
 
-  // Friction loss is characterized in 2022 PTO simulation paper
-  // Friction loss is a function of RPM
-  // Units of power returned
-  double MotorDriveFrictionLoss(double N) const
+
+  // Electric Motor Friction  is characterized in 2022 PTO simulation paper
+  // Friction is a function of RPM
+  // Units of N-m returned
+  double ElectricMotorFrictionTorque(double N) const
   {
-    return fabs(
-      (tau_c * tanh(2 * M_PI * N / buoy_utils::SecondsPerMinute / k_th) +
-      k_v * N / 1000.0) * N);
+    return -(tau_c * tanh(N / k_th) + k_v * N);
   }
 
+
   // Switching Loss is from measurements as a function of bus voltage.
-  // ~ 10% of Voltage in Watts...
+  // ~ 5% of Voltage in Watts...
   // Units of power returned
-  double MotorDriveSwitchingLoss(double V) const
+  double MotorDriveSwitchingLoss(double N, double IWind, double V) const
   {
-    return k_switch * fabs(V);
+    double SwitchLoss = 0.0;
+    if ((fabs(N) > 300) || (fabs(IWind) > 0.1)) {
+      SwitchLoss = k_switch * V * V;
+    }
+    return SwitchLoss;
   }
 
   // Winding ISquaredR Losses,
@@ -158,66 +162,75 @@ struct ElectroHydraulicSoln : Functor<double>
   }
 
   // x[0] = RPM
-  // x[1] = Pressure (psi)
+  // x[1] = Pressure Delta (psi)
   // x[2] = Bus Voltage (Volts)
   int operator()(const Eigen::VectorXd & x, Eigen::VectorXd & fvec) const
   {
     const int n = x.size();
     assert(fvec.size() == n);
 
-    const double rpm = fabs(x[0U]);
-    const double pressure = fabs(x[1U]);
-    const double eff_m = this->hyd_eff_m.eval(rpm);
-    const double eff_v = this->hyd_eff_v.eval(pressure);
-    // const double eff_m = 1.0 - (.1 / 6000.0) * rpm;
-    // const double eff_v = 1.0 - (.1 / 3500.0) * pressure;
+    const double eff_m = 0.8;  // this->hyd_eff_m.eval(fabs(x[0U]);
+    const double eff_v = 1.0;  // this->hyd_eff_v.eval(fabs(x[1U]);
 
     double WindCurr = this->I_Wind(x[0U]);
-    // 1.375 fudge factor required to match experiments, not yet sure why.
-    const double T_applied =
-      1.375 * this->I_Wind.TorqueConstantInLbPerAmp * WindCurr;
-
-    ShaftMechPower = -T_applied * buoy_utils::NM_PER_INLB *
+    const double T_applied = 1.00 * this->I_Wind.TorqueConstantInLbPerAmp * WindCurr;
+    //  The 1.05 factor above is here due to a discremency in the motor torque constant
+    //  The at-sea code uses the motor spec'd value of 0.438 to compute motor current
+    //  as a function of RPM.  But, bench-testing of the motor itself reveals the actual
+    //  motor torque constant is 0.458.  Because of this a factor of 1.05 is applied here
+    //  so that the applied torque in simulation matches what is physically happening in
+    //  the system.
+
+    double T_ElectricMotorFriction = ElectricMotorFrictionTorque(x[0U]);  // Returns N-m
+    ElectricMotorFrictionLoss = fabs(
+      T_ElectricMotorFriction * 2 * M_PI * x[0U] / buoy_utils::SecondsPerMinute);
+    T_ElectricMotorFriction = T_ElectricMotorFriction / buoy_utils::NM_PER_INLB;
+    MotorEMFPower = -(T_applied * buoy_utils::NM_PER_INLB) *
       x[0U] * buoy_utils::RPM_TO_RAD_PER_SEC;
-    FrictionLoss = MotorDriveFrictionLoss(x[0U]);
-    SwitchingLoss = MotorDriveSwitchingLoss(x[2U]);
+    SwitchingLoss = MotorDriveSwitchingLoss(x[1U], WindCurr, x[2U]);
     I2RLoss = MotorDriveISquaredRLoss(WindCurr);
-    BusPower = ShaftMechPower - (FrictionLoss + SwitchingLoss + I2RLoss);
+
+    BusPower = MotorEMFPower - (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;
     }
-    ReliefValveLoss = Q_Relief * x[1U] / buoy_utils::INLB_PER_NM;  // Result is Watts
-
-    double Q_Motor = this->Q - Q_Relief;
     double Q_Ideal = x[0U] * this->HydMotorDisp / buoy_utils::SecondsPerMinute;
+    double Q_Motor = this->Q - Q_Relief;
     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]);
-
-    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);
+    double T_HydMotFrict = -(1.0 - eff_m) * std::max(fabs(T_applied), fabs(T_Fluid)) * sgn(x[0]);
 
-    fvec[0U] = Q_Motor - Q_Leak - Q_Ideal;
-    fvec[1U] = T_applied + T_Friction + T_Fluid;
+    fvec[0U] = this->Q - Q_Relief - Q_Leak - Q_Ideal;
+    fvec[1U] = T_applied + T_ElectricMotorFriction + T_HydMotFrict + T_Fluid;
     fvec[2U] = BusPower - (x[2U] - VBattEMF) * x[2U] / this->Ri;
 
+    ReliefValveLoss = Q_Relief * x[1U] / buoy_utils::INLB_PER_NM;  // Result is Watts
+    HydraulicMotorLoss = Q_Leak * x[1U] / buoy_utils::INLB_PER_NM -  // Result is Watts
+      T_HydMotFrict * x[0U] * 2.0 * M_PI / (buoy_utils::INLB_PER_NM * buoy_utils::SecondsPerMinute);
+
     return 0;
   }
 };
+// const std::vector<double> ElectroHydraulicSoln::Peff{
+//  0.0, 145.0, 290.0, 435.0, 580.0, 725.0, 870.0,
+//  1015.0, 1160.0, 1305.0, 1450.0, 1595.0, 1740.0, 1885.0,
+//  2030.0, 2175.0, 2320.0, 2465.0, 2610.0, 2755.0, 3500.0, 10000.0};
+// const std::vector<double> ElectroHydraulicSoln::Eff_V{
+//  1.0000, 0.980, 0.97, 0.960, 0.9520, 0.950, 0.949, 0.9480,
+//  0.9470, 0.946, 0.9450, 0.9440, 0.9430, 0.9420, 0.9410, 0.9400,
+//  0.9390, 0.9380, 0.9370, 0.9350, 0.9100, .6000};
+
 const std::vector<double> ElectroHydraulicSoln::Peff{
-  0.0, 145.0, 290.0, 435.0, 580.0, 725.0, 870.0,
-  1015.0, 1160.0, 1305.0, 1450.0, 1595.0, 1740.0, 1885.0,
-  2030.0, 2175.0, 2320.0, 2465.0, 2610.0, 2755.0, 3500.0, 10000.0};
+  0.0, 500.0, 1000.0, 3000.0, 10000.0};
 
 const std::vector<double> ElectroHydraulicSoln::Eff_V{
-  1.0000, 0.980, 0.97, 0.960, 0.9520, 0.950, 0.949, 0.9480,
-  0.9470, 0.946, 0.9450, 0.9440, 0.9430, 0.9420, 0.9410, 0.9400,
-  0.9390, 0.9380, 0.9370, 0.9350, 0.9100, .6000};
+  1.0, 1.0, 1.0, 1.0, 1.0};  // 1.0, 0.97, .96, .95, .75};
 
 
 const std::vector<double> ElectroHydraulicSoln::Neff{

buoy_gazebo/src/ElectroHydraulicPTO/WindingCurrentTarget.hpp

@@ -27,7 +27,7 @@
 #include <simple_interp/interp1d.hpp>
 
 // Defines from Controller Firmware, behavior replicated here
-#define TORQUE_CONSTANT 0.438   // 0.62 N-m/ARMS  0.428N-m/AMPS Flux Current
+#define TORQUE_CONSTANT .438  // 0.62 N-m/ARMS  0.428 N-m/AMPS Flux Current
 #define CURRENT_CMD_RATELIMIT 200  // A/second.  Set to zero to disable feature
 #define TORQUE_CMD_TIMEOUT 2  // Torque Command Timeut, in secs. Set to zero to disable timeout
 #define BIAS_CMD_TIMEOUT 10  // Bias Current Command Timeut, secs. Set to zero to disable timeout

buoy_gazebo/src/LatentData/LatentData/LatentData.hpp

@@ -114,7 +114,7 @@ struct ElectroHydraulic
   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_emf_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
@@ -132,7 +132,7 @@ struct ElectroHydraulic
     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_emf_power - that.motor_emf_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;
@@ -150,7 +150,6 @@ struct WaveBody
 
   gz::math::Pose3<double> pose{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
   gz::math::Pose3<double> 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};
@@ -175,7 +174,6 @@ struct WaveBody
     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;
   }
 };

buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp

@@ -279,8 +279,8 @@ void LatentDataPublisher::PostUpdate(
     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_emf_power =
+    latent_data.electro_hydraulic.motor_emf_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 =
@@ -372,6 +372,8 @@ void LatentDataPublisher::PostUpdate(
 
   this->dataPtr->data_valid_ = latent_data.valid();
 
+  this->dataPtr->data_valid_ = latent_data.valid();
+
   data.unlock();
 }
 }  // namespace buoy_gazebo

buoy_tests/launch/experiment_comparison.launch.py

@@ -58,10 +58,10 @@ def find_proc(ld):
             if type(entity._Action__condition) is UnlessCondition:
                 return entity
         elif type(entity) is OpaqueFunction:
-            for node in entity._OpaqueFunction__args:
-                if type(node) is Node:
-                    if type(node._Action__condition) is UnlessCondition:
-                        return node
+            for arg in entity._OpaqueFunction__args:
+                if type(arg) is Node:
+                    if type(arg._Action__condition) is UnlessCondition:
+                        return arg
 
 
 class BuoyExperimentComparisonTest(unittest.TestCase):