From 65eebf7838172b6dbad6fa5bd6404807e86d8992 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 7 Aug 2022 00:19:48 +0200
Subject: [PATCH] Fix m and com tests with more KFs.
---
...problem_force_torque_inertial_dynamics.cpp | 44 ++++++++++++++++---
1 file changed, 39 insertions(+), 5 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index f152da8..9805d12 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -135,6 +135,22 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
WOLF_INFO("True mass : ", mass_true, " Kg.");
WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+
+
+ CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
+ {
+ C->setTimeStamp(t);
+ C->process();
+ p->getOrigin()->getFrame()->fix();
+ report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("-----------------------------");
+ }
+
+
+
+ ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
}
TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hovering)
@@ -175,6 +191,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
C_KF0->getFrame()->fix();
+ C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
C_KF1->getFrame()->fix();
P->print(4, 1, 1, 1);
@@ -188,6 +205,23 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
+
+
+
+ CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
+ for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 11.0 ; t += 1.0)
+ {
+ C->setTimeStamp(t);
+ C->process();
+ p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
+ p->getOrigin()->getFrame()->fix();
+ report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("-----------------------------");
+ }
+
+
+ ASSERT_MATRIX_APPROX(S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
}
TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
@@ -229,7 +263,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
C_KF0->getFrame()->fix();
- C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero());
+ C_KF1->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
C_KF1->getFrame()->fix();
P->print(4, 1, 1, 1);
@@ -239,10 +273,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
WOLF_INFO(report); // should show a very low iteration number (possibly 1)
WOLF_INFO("True mass : ", mass_true, " Kg.");
- WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
- WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
@@ -258,7 +292,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
{
C->setTimeStamp(t);
C->process();
- p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero());
+ p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
p->getOrigin()->getFrame()->fix();
report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
@@ -277,6 +311,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.mass_and_cdm_hovering";
+ // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.cdm_only_hovering";
return RUN_ALL_TESTS();
}
--
GitLab