From 480e3ca86f8cb0fdff3eaeee471d28e305ef456f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 7 Aug 2022 10:40:09 +0200
Subject: [PATCH] cosmetics
---
...problem_force_torque_inertial_dynamics.cpp | 36 ++++++++-----------
1 file changed, 14 insertions(+), 22 deletions(-)
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 9805d12..9944154 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -49,7 +49,7 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
ProblemPtr P;
SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialPreintDynamicsPtr p;
- SolverCeresPtr solver_;
+ SolverCeresPtr solver;
Vector3d cdm_true;
Vector3d inertia_true;
double mass_true;
@@ -63,13 +63,11 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
ParamsServer server = ParamsServer(parser.getParams());
P = Problem::autoSetup(server);
// CERES WRAPPER
- solver_ = std::make_shared<SolverCeres>(P);
- // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;ceres::LINE_SEARCH
- // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
- // solver_->getSolverOptions().max_num_iterations = 1e4;
- // solver_->getSolverOptions().function_tolerance = 1e-15;
- // solver_->getSolverOptions().gradient_tolerance = 1e-15;
- // solver_->getSolverOptions().parameter_tolerance = 1e-15;
+ solver = std::make_shared<SolverCeres>(P);
+ // solver->getSolverOptions().max_num_iterations = 1e4;
+ // solver->getSolverOptions().function_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());
p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
@@ -127,14 +125,13 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
P->print(4, 1, 1, 1);
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);
- P->print(4, 1, 1, 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("-----------------------------");
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
C->setTimeStamp(t);
C->process();
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("-----------------------------");
}
-
-
ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
}
@@ -197,16 +192,13 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
P->print(4, 1, 1, 1);
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)
- P->print(4, 1, 1, 1);
-
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.");
-
-
+ WOLF_INFO("-----------------------------");
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)
@@ -215,7 +207,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
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);
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_INFO("Estimated cdm : ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
}
@@ -269,7 +261,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
P->print(4, 1, 1, 1);
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("True mass : ", mass_true, " Kg.");
@@ -294,7 +286,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
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);
+ 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("-----------------------------");
--
GitLab