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

new simulation test with more complicated data

parent 989e74b8
No related branches found
No related tags found
2 merge requests!31devel->main,!30Complete UAV identification setup
......@@ -37,5 +37,7 @@ wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
wolf_add_gtest(gtest_sensor_inertial_kinematics gtest_sensor_inertial_kinematics.cpp)
wolf_add_gtest(gtest_simulation_problem_force_torque_inertial_dynamics gtest_simulation_problem_force_torque_inertial_dynamics.cpp)
wolf_add_gtest(gtest_solve_problem_force_torque_inertial_dynamics gtest_solve_problem_force_torque_inertial_dynamics.cpp)
This diff is collapsed.
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
#include "bodydynamics/factor/factor_angular_momentum.h"
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
#include "bodydynamics/capture/capture_force_torque_inertial.h"
#include <core/sensor/factory_sensor.h>
#include <core/utils/utils_gtest.h>
#include <core/utils/logging.h>
#include <core/utils/params_server.h>
#include <core/yaml/parser_yaml.h>
#include <core/state_block/state_block_derived.h>
#include <core/state_block/factory_state_block.h>
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/math/rotations.h>
#include <core/tree_manager/tree_manager_sliding_window.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
using namespace wolf;
using namespace bodydynamics::fti;
WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
{
public:
ProblemPtr P;
SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialDynamicsPtr p;
SolverCeresPtr solver;
Vector6d bias_true;
Vector3d cdm_true;
Vector3d inertia_true;
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()
{
std::ifstream data_simulation;
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 = ',';
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;
while (std::getline(data_simulation, line_data))
{
std::stringstream data(line_data);
// time stamp
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;
string string_pos_y;
string string_pos_z;
std::getline(data, string_pos_x, delimiter);
std::getline(data, string_pos_y, delimiter);
std::getline(data, string_pos_z, delimiter);
position_i(0) = std::stod(string_pos_x);
position_i(1) = std::stod(string_pos_y);
position_i(2) = std::stod(string_pos_z);
position.push_back(position_i);
// quaternion
string string_quat_x;
string string_quat_y;
string string_quat_z;
string string_quat_w;
std::getline(data, string_quat_x, delimiter);
std::getline(data, string_quat_y, delimiter);
std::getline(data, string_quat_z, delimiter);
std::getline(data, string_quat_w, delimiter);
Quaterniond quaternion_i(std::stod(string_quat_w),
std::stod(string_quat_x),
std::stod(string_quat_y),
std::stod(string_quat_z));
quaternion.push_back(quaternion_i);
// linear velocity
string string_vlin_x;
string string_vlin_y;
string string_vlin_z;
std::getline(data, string_vlin_x, delimiter);
std::getline(data, string_vlin_y, delimiter);
std::getline(data, string_vlin_z, delimiter);
vlin_i(0) = std::stod(string_vlin_x);
vlin_i(1) = std::stod(string_vlin_y);
vlin_i(2) = std::stod(string_vlin_z);
vlin.push_back(vlin_i);
// angular velocity
string string_vang_x;
string string_vang_y;
string string_vang_z;
std::getline(data, string_vang_x, delimiter);
std::getline(data, string_vang_y, delimiter);
std::getline(data, string_vang_z, delimiter);
vang_i(0) = std::stod(string_vang_x);
vang_i(1) = std::stod(string_vang_y);
vang_i(2) = std::stod(string_vang_z);
vang.push_back(vang_i);
// force
string string_force_x;
string string_force_y;
string string_force_z;
std::getline(data, string_force_x, delimiter);
std::getline(data, string_force_y, delimiter);
std::getline(data, string_force_z, delimiter);
force_i(0) = std::stod(string_force_x);
force_i(1) = std::stod(string_force_y);
force_i(2) = std::stod(string_force_z);
force.push_back(force_i);
// torque
string string_torque_x;
string string_torque_y;
string string_torque_z;
std::getline(data, string_torque_x, delimiter);
std::getline(data, string_torque_y, delimiter);
std::getline(data, string_torque_z, delimiter);
torque_i(0) = std::stod(string_torque_x);
torque_i(1) = std::stod(string_torque_y);
torque_i(2) = std::stod(string_torque_z);
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.push_back(a_meas_i);
//We have to be careful with the index
}
counter++;
}
}
void SetUp() override
{
ExtractAndCompleteData();
std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
ParserYaml parser =
ParserYaml("problem_force_torque_inertial_dynamics_solve_test.yaml", wolf_root + "/test/yaml/");
ParamsServer server = ParamsServer(parser.getParams());
P = Problem::autoSetup(server);
// CERES WRAPPER
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<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
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)
{
FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
}
TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, simulation)
{
// Calibration params
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;
S->getStateBlock('m')->setState(Vector1d(mass_guess));
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);
std::string report;
for (int i = 1; i < time_stamp.size(); i++)
{
//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 = MatrixXd::Identity(12, 12);
// 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("-----------------------------");
}
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("-----------------------------");
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";
return RUN_ALL_TESTS();
}
......@@ -69,10 +69,10 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
P = Problem::autoSetup(server);
// CERES WRAPPER
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;
// 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<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
......@@ -208,7 +208,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0; t <= 22.0; t += 1.0)
double dt = 0.2;
double time_last = 5.0;
for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
......@@ -287,7 +290,11 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
// which if not eliminated contaminate the overall solution.
// This is due to these first factors relying on the linearization Jacobian to correct the
// poorly preintegrated delta.
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0; t <= 50.0; t += 1.0)
double dt = 0.2;
double time_last = 5.0;
for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
......@@ -362,7 +369,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0; t <= 30.0; t += 1.0)
double dt = 0.2;
double time_last = 10.0;
for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
......@@ -444,7 +454,11 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
// which if not eliminated contaminate the overall solution.
// This is due to these first factors relying on the linearization Jacobian to correct the
// poorly preintegrated delta.
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0; t <= 26.0; t += 1.0)
double dt = 0.2;
double time_last = 10.0;
for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
......@@ -494,12 +508,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
// add regularization to unobservable states
S->addPriorParameter('C', // cdm
Vector1d(cdm_true(2)), // cdm Z
Matrix1d::Identity() * 1e2, // cov Z
Matrix1d::Identity() * 1, // cov Z
2, // start: Z coordinate
1); // size
S->addPriorParameter('i', // inertia
Vector2d(inertia_true.segment<2>(0)), // inertia XY
Matrix2d::Identity() * 1e2, // cov XY
Matrix2d::Identity() * 0.01, // cov XY
0, // start: X coordinate
2); // size: 2
......@@ -547,7 +561,11 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
// which if not eliminated contaminate the overall solution.
// This is due to these first factors relying on the linearization Jacobian to correct the
// poorly preintegrated delta.
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0; t <= 30.0; t += 1.0)
double dt = 0.2;
double time_last = 10.0;
for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
{
CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
......@@ -614,12 +632,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant)
Vector3d ang_acc_true;
Vector3d angle_true;
Quaterniond quat_true(1, 0, 0, 0);
double dt = 1; // time increment
double dt = 0.2; // time increment
Vector3d position_data(0, 0, 0);
Quaterniond q_data;
std::string report;
for (TimeStamp t = 1.0; t <= 18.0; t += 1.0)
for (TimeStamp t = dt; t <= 5.0; t += dt)
{
// SIMULATOR
......@@ -729,11 +747,11 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
// initialiation of all the variables needed in the following iteration process
// Vector3d dL;
double dt = 0.2; // time increment
double dt = 0.1; // time increment
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
std::string report;
for (TimeStamp t = dt; t <= 8.0; t += dt)
for (TimeStamp t = dt; t <= 10.0; t += dt)
{
// SIMULATOR
......@@ -865,12 +883,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, torque_yaw_constant__u
// add regularization to unobservable states
S->addPriorParameter('C', // cdm
Vector1d(cdm_true(2)), // cdm Z
Matrix1d::Identity() * 1e2, // cov Z
Matrix1d::Identity() * 1, // cov Z
2, // start: Z coordinate
1); // size
S->addPriorParameter('i', // inertia
Vector2d(inertia_true.segment<2>(0)), // inertia XY
Matrix2d::Identity() * 1e2, // cov XY
Matrix2d::Identity() * 0.01, // cov XY
0, // start: X coordinate
2); // size: 2
......@@ -976,7 +994,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
Vector3d acc_true = -gravity();
Vector3d ang_vel_true = Vector3d(0, 0, 0);
Vector3d force_true = -mass_true * gravity();
Vector3d torque_true = Vector3d(0, 0, 0.01);
Vector3d torque_true = Vector3d(0, 0, 0.1);
Vector3d position_true = Vector3d(0, 0, 0);
Quaterniond quat_true = Quaterniond(1, 0, 0, 0);
Vector3d ang_acc_true = inertia_true.asDiagonal().inverse() * torque_true;
......@@ -987,10 +1005,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
// bias_true.setZero();
// Noise
Vector3d acc_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
Vector3d gyro_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
Vector3d force_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
Vector3d torque_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
Vector3d acc_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
Vector3d gyro_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
Vector3d force_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
Vector3d torque_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
// Data -- hovering and constant torque in yaw
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
......@@ -1025,12 +1043,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
// add regularization to unobservable states
S->addPriorParameter('C', // cdm
Vector1d(cdm_true(2)), // cdm Z
Matrix1d::Identity() * .1, // cov Z
Matrix1d::Identity() * 1, // cov Z
2, // start: Z coordinate
1); // size
S->addPriorParameter('i', // inertia
Vector2d(inertia_true.segment<2>(0)), // inertia XY
Matrix2d::Identity() * .01, // cov XY
Matrix2d::Identity() * .001, // cov XY
0, // start: X coordinate
2); // size: 2
......@@ -1047,8 +1065,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
C_KF0->getFrame()->getStateBlock('O')->setState(orient_data);
// initialiation of all the variables needed in the following iteration process
double t_final = 24.0; // total run time
double dt = 0.1; // time increment
double t_final = 2.40; // total run time
double dt = 0.01; // time increment
unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
std::string report;
......@@ -1065,9 +1083,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
// noises
acc_noise = Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->acc_noise_std;
gyro_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
force_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
torque_noise = 0*Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
gyro_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->gyro_noise_std;
force_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->force_noise_std;
torque_noise = 0 * Vector3d::Random() * S->getParamsSensorForceTorqueInertial()->torque_noise_std;
// FTI measurements
data.segment<3>(0) = acc_true + acc_bias_true + acc_noise;
......@@ -1099,8 +1117,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
// solve!
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
//WOLF_INFO(report);
P->print(4,1,1,1);
// WOLF_INFO(report);
// P->print(4,1,1,1);
// results of this iteration
WOLF_INFO("Total angle turned : ", angle_true.transpose(), " rad.");
......@@ -1141,8 +1159,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
::testing::GTEST_FLAG(filter) =
"Test_SolveProblemForceTorqueInertialDynamics_yaml.torque_yaw_constant__unfix_all__nonzero_bias_and_nonzero_"
"noise";
// ::testing::GTEST_FLAG(filter) =
// "Test_SolveProblemForceTorqueInertialDynamics_yaml.torque_yaw_constant__unfix_all__nonzero_bias_and_nonzero_noise";
return RUN_ALL_TESTS();
}
......@@ -18,8 +18,8 @@ config:
tree_manager:
type: "TreeManagerSlidingWindow"
plugin: "core"
n_frames: 3
n_fix_first_frames: 1
n_frames: 6
n_fix_first_frames: 0
viral_remove_empty_parent: true
map:
type: "MapBase"
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment