Skip to content
Snippets Groups Projects
Commit 480e3ca8 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

cosmetics

parent 65eebf78
No related branches found
No related tags found
3 merge requests!31devel->main,!29Add functionality for UAV,!27Fix test with acc_x = 2 m/s2
Hide whitespace changes
Inline Side-by-side
test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+ 14
− 22
View file @ 480e3ca8
...@@ -49,7 +49,7 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing:: ...@@ -49,7 +49,7 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
ProblemPtr P; ProblemPtr P;
SensorForceTorqueInertialPtr S; SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialPreintDynamicsPtr p; ProcessorForceTorqueInertialPreintDynamicsPtr p;
SolverCeresPtr solver_; SolverCeresPtr solver;
Vector3d cdm_true; Vector3d cdm_true;
Vector3d inertia_true; Vector3d inertia_true;
double mass_true; double mass_true;
...@@ -63,13 +63,11 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing:: ...@@ -63,13 +63,11 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
ParamsServer server = ParamsServer(parser.getParams()); ParamsServer server = ParamsServer(parser.getParams());
P = Problem::autoSetup(server); P = Problem::autoSetup(server);
// CERES WRAPPER // CERES WRAPPER
solver_ = std::make_shared<SolverCeres>(P); solver = std::make_shared<SolverCeres>(P);
// solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;ceres::LINE_SEARCH // solver->getSolverOptions().max_num_iterations = 1e4;
// solver_->getSolverOptions().max_line_search_step_contraction = 1e-3; // solver->getSolverOptions().function_tolerance = 1e-15;
// solver_->getSolverOptions().max_num_iterations = 1e4; // solver->getSolverOptions().gradient_tolerance = 1e-15;
// solver_->getSolverOptions().function_tolerance = 1e-15; // solver->getSolverOptions().parameter_tolerance = 1e-15;
// solver_->getSolverOptions().gradient_tolerance = 1e-15;
// solver_->getSolverOptions().parameter_tolerance = 1e-15;
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front()); S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front()); p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
...@@ -127,14 +125,13 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri ...@@ -127,14 +125,13 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
P->print(4, 1, 1, 1); P->print(4, 1, 1, 1);
WOLF_INFO("======== SOLVE PROBLEM =======") WOLF_INFO("======== SOLVE PROBLEM =======")
std::string report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL); std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); WOLF_INFO(report);
P->print(4, 1, 1, 1);
WOLF_INFO("True mass : ", mass_true, " Kg."); WOLF_INFO("True mass : ", mass_true, " Kg.");
WOLF_INFO("Guess mass : ", mass_guess, " Kg."); WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg."); WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
WOLF_INFO("-----------------------------");
CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr); CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
...@@ -143,13 +140,11 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri ...@@ -143,13 +140,11 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
C->setTimeStamp(t); C->setTimeStamp(t);
C->process(); C->process();
p->getOrigin()->getFrame()->fix(); p->getOrigin()->getFrame()->fix();
report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL); report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg."); WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
WOLF_INFO("-----------------------------"); WOLF_INFO("-----------------------------");
} }
ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3); ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
} }
...@@ -197,16 +192,13 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin ...@@ -197,16 +192,13 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
P->print(4, 1, 1, 1); P->print(4, 1, 1, 1);
WOLF_INFO("======== SOLVE PROBLEM =======") WOLF_INFO("======== SOLVE PROBLEM =======")
std::string report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL); std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); // should show a very low iteration number (possibly 1) WOLF_INFO(report); // should show a very low iteration number (possibly 1)
P->print(4, 1, 1, 1);
WOLF_INFO("True cdm : ", cdm_true.transpose(), " m."); WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m."); WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m."); WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr); 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) for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 11.0 ; t += 1.0)
...@@ -215,7 +207,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin ...@@ -215,7 +207,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
C->process(); C->process();
p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
p->getOrigin()->getFrame()->fix(); p->getOrigin()->getFrame()->fix();
report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL); report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m."); WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------"); WOLF_INFO("-----------------------------");
} }
...@@ -269,7 +261,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov ...@@ -269,7 +261,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
P->print(4, 1, 1, 1); P->print(4, 1, 1, 1);
WOLF_INFO("======== SOLVE PROBLEM =======") WOLF_INFO("======== SOLVE PROBLEM =======")
std::string report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL); std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report); // should show a very low iteration number (possibly 1) WOLF_INFO(report); // should show a very low iteration number (possibly 1)
WOLF_INFO("True mass : ", mass_true, " Kg."); WOLF_INFO("True mass : ", mass_true, " Kg.");
...@@ -294,7 +286,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov ...@@ -294,7 +286,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
C->process(); C->process();
p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
p->getOrigin()->getFrame()->fix(); p->getOrigin()->getFrame()->fix();
report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL); report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg."); WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m."); WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------"); WOLF_INFO("-----------------------------");
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment