Fix added mass contribution. (#59)

I noticed that added mass was acting in the wrong direction. This was after visuallizing the individual forces separately (in this case added mass and damping forces). To be honest it still doesn't fix any of our outstanding issues. Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
osrf · Mar 16, 2022 · e388214 · e388214
1 parent 248e5ba
commit e388214
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 5 deletions.
diff --git a/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc b/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc
@@ -274,7 +274,7 @@ void HydrodynamicsPlugin::PreUpdate(
   Ma(3,3) = this->dataPtr->paramKdotP;
   Ma(4,4) = this->dataPtr->paramMdotQ;
   Ma(5,5) = this->dataPtr->paramNdotR;
-  const Eigen::VectorXd kAmassVec = Ma * stateDot;
+  const Eigen::VectorXd kAmassVec = - Ma * stateDot;
 
   // Coriollis and Centripetal forces for under water vehicles (Fossen P. 37)
   // Note: this is significantly different from VRX because we need to account

diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc
@@ -60,7 +60,7 @@ TEST_F(LrauvTestFixture, DepthVBS)
 
   // Run enough iterations (chosen empirically) to reach steady state, then kill
   // the controller
-  int targetIterations{50000};
+  int targetIterations{100000};
   int maxSleep{100};
   int sleep{0};
   for (; sleep < maxSleep && lrauvRunning && this->iterations < targetIterations; ++sleep)
@@ -76,7 +76,7 @@ TEST_F(LrauvTestFixture, DepthVBS)
 
   ignmsg << "Logged [" << this->tethysPoses.size() << "] poses" << std::endl;
 
-  int maxIterations{28000};
+  int maxIterations{50000};
   ASSERT_LT(maxIterations, this->tethysPoses.size());
 
   ignition::math::Vector3d maxVel(0, 0, 0);

diff --git a/lrauv_ignition_plugins/test/test_state_msg.cc b/lrauv_ignition_plugins/test/test_state_msg.cc
@@ -248,10 +248,10 @@ TEST_F(LrauvTestFixture, State)
   EXPECT_NEAR(0.0002, latest.buoyancyposition_(), 1e-3);
 
   // Position
-  EXPECT_LT(0.3, latest.depth_());
+  EXPECT_LT(0.2, latest.depth_());
 
   // NED world frame: higher Z is deeper
-  EXPECT_LT(0.3, latest.pos_().z());
+  EXPECT_LT(0.2, latest.pos_().z());
 
   // Velocity
   // NED world frame: sinking to higher Z