From fb4d8183e162c188ea7971687fdd4669c7b214f1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 7 Sep 2022 16:20:11 +0200
Subject: [PATCH] Make batch test and online tests. Both work.
---
...problem_force_torque_inertial_dynamics.cpp | 191 ++++++++++++------
...que_inertial_dynamics_simulation_test.yaml | 2 +-
2 files changed, 133 insertions(+), 60 deletions(-)
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index bcad107..59eb9dd 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -80,8 +80,6 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
string line_data;
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));
int counter = 0;
@@ -187,31 +185,10 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
torque.push_back(torque_i);
-
+ // acceleration -- need to compute from other data
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 = force_i/mass_true;
- // //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
- // }
-
- // if (counter != 0)
- // {
- // dt = time_stamp[counter] - time_stamp[counter - 1];
- // WOLF_INFO("Massa : ", mass_true);
- // WOLF_INFO("Forces : ", (force_i).transpose());
- // WOLF_INFO("Acc mesurada amb forces: ", (force_i/mass_true).transpose());
- // WOLF_INFO("Acc mesurada amb la vel: ", ((vlin[counter] - vlin[counter - 1]) / dt - quaternion[counter].conjugate() * gravity()).transpose())
- // //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++;
}
@@ -235,7 +212,7 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
P = Problem::autoSetup(server);
solver = std::make_shared<SolverCeres>(P);
- // auto options = solver->getSolverOptions();
+ auto options = solver->getSolverOptions();
// solver->getSolverOptions().max_num_iterations = 1e4;
// solver->getSolverOptions().function_tolerance = 1e-15;
// solver->getSolverOptions().gradient_tolerance = 1e-15;
@@ -359,7 +336,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
fout.close();
}
-TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online)
{
// Calibration params
Vector6d bias_guess = S->getStateBlock('I')->getState();
@@ -376,25 +353,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
S->setStateBlockDynamic('I', false);
- // // add regularization, uncomment if the parameter is not fixed
+ // add regularization, uncomment if the parameter is not fixed
- // S->addPriorParameter('C', // cdm
- // cdm_guess, // cdm
- // 0.01*0.01*Matrix3d::Identity(), // cov
- // 0, // start: X coordinate
- // 3); // size
+ S->addPriorParameter('I', // cdm
+ bias_guess, // cdm
+ 0.1*0.1*Matrix6d::Identity()); // cov
- // S->addPriorParameter('i', // inertia
- // inertia_guess, // inertia
- // 0.01*0.01*Matrix3d::Identity(), // cov
- // 0, // start: X coordinate
- // 3);
+ S->addPriorParameter('C', // cdm
+ cdm_guess, // cdm
+ 0.1*0.1*Matrix3d::Identity()); // cov
- // S->addPriorParameter('m', // mass
- // Vector1d(mass_guess), // mass
- // 0.1 * 0.1 * Matrix1d::Identity(), // cov
- // 0, // start
- // 1);
+ S->addPriorParameter('i', // inertia
+ inertia_guess, // inertia
+ 0.01*0.01*Matrix3d::Identity()); // cov
+
+ S->addPriorParameter('m', // mass
+ Vector1d(mass_guess), // mass
+ 0.1 * 0.1 * Matrix1d::Identity()); // cov
std::string report;
@@ -410,10 +385,6 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
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]);
-
- P->print(4, 1, 1, 1);
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
@@ -436,8 +407,6 @@ 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())
{
@@ -448,16 +417,15 @@ 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(4, 1, 1, 1);
// results of this iteration
+ WOLF_INFO("Time : ", t, " s.");
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/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.");
@@ -468,18 +436,123 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ WOLF_INFO(report);
+
// FINAL RESULTS
- WOLF_INFO("True bias : ", bias_true.transpose());
- WOLF_INFO("Guess bias : ", bias_guess.transpose());
- WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose());
- WOLF_INFO("True center of mass : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
+ WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
+ WOLF_INFO("-----------------------------");
+}
+
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
+{
+ // Calibration params
+ Vector6d bias_guess = S->getStateBlock('I')->getState();
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ Vector3d inertia_guess = S->getStateBlock('i')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->unfix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I', false);
+
+
+ // // add regularization, uncomment if the parameter is not fixed
+
+ // S->addPriorParameter('I', // bias
+ // bias_guess, // bias
+ // 0.1 * 0.1 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('C', // cdm
+ // cdm_guess, // cdm
+ // 0.01 * 0.01 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('i', // inertia
+ // inertia_guess, // inertia
+ // 0.01 * 0.01 * Matrix3d::Identity()); // cov
+
+ // S->addPriorParameter('m', // mass
+ // Vector1d(mass_guess), // mass
+ // 0.1 * 0.1 * Matrix1d::Identity()); // cov
+
+ // 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());
+
+ unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+
+ 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();
+
+ // check if new KF
+ if (last_kf_id != p->getOrigin()->getFrame()->id())
+ {
+ last_kf_id = p->getOrigin()->getFrame()->id();
+
+ // fix measured position and orientation
+ p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
+ p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+ p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
+ }
+ }
+
+ std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+
+ // FINAL RESULTS
+ WOLF_INFO("-----------------------------");
+ WOLF_INFO(report);
+ WOLF_INFO("True bias * : ", bias_true.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Guess bias : ", bias_guess.transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("Estimated bias : ", S->getStateBlock('I')->getState().transpose(), " m/s2 - rad/s.");
+ WOLF_INFO("True center of mass * : ", cdm_true.transpose(), " m.");
WOLF_INFO("Guess center of mass : ", cdm_guess.transpose(), " m.");
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
- WOLF_INFO("True inertia : ", inertia_true.transpose(), " m^2 Kg.");
+ WOLF_INFO("True inertia * : ", inertia_true.transpose(), " m^2 Kg.");
WOLF_INFO("Guess inertia : ", inertia_guess.transpose(), " m^2 Kg.");
WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
- WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("True mass * : ", mass_true, " Kg.");
WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
@@ -491,7 +564,7 @@ int main(int argc, char** argv)
// ::testing::GTEST_FLAG(filter) =
// "Test_SimulationProblemForceTorqueInertialDynamics_yaml.preintegration_and_csv";
::testing::GTEST_FLAG(filter) =
- "Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation";
+ "Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation*";
return RUN_ALL_TESTS();
}
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
index df68db8..d2878a2 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
@@ -46,7 +46,7 @@ config:
# Dynamics
com: [0.005,0.005,0.01] # center of mass [m]
- inertia: [0.013,0.013,0.024] # moments of inertia i_xx, i_yy, i_zz [kg m2]
+ inertia: [0.015,0.015,0.030] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 2.00 # mass [kg]
processors:
--
GitLab