diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp index 066973c22f8f8b53d4b3ddd3fa9a6cd66c0d896b..9c4a5981605da4b2f6cb769f819cea4d4d284acd 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(); }