diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
index 2d509f80415ce98f27d84ee11440086f0fc83595..d9cff87b4f4d8e57588e1b71ec9e9b22e90f7146 100644
--- a/include/bodydynamics/sensor/sensor_force_torque_inertial.h
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -37,6 +37,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
// noise std dev
double acc_noise_std;
double gyro_noise_std;
+ double accb_initial_std;
+ double gyrob_initial_std;
double acc_drift_std;
double gyro_drift_std;
double force_noise_std;
@@ -49,29 +51,33 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
: ParamsSensorBase(_unique_name, _server)
{
- acc_noise_std = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std");
- gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
- acc_drift_std = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
- gyro_drift_std = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std");
- force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
- torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
- com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
- inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
- mass = _server.getParam<double>(prefix + _unique_name + "/mass");
+ acc_noise_std = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std");
+ gyro_noise_std = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
+ accb_initial_std = _server.getParam<double>(prefix + _unique_name + "/accb_initial_std");
+ gyrob_initial_std = _server.getParam<double>(prefix + _unique_name + "/gyrob_initial_std");
+ acc_drift_std = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
+ gyro_drift_std = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std");
+ force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
+ torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
+ com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
+ inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
+ mass = _server.getParam<double>(prefix + _unique_name + "/mass");
}
~ParamsSensorForceTorqueInertial() override = default;
std::string print() const override
{
- return ParamsSensorBase::print() + "\n" //
- + "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" //
- + "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" //
- + "acc_drift_std: " + std::to_string(acc_drift_std) + "\n" //
- + "gyro_drift_std: " + std::to_string(gyro_drift_std) + "\n" //
- + "force_noise_std: " + std::to_string(force_noise_std) + "\n" //
- + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" //
- + "com: print not implemented." + "\n" //
- + "inertia: print not implemented. \n" //
- + "mass: " + std::to_string(mass) + "\n"; //
+ return ParamsSensorBase::print() + "\n" //
+ + "acc_noise_std: " + std::to_string(acc_noise_std) + "\n" //
+ + "gyro_noise_std: " + std::to_string(gyro_noise_std) + "\n" //
+ + "accb_initial_std: " + std::to_string(accb_initial_std) + "\n" //
+ + "gyrob_initial_std: " + std::to_string(gyrob_initial_std) + "\n" //
+ + "acc_drift_std: " + std::to_string(acc_drift_std) + "\n" //
+ + "gyro_drift_std: " + std::to_string(gyro_drift_std) + "\n" //
+ + "force_noise_std: " + std::to_string(force_noise_std) + "\n" //
+ + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n" //
+ + "com: print not implemented." + "\n" //
+ + "inertia: print not implemented. \n" //
+ + "mass: " + std::to_string(mass) + "\n"; //
}
};
@@ -90,14 +96,17 @@ class SensorForceTorqueInertial : public SensorBase
WOLF_SENSOR_CREATE(SensorForceTorqueInertial, ParamsSensorForceTorqueInertial, 7);
// getters return by copy
- Vector6d getImuBias() const; // Imu bias [acc, gyro]
- Vector3d getAccBias() const; // Acc bias
- Vector3d getGyroBias() const; // Gyro bias
- double getMass() const; // Total mass
- Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix)
- Vector3d getCom() const; // centre of mass
- Vector7d getModel() const; // dynamic model [com, inertia, mass]
- ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial(){return params_fti_;}
+ Vector6d getImuBias() const; // Imu bias [acc, gyro]
+ Vector3d getAccBias() const; // Acc bias
+ Vector3d getGyroBias() const; // Gyro bias
+ double getMass() const; // Total mass
+ Vector3d getInertia() const; // Inertia vector (diagonal of inertia matrix)
+ Vector3d getCom() const; // centre of mass
+ Vector7d getModel() const; // dynamic model [com, inertia, mass]
+ ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial()
+ {
+ return params_fti_;
+ }
};
inline double SensorForceTorqueInertial::getMass() const
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index 730eefca64a9fa6aa29ade7f7603c75ddba3ac40..3cf239ae7564a2080533bddfdf00325a430d596a 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -195,10 +195,23 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
// 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++;
}
@@ -206,6 +219,11 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
void SetUp() override
{
+ bias_true = Vector6d::Zero();
+ cdm_true = {0, 0, 0};
+ inertia_true = {0.0134943, 0.0141622, 0.0237319}; // rounded {0.017598, 0.017957, 0.029599}
+ mass_true = 1.9828;
+
std::string wolf_bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
extractAndCompleteData(wolf_bodydynamics_root + "/test/dades_simulacio_pep.csv");
@@ -217,7 +235,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;
@@ -226,11 +244,6 @@ 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
- bias_true = Vector6d::Zero();
- cdm_true = {0, 0, 0};
- inertia_true = {0.0134943, 0.0141622, 0.0237319}; // rounded {0.017598, 0.017957, 0.029599}
- mass_true = 1.9828;
}
};
@@ -334,6 +347,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
// 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) << ","
@@ -347,39 +361,31 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
{
// Calibration params
- 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);
- S->getStateBlock('i')->setState(inertia_guess);
- S->getStateBlock('m')->setState(Vector1d(mass_guess));
+ 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')->fix();
+ S->getStateBlock('I')->unfix();
S->getStateBlock('C')->unfix();
- S->getStateBlock('i')->fix();
- S->getStateBlock('m')->fix();
+ S->getStateBlock('i')->unfix();
+ S->getStateBlock('m')->unfix();
S->setStateBlockDynamic('I', false);
// // add regularization, uncomment if the parameter is not fixed
+
// S->addPriorParameter('C', // cdm
// cdm_guess, // cdm
- // Matrix3d::Identity(), // cov
+ // 0.01*0.01*Matrix3d::Identity(), // cov
// 0, // start: X coordinate
// 3); // size
// S->addPriorParameter('i', // inertia
// inertia_guess, // inertia
- // Matrix3d::Identity(), // cov
+ // 0.01*0.01*Matrix3d::Identity(), // cov
// 0, // start: X coordinate
// 3);
@@ -428,7 +434,7 @@ 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
@@ -451,34 +457,11 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
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("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: ",
- // (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;
}
}
@@ -486,6 +469,9 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
// 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("Guess center of mass : ", cdm_guess.transpose(), " m.");
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
@@ -501,7 +487,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) =
+ // "Test_SimulationProblemForceTorqueInertialDynamics_yaml.preintegration_and_csv";
::testing::GTEST_FLAG(filter) =
"Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation";
+
return RUN_ALL_TESTS();
}
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
index 233368f38accb87f4bbd4500e37bede35dd1378a..d52ed3d01b5fcc803d268085e5b361a8a6f79343 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
@@ -31,6 +31,8 @@ config:
torque_noise_std: 0.1 # std dev of torque noise in N/m
acc_noise_std: 0.01 # std dev of acc noise in m/s2
gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)
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 e791b8e0cd31c713c17caada86a249c24d5cf787..df68db87b3addb39266f2e09ce23aa4cf6693807 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_simulation_test.yaml
@@ -35,6 +35,8 @@ config:
# IMU
acc_noise_std: 0.001 # std dev of acc noise in m/s2
gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
@@ -43,9 +45,9 @@ config:
torque_noise_std: 0.0001 # std dev of torque noise in N/m
# Dynamics
- com: [0,0,0.0341] # center of mass [m]
- inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2]
- mass: 1.952 # mass [kg]
+ 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]
+ mass: 2.00 # mass [kg]
processors:
-
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 c76464f63119b98989d70e69fbc3cc03cd2527b7..4b43965f98c8a50e6098d0c0f7129701f8367e6e 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -35,6 +35,8 @@ config:
# IMU
acc_noise_std: 0.001 # std dev of acc noise in m/s2
gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
diff --git a/test/yaml/sensor_force_torque_inertial.yaml b/test/yaml/sensor_force_torque_inertial.yaml
index e6d323ba4b29ca53840bd4ae4618ca6348d2d057..746d4f54683062ace30d1aecfa18d212e774c596 100644
--- a/test/yaml/sensor_force_torque_inertial.yaml
+++ b/test/yaml/sensor_force_torque_inertial.yaml
@@ -2,6 +2,8 @@ force_noise_std: 0.1 # std dev of force noise in N
torque_noise_std: 0.1 # std dev of torque noise in N/m
acc_noise_std: 0.01 # std dev of acc noise in m/s2
gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+accb_initial_std: 0 # m/s2 - initial bias
+gyrob_initial_std: 0 # rad/sec - initial bias
acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s)
gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s)