Refine Spring

buoy_description/models/mbari_wec/model.sdf

@@ -13,22 +13,25 @@
       <RotorInertia>1</RotorInertia>
     </plugin>
 
-    <!-- Add Upper/Lower Polytropic Spring plugin -->
+    <!-- Upper Polytropic Spring plugin -->
     <plugin filename="PolytropicPneumaticSpring" name="buoy_gazebo::PolytropicPneumaticSpring">
       <JointName>HydraulicRam</JointName>
       <chamber>upper_polytropic</chamber>
       <is_upper>true</is_upper>
       <!-- measure of valve opening cross-section and duration (meter-seconds) -->
-      <valve_absement>49e-7</valve_absement>
-      <pump_absement>18e-8</pump_absement>
+      <valve_absement>4.75e-9</valve_absement>
+      <pump_absement>2.75e-11</pump_absement>
       <pump_pressure>1.7e+6</pump_pressure>
       <stroke>2.03</stroke>
       <piston_area>0.0127</piston_area>
-      <dead_volume>0.0266</dead_volume>
-      <T0>283.15</T0>
+      <dead_volume>0.0226</dead_volume>
+      <T0>263.15</T0>
+      <r>0.1</r>  <!-- coef of heat transfer -->
       <R_specific>0.2968</R_specific>
       <c_p>1.04</c_p>
       <hysteresis>true</hysteresis>
+      <velocity_deadzone_lower>-0.1</velocity_deadzone_lower>
+      <velocity_deadzone_upper>0.05</velocity_deadzone_upper>
       <n1>1.1725</n1>
       <n2>1.2139</n2>
       <x1>1.6159</x1>
@@ -37,30 +40,32 @@
       <P2>410190</P2>
     </plugin>
 
-    <!-- <debug_prescribed_velocity>true</debug_prescribed_velocity>-->
+    <!-- Lower Polytropic Spring plugin -->
     <plugin filename="PolytropicPneumaticSpring" name="buoy_gazebo::PolytropicPneumaticSpring">
       <JointName>HydraulicRam</JointName>
       <chamber>lower_polytropic</chamber>
       <is_upper>false</is_upper>
       <!-- measure of valve opening cross-section and duration (meter-seconds) -->
-      <valve_absement>49e-7</valve_absement>
-      <pump_absement>18e-8</pump_absement>
+      <valve_absement>4.75e-9</valve_absement>
+      <pump_absement>2.75e-11</pump_absement>
       <pump_pressure>1.7e+6</pump_pressure>
       <stroke>2.03</stroke>
       <piston_area>0.0115</piston_area>
-      <dead_volume>0.0523</dead_volume>
+      <dead_volume>0.0463</dead_volume>
       <T0>283.15</T0>
+      <r>0.1</r>  <!-- coef of heat transfer -->
       <R_specific>0.2968</R_specific>
       <c_p>1.04</c_p>
       <hysteresis>true</hysteresis>
+      <velocity_deadzone_lower>-0.01</velocity_deadzone_lower>
+      <velocity_deadzone_upper>0.1</velocity_deadzone_upper>
       <n1>1.1967</n1>
       <n2>1.1944</n2>
       <x1>0.9695</x1>
       <x2>1.1850</x2>
       <P1>1212060</P1>
       <P2>1212740</P2>
     </plugin>
-    <!-- <debug_prescribed_velocity>true</debug_prescribed_velocity>-->
 
   </model>
 </sdf>

buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp

@@ -29,6 +29,7 @@
 
 #include <ignition/msgs/double.pb.h>
 
+#include <algorithm>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -48,7 +49,10 @@ struct PolytropicPneumaticSpringConfig
   bool is_upper{false};
 
   /// \brief is hysteresis present in piston travel direction
-  bool hysteresis;
+  bool hysteresis{false};
+
+  /// \brief hysteresis velocity deadzone (m/s)
+  double vel_dz_lower{-0.1}, vel_dz_upper{0.1};
 
   /// \brief adiabatic index (gamma) == 1.4 for diatomic gas (e.g. N2)
   static constexpr double ADIABATIC_INDEX{1.4};
@@ -85,6 +89,12 @@ struct PolytropicPneumaticSpringConfig
   /// \brief initial Temp (K)
   double T0{283.15};
 
+  /// \brief coef of heat transfer (1/s)
+  double r{1.0};
+
+  /// \brief Temp of environment (seawater) (K)
+  double Tenv{283.15};
+
   /// \brief R (specific gas)
   double R{0.2968};
 
@@ -191,9 +201,9 @@ void PolytropicPneumaticSpring::openValve(
   }
 
   // Ideal Gas Law
-  const double mRT0 = this->dataPtr->mass * this->dataPtr->config_->R * this->dataPtr->config_->T0;
-  P1 = mRT0 / V1;
-  P2 = mRT0 / V2;
+  const double mRT = this->dataPtr->mass * this->dataPtr->config_->R * this->dataPtr->T;
+  P1 = mRT / V1;
+  P2 = mRT / V2;
 }
 
 ////////////////////////////////////////////////
@@ -228,19 +238,45 @@ void PolytropicPneumaticSpring::pumpOn(
   }
 
   // Ideal Gas Law
-  const double mRT0 = this->dataPtr->mass * this->dataPtr->config_->R * this->dataPtr->config_->T0;
-  P1 = mRT0 / V1;
-  P2 = mRT0 / V2;
+  const double mRT = this->dataPtr->mass * this->dataPtr->config_->R * this->dataPtr->T;
+  P1 = mRT / V1;
+  P2 = mRT / V2;
+}
+
+void PolytropicPneumaticSpring::computeLawOfCoolingForce(const double & x, const int & dt_nano)
+{
+  // geometry: V = V_dead + A*x
+  this->dataPtr->V = this->dataPtr->config_->dead_volume + x * this->dataPtr->config_->piston_area;
+
+  // Newton's Law of Cooling (non-dimensionalized):
+  // Tdot = r*(T_env - T(t)) -> T[n] = dt*r*(Tenv - T[n-1]) + T[n-1] (using forward difference)
+  const double dt_sec = dt_nano * IGN_NANO_TO_SEC;
+  const double dT =
+    dt_sec * this->dataPtr->config_->r * (this->dataPtr->config_->Tenv - this->dataPtr->T);
+  this->dataPtr->T += dT;
+
+  // TODO(andermi) find Qdot (rate of heat transfer) from h, A, dT (Qdot = h*A*dT)
+  const double radius = 0.045;
+  const double A = (2.0 * this->dataPtr->config_->piston_area) * radius * x;
+  const double h = 11.3;  // (W/(m^2*K)) -- Water<->Mild Steel<->Gas
+  this->dataPtr->Q_rate = h * A * dT;
+
+  // Ideal Gas Law: P = (m*R)*T/V
+  this->dataPtr->P = this->dataPtr->config_->c * this->dataPtr->T / this->dataPtr->V;
+
+  // F = P*A
+  this->dataPtr->F = this->dataPtr->P * this->dataPtr->config_->piston_area;
 }
 
 //////////////////////////////////////////////////
-void PolytropicPneumaticSpring::computeForce(const double & x, const double & v)
+void PolytropicPneumaticSpring::computePolytropicForce(const double & x, const double & v)
 {
+  const double V0 = this->dataPtr->V;
+  const double P0 = this->dataPtr->P;
   // geometry: V = V_dead + A*x
   this->dataPtr->V = this->dataPtr->config_->dead_volume + x * this->dataPtr->config_->piston_area;
   // polytropic relationship: P = P0*(V0/V)^n
-  this->dataPtr->P = this->dataPtr->P0 *
-    pow(this->dataPtr->V0 / this->dataPtr->V, this->dataPtr->n);
+  this->dataPtr->P = P0 * pow(V0 / this->dataPtr->V, this->dataPtr->n);
   // Ideal Gas Law: T = P*V/(m*R)
   this->dataPtr->T = this->dataPtr->P * this->dataPtr->V / this->dataPtr->config_->c;
 
@@ -250,11 +286,14 @@ void PolytropicPneumaticSpring::computeForce(const double & x, const double & v)
     // Rodrigues, M. J. (June 5, 2014). Heat Transfer During the Piston-Cylinder Expansion of a Gas
     // (Master's thesis, Oregon State University).
     // Retrieved from https://ir.library.oregonstate.edu/downloads/ww72bf399
-    // heat loss rate for polytropic idea gas:
+    // 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
+    const double r = 0.045;
+    const double A = (2.0 * this->dataPtr->config_->piston_area) * r * x;
     this->dataPtr->Q_rate =
       (1.0 - this->dataPtr->n / PolytropicPneumaticSpringConfig::ADIABATIC_INDEX) * cp_R *
-      this->dataPtr->P * this->dataPtr->config_->piston_area * v;
+      this->dataPtr->P * A * v;
   }
 
   // F = P*A
@@ -293,8 +332,11 @@ void PolytropicPneumaticSpring::Configure(
   config.VelMode = _sdf->Get<bool>(
     "VelMode", false).first;
   config.T0 = SdfParamDouble(_sdf, "T0", config.T0);
+  config.r = SdfParamDouble(_sdf, "r", config.r);
 
   config.hysteresis = _sdf->Get<bool>("hysteresis", false).first;
+  config.vel_dz_lower = SdfParamDouble(_sdf, "velocity_deadzone_lower", config.vel_dz_lower);
+  config.vel_dz_upper = SdfParamDouble(_sdf, "velocity_deadzone_upper", config.vel_dz_upper);
   if (config.hysteresis) {
     config.n1 = SdfParamDouble(_sdf, "n1", PolytropicPneumaticSpringConfig::ADIABATIC_INDEX);
     config.n2 = SdfParamDouble(_sdf, "n2", PolytropicPneumaticSpringConfig::ADIABATIC_INDEX);
@@ -308,6 +350,7 @@ void PolytropicPneumaticSpring::Configure(
     this->dataPtr->n = config.n1;
     this->dataPtr->P1 = SdfParamDouble(_sdf, "P1", this->dataPtr->P1);
     this->dataPtr->P2 = SdfParamDouble(_sdf, "P2", this->dataPtr->P2);
+    this->dataPtr->P0 = this->dataPtr->P1;
     config.x1 = SdfParamDouble(_sdf, "x1", config.x1);
     config.x2 = SdfParamDouble(_sdf, "x2", config.x2);
 
@@ -344,6 +387,11 @@ void PolytropicPneumaticSpring::Configure(
     igndbg << "mass: " << this->dataPtr->mass << std::endl;
   }
 
+  this->dataPtr->V = this->dataPtr->V0;
+  this->dataPtr->P = this->dataPtr->P0;
+  // Ideal Gas Law: T = P*V/(m*R)
+  this->dataPtr->T = this->dataPtr->P * this->dataPtr->V / config.c;
+
   config.model = ignition::gazebo::Model(_entity);
   if (!config.model.Valid(_ecm)) {
     ignerr << "PolytropicPneumaticSpring plugin should be attached to a model entity. " <<
@@ -494,47 +542,44 @@ void PolytropicPneumaticSpring::PreUpdate(
     v *= -1.0;
   }
 
+  x = std::min(std::max(x, 0.0), this->dataPtr->config_->stroke);
+
   const int dt_nano =
     static_cast<int>(std::chrono::duration_cast<std::chrono::nanoseconds>(_info.dt).count());
 
   static const double PASCAL_TO_PSI = 1.450377e-4;  // PSI/Pascal
   const double pressure_diff = (spring_state.lower_psi - spring_state.upper_psi) / PASCAL_TO_PSI;
 
-  if (this->dataPtr->config_->hysteresis) {
-    if (spring_state.valve_command) {
-      openValve(
-        dt_nano, pressure_diff,
-        this->dataPtr->P1, this->dataPtr->config_->V1,
-        this->dataPtr->P2, this->dataPtr->config_->V2);
-    } else if (spring_state.pump_command) {
-      pumpOn(
-        dt_nano,
-        this->dataPtr->P1, this->dataPtr->config_->V1,
-        this->dataPtr->P2, this->dataPtr->config_->V2);
-    }
+  if (spring_state.valve_command) {
+    openValve(
+      dt_nano, pressure_diff,
+      this->dataPtr->P, this->dataPtr->V);
+  } else if (spring_state.pump_command) {
+    pumpOn(
+      dt_nano,
+      this->dataPtr->P, this->dataPtr->V);
+  }
 
-    if (v >= 0.0) {
+  if (this->dataPtr->config_->hysteresis) {
+    if (v >= this->dataPtr->config_->vel_dz_upper) {
       this->dataPtr->n = this->dataPtr->config_->n1;
-      this->dataPtr->V0 = this->dataPtr->config_->V1;
-      this->dataPtr->P0 = this->dataPtr->P1;
-    } else {
+      computePolytropicForce(x, v);
+    } else if (v <= this->dataPtr->config_->vel_dz_lower) {
       this->dataPtr->n = this->dataPtr->config_->n2;
-      this->dataPtr->V0 = this->dataPtr->config_->V2;
-      this->dataPtr->P0 = this->dataPtr->P2;
+      computePolytropicForce(x, v);
+    } else {
+      computeLawOfCoolingForce(x, dt_nano);
     }
   } else {
-    if (spring_state.valve_command) {
-      openValve(
-        dt_nano, pressure_diff,
-        this->dataPtr->P0, this->dataPtr->V0);
-    } else if (spring_state.pump_command) {
-      pumpOn(
-        dt_nano,
-        this->dataPtr->P0, this->dataPtr->V0);
+    if (this->dataPtr->config_->vel_dz_lower <= v &&
+      v <= this->dataPtr->config_->vel_dz_upper)
+    {
+      computeLawOfCoolingForce(x, dt_nano);
+    } else {
+      computePolytropicForce(x, v);
     }
   }
 
-  computeForce(x, v);
   if (this->dataPtr->config_->is_upper) {
     this->dataPtr->F *= -1.0;
   }

buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp

@@ -101,7 +101,8 @@ class PolytropicPneumaticSpring : public ignition::gazebo::System,
     const int dt_nano,
     double & P1, const double & V1,
     double & P2, const double & V2);
-  void computeForce(const double & x, const double & v);
+  void computeLawOfCoolingForce(const double & x, const int & dt_nano);
+  void computePolytropicForce(const double & x, const double & v);
 
   ignition::transport::Node node;
   ignition::transport::Node::Publisher force_pub, pressure_pub, volume_pub,

buoy_tests/launch/pc_commands_ros_feedback_py.launch.py

@@ -214,7 +214,7 @@ def test_pc_commands_ros(self, gazebo_test_fixture, proc_info):
         self.assertLess(self.node.bias_curr_, bc + 0.1)
 
         # TODO(andermi) fix this comparison when motor mode is fixed
-        self.assertLess(self.node.range_finder_, 1.1)  # meters
+        self.assertLess(self.node.range_finder_, 2.03)  # meters
 
         self.test_helper.run(bias_curr_timeout_iterations - bias_curr_iterations +
                              feedbackCheckIterations)

buoy_tests/launch/sc_pump_ros_feedback_py.launch.py

@@ -33,9 +33,9 @@ def test_sc_pump_ros(self, gazebo_test_fixture, proc_info):
         self.assertEqual(t, 0)
         self.assertEqual(self.test_helper.iterations, 0)
 
-        preCmdIterations = 15000
+        preCmdIterations = 40000
         statusCheckIterations = 1000
-        postCmdIterations = 20000
+        postCmdIterations = 60000
 
         # Run simulation server and allow piston to settle
         self.test_helper.run(preCmdIterations)
@@ -67,8 +67,8 @@ def test_sc_pump_ros(self, gazebo_test_fixture, proc_info):
         self.assertFalse(self.node.sc_status_ & SCRecord.LR_FAULT)
         self.assertFalse(self.node.sc_status_ & SCRecord.LR_FAULT)
 
-        # Now send Pump command to run for 20 seconds
-        self.node.send_pump_command(20.0 / 60.0)
+        # Now send Pump command to run for 60 seconds
+        self.node.send_pump_command(1.0)
         self.assertEqual(self.node.pump_future_.result().result.value,
                          self.node.pump_future_.result().result.OK)
 
@@ -133,14 +133,14 @@ def test_sc_pump_ros(self, gazebo_test_fixture, proc_info):
         post_pump_range_finder = self.node.range_finder_
 
         self.assertGreater(post_pump_range_finder,
-                           pre_pump_range_finder - 2.2 * 0.0254 * 20.0 / 60.0,
-                           'Piston should retract 2 inches/min for 20 seconds')
+                           pre_pump_range_finder - 2.2 * 0.0254,
+                           'Piston should retract 2 inches/min for 1 minute')
 
         self.assertLess(post_pump_range_finder,
-                        pre_pump_range_finder - 1.8 * 0.0254 * 20.0 / 60.0,
-                        'Piston should retract 2 inches/min for 20 seconds')
+                        pre_pump_range_finder - 1.8 * 0.0254,
+                        'Piston should retract 2 inches/min for 1 minute')
 
         # TODO(anyone) remove once TestFixture is fixed upstream
         self.test_helper.stop()
 
-        proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=30)
+        proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=60)

buoy_tests/launch/sc_valve_ros_feedback_py.launch.py

@@ -33,7 +33,7 @@ def test_sc_valve_ros(self, gazebo_test_fixture, proc_info):
         self.assertEqual(t, 0)
         self.assertEqual(self.test_helper.iterations, 0)
 
-        preCmdIterations = 15000
+        preCmdIterations = 40000
         statusCheckIterations = 1000
         postCmdIterations = 5000