added spring and pto (still need a couple pto losses)

Signed-off-by: Michael Anderson <[email protected]>

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,13 @@
 #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
 {
@@ -274,6 +277,17 @@ void ElectroHydraulicPTO::PreUpdate(
     pto_loss = buoy_gazebo::ElectroHydraulicLoss(pto_loss_comp->Data());
   }
 
+  buoy_gazebo::LatentData latent_data;
+  if (_ecm.EntityHasComponentType(
+      this->dataPtr->model.Entity(),
+      buoy_gazebo::components::LatentData().TypeId()))
+  {
+    auto latent_data_comp =
+      _ecm.Component<buoy_gazebo::components::LatentData>(this->dataPtr->model.Entity());
+
+    latent_data = buoy_gazebo::LatentData(latent_data_comp->Data());
+  }
+
   if (pto_state.scale_command) {
     this->dataPtr->functor.I_Wind.ScaleFactor = pto_state.scale_command.value();
   } else {
@@ -406,11 +420,20 @@ void ElectroHydraulicPTO::PreUpdate(
 
   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.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;
 
+  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.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;
+
   _ecm.SetComponentData<buoy_gazebo::components::BatteryState>(
     this->dataPtr->PrismaticJointEntity,
     battery_state);
@@ -425,6 +448,10 @@ void ElectroHydraulicPTO::PreUpdate(
     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);
   pistonvel.set_data(xdot);

buoy_gazebo/src/LatentData/IncWaveHeight/IncWaveHeight.cpp

@@ -352,6 +352,7 @@ void IncWaveHeight::PreUpdate(
   auto sec_nsec = gz::math::durationToSecNsec(this->dataPtr->current_time_);
   latent_data.inc_wave_heights.sec = sec_nsec.first;
   latent_data.inc_wave_heights.nsec = sec_nsec.second;
+  latent_data.inc_wave_heights.valid = this->dataPtr->inc_wave_valid_;
 
   std::size_t idx = 0U;
   for (; idx < latent_data.inc_wave_heights.points.size(); ++idx) {

buoy_gazebo/src/LatentData/LatentData/LatentData.hpp

@@ -52,6 +52,7 @@ struct IncWaveHeightPoint
 struct IncWaveHeights {
   int32_t sec{0};
   uint32_t nsec{0U};
+  bool valid;
   std::vector<IncWaveHeightPoint> points;
 
   bool operator==(const IncWaveHeights & that) const
@@ -69,18 +70,63 @@ struct IncWaveHeights {
       equal &= (this->points[idx] == that.points[idx]);
     }
 
+    equal &= this->valid == that.valid;
+
+    return equal;
+  }
+};
+
+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};
+
+  bool operator==(const AirSpring & that) const
+  {
+    bool equal = (this->valid == that.valid);
+    equal &= (this->force == that.force);
+    equal &= (this->T == that.T);
+    equal &= (this->dQ_dt == that.dQ_dt);
+
     return equal;
   }
 };
 
+struct ElectroHydraulic
+{
+  bool valid{false};
+  double inst_power{0.0};
+  double rpm{0.0};
+  double motor_drive_i2r_loss{0.0};
+  double motor_drive_friction_loss{0.0};
+  double motor_drive_switching_loss{0.0};
+  double battery_i2r_loss{0.0};
+};
+
 /// \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;
+
+  bool valid() const
+  {
+    return inc_wave_heights.valid && \
+      upper_spring.valid && lower_spring.valid && \
+      electro_hydraulic.valid;
+  }
 
   bool operator==(const LatentData & that) const
   {
     bool equal = (this->inc_wave_heights == that.inc_wave_heights);
+    equal &= (this->upper_spring == that.upper_spring);
+    equal &= (this->lower_spring == that.lower_spring);
     // equal &= fabs(this-> - that.) < 1e-7F;
     return equal;
   }

buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp

@@ -218,9 +218,9 @@ void LatentDataPublisher::PostUpdate(
       _ecm.Component<buoy_gazebo::components::LatentData>(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
@@ -242,8 +242,38 @@ void LatentDataPublisher::PostUpdate(
       this->dataPtr->latent_data_.inc_wave_heights[idx]);
   }
 
+  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.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.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.battery_i2r_loss =
+    latent_data.electro_hydraulic.battery_i2r_loss;
+
   // TODO(andermi) fill in other stuff
 
+  this->dataPtr->data_valid_ = latent_data.valid();
+
   data.unlock();
 }
 }  // namespace buoy_gazebo

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}
 )

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 <gz/msgs/double.pb.h>
 #include <gz/msgs/time.pb.h>
 
@@ -35,8 +37,8 @@
 #include <gz/sim/Joint.hh>
 #include <gz/sim/Model.hh>
 
-#include "PolytropicPneumaticSpring.hpp"
 #include "SpringState.hpp"
+#include <LatentData/LatentData.hpp>
 
 
 using namespace std::chrono_literals;
@@ -238,8 +240,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 +274,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;
@@ -559,6 +563,7 @@ 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<gz::msgs::Time>(_info.simTime);
@@ -574,6 +579,37 @@ void PolytropicPneumaticSpring::PreUpdate(
     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<buoy_gazebo::components::LatentData>(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;
+  } 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;
+  }
+
+  _ecm.SetComponentData<buoy_gazebo::components::LatentData>(
+    this->dataPtr->config_->model.Entity(),
+    latent_data);
+
   gz::msgs::Double force;
   force.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
   force.set_data(this->dataPtr->F);