Skip to content

Commit

Permalink
Refine Spring (#102)
Browse files Browse the repository at this point in the history 
* matched spring pretty well; need to do linters/tests

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

* make spring pressure continuous across hysteresis

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

* better fitting curves; almost there

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

* fit model to test bench

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

* test;linters

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

* avoiding timeout from longer test

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

* avoid timeout

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

Signed-off-by: Michael Anderson <[email protected]>
  • Loading branch information
@andermi
andermi committed Oct 28, 2022
1 parent 6576e7c commit a51b8ec
Show file tree
Hide file tree
Showing 14 changed files with 47,147 additions and 47,084 deletions.
25 changes: 15 additions & 10 deletions buoy_description/models/mbari_wec/model.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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>
123 changes: 84 additions & 39 deletions buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

#include <ignition/msgs/double.pb.h>

#include <algorithm>
#include <memory>
#include <mutex>
#include <string>
Expand All @@ -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};
Expand Down Expand Up @@ -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};

Expand Down Expand Up @@ -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;
}

////////////////////////////////////////////////
Expand Down Expand Up @@ -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;

Expand All @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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);

Expand Down Expand Up @@ -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. " <<
Expand Down Expand Up @@ -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;
}
Expand Down
3 changes: 2 additions & 1 deletion buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
4 changes: 2 additions & 2 deletions buoy_tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ if(BUILD_TESTING)

if(buoy_add_gtest_LAUNCH_TEST)
add_launch_test(launch/${TEST_NAME}.launch.py
TIMEOUT 300
TIMEOUT 600
)
else()
ament_add_gtest_test(${TEST_NAME})
Expand Down Expand Up @@ -127,7 +127,7 @@ if(BUILD_TESTING)
endif()

add_launch_test(launch/${PYTEST_NAME}_py.launch.py
TIMEOUT 300
TIMEOUT 600
)
endfunction()

Expand Down
2 changes: 1 addition & 1 deletion buoy_tests/launch/pc_commands_ros_feedback.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ def generate_test_description():
class PCCommandsROSTest(unittest.TestCase):

def test_termination(self, gazebo_test_fixture, proc_info):
proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=200)
proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=600)


@launch_testing.post_shutdown_test()
Expand Down
2 changes: 1 addition & 1 deletion buoy_tests/launch/pc_commands_ros_feedback_py.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion buoy_tests/launch/sc_commands_ros_feedback.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ def generate_test_description():
class SCCommandsROSTest(unittest.TestCase):

def test_termination(self, gazebo_test_fixture, proc_info):
proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=300)
proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=600)


@launch_testing.post_shutdown_test()
Expand Down
18 changes: 9 additions & 9 deletions buoy_tests/launch/sc_pump_ros_feedback_py.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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 1 minute
self.node.send_pump_command(1.0)
self.assertEqual(self.node.pump_future_.result().result.value,
self.node.pump_future_.result().result.OK)

Expand Down Expand Up @@ -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=600)
2 changes: 1 addition & 1 deletion buoy_tests/launch/sc_valve_ros_feedback_py.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
Loading

0 comments on commit a51b8ec

Please sign in to comment.