From cbaf09d124c6fc20a605c5074f1e419fc082a018 Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Tue, 9 Aug 2022 18:20:17 +0200
Subject: [PATCH] added estimated ang mom to gtest

---
 ...solve_problem_force_torque_inertial_dynamics.cpp | 13 ++++++++-----
 1 file changed, 8 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 066973c..9c4a598 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -205,7 +205,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
     WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
     WOLF_INFO("-----------------------------");
 
-    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 60.0 ; t += 1.0)
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
     {
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
         C->process();
@@ -284,7 +284,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
     // which if not eliminated contaminate the overall solution.
     // This is due to these first factors relying on the linearization Jacobian to correct the
     // poorly preintegrated delta.
-    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 60.0 ; t += 1.0)
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
     {
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->setTimeStamp(t);
@@ -359,7 +359,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
     WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
     WOLF_INFO("-----------------------------");
 
-    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 60.0 ; t += 1.0)
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
     {
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
         C->process();
@@ -367,6 +367,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
         p->getOrigin()->getFrame()->getStateBlock('L')->unfix();
         report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
         WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+        WOLF_INFO("Estimated ang mom : ", p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(), " m^2 kg/s.");
         WOLF_INFO("-----------------------------");
     }
 
@@ -439,7 +440,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
     // which if not eliminated contaminate the overall solution.
     // This is due to these first factors relying on the linearization Jacobian to correct the
     // poorly preintegrated delta.
-    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 60.0 ; t += 1.0)
+    for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 50.0 ; t += 1.0)
     {
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->setTimeStamp(t);
@@ -449,6 +450,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
         report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
         WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
         WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
+        WOLF_INFO("Estimated ang mom : ", p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(), " m^2 kg/s.");
+
         WOLF_INFO("-----------------------------");
     }
 
@@ -463,6 +466,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.mass_and_cdm_and_ang_mom_hovering";
+    //::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.mass_and_cdm_and_ang_mom_hovering";
     return RUN_ALL_TESTS();
 }
-- 
GitLab