Document propOmega, flip thruster, fix coefficient (#76)

Signed-off-by: Louise Poubel <[email protected]>

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:/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());
 }