Skip to content
Snippets Groups Projects
Commit 52f3d8f1 authored by Amanda Sanjuan Sánchez's avatar Amanda Sanjuan Sánchez
Browse files

fixed errors from the simulation test

parent 7824c199
No related branches found
No related tags found
2 merge requests!31devel->main,!30Complete UAV identification setup
Hide whitespace changes
Inline Side-by-side
test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+ 189
− 57
View file @ 52f3d8f1
......@@ -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();
}
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