From 4aa88dec0e903c0202ff1865e74c72bc984f2bb4 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 3 Nov 2021 16:17:50 +0800 Subject: [PATCH 1/3] Fix added mass contribution. 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 --- lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc b/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc index f80fe3ee..c10c63ed 100644 --- a/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc +++ b/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc @@ -275,7 +275,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 From 5ac10ac9c7a871d18610c90a67c0ceae50f8f72a Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 17 Nov 2021 20:27:49 +0800 Subject: [PATCH 2/3] fic buoyancy action test Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_buoyancy_action.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_buoyancy_action.cc b/lrauv_ignition_plugins/test/test_buoyancy_action.cc index 9894c4ae..130887e4 100644 --- a/lrauv_ignition_plugins/test/test_buoyancy_action.cc +++ b/lrauv_ignition_plugins/test/test_buoyancy_action.cc @@ -57,7 +57,7 @@ TEST_F(LrauvTestFixture, Sink) EXPECT_GT(targetZ, this->tethysPoses.back().Pos().Z()); // It pitches backwards because the buoyancy engine is closer to the back - EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 0.13); + EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 0.14); // Roll and yaw aren't affected EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Roll(), 1e-6); @@ -65,7 +65,7 @@ TEST_F(LrauvTestFixture, Sink) // TODO(anyone) Fix residual movement // https://github.com/osrf/lrauv/issues/132 - EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 2.5); + EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 2.8); EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().Y(), 1.1); } From c28986b00a352baff492b83493fca73d1bb315f8 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 11 Mar 2022 12:50:54 +0800 Subject: [PATCH 3/3] Because added mass now opposes the direction of acceleration, the vehicle sinks slower now. Thus the tests have had to be updated. Signed-off-by: Arjo Chakravarty --- lrauv_ignition_plugins/test/test_mission_depth_vbs.cc | 4 ++-- lrauv_ignition_plugins/test/test_state_msg.cc | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc b/lrauv_ignition_plugins/test/test_mission_depth_vbs.cc index 6a45e8c4..5acecef7 100644 --- 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 index 7d9b8863..2dfc5d0c 100644 --- 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