Use REP103 SI units convention

.github/workflows/build-and-test.sh

@@ -25,6 +25,8 @@ cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC
 wget https://raw.githubusercontent.com/osrf/buoy_entrypoint/main/buoy_all.yaml
 vcs import --skip-existing < buoy_all.yaml
 
+rm -rf buoy_examples
+
 rosdep init
 rosdep update
 rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO

buoy_description/models/mbari_wec/model.sdf

@@ -20,7 +20,7 @@
       <is_upper>true</is_upper>
       <!-- measure of valve opening cross-section and duration (meter-seconds) -->
       <valve_absement>49e-7</valve_absement>
-      <pump_absement>13e-8</pump_absement>
+      <pump_absement>18e-8</pump_absement>
       <pump_pressure>1.7e+6</pump_pressure>
       <stroke>2.03</stroke>
       <piston_area>0.0127</piston_area>
@@ -29,12 +29,12 @@
       <R_specific>0.2968</R_specific>
       <c_p>1.04</c_p>
       <hysteresis>true</hysteresis>
-      <n1>1.2371</n1>
-      <n2>1.2307</n2>
-      <x1>0.9790</x1>
-      <x2>1.1912</x2>
-      <P1>410291</P1>
-      <P2>410266</P2>
+      <n1>1.1725</n1>
+      <n2>1.2139</n2>
+      <x1>1.6159</x1>
+      <x2>1.8578</x2>
+      <P1>410152</P1>
+      <P2>410190</P2>
     </plugin>
 
     <!-- <debug_prescribed_velocity>true</debug_prescribed_velocity>-->
@@ -44,7 +44,7 @@
       <is_upper>false</is_upper>
       <!-- measure of valve opening cross-section and duration (meter-seconds) -->
       <valve_absement>49e-7</valve_absement>
-      <pump_absement>13e-8</pump_absement>
+      <pump_absement>18e-8</pump_absement>
       <pump_pressure>1.7e+6</pump_pressure>
       <stroke>2.03</stroke>
       <piston_area>0.0115</piston_area>
@@ -53,12 +53,12 @@
       <R_specific>0.2968</R_specific>
       <c_p>1.04</c_p>
       <hysteresis>true</hysteresis>
-      <n1>1.2327</n1>
-      <n2>1.2518</n2>
-      <x1>1.1958</x1>
-      <x2>1.3935</x2>
-      <P1>1212090</P1>
-      <P2>1212140</P2>
+      <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>-->
 

buoy_gazebo/CMakeLists.txt

@@ -16,6 +16,8 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(buoy_description REQUIRED)
+# find_package(buoy_interfaces REQUIRED)
+# find_package(rclcpp REQUIRED)
 
 find_package(ignition-cmake2 REQUIRED)
 find_package(ignition-plugin1 REQUIRED COMPONENTS register)
@@ -98,6 +100,36 @@ endfunction()
 
 add_subdirectory(src)
 
+add_library(BuoyECMStateData INTERFACE)
+target_include_directories(BuoyECMStateData INTERFACE
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>"
+  "$<INSTALL_INTERFACE:include>"
+)
+
+install(TARGETS BuoyECMStateData
+  EXPORT export_BuoyECMStateData
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib
+  INCLUDES DESTINATION include
+)
+
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src/buoy_utils
+  DESTINATION include
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src/ElectroHydraulicPTO
+  DESTINATION include/buoy_gazebo
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src/PolytropicPneumaticSpring
+  DESTINATION include/buoy_gazebo
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+ament_export_targets(export_BuoyECMStateData)
 
 # Resources
 install(DIRECTORY

buoy_gazebo/package.xml

@@ -20,6 +20,7 @@
   <depend>rviz2</depend>
   <depend>sdformat_urdf</depend>
   <depend>splinter_ros</depend>
+  <depend>libgnuplot-iostream-dev</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp

@@ -140,69 +140,12 @@ void ElectroHydraulicPTO::Configure(
 
   this->dataPtr->x.setConstant(2.0, 0.0);
 
-
   std::string pistonvel_topic = std::string("/pistonvel_") + PrismaticJointName;
   pistonvel_pub = node.Advertise<ignition::msgs::Double>(pistonvel_topic);
   if (!pistonvel_pub) {
     ignerr << "Error advertising topic [" << pistonvel_topic << "]" << std::endl;
     return;
   }
-
-  std::string rpm_topic = std::string("/rpm_") + PrismaticJointName;
-  rpm_pub = node.Advertise<ignition::msgs::Double>(rpm_topic);
-  if (!rpm_pub) {
-    ignerr << "Error advertising topic [" << rpm_topic << "]" << std::endl;
-    return;
-  }
-
-  std::string deltaP_topic = std::string("/deltaP_") + PrismaticJointName;
-  deltaP_pub = node.Advertise<ignition::msgs::Double>(deltaP_topic);
-  if (!deltaP_pub) {
-    ignerr << "Error advertising topic [" << deltaP_topic << "]" << std::endl;
-    return;
-  }
-
-  std::string targwindcurr_topic = std::string("/targwindcurr_") + PrismaticJointName;
-  targwindcurr_pub = node.Advertise<ignition::msgs::Double>(targwindcurr_topic);
-  if (!targwindcurr_pub) {
-    ignerr << "Error advertising topic [" << targwindcurr_topic << "]" << std::endl;
-    return;
-  }
-
-  std::string windcurr_topic = std::string("/windcurr_") + PrismaticJointName;
-  windcurr_pub = node.Advertise<ignition::msgs::Double>(windcurr_topic);
-  if (!windcurr_pub) {
-    ignerr << "Error advertising topic [" << windcurr_topic << "]" << std::endl;
-    return;
-  }
-
-  std::string battcurr_topic = std::string("/battcurr_") + PrismaticJointName;
-  battcurr_pub = node.Advertise<ignition::msgs::Double>(battcurr_topic);
-  if (!battcurr_pub) {
-    ignerr << "Error advertising topic [" << battcurr_topic << "]" << std::endl;
-    return;
-  }
-
-  std::string loadcurr_topic = std::string("/loadcurr_") + PrismaticJointName;
-  loadcurr_pub = node.Advertise<ignition::msgs::Double>(loadcurr_topic);
-  if (!loadcurr_pub) {
-    ignerr << "Error advertising topic [" << loadcurr_topic << "]" << std::endl;
-    return;
-  }
-
-  std::string scalefactor_topic = std::string("/scalefactor_") + PrismaticJointName;
-  scalefactor_pub = node.Advertise<ignition::msgs::Double>(scalefactor_topic);
-  if (!scalefactor_pub) {
-    ignerr << "Error advertising topic [" << scalefactor_topic << "]" << std::endl;
-    return;
-  }
-
-  std::string retractfactor_topic = std::string("/retractfactor_") + PrismaticJointName;
-  retractfactor_pub = node.Advertise<ignition::msgs::Double>(retractfactor_topic);
-  if (!retractfactor_pub) {
-    ignerr << "Error advertising topic [" << retractfactor_topic << "]" << std::endl;
-    return;
-  }
 }
 
 //////////////////////////////////////////////////
@@ -250,6 +193,9 @@ void ElectroHydraulicPTO::PreUpdate(
   double xdot = prismaticJointVelComp->Data().at(0);
   this->dataPtr->functor.Q = xdot * 39.4 * this->dataPtr->PistonArea;  // inch^3/second
 
+  // Need to set to actual ram position for soft-stop at ends, mid-span for now
+  this->dataPtr->functor.I_Wind.RamPosition = 40.0;
+
   // Compute Resulting Rotor RPM and Force applied to Piston based on kinematics
   // and quasistatic forces.  These neglect oil compressibility and rotor inertia,
   // but do include mechanical and volumetric efficiency of hydraulic motor.
@@ -300,12 +246,17 @@ void ElectroHydraulicPTO::PreUpdate(
 
 
   // Solve Electrical
+  // TODO(hamilton) temporary fix for NaN situation. Should make this more robust
+  // or at least parameterized.
+  // Problem: If I repeatedly smash the PC with a -30 Amp winding current command, this solution
+  // becomes unstable and rpm/pressure reach NaN and gazebo crashes. I'm clipping it
+  // to the max absolute rpm from the winding current interpolation
+  // (no extrapolation, default torque controller).
+  // const double N = std::min(std::max(this->dataPtr->x[0U], -6790.0), 6790.0);
   const double N = this->dataPtr->x[0U];
   double deltaP = this->dataPtr->x[1U];
   this->dataPtr->TargetWindingCurrent = this->dataPtr->functor.I_Wind.I;
-  unsigned int seed{1U};
-  this->dataPtr->WindingCurrent = this->dataPtr->TargetWindingCurrent + 0.001 *
-    (rand_r(&seed) % 200 - 100);
+  this->dataPtr->WindingCurrent = this->dataPtr->TargetWindingCurrent;
 
   static const double eff_e = 0.85;
   static const double RPM_TO_RAD_PER_SEC = 2.0 * M_PI / 60.0;
@@ -357,82 +308,17 @@ void ElectroHydraulicPTO::PreUpdate(
     this->dataPtr->PrismaticJointEntity,
     pto_state);
 
-
   auto stampMsg = ignition::gazebo::convert<ignition::msgs::Time>(_info.simTime);
 
   ignition::msgs::Double pistonvel;
   pistonvel.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
   pistonvel.set_data(xdot);
 
-  ignition::msgs::Double rpm;
-  rpm.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
-  rpm.set_data(N);
-
-  ignition::msgs::Double deltap;
-  deltap.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
-  deltap.set_data(deltaP);
-
-  ignition::msgs::Double targwindcurr;
-  targwindcurr.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
-  targwindcurr.set_data(this->dataPtr->TargetWindingCurrent);
-
-  ignition::msgs::Double windcurr;
-  windcurr.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
-  windcurr.set_data(this->dataPtr->WindingCurrent);
-
-  ignition::msgs::Double battcurr;
-  battcurr.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
-  battcurr.set_data(I_Batt);
-
-  ignition::msgs::Double loadcurr;
-  loadcurr.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
-  loadcurr.set_data(I_Load);
-
-  ignition::msgs::Double scalefactor;
-  scalefactor.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
-  scalefactor.set_data(this->dataPtr->functor.I_Wind.ScaleFactor);
-
-  ignition::msgs::Double retractfactor;
-  retractfactor.mutable_header()->mutable_stamp()->CopyFrom(stampMsg);
-  retractfactor.set_data(this->dataPtr->functor.I_Wind.RetractFactor);
 
   if (!pistonvel_pub.Publish(pistonvel)) {
     ignerr << "could not publish pistonvel" << std::endl;
   }
 
-  if (!rpm_pub.Publish(rpm)) {
-    ignerr << "could not publish rpm" << std::endl;
-  }
-
-  if (!deltaP_pub.Publish(deltap)) {
-    ignerr << "could not publish deltaP" << std::endl;
-  }
-
-  if (!targwindcurr_pub.Publish(targwindcurr)) {
-    ignerr << "could not publish targwindcurr" << std::endl;
-  }
-
-  if (!windcurr_pub.Publish(windcurr)) {
-    ignerr << "could not publish windcurr" << std::endl;
-  }
-
-  if (!battcurr_pub.Publish(battcurr)) {
-    ignerr << "could not publish battcurr" << std::endl;
-  }
-
-  if (!loadcurr_pub.Publish(loadcurr)) {
-    ignerr << "could not publish loadcurr" << std::endl;
-  }
-
-  if (!scalefactor_pub.Publish(scalefactor)) {
-    ignerr << "could not publish scalefactor" << std::endl;
-  }
-
-  if (!retractfactor_pub.Publish(retractfactor)) {
-    ignerr << "could not publish retractfactor" << std::endl;
-  }
-
-
   // 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) {