Document propOmega, flip thruster, fix coefficient

lrauv_description/models/tethys/model.sdf

@@ -344,7 +344,7 @@
     </joint>
 
     <joint name="propeller_joint" type="revolute">
-      <pose>0 0 0 0 0 0</pose>
+      <pose degrees="true">0 0 0   0 180 0</pose>
       <parent>base_link</parent>
       <child>propeller</child>
       <axis>

lrauv_description/models/tethys_equipped/model.sdf

@@ -93,7 +93,8 @@
         name="ignition::gazebo::systems::Thruster">
         <namespace>tethys</namespace>
         <joint_name>propeller_joint</joint_name>
-        <thrust_coefficient>0.004422</thrust_coefficient>
+        <!-- Be sure to update TethysComm when updating these numbers -->
+        <thrust_coefficient>0.00004422</thrust_coefficient>
         <fluid_density>1000</fluid_density>
         <propeller_diameter>0.2</propeller_diameter>
         <velocity_control>true</velocity_control>

lrauv_ignition_plugins/example/example_thruster.cc

@@ -21,7 +21,10 @@
  */
 
 /**
- * Send velocity commands to the thruster.
+ * Send angular velocity commands to the propeller.
+ *
+ * Positive values rotate the propeller clockwise when looking
+ * from the back, and propel the vehicle forward.
  *
  * Usage:
  *   $ LRAUV_example_thruster <vehicle_name> <rad_per_sec>

lrauv_ignition_plugins/proto/lrauv_command.proto

@@ -37,7 +37,12 @@ message LRAUVCommand
   /// \brief Optional header data
   ignition.msgs.Header header  = 1;
 
-  float propOmega_             = 2;
+  float propOmega_             = 2;  // Angular velocity that the controller
+                                     // believes the propeller is currently at.
+                                     // Unit: rad / s. Positive values rotate
+                                     // the propeller clockwise when looking
+                                     // from the back, and propel the vehicle
+                                     // forward.
   float rudderAngle_           = 3;  // Angle that the controller believes the
                                      // rudder is currently at. Unit: radians.
                                      // Higher values have the vertical fins
@@ -60,7 +65,11 @@ message LRAUVCommand
   bool dropWeightState_        = 7;  // Indicator "dropweight OK"
                                      // 1 = in place, 0 = dropped
 
-  float propOmegaAction_       = 8;
+  float propOmegaAction_       = 8;  // Target angular velocity for the
+                                     // propeller. Unit: rad / s. Positive
+                                     // values rotate the propeller clockwise
+                                     // when looking from the back, and propel
+                                     // the vehicle forward.
   float rudderAngleAction_     = 9;  // Target angle for rudder joint. Unit:
                                      // radians. Higher values rotate the
                                      // vertical fins clockwise when looking

lrauv_ignition_plugins/proto/lrauv_state.proto

@@ -43,9 +43,13 @@ message LRAUVState
   int32 errorPad_                 = 2;
   int32 utmZone_                  = 3;
   int32 northernHemi_             = 4;
-  float propOmega_                = 5;
-  float propThrust_               = 6;
-  float propTorque_               = 7;
+  float propOmega_                = 5;  // Current angular velocity of the
+                                        // propeller. Unit: rad / s. Positive
+                                        // values rotate the propeller
+                                        // clockwise when looking from the back,
+                                        // and propel the vehicle forward.
+  float propThrust_               = 6;  // Not populated
+  float propTorque_               = 7;  // Not populated
   float rudderAngle_              = 8;  // Angle that the rudder joint is
                                         // currently at. Unit: radians.
                                         // Higher values have the vertical fins
@@ -64,7 +68,8 @@ message LRAUVState
 
   ignition.msgs.Vector3d rph_     = 13;  // roll_, pitch_, heading_ in SimResultStruct (rad)
 
-  float speed_                    = 14;
+  float speed_                    = 14;  // Magnitude of linear velocity in the
+                                         // world frame. Unit: m / s
   double latitudeDeg_             = 15;
   double longitudeDeg_            = 16;
   float netBuoy_                  = 17;  // Net buoyancy forces applied to the

lrauv_ignition_plugins/src/TethysCommPlugin.cc

@@ -414,10 +414,15 @@ void TethysCommPlugin::CommandCallback(
   // Thruster
   ignition::msgs::Double thrusterMsg;
   // TODO(arjo):
-  // Conversion from rpm-> force b/c thruster plugin takes force
+  // Conversion from angular velocity to force b/c thruster plugin takes force
   // Maybe we should change that?
+  // https://github.com/osrf/lrauv/issues/75
   auto angVel = _msg.propomegaaction_();
-  auto force = -0.004422 * 1000 * 0.0016 * angVel * angVel;
+
+  // force = thrust_coefficient * fluid_density * omega ^ 2 *
+  //         propeller_diameter ^ 4
+  // These values are defined in the model's Thruster plugin's SDF
+  auto force = 0.00004422 * 1000 * 0.2 * angVel * angVel;
   if (angVel < 0)
   {
     force *=-1;
@@ -479,8 +484,8 @@ void TethysCommPlugin::PostUpdate(
   const ignition::gazebo::UpdateInfo &_info,
   const ignition::gazebo::EntityComponentManager &_ecm)
 {
-  ignition::gazebo::Link baseLink(modelLink);
-  auto modelPose = ignition::gazebo::worldPose(modelLink, _ecm);
+  ignition::gazebo::Link baseLink(this->modelLink);
+  auto modelPose = ignition::gazebo::worldPose(this->modelLink, _ecm);
 
   // Publish state
   lrauv_ignition_plugins::msgs::LRAUVState stateMsg;
@@ -500,7 +505,7 @@ void TethysCommPlugin::PostUpdate(
     ignerr << "Propeller joint has wrong size\n";
     return;
   }
-  stateMsg.set_propomega_(-propAngVelComp->Data()[0]);
+  stateMsg.set_propomega_(propAngVelComp->Data()[0]);
 
   // Rudder joint position
   auto rudderPosComp =
@@ -553,7 +558,7 @@ void TethysCommPlugin::PostUpdate(
   // Speed
   auto linearVelocity =
     _ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
-    modelLink);
+    this->modelLink);
   stateMsg.set_speed_(linearVelocity->Data().Length());
 
   // Lat long

lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh

@@ -71,6 +71,8 @@ class LrauvTestFixture : public ::testing::Test
       [&](const ignition::gazebo::UpdateInfo &_info,
       const ignition::gazebo::EntityComponentManager &_ecm)
       {
+        this->dt = _info.dt;
+
         auto worldEntity = ignition::gazebo::worldEntity(_ecm);
         ignition::gazebo::World world(worldEntity);
 
@@ -233,6 +235,9 @@ class LrauvTestFixture : public ::testing::Test
   /// \brief How many times has OnPostUpdate been run
   public: unsigned int iterations{0u};
 
+  /// \brief Latest simulation time step.
+  public: std::chrono::steady_clock::duration dt{0};
+
   /// \brief All tethys world poses in order
   public: std::vector<ignition::math::Pose3d> tethysPoses;
 

lrauv_ignition_plugins/test/test_controller.cc

@@ -37,23 +37,52 @@ TEST_F(LrauvTestFixture, Command)
   EXPECT_EQ(100, this->tethysPoses.size());
   EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 1e-6);
 
-  // Propel vehicle
+  // Propel vehicle forward by giving the propeller a positive angular velocity
+  // Vehicle is supposed to move at around 1 m/s with 300 RPM.
+  // 300 RPM = 300 * 2 pi / 60 = 10 pi rad/s
   lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
-  cmdMsg.set_propomegaaction_(30);
+  cmdMsg.set_propomegaaction_(10 * IGN_PI);
 
   // Neutral buoyancy
+  cmdMsg.set_dropweightstate_(true);
   cmdMsg.set_buoyancyaction_(0.0005);
 
   // Run server until the command is processed and the model reaches a certain
   // point ahead (the robot moves towards -X)
-  double targetX{-1.5};
+  double targetX{-100.0};
   this->PublishCommandWhile(cmdMsg, [&]()
   {
     return this->tethysPoses.back().Pos().X() > targetX;
   });
 
-  EXPECT_LT(100, this->iterations);
-  EXPECT_LT(100, this->tethysPoses.size());
+  int minIterations{5100};
+  EXPECT_LT(minIterations, this->iterations);
+  EXPECT_LT(minIterations, this->tethysPoses.size());
+
+  // Check final position
   EXPECT_GT(targetX, this->tethysPoses.back().Pos().X());
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Y(), 1e-3);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Z(), 0.05);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Roll(), 1e-2);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 1e-3);
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Yaw(), 1e-3);
+
+  // Check approximate instantaneous speed after initial acceleration
+  double dtSec = std::chrono::duration<double>(this->dt).count();
+  ASSERT_LT(0.0, dtSec);
+  double time100it = 100 * dtSec;
+  for (unsigned int i = 1000; i < this->tethysPoses.size(); i += 100)
+  {
+    auto prevPose = this->tethysPoses[i - 100];
+    auto pose = this->tethysPoses[i];
+
+    auto dist = (pose.Pos() - prevPose.Pos()).Length();
+
+    auto linVel = dist / time100it;
+    EXPECT_LT(0.0, linVel);
+
+    // TODO(chapulina) Decrease tolerance
+    EXPECT_NEAR(1.0, linVel, 0.19) << i;
+  }
 }
 

lrauv_ignition_plugins/test/test_mission_yoyo_circle.cc

@@ -75,137 +75,46 @@ TEST_F(LrauvTestFixture, YoYoCircle)
 
   ignmsg << "Logged [" << this->tethysPoses.size() << "] poses" << std::endl;
 
-  int maxIterations{28000};
-  ASSERT_LT(maxIterations, this->tethysPoses.size());
-
-  // Uncomment to get new expectations
-  // for (int i = 500; i <= maxIterations; i += 500)
-  // {
-  //   auto pose = this->tethysPoses[i];
-  //   std::cout << "this->CheckRange(" << i << ", {"
-  //       << std::setprecision(2) << std::fixed
-  //       << pose.Pos().X() << ", "
-  //       << pose.Pos().Y() << ", "
-  //       << pose.Pos().Z() << ", "
-  //       << pose.Rot().Roll() << ", "
-  //       << pose.Rot().Pitch() << ", "
-  //       << pose.Rot().Yaw() << "}, {12.0, 1.57});"
-  //       << std::endl;
-  // }
-
-  // TODO(louise) Make expectations tighter as various inputs are updated
-
-  //          Y
-  //            ^
-  //            |
-  //            |         View from the top
-  //            |
-  //            |
-  //            |     _
-  //            |    / |
-  //       ,--------'  '_
-  //      (             _+ -----------> X
-  //       `--------,  ,
-  //                 \_|
-  //
-  // * (X/Y): With a positive rudder angle, the robot rotates counter-clockwise.
-  //          So it stays on the -Y side and goes:
-  //          -X-Y -> +X-Y -> +X+Y -> -X+Y -> repeat
-  // * (Z): starts at zero, but once it goes below -2 m, it never goes deeper than
-  //        -20 m, and never higher than -2 m.
-  // * (Pitch): always between -20 deg and 20 deg
-
-  // With 0.02 step size, the vehicle does a lap every 7000 iterations or so
-
-  // -X: decrease X
-  // -Y: decrease Y
-  this->CheckRange(500, {-0.16, 0.00, -0.04, 0.01, -0.00, 0.00}, {12.0, 1.57});
-  this->CheckRange(1000, {-3.61, 0.09, 0.09, 0.01, -0.04, 0.15}, {12.0, 1.57});
-  this->CheckRange(1500, {-10.30, -1.30, 0.04, -0.00, -0.11, 0.47}, {12.0, 1.57});
-  this->CheckRange(2000, {-17.51, -5.89, -0.58, -0.01, -0.17, 0.88}, {12.0, 1.57});
-  this->CheckRange(2500, {-22.54, -13.64, -1.73, -0.01, -0.21, 1.33}, {12.0, 1.57});
-  this->CheckRange(3000, {-23.68, -23.05, -3.24, -0.01, -0.24, 1.79}, {12.0, 1.57});
-
-  // +X: increase X
-  this->CheckRange(3500, {-20.43, -32.02, -5.03, -0.01, -0.26, 2.27}, {12.0, 1.57});
-  this->CheckRange(4000, {-13.43, -38.51, -6.97, -0.00, -0.27, 2.74}, {12.0, 1.57});
-  this->CheckRange(4500, {-4.24, -41.06, -9.00, 0.00, -0.29, -3.06}, {12.0, 1.57});
-  this->CheckRange(5000, {5.05, -39.10, -11.18, 0.01, -0.29, -2.58}, {12.0, 1.57});
-
-  // +Y: increase Y
-  this->CheckRange(5500, {12.38, -33.08, -13.39, 0.02, -0.30, -2.10}, {12.0, 1.57});
-  this->CheckRange(6000, {16.10, -24.36, -15.63, 0.02, -0.31, -1.62}, {12.0, 1.57});
-
-  // -X: decrease X
-  this->CheckRange(6500, {15.37, -14.96, -17.99, 0.02, -0.32, -1.14}, {12.0, 1.57});
-  this->CheckRange(7000, {10.39, -7.01, -20.55, 0.02, -0.16, -0.66}, {12.0, 1.57});
-  this->CheckRange(7500, {2.00, -2.06, -21.82, 0.00, -0.04, -0.19}, {12.0, 1.57});
-  this->CheckRange(8000, {-7.86, -1.46, -22.27, -0.01, 0.01, 0.28}, {12.0, 1.57});
-
-  // -Y: decrease Y
-  this->CheckRange(8500, {-16.95, -5.44, -22.31, -0.02, 0.06, 0.76}, {12.0, 1.57});
-  this->CheckRange(9000, {-23.22, -13.11, -21.99, -0.02, 0.10, 1.23}, {12.0, 1.57});
-  this->CheckRange(9500, {-25.29, -22.76, -21.37, -0.01, 0.13, 1.70}, {12.0, 1.57});
-
-  // +X: increase X
-  this->CheckRange(10000, {-22.75, -32.26, -20.48, 0.01, 0.16, 2.18}, {12.0, 1.57});
-  this->CheckRange(10500, {-16.18, -39.51, -19.34, 0.02, 0.18, 2.65}, {12.0, 1.57});
-  this->CheckRange(11000, {-7.07, -42.95, -17.99, 0.02, 0.21, 3.13}, {12.0, 1.57});
-
-  // +Y: increase Y
-  this->CheckRange(11500, {2.56, -41.84, -16.46, 0.02, 0.23, -2.68}, {12.0, 1.57});
-  this->CheckRange(12000, {10.57, -36.46, -14.75, 0.01, 0.24, -2.20}, {12.0, 1.57});
-  this->CheckRange(12500, {15.21, -28.04, -12.94, -0.00, 0.26, -1.72}, {12.0, 1.57});
-  this->CheckRange(13000, {15.46, -18.47, -10.98, -0.01, 0.27, -1.25}, {12.0, 1.57});
-
-  // -X: decrease X
-  this->CheckRange(13500, {11.27, -9.88, -8.96, -0.02, 0.28, -0.77}, {12.0, 1.57});
-  this->CheckRange(14000, {3.64, -4.21, -6.82, -0.02, 0.30, -0.29}, {12.0, 1.57});
-  this->CheckRange(14500, {-5.72, -2.71, -4.59, -0.03, 0.30, 0.19}, {12.0, 1.57});
-
-  // -Y: decrease Y
-  this->CheckRange(15000, {-14.67, -5.69, -2.15, -0.02, 0.22, 0.68}, {12.0, 1.57});
-  this->CheckRange(15500, {-21.37, -12.64, -0.51, -0.01, 0.06, 1.15}, {12.0, 1.57});
-  this->CheckRange(16000, {-24.23, -22.08, 0.09, 0.01, -0.00, 1.62}, {12.0, 1.57});
-
-  // +X: increase X
-  this->CheckRange(16500, {-22.48, -31.83, 0.20, 0.02, -0.05, 2.09}, {12.0, 1.57});
-  this->CheckRange(17000, {-16.47, -39.71, -0.05, 0.02, -0.09, 2.57}, {12.0, 1.57});
-  this->CheckRange(17500, {-7.55, -43.97, -0.61, 0.01, -0.12, 3.04}, {12.0, 1.57});
-
-  // +Y: increase Y
-  this->CheckRange(18000, {2.29, -43.69, -1.45, -0.01, -0.15, -2.77}, {12.0, 1.57});
-  this->CheckRange(18500, {10.87, -38.98, -2.53, -0.02, -0.18, -2.29}, {12.0, 1.57});
-  this->CheckRange(19000, {16.33, -30.90, -3.83, -0.03, -0.20, -1.82}, {12.0, 1.57});
-  this->CheckRange(19500, {17.48, -21.26, -5.34, -0.02, -0.22, -1.34}, {12.0, 1.57});
-
-  // -X: decrease X
-  this->CheckRange(20000, {14.10, -12.22, -7.00, -0.01, -0.24, -0.86}, {12.0, 1.57});
-  this->CheckRange(20500, {6.97, -5.76, -8.81, 0.00, -0.25, -0.39}, {12.0, 1.57});
-  this->CheckRange(21000, {-2.29, -3.31, -10.74, 0.02, -0.27, 0.09}, {12.0, 1.57});
-
-  // -Y: decrease Y
-  this->CheckRange(21500, {-11.60, -5.40, -12.79, 0.03, -0.28, 0.57}, {12.0, 1.57});
-  this->CheckRange(22000, {-18.89, -11.54, -14.88, 0.04, -0.29, 1.05}, {12.0, 1.57});
-  this->CheckRange(22500, {-22.50, -20.31, -17.10, 0.04, -0.31, 1.53}, {12.0, 1.57});
-
-  // +X: increase X
-  this->CheckRange(23000, {-21.65, -29.69, -19.53, 0.03, -0.24, 2.02}, {12.0, 1.57});
-  this->CheckRange(23500, {-16.42, -37.76, -21.33, 0.01, -0.07, 2.49}, {12.0, 1.57});
-  this->CheckRange(24000, {-7.88, -42.67, -21.96, -0.02, -0.00, 2.96}, {12.0, 1.57});
-  this->CheckRange(24500, {2.01, -43.16, -22.10, -0.04, 0.05, -2.85}, {12.0, 1.57});
-
-  // +Y: increase Y
-  this->CheckRange(25000, {11.04, -39.07, -21.87, -0.03, 0.09, -2.37}, {12.0, 1.57});
-  this->CheckRange(25500, {17.19, -31.34, -21.31, -0.01, 0.13, -1.90}, {12.0, 1.57});
-  this->CheckRange(26000, {19.12, -21.68, -20.48, 0.01, 0.15, -1.42}, {12.0, 1.57});
-
-  // -X: decrease X
-  this->CheckRange(26500, {16.43, -12.26, -19.39, 0.03, 0.18, -0.95}, {12.0, 1.57});
-  this->CheckRange(27000, {9.76, -5.16, -18.07, 0.04, 0.20, -0.47}, {12.0, 1.57});
-  this->CheckRange(27500, {0.62, -1.91, -16.59, 0.03, 0.22, 0.01}, {12.0, 1.57});
-
-  // -Y: decrease Y
-  this->CheckRange(28000, {-8.96, -3.21, -14.93, 0.02, 0.23, 0.49}, {12.0, 1.57});
+  int minIterations{28000};
+  ASSERT_LT(minIterations, this->tethysPoses.size());
+
+  // Check bounds
+  double dtSec = std::chrono::duration<double>(this->dt).count();
+  ASSERT_LT(0.0, dtSec);
+  double time100it = 100 * dtSec;
+  for (unsigned int i = 100; i < this->tethysPoses.size(); i += 100)
+  {
+    auto prevPose = this->tethysPoses[i - 100];
+    auto pose = this->tethysPoses[i];
+
+    // Speed is about 1 m / s
+    auto dist = (pose.Pos() - prevPose.Pos()).Length();
+
+    auto linVel = dist / time100it;
+    EXPECT_LT(0.0, linVel);
+
+    EXPECT_NEAR(1.0, linVel, 1.0) << i;
+
+    // Depth is above 20m, and below 2m after initial descent, with some
+    // tolerance
+    EXPECT_LT(-22.1, pose.Pos().Z()) << i;
+    if (i > 2000)
+    {
+      EXPECT_GT(0.0, pose.Pos().Z()) << i;
+    }
+
+    // Pitch is between -20 and 20 deg
+    EXPECT_LT(IGN_DTOR(-20), pose.Rot().Pitch()) << i;
+    EXPECT_GT(IGN_DTOR(20), pose.Rot().Pitch()) << i;
+
+    // Trajectory projected onto the horizontal plane is roughly a circle
+    ignition::math::Vector2d planarPos{pose.Pos().X(), pose.Pos().Y()};
+
+    ignition::math::Vector2d center{-4.0, -23.0};
+    planarPos -= center;
+
+    double meanRadius{20.0};
+    EXPECT_NEAR(20.0, planarPos.Length(), 4.0) << i;
+  }
 }
 

lrauv_ignition_plugins/test/test_rudder.cc

@@ -63,6 +63,6 @@ TEST_F(LrauvTestFixture, Rudder)
 
   // Vehicle makes a clockwise arch looking from the top
   EXPECT_GT(targetY, this->tethysPoses.back().Pos().Y());
-  EXPECT_GT(-1.5, this->tethysPoses.back().Pos().X());
+  EXPECT_GT(-1.0, this->tethysPoses.back().Pos().X());
 }