add ehpto force

Signed-off-by: Michael Anderson <[email protected]>
osrf · Sep 1, 2023 · 5251ce3 · 5251ce3
1 parent 54bd6ed
commit 5251ce3
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 7 deletions.
diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp
@@ -426,9 +426,12 @@ void ElectroHydraulicPTO::PreUpdate(
   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.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;
@@ -464,7 +467,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);

diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp
@@ -36,6 +36,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
@@ -81,11 +82,11 @@ struct IncWaveHeights
 struct AirSpring
 {
   bool valid{false};
-  double force{0.0};
-  double T{0.0};
-  double dQ_dt{0.0};
-  double piston_position{0.0};
-  double piston_velocity{0.0};
+  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
 
   bool operator==(const AirSpring & that) const
   {
@@ -101,8 +102,9 @@ struct AirSpring
 struct ElectroHydraulic
 {
   bool valid{false};
-  double inst_power{0.0};
+  double inst_power{0.0};  // Watts
   double rpm{0.0};
+  double force{0.0};  // Newtons
   double motor_drive_i2r_loss{0.0};
   double motor_drive_friction_loss{0.0};
   double motor_drive_switching_loss{0.0};

diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp
@@ -262,6 +262,8 @@ void LatentDataPublisher::PostUpdate(
     latent_data.electro_hydraulic.inst_power;
   this->dataPtr->latent_data_.electro_hydraulic.rpm =
     latent_data.electro_hydraulic.rpm;
+  this->dataPtr->latent_data_.electro_hydraulic.force =
+    latent_data.electro_hydraulic.force;
   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 =