diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp index 395b253961f01ed16d87eb42abded8692424aa7a..15c72e2f7a5dda65029222ad07e00b37c144dafe 100644 --- a/src/processor/processor_force_torque_inertial_dynamics.cpp +++ b/src/processor/processor_force_torque_inertial_dynamics.cpp @@ -404,7 +404,7 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect Matrix<double, 18, 12> J_delta_tangent; Matrix<double, 18, 7> J_delta_model; fti::tangent2delta( - tangent, model, _dt, _delta, J_delta_tangent, J_delta_model); // Aquà ja farà bé ell sol el J_delta_model? + tangent, model, _dt, _delta, J_delta_tangent, J_delta_model); // 3. Compose jacobians Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias; diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp index ad44a282cb1c2b914848ab2aefbbf4c40dbb2ca6..ad54fa8e05b66fa310efe27fc09f3344d6720198 100644 --- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp +++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp @@ -52,7 +52,7 @@ using namespace bodydynamics::fti; WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d); -class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test +class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::Test { public: ProblemPtr P; @@ -65,21 +65,21 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test double mass_true; std::vector<double> time_stamp; double dt; - Vector3d position_i, vlin_i, vang_i, force_i, torque_i, a_meas_i; std::vector<Vector3d> position, vlin, vang, force, torque, a_meas; std::vector<Quaterniond> quaternion; protected: void ExtractAndCompleteData() { + Vector3d position_i, vlin_i, vang_i, force_i, torque_i, a_meas_i; std::ifstream data_simulation; - string data_file = "/home/asanjuan/dev/wolf_lib/bodydynamics/test/dades_simulacio_pep.csv"; + string data_file = "/home/asanjuan/dev/wolf_lib/bodydynamics/test/dades_simulacio_pep.csv"; data_simulation.open(data_file, std::ios::in); - string line_data; - char delimiter = ','; + 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)); + // 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; @@ -91,7 +91,6 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test string string_time_stamp; std::getline(data, string_time_stamp, delimiter); time_stamp.push_back(std::stod(string_time_stamp)); - WOLF_INFO(time_stamp[counter]); // position string string_pos_x; @@ -189,13 +188,13 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test 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].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 + // We have to be careful with the index } counter++; } - } void SetUp() override @@ -204,7 +203,7 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR; ParserYaml parser = - ParserYaml("problem_force_torque_inertial_dynamics_solve_test.yaml", wolf_root + "/test/yaml/"); + ParserYaml("problem_force_torque_inertial_dynamics_simulation_test.yaml", wolf_root + "/test/yaml/"); ParamsServer server = ParamsServer(parser.getParams()); P = Problem::autoSetup(server); // CERES WRAPPER @@ -217,44 +216,79 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test 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 bias_true = Vector6d::Zero(); cdm_true = {0, 0, 0.0341}; inertia_true = {0.0176, 0.0180, 0.0296}; // rounded {0.017598, 0.017957, 0.029599} mass_true = 1.952; - } }; -TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, force_registraion) +TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, force_registraion) { FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create); } -TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, simulation) +TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation) { // Calibration params + Vector6d bias_guess(Vector6d::Zero()); + S->getStateBlock('I')->setState(bias_guess); Vector3d cdm_guess(0.005, 0.005, 0.1); S->getStateBlock('C')->setState(cdm_guess); Vector3d inertia_guess(0.01, 0.01, 0.02); S->getStateBlock('i')->setState(inertia_guess); - double mass_guess = 2.0; + double mass_guess = 1.5; S->getStateBlock('m')->setState(Vector1d(mass_guess)); S->getStateBlock('P')->fix(); S->getStateBlock('O')->fix(); - S->getStateBlock('I')->unfix(); + S->getStateBlock('I')->fix(); S->getStateBlock('C')->unfix(); S->getStateBlock('i')->unfix(); S->getStateBlock('m')->unfix(); S->setStateBlockDynamic('I', false); - + + // add regularization + S->addPriorParameter('C', // cdm + cdm_guess, // cdm + Matrix3d::Identity(), // cov + 0, // start: X coordinate + 3); // size + + S->addPriorParameter('i', // inertia + inertia_guess, // inertia + .00001*Matrix3d::Identity(), // cov + 0, // start: X coordinate + 3); + + S->addPriorParameter('m', // mass + Vector1d(mass_guess), // mass + Matrix1d::Identity(), // cov + 0, // start + 1); std::string report; - for (int i = 1; i < time_stamp.size(); i++) + // 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(); + // fix measured position and orientation + p->getOrigin()->getFrame()->getStateBlock('P')->fix(); + p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[0]); + p->getOrigin()->getFrame()->getStateBlock('O')->fix(); + p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[0].coeffs()); + + + 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 { - //SIMULATOR - + // SIMULATOR + TimeStamp t = time_stamp[i]; // Data @@ -263,27 +297,44 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, simulation) data.segment<3>(3) = vang[i]; data.segment<3>(6) = force[i]; data.segment<3>(9) = torque[i]; - MatrixXd data_cov = MatrixXd::Identity(12, 12); - + MatrixXd data_cov = S->getNoiseCov(); // ESTIMATOR CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr); C->process(); - p->getOrigin()->getFrame()->unfix(); - 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()); - - report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL); - WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m."); - WOLF_INFO("Estimated inertia : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg."); - WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg."); - WOLF_INFO("-----------------------------"); + // 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()); + + + + // solve! + report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF); + + WOLF_INFO(report); + P->print(1, 0, 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("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("-----------------------------"); + } } + // FINAL RESULTS 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."); @@ -294,13 +345,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, simulation) WOLF_INFO("Guess mass : ", mass_guess, " Kg."); WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg."); WOLF_INFO("-----------------------------"); - - WOLF_INFO("Acceleration : ", a_meas[1].transpose()); } int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); // ::testing::GTEST_FLAG(filter) = // "Test_SolveProblemForceTorqueInertialDynamics_yaml.simulation"; diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp index 100292809f8a91411143c8ec3823c87a81afd34c..9508e6cee38df8090ac3748e98a58d70a1ae1197 100644 --- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp +++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp @@ -208,7 +208,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering) WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m."); WOLF_INFO("-----------------------------"); - double dt = 0.2; + double dt = 0.2; double time_last = 5.0; for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt) @@ -291,7 +291,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering) // This is due to these first factors relying on the linearization Jacobian to correct the // poorly preintegrated delta. - double dt = 0.2; + double dt = 0.2; double time_last = 5.0; for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt) @@ -369,7 +369,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m."); WOLF_INFO("-----------------------------"); - double dt = 0.2; + double dt = 0.2; double time_last = 10.0; for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt) @@ -455,7 +455,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m // This is due to these first factors relying on the linearization Jacobian to correct the // poorly preintegrated delta. - double dt = 0.2; + double dt = 0.2; double time_last = 10.0; for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt) @@ -562,7 +562,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_ // This is due to these first factors relying on the linearization Jacobian to correct the // poorly preintegrated delta. - double dt = 0.2; + double dt = 0.2; double time_last = 10.0; for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt) @@ -1092,6 +1092,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, data.segment<3>(3) = ang_vel_true + gyro_bias_true + gyro_noise; data.segment<3>(6) = force_true + force_noise; data.segment<3>(9) = torque_true + torque_noise; + data_cov = S->getNoiseCov(); // Pose measurements (simulate motion capture) position_data = position_true; diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml index cb6915f7f862657f2a18ce16f297f91e027ee73c..c76464f63119b98989d70e69fbc3cc03cd2527b7 100644 --- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml +++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml @@ -61,7 +61,7 @@ config: n_propellers: 3 keyframe_vote: voting_active: true - max_time_span: 0.995 + max_time_span: 0.0995 max_buff_length: 999 # motion deltas dist_traveled: 999 # meters angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)