From 52f3d8f135256a78de04fc5ff4a01ec21cb19ea0 Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Tue, 6 Sep 2022 11:02:17 +0200
Subject: [PATCH] fixed errors from the simulation test
---
...problem_force_torque_inertial_dynamics.cpp | 246 ++++++++++++++----
1 file changed, 189 insertions(+), 57 deletions(-)
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index 7fe58bd..730eefc 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -67,6 +67,8 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
double dt;
std::vector<Vector3d> position, vlin, vang, force, torque, a_meas;
std::vector<Quaterniond> quaternion;
+ // Fitxer CSV
+ std::fstream fout;
protected:
void extractAndCompleteData(const std::string& file_path_name)
@@ -79,7 +81,7 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
char delimiter = ',';
std::getline(data_simulation, line_data);
// this acceleration is just to fill a gap, it is not going to be used and it is not true
- a_meas.push_back(Vector3d(0, 0, 0));
+ // a_meas.push_back(Vector3d(0, 0, 0));
int counter = 0;
@@ -185,14 +187,19 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
torque.push_back(torque_i);
- if (counter != 0)
- {
- dt = time_stamp[counter] - time_stamp[counter - 1];
- a_meas_i = quaternion[counter].conjugate() * ((vlin[counter] - vlin[counter - 1]) / dt - gravity());
- // a_meas_i = quaternion[counter] * ((vlin[counter] - vlin[counter - 1]) / dt - gravity());
- a_meas.push_back(a_meas_i);
- // We have to be careful with the index
- }
+
+ a_meas_i = force_i/mass_true;
+
+ a_meas.push_back(a_meas_i);
+
+ // if (counter != 0)
+ // {
+ // dt = time_stamp[counter] - time_stamp[counter - 1];
+ // //a_meas_i = (vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity();
+ // a_meas.push_back(a_meas_i);
+ // // We have to be careful with the index
+ // }
+
counter++;
}
}
@@ -202,13 +209,15 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
extractAndCompleteData(wolf_bodydynamics_root + "/test/dades_simulacio_pep.csv");
+
- ParserYaml parser =
- ParserYaml("problem_force_torque_inertial_dynamics_simulation_test.yaml", wolf_bodydynamics_root + "/test/yaml/");
+ ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics_simulation_test.yaml",
+ wolf_bodydynamics_root + "/test/yaml/");
ParamsServer server = ParamsServer(parser.getParams());
P = Problem::autoSetup(server);
- solver = std::make_shared<SolverCeres>(P);
+ solver = std::make_shared<SolverCeres>(P);
+ auto options = solver->getSolverOptions();
// solver->getSolverOptions().max_num_iterations = 1e4;
// solver->getSolverOptions().function_tolerance = 1e-15;
// solver->getSolverOptions().gradient_tolerance = 1e-15;
@@ -217,7 +226,7 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
- //need to change with the real values, waiting for Pep's answer
+ // need to change with the real values, waiting for Pep's answer
bias_true = Vector6d::Zero();
cdm_true = {0, 0, 0};
inertia_true = {0.0134943, 0.0141622, 0.0237319}; // rounded {0.017598, 0.017957, 0.029599}
@@ -225,18 +234,127 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
}
};
-TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, force_registraion)
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, force_registration)
{
FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
}
+//this test checks the pre-integration evolution along the time
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_and_csv)
+{
+ std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ // creem un nou arxiu CSV per imprimir els estats que estem rebent ara
+ fout.open(wolf_bodydynamics_root + "/test/dades_simulacio_pep_estimador.csv",
+ std::fstream::out | std::fstream::trunc);
+
+ S->getStateBlock('I')->setState(bias_true);
+ S->getStateBlock('C')->setState(cdm_true);
+ S->getStateBlock('i')->setState(inertia_true);
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->fix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->fix();
+ S->setStateBlockDynamic('I', false);
+
+ //Writing the first lines to know how the data will be distributed in the csv
+ fout << "time stamp"
+ << ","
+ << "position_x_true"
+ << ","
+ << "position_y_true"
+ << ","
+ << "position_z_true"
+ << ","
+ << "quaternion_x_true"
+ << ","
+ << "quaternion_y_true"
+ << ","
+ << "quaternion_z_true"
+ << ","
+ << "quaternion_w_true"
+ << ","
+ << "position_x_est"
+ << ","
+ << "position_y_est"
+ << ","
+ << "position_z_est"
+ << ","
+ << "quaternion_x_est"
+ << ","
+ << "quaternion_y_est"
+ << ","
+ << "quaternion_z_est"
+ << ","
+ << "quaternion_w_est"
+ << "\n";
+
+ // Force processor to make a KF at t=0
+ CaptureMotionPtr C0 =
+ std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
+ C0->process();
+
+ int i_init = 0;
+
+ // fix measured position and orientation
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
+ p->getOrigin()->getFrame()->getStateBlock('V')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('V')->setState(quaternion[i_init].conjugate() * vlin[i_init]);
+ Vector3d ang_mom_init(
+ vang[i_init](0) * inertia_true(0), vang[i_init](1) * inertia_true(1), vang[i_init](2) * inertia_true(2));
+ p->getOrigin()->getFrame()->getStateBlock('L')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('L')->setState(ang_mom_init);
+
+
+ for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
+ {
+ // SIMULATOR
+
+ TimeStamp t = time_stamp[i];
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = a_meas[i];
+ data.segment<3>(3) = vang[i];
+ data.segment<3>(6) = force[i];
+ data.segment<3>(9) = torque[i];
+ MatrixXd data_cov = S->getNoiseCov();
+
+ // ESTIMATOR
+
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
+ C->process();
+
+ // results printed in the csv in the order established before
+ Vector3d position_est(p->getLast()->getFrame()->getStateBlock('P')->getState());
+ Vector4d quaternion_coeffs_est(p->getLast()->getFrame()->getStateBlock('O')->getState());
+ fout << time_stamp[i] << "," << position[i](0) << "," << position[i](1) << "," << position[i](2) << ","
+ << quaternion[i].coeffs()(0) << "," << quaternion[i].coeffs()(1) << "," << quaternion[i].coeffs()(2)
+ << "," << quaternion[i].coeffs()(3) << "," << position_est(0) << "," << position_est(1) << ","
+ << position_est(2) << "," << quaternion_coeffs_est(0) << "," << quaternion_coeffs_est(1) << ","
+ << quaternion_coeffs_est(2) << "," << quaternion_coeffs_est(3)
+ << "\n";
+
+ }
+}
+
TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
{
// Calibration params
- Vector6d bias_guess(Vector6d::Zero());
- Vector3d cdm_guess(0.005, 0.005, 0.1);
- Vector3d inertia_guess(0.01, 0.01, 0.02);
- double mass_guess = 1.5;
+ Vector6d bias_guess = bias_true;
+ // Vector3d cdm_guess(0.005, 0.005, 0.01);
+ Vector3d cdm_guess = cdm_true;
+ // Vector3d inertia_guess(0.013, 0.013, 0.024);
+ Vector3d inertia_guess = inertia_true;
+ // double mass_gues_s = 2.00;
+ double mass_guess = mass_true;
+
S->getStateBlock('I')->setState(bias_guess);
S->getStateBlock('C')->setState(cdm_guess);
@@ -247,14 +365,15 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
S->getStateBlock('O')->fix();
S->getStateBlock('I')->fix();
S->getStateBlock('C')->unfix();
- S->getStateBlock('i')->unfix();
- S->getStateBlock('m')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->fix();
S->setStateBlockDynamic('I', false);
- // // add regularization
+
+ // // add regularization, uncomment if the parameter is not fixed
// S->addPriorParameter('C', // cdm
- // cdm_guess, // cdm
- // Matrix3d::Identity(), // cov
+ // cdm_guess, // cdm
+ // Matrix3d::Identity(), // cov
// 0, // start: X coordinate
// 3); // size
@@ -264,10 +383,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
// 0, // start: X coordinate
// 3);
- // S->addPriorParameter('m', // mass
- // Vector1d(mass_guess), // mass
- // Matrix1d::Identity(), // cov
- // 0, // start
+ // S->addPriorParameter('m', // mass
+ // Vector1d(mass_guess), // mass
+ // 0.1 * 0.1 * Matrix1d::Identity(), // cov
+ // 0, // start
// 1);
std::string report;
@@ -276,18 +395,22 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
CaptureMotionPtr C0 =
std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
C0->process();
+
+ int i_init = 0;
+
// fix measured position and orientation
p->getOrigin()->getFrame()->getStateBlock('P')->fix();
- p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[0]);
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i_init]);
p->getOrigin()->getFrame()->getStateBlock('O')->fix();
- p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[0].coeffs());
-
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i_init].coeffs());
+ // p->getOrigin()->getFrame()->getStateBlock('V')->fix();
+ // p->getOrigin()->getFrame()->getStateBlock('V')->setState(quaternion[i_init].conjugate() * vlin[i_init]);
P->print(4, 1, 1, 1);
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
- for (int i = 1; i < time_stamp.size(); i++) // start with second data
+ for (int i = i_init + 1; i < time_stamp.size(); i++) // start with second data
{
// SIMULATOR
@@ -305,6 +428,8 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
+
+ P->print(4, 1, 1, 1);
// check if new KF
if (last_kf_id != p->getOrigin()->getFrame()->id())
@@ -316,43 +441,50 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
p->getOrigin()->getFrame()->getStateBlock('O')->fix();
p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
-
-
+ // p->getOrigin()->getFrame()->getStateBlock('V')->fix();
+ // p->getOrigin()->getFrame()->getStateBlock('V')->setState(quaternion[i].conjugate() * vlin[i_init]);
// solve!
- report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ // report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
// WOLF_INFO(report);
- // P->print(1, 0, 1, 1);
+ P->print(4, 1, 1, 1);
// results of this iteration
- WOLF_INFO("Torque : ", torque[i].transpose()," N m .");
- WOLF_INFO("Angular velocity : ", vang[i].transpose()," rad/s .");
- WOLF_INFO("Angular momentum : ", p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(), " kg m²/s .");
+ WOLF_INFO("Torque : ", torque[i].transpose(), " N m .");
+ WOLF_INFO("Angular velocity : ", vang[i].transpose(), " rad/s .");
+ WOLF_INFO("Angular momentum : ",
+ p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(),
+ " kg m²/s .");
WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
- for (auto ftr : p->getOrigin()->getFeatureList())
- {
- if (std::dynamic_pointer_cast<FeatureMotion>(ftr) != nullptr)
- {
- auto fac = std::static_pointer_cast<FactorForceTorqueInertialDynamics>(ftr->getFactorList().front());
- WOLF_INFO("Residual FTI: ", fac->residual().transpose());
- }
- else
- {
- auto fac = std::static_pointer_cast<FactorAngularMomentum>(ftr->getFactorList().front());
- WOLF_INFO("Residual L: ", fac->residual().transpose());
- }
- }
-
-
- if (i>100)break;
+ // for (auto ftr : p->getOrigin()->getFeatureList())
+ // {
+ // if (std::dynamic_pointer_cast<FeatureMotion>(ftr) != nullptr)
+ // {
+ // auto fac =
+ // std::static_pointer_cast<FactorForceTorqueInertialDynamics>(ftr->getFactorList().front());
+ // WOLF_INFO(
+ // "Residual FTI: ",
+ // (ftr->getMeasurementSquareRootInformationUpper().inverse() * fac->residual()).transpose());
+ // }
+ // else
+ // {
+ // auto fac = std::static_pointer_cast<FactorAngularMomentum>(ftr->getFactorList().front());
+ // WOLF_INFO("Residual L: ", fac->residual().transpose());
+ // }
+ // }
+
+ // if (i>100)break;
}
}
+ report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+
// FINAL RESULTS
WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m.");
WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
@@ -366,10 +498,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
WOLF_INFO("-----------------------------");
}
-int main(int argc, char **argv)
+int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) =
- // "Test_SolveProblemForceTorqueInertialDynamics_yaml.simulation";
+ ::testing::GTEST_FLAG(filter) =
+ "Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation";
return RUN_ALL_TESTS();
}
--
GitLab