diff --git a/include/bodydynamics/capture/capture_force_torque.h b/include/bodydynamics/capture/capture_force_torque.h
index c6289b44281ae967b45754550db7d7bda89c1320..bd0546fc3da997e92e304d4d3016b7ab3877cbc5 100644
--- a/include/bodydynamics/capture/capture_force_torque.h
+++ b/include/bodydynamics/capture/capture_force_torque.h
@@ -47,8 +47,8 @@
#include <core/capture/capture_motion.h>
#include <core/state_block/vector_composite.h>
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureForceTorque);
class CaptureForceTorque : public CaptureMotion
diff --git a/include/bodydynamics/capture/capture_force_torque_inertial.h b/include/bodydynamics/capture/capture_force_torque_inertial.h
index e79866fb29180addbf02774d79beca6dabcec18d..d3d9b8bbf8a47e7ac9bfc1c4fd2c061f38f46f75 100644
--- a/include/bodydynamics/capture/capture_force_torque_inertial.h
+++ b/include/bodydynamics/capture/capture_force_torque_inertial.h
@@ -43,7 +43,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(CaptureForceTorqueInertial);
class CaptureForceTorqueInertial : public CaptureMotion
diff --git a/include/bodydynamics/factor/factor_angular_momentum.h b/include/bodydynamics/factor/factor_angular_momentum.h
index a80c3a752c634e64b25e79cb275a032ec33496e3..a8ff43337da5ffe1c7eefcca58910a4a348bdad7 100644
--- a/include/bodydynamics/factor/factor_angular_momentum.h
+++ b/include/bodydynamics/factor/factor_angular_momentum.h
@@ -87,11 +87,11 @@ class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3,
T* _res) const; // residual
template <typename D1, typename D2, typename D3, typename D4, typename D5>
- bool residual(const MatrixBase<D1>& _L,
- const MatrixBase<D2>& _I,
- const MatrixBase<D3>& _i,
+ bool residual(const MatrixBase<D1>& _L,
+ const MatrixBase<D2>& _I,
+ const MatrixBase<D3>& _i,
const QuaternionBase<D4>& _q,
- MatrixBase<D5>& _res) const;
+ MatrixBase<D5>& _res) const;
Vector3d residual() const;
@@ -126,11 +126,11 @@ inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr
}
template <typename D1, typename D2, typename D3, typename D4, typename D5>
-inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
- const MatrixBase<D2>& _I,
- const MatrixBase<D3>& _i,
+inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
+ const MatrixBase<D2>& _I,
+ const MatrixBase<D3>& _i,
const QuaternionBase<D4>& _q,
- MatrixBase<D5>& _res) const
+ MatrixBase<D5>& _res) const
{
MatrixSizeCheck<3, 1>::check(_res);
@@ -153,13 +153,13 @@ inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
Matrix<T, 3, 1> w_real = measurement_ang_vel_ - _I.template segment<3>(3);
Matrix<T, 3, 1> L_local;
- L_local = _q.conjugate()*_L; //we transform que _L from the global frame to the local frame
- const auto& Lx = L_local(0);
- const auto& Ly = L_local(1);
- const auto& Lz = L_local(2);
- const auto& ixx = _i(0);
- const auto& iyy = _i(1);
- const auto& izz = _i(2);
+ L_local = _q.conjugate() * _L; // we transform que _L from the global frame to the local frame
+ const auto& Lx = L_local(0);
+ const auto& Ly = L_local(1);
+ const auto& Lz = L_local(2);
+ const auto& ixx = _i(0);
+ const auto& iyy = _i(1);
+ const auto& izz = _i(2);
Matrix<T, 3, 1> w_est(Lx / ixx, Ly / iyy, Lz / izz);
Matrix<T, 3, 1> err = w_real - w_est;
_res = sqrt_info_upper_ * err;
@@ -169,10 +169,10 @@ inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
inline Vector3d FactorAngularMomentum::residual() const
{
- Vector3d res(3);
- Vector3d L = getFrame()->getStateBlock('L')->getState();
- Vector6d I = getCapture()->getStateBlock('I')->getState();
- Vector3d i = getSensor()->getStateBlock('i')->getState();
+ Vector3d res(3);
+ Vector3d L = getFrame()->getStateBlock('L')->getState();
+ Vector6d I = getCapture()->getStateBlock('I')->getState();
+ Vector3d i = getSensor()->getStateBlock('i')->getState();
Quaterniond q;
q.coeffs() = getFrame()->getStateBlock('O')->getState();
@@ -181,12 +181,16 @@ inline Vector3d FactorAngularMomentum::residual() const
}
template <typename T>
-inline bool FactorAngularMomentum::operator()(const T* const _L, const T* const _I, const T* const _i, const T* const _q, T* _res) const
+inline bool FactorAngularMomentum::operator()(const T* const _L,
+ const T* const _I,
+ const T* const _i,
+ const T* const _q,
+ T* _res) const
{
Map<const Matrix<T, 3, 1>> L(_L);
Map<const Matrix<T, 6, 1>> I(_I);
Map<const Matrix<T, 3, 1>> i(_i);
- Map<const Quaternion<T>> q(_q);
+ Map<const Quaternion<T>> q(_q);
Map<Matrix<T, 3, 1>> res(_res);
residual(L, I, i, q, res);
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 74c009f9442de0cf17702d01d07e15d5ec0108ea..eaed3797aec2ae31ec0a9f0c11f19a13691169c3 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -50,7 +50,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorForceTorque);
// class
diff --git a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
index fc0431286dd1d4d99765cd6a506d0da212335221..9ea6076f8c646a9146d920fc5abd6888f8c25d84 100644
--- a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
+++ b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
@@ -159,18 +159,18 @@ inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(cons
_processor, // processor
_apply_loss_function,
_status,
- _capture_origin->getFrame()->getStateBlock('P'), // previous frame P
- _capture_origin->getFrame()->getStateBlock('O'), // previous frame O
- _capture_origin->getFrame()->getStateBlock('V'), // previous frame V
- _capture_origin->getFrame()->getStateBlock('L'), // previous frame L
- _capture_origin->getStateBlock('I'), // previous frame IMU bias
- _ftr->getFrame()->getStateBlock('P'), // current frame P
- _ftr->getFrame()->getStateBlock('O'), // current frame O
- _ftr->getFrame()->getStateBlock('V'), // current frame V
- _ftr->getFrame()->getStateBlock('L'), // current frame L
- _ftr->getCapture()->getStateBlock('C'), // sensor C
- _ftr->getCapture()->getStateBlock('i'), // sensor i
- _ftr->getCapture()->getStateBlock('m')), // sensor m
+ _capture_origin->getFrame()->getStateBlock('P'), // previous frame P
+ _capture_origin->getFrame()->getStateBlock('O'), // previous frame O
+ _capture_origin->getFrame()->getStateBlock('V'), // previous frame V
+ _capture_origin->getFrame()->getStateBlock('L'), // previous frame L
+ _capture_origin->getStateBlock('I'), // previous frame IMU bias
+ _ftr->getFrame()->getStateBlock('P'), // current frame P
+ _ftr->getFrame()->getStateBlock('O'), // current frame O
+ _ftr->getFrame()->getStateBlock('V'), // current frame V
+ _ftr->getFrame()->getStateBlock('L'), // current frame L
+ _ftr->getCapture()->getStateBlock('C'), // sensor C
+ _ftr->getCapture()->getStateBlock('i'), // sensor i
+ _ftr->getCapture()->getStateBlock('m')), // sensor m
dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()),
delta_preint_(_ftr->getDeltaPreint()),
calib_preint_(_ftr->getCalibrationPreint()),
diff --git a/include/bodydynamics/math/force_torque_inertial_delta_tools.h b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
index a4648d62521166373db075b3af85727a1c0d7e1d..d735f2c06ec35ca1142dfe0b319479b9e8de53ff 100644
--- a/include/bodydynamics/math/force_torque_inertial_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_inertial_delta_tools.h
@@ -383,8 +383,25 @@ inline void compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt, Ma
Map<Matrix<typename D3::Scalar, 3, 1>> sum_L(&sum(12));
Map<Quaternion<typename D3::Scalar>> sum_q(&sum(15));
- compose(dpi1, dvi1, dpd1, dvd1, dL1, dq1, dpi2, dvi2, dpd2, dvd2, dL2, dq2, dt, sum_pi, sum_vi, sum_pd, sum_vd,
- sum_L, sum_q);
+ compose(dpi1,
+ dvi1,
+ dpd1,
+ dvd1,
+ dL1,
+ dq1,
+ dpi2,
+ dvi2,
+ dpd2,
+ dvd2,
+ dL2,
+ dq2,
+ dt,
+ sum_pi,
+ sum_vi,
+ sum_pd,
+ sum_vd,
+ sum_L,
+ sum_q);
}
template <typename D1, typename D2, class T>
@@ -428,7 +445,7 @@ inline void compose(const MatrixBase<D1>& d1,
// sum_vi = dvi1 + dq1*dvi2; // 0, I , 0, 0 , 0, -dR1 * dvi2_x // 0, dR1, 0, 0, 0, 0
// sum_pd = dpd1 + dvd1*dt + dq1*dpd2; // 0, 0 , I, I*dt, 0, -dR1 * dpd2_x // 0, 0, dR1, 0, 0, 0
// sum_vd = dvd1 + dq1*dvd2; // 0, 0 , 0, I , 0, -dR1 * dvd2_x // 0, 0, 0, dR1, 0, 0
- // sum_L = dL1 + dq1*dL2; // 0, 0 , 0, 0 , I, -dR1 * dL2_x // 0, 0, 0, 0, dR1, 0
+ // sum_L = dL1 + dq1*dL2; // 0, 0 , 0, 0 , I, -dR1 * dL2_x // 0, 0, 0, 0, dR1, 0
// sum_q = dq1*dq2; // 0, 0 , 0, 0 , 0, dR2.tr // 0, 0, 0, 0, 0, I
// // Jac wrt first delta
@@ -544,8 +561,25 @@ inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt, Ma
Map<Matrix<typename D3::Scalar, 3, 1>> bet_L(&d2_minus_d1(12));
Map<Quaternion<typename D3::Scalar>> bet_q(&d2_minus_d1(15));
- between(dpi1, dvi1, dpd1, dvd1, dL1, dq1, dpi2, dvi2, dpd2, dvd2, dL2, dq2, dt, bet_pi, bet_vi, bet_pd, bet_vd,
- bet_L, bet_q);
+ between(dpi1,
+ dvi1,
+ dpd1,
+ dvd1,
+ dL1,
+ dq1,
+ dpi2,
+ dvi2,
+ dpd2,
+ dvd2,
+ dL2,
+ dq2,
+ dt,
+ bet_pi,
+ bet_vi,
+ bet_pd,
+ bet_vd,
+ bet_L,
+ bet_q);
}
template <typename D1, typename D2, class T>
@@ -893,8 +927,24 @@ inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& diff2, MatrixBa
Map<Matrix<typename D3::Scalar, 3, 1>> d_L(&d(12));
Map<Quaternion<typename D3::Scalar>> d_q(&d(15));
- plus(dpi1, dvi1, dpd1, dvd1, dL1, dq1, diff_dpi2, diff_dvi2, diff_dpd2, diff_dvd2, diff_dL2, diff_do2, d_pi, d_vi,
- d_pd, d_vd, d_L, d_q);
+ plus(dpi1,
+ dvi1,
+ dpd1,
+ dvd1,
+ dL1,
+ dq1,
+ diff_dpi2,
+ diff_dvi2,
+ diff_dpd2,
+ diff_dvd2,
+ diff_dL2,
+ diff_do2,
+ d_pi,
+ d_vi,
+ d_pd,
+ d_vd,
+ d_L,
+ d_q);
}
template <typename D1, typename D2>
@@ -972,8 +1022,24 @@ inline void diff(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<
Map<Matrix<typename D3::Scalar, 3, 1>> diff_L(&diff_d1_d2(12));
Map<Matrix<typename D3::Scalar, 3, 1>> diff_o(&diff_d1_d2(15));
- diff(dpi1, dvi1, dpd1, dvd1, dL1, dq1, dpi2, dvi2, dpd2, dvd2, dL2, dq2, diff_pi, diff_vi, diff_pd, diff_vd,
- diff_L, diff_o);
+ diff(dpi1,
+ dvi1,
+ dpd1,
+ dvd1,
+ dL1,
+ dq1,
+ dpi2,
+ dvi2,
+ dpd2,
+ dvd2,
+ dL2,
+ dq2,
+ diff_pi,
+ diff_vi,
+ diff_pd,
+ diff_vd,
+ diff_L,
+ diff_o);
}
template <typename D1, typename D2>
@@ -1404,10 +1470,19 @@ void tangent2delta(const MatrixBase<D1>& _tangent,
Matrix<typename D1::Scalar, 3, 3> J_accd_inertia;
Matrix<typename D1::Scalar, 3, 1> J_accd_mass;
- forces2acc(force, torque, angvel, com, inertia, mass,
- acc_d,
- J_accd_force, J_accd_torque, J_accd_angvel,
- J_accd_com, J_accd_inertia, J_accd_mass);
+ forces2acc(force,
+ torque,
+ angvel,
+ com,
+ inertia,
+ mass,
+ acc_d,
+ J_accd_force,
+ J_accd_torque,
+ J_accd_angvel,
+ J_accd_com,
+ J_accd_inertia,
+ J_accd_mass);
T dt2 = _dt * _dt;
@@ -1509,7 +1584,6 @@ void data2delta(const MatrixBase<D1>& _data,
_J_delta_model = _J_delta_model + J_delta_tangent * J_tangent_model;
}
-
} // namespace fti
} // namespace bodydynamics
} // namespace wolf
diff --git a/include/bodydynamics/processor/processor_force_torque.h b/include/bodydynamics/processor/processor_force_torque.h
index c12c0dfb992231c3af3d6e90ae1e46a777973ae7..ee94dff2ad1724ad60ee5210ecf997538e68a30c 100644
--- a/include/bodydynamics/processor/processor_force_torque.h
+++ b/include/bodydynamics/processor/processor_force_torque.h
@@ -112,7 +112,6 @@ class ProcessorForceTorque : public ProcessorMotion
namespace wolf
{
-
inline void ProcessorForceTorque::configure(SensorBasePtr _sensor)
{
sensor_ikin_ = _sensor->getProblem()->findSensor(params_motion_force_torque_->sensor_ikin_name);
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial.h b/include/bodydynamics/processor/processor_force_torque_inertial.h
index 607da44e7ba05cd5a83b7ee0803b0b498c843367..ce18e1693ab315ebe295ebdd5f44f016c95aa7b0 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial.h
@@ -55,7 +55,6 @@
namespace wolf
{
-
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertial);
struct ParamsProcessorForceTorqueInertial : public ParamsProcessorMotion
@@ -80,8 +79,7 @@ WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertial);
class ProcessorForceTorqueInertial : public ProcessorMotion
{
public:
- ProcessorForceTorqueInertial(
- ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial);
+ ProcessorForceTorqueInertial(ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial);
~ProcessorForceTorqueInertial() override;
void configure(SensorBasePtr _sensor) override;
WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertial, ParamsProcessorForceTorqueInertial);
@@ -247,7 +245,6 @@ class ProcessorForceTorqueInertial : public ProcessorMotion
void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
-
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
public:
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
index b4237dd9c8067a4f39a497cae29586530231403d..b39787cee78b2272905c4b39aa8d9a8330a553a3 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
@@ -55,7 +55,6 @@
namespace wolf
{
-
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialDynamics);
struct ParamsProcessorForceTorqueInertialDynamics : public ParamsProcessorMotion
@@ -84,13 +83,12 @@ class ProcessorForceTorqueInertialDynamics : public ProcessorMotion
ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics_);
~ProcessorForceTorqueInertialDynamics() override;
void configure(SensorBasePtr _sensor) override;
- WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialDynamics,
- ParamsProcessorForceTorqueInertialDynamics);
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialDynamics, ParamsProcessorForceTorqueInertialDynamics);
protected:
ParamsProcessorForceTorqueInertialDynamicsPtr params_force_torque_inertial_dynamics_;
SensorForceTorqueInertialPtr sensor_fti_;
- Matrix6d imu_drift_cov_;
+ Matrix6d imu_drift_cov_;
public:
/** \brief convert raw CaptureMotion data to the delta-state format.
@@ -250,12 +248,12 @@ class ProcessorForceTorqueInertialDynamics : public ProcessorMotion
const CaptureBasePtr& _capture_origin_ptr) override;
/** \brief Emplace features and factors
- *
+ *
* This processor emplaces tree possible factors (with its features):
* - A Force-torque-inertial pre-integrated factor -- the motion factor
* - An Angular-momentum factor -- links angular momentum with angular velocity and inertia
* - An IMU bias drift factor -- this one only if the IMU biases are dynamic
- *
+ *
* \param _capture_origin: origin of the integrated delta
* \param _capture_own: final of the integrated delta
*/
diff --git a/include/bodydynamics/sensor/sensor_force_torque.h b/include/bodydynamics/sensor/sensor_force_torque.h
index 4f972a7ded8f9f6756971b38010395599d249293..ab2af6a100542d02c77da86fba00767951dcba17 100644
--- a/include/bodydynamics/sensor/sensor_force_torque.h
+++ b/include/bodydynamics/sensor/sensor_force_torque.h
@@ -44,7 +44,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(SensorForceTorque);
class SensorForceTorque : public SensorBase
@@ -56,7 +55,7 @@ class SensorForceTorque : public SensorBase
double std_tau_; // standard deviation of the torque sensors (N.m)
public:
- SensorForceTorque(const YAML::Node &_params);
+ SensorForceTorque(const YAML::Node& _params);
WOLF_SENSOR_CREATE(SensorForceTorque);
diff --git a/include/bodydynamics/sensor/sensor_force_torque_inertial.h b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
index 1da66d9d6b6fdf1ef2962619d049f3c684e61fd8..bb22937ff005737a3fabc657c4c355d14887e418 100644
--- a/include/bodydynamics/sensor/sensor_force_torque_inertial.h
+++ b/include/bodydynamics/sensor/sensor_force_torque_inertial.h
@@ -48,7 +48,6 @@
namespace wolf
{
-
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorForceTorqueInertial);
struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
@@ -67,11 +66,10 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
Vector3d inertia;
double mass;
bool imu_bias_fix, com_fix, inertia_fix, mass_fix;
- bool set_mass_prior,set_com_prior,set_inertia_prior,set_bias_prior;
+ bool set_mass_prior, set_com_prior, set_inertia_prior, set_bias_prior;
double prior_mass_std;
Vector3d prior_com_std, prior_inertia_std;
- Vector6d bias,prior_bias_std;
-
+ Vector6d bias, prior_bias_std;
ParamsSensorForceTorqueInertial() = default;
ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
@@ -83,10 +81,10 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
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");
-
+
+ force_noise_std = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
+ torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
+
if (_server.hasParam(prefix + _unique_name + "/acc_bias"))
acc_bias = _server.getParam<Vector3d>(prefix + _unique_name + "/acc_bias");
else
@@ -95,12 +93,12 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
gyro_bias = _server.getParam<Vector3d>(prefix + _unique_name + "/gyro_bias");
else
gyro_bias.setZero();
-
- com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
- inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
- mass = _server.getParam<double>(prefix + _unique_name + "/mass");
- bias = _server.getParam<Vector6d>(prefix + _unique_name + "/bias");
-
+
+ com = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
+ inertia = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
+ mass = _server.getParam<double>(prefix + _unique_name + "/mass");
+ bias = _server.getParam<Vector6d>(prefix + _unique_name + "/bias");
+
imu_bias_fix = _server.getParam<bool>(prefix + _unique_name + "/imu_bias_fix");
com_fix = _server.getParam<bool>(prefix + _unique_name + "/com_fix");
inertia_fix = _server.getParam<bool>(prefix + _unique_name + "/inertia_fix");
@@ -109,29 +107,32 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
// regularization
if (_server.hasParam(prefix + _unique_name + "/set_mass_prior"))
{
- set_mass_prior = _server.getParam<bool>(prefix + _unique_name + "/set_mass_prior");
- prior_mass_std = _server.getParam<double>(prefix + _unique_name + "/prior_mass_std");
- } else
+ set_mass_prior = _server.getParam<bool>(prefix + _unique_name + "/set_mass_prior");
+ prior_mass_std = _server.getParam<double>(prefix + _unique_name + "/prior_mass_std");
+ }
+ else
set_mass_prior = false;
if (_server.hasParam(prefix + _unique_name + "/set_com_prior"))
{
- set_com_prior = _server.getParam<bool>(prefix + _unique_name + "/set_com_prior");
- prior_com_std = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_com_std");
- } else
+ set_com_prior = _server.getParam<bool>(prefix + _unique_name + "/set_com_prior");
+ prior_com_std = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_com_std");
+ }
+ else
set_com_prior = false;
if (_server.hasParam(prefix + _unique_name + "/set_inertia_prior"))
{
- set_inertia_prior = _server.getParam<bool>(prefix + _unique_name + "/set_inertia_prior");
- prior_inertia_std = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_inertia_std");
- } else
+ set_inertia_prior = _server.getParam<bool>(prefix + _unique_name + "/set_inertia_prior");
+ prior_inertia_std = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_inertia_std");
+ }
+ else
set_inertia_prior = false;
if (_server.hasParam(prefix + _unique_name + "/set_bias_prior"))
{
- set_bias_prior = _server.getParam<bool>(prefix + _unique_name + "/set_bias_prior");
- prior_bias_std = _server.getParam<Eigen::Vector6d>(prefix + _unique_name + "/prior_bias_std");
- } else
+ set_bias_prior = _server.getParam<bool>(prefix + _unique_name + "/set_bias_prior");
+ prior_bias_std = _server.getParam<Eigen::Vector6d>(prefix + _unique_name + "/prior_bias_std");
+ }
+ else
set_bias_prior = false;
-
}
~ParamsSensorForceTorqueInertial() override = default;
std::string print() const override
diff --git a/include/bodydynamics/sensor/sensor_inertial_kinematics.h b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
index b9fa6dc938d21f81ac73f9b455cc4d611cf1e7a1..95035dd788fd4b37566a95a250b0c45e8b2dea9d 100644
--- a/include/bodydynamics/sensor/sensor_inertial_kinematics.h
+++ b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
@@ -44,7 +44,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(SensorInertialKinematics);
class SensorInertialKinematics : public SensorBase
@@ -55,7 +54,7 @@ class SensorInertialKinematics : public SensorBase
double std_vbc_; // standard deviation of the com velocity measurement relative to the base frame (m/s)
public:
- SensorInertialKinematics(const YAML::Node &_params);
+ SensorInertialKinematics(const YAML::Node& _params);
WOLF_SENSOR_CREATE(SensorInertialKinematics);
diff --git a/include/bodydynamics/sensor/sensor_point_feet_nomove.h b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
index a218ab59433e8b1548b72363ec7a41e25aa6b2d8..5706793f00ec0e5f640fecfde035e55c730bf9f9 100644
--- a/include/bodydynamics/sensor/sensor_point_feet_nomove.h
+++ b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
@@ -79,7 +79,7 @@ class SensorPointFeetNomove : public SensorBase
protected:
Matrix3d cov_foot_nomove_; // random walk covariance in (m/s^2/sqrt(Hz))^2
Matrix1d cov_altitude_; // m^2
- double foot_radius_; // m
+ double foot_radius_; // m
public:
SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics, const ParamsSensorPointFeetNomovePtr& _intr_pfnm);
diff --git a/src/feature/feature_force_torque.cpp b/src/feature/feature_force_torque.cpp
index 58dc7b7531a41dbe5d15dca9b6c83c642777c4bc..cacf00b548efbfe1a70f1264c7b2143e2948e5a0 100644
--- a/src/feature/feature_force_torque.cpp
+++ b/src/feature/feature_force_torque.cpp
@@ -40,7 +40,6 @@
#include "bodydynamics/feature/feature_force_torque.h"
namespace wolf
{
-
FeatureForceTorque::FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::Vector6d& _biases_preint,
diff --git a/src/processor/processor_force_torque.cpp b/src/processor/processor_force_torque.cpp
index e43a3e0052585a3c854bb9a347f0062d5b9a3bba..1df9b28a09fe78389b5c1b1a9d85aa07e96db481 100644
--- a/src/processor/processor_force_torque.cpp
+++ b/src/processor/processor_force_torque.cpp
@@ -47,7 +47,6 @@
namespace wolf
{
-
int compute_data_size(int nbc, int dimc)
{
// compute the size of the data/body vectors used by processorMotion API depending
diff --git a/src/processor/processor_force_torque_inertial.cpp b/src/processor/processor_force_torque_inertial.cpp
index e51d59d9967a2d11169079081a62b0167de34811..0659c9b8d304b12f51fed1bc47e807e8724dc035 100644
--- a/src/processor/processor_force_torque_inertial.cpp
+++ b/src/processor/processor_force_torque_inertial.cpp
@@ -57,7 +57,6 @@
namespace wolf
{
-
ProcessorForceTorqueInertial::ProcessorForceTorqueInertial(
ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial)
: ProcessorMotion("ProcessorForceTorqueInertial",
@@ -96,7 +95,7 @@ CaptureMotionPtr ProcessorForceTorqueInertial::emplaceCapture(const FrameBasePtr
}
void ProcessorForceTorqueInertial::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
- CaptureMotionPtr _capture_own)
+ CaptureMotionPtr _capture_own)
{
// 1. Feature and factor FTI -- force-torque-inertial
//----------------------------------------------------
@@ -111,7 +110,6 @@ void ProcessorForceTorqueInertial::emplaceFeaturesAndFactors(CaptureBasePtr _c
FactorBase::emplace<FactorForceTorqueInertial>(
ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function);
-
// 2. Feature and factor bias -- IMU bias drift for acc and gyro
//---------------------------------------------------------------
// - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
@@ -147,7 +145,6 @@ void ProcessorForceTorqueInertial::emplaceFeaturesAndFactors(CaptureBasePtr _c
}
}
-
void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
_capture->getStateBlock('I')->setState(_calibration); // Set IMU bias
@@ -157,13 +154,12 @@ void ProcessorForceTorqueInertial::configure(SensorBasePtr _sensor)
{
sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
-
auto acc_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->acc_drift_std;
auto gyro_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_drift_std;
ArrayXd imu_drift_sigmas(6);
imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
- imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
}
void ProcessorForceTorqueInertial::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index fce77e92dd4be35cb3e731a71e87aeb42f2a4662..f7bad625c5142899bf3ab494dce64533af0a1bf8 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -59,7 +59,6 @@
namespace wolf
{
-
ProcessorForceTorqueInertialDynamics::ProcessorForceTorqueInertialDynamics(
ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics)
: ProcessorMotion("ProcessorForceTorqueInertialDynamics",
@@ -155,7 +154,7 @@ void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBase
0, // take all of first second block
-1, // take all of first second block
shared_from_this(), // this processor
- params_->apply_loss_function); // loss function
+ params_->apply_loss_function); // loss function
}
}
}
@@ -177,7 +176,7 @@ void ProcessorForceTorqueInertialDynamics::configure(SensorBasePtr _sensor)
ArrayXd imu_drift_sigmas(6);
imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
- imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
}
void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _data,
@@ -313,9 +312,9 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
*
* these are obtained directly by the function g() = tangent2delta(). This function does:
*
- * angacc = I.inv * ( tt - wt x ( I * wt ) )
+ * angacc = I.inv * ( tt - wt x ( I * wt ) )
* --> J_aa_i, J_aa_tt, J_aa_wt
- * acc_dyn = ft / _mass - angacc x com - wt x ( wt x com )
+ * acc_dyn = ft / _mass - angacc x com - wt x ( wt x com )
* --> J_ad_ft, J_ad_m, J_ad_aa, J_ad_c, J_ad_wt
* ==> J_ad_wt, J_ad_ft, J_ad_tt, J_ad_c, J_ad_i, J_ad_m
*
@@ -325,9 +324,9 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
* delta_v_dyn = acc_dyn * _dt --> J_dvd_ad
* delta_L = tt * _dt --> J_dL_tt
* delta_q = exp_q(wt * _dt) --> J_dq_wt
- *
+ *
* Assembling Jacobians gives:
- *
+ *
* at wt ft tt
* J_delta_tangent = [ J_dpi_at 0 0 0
* J_dvi_at 0 0 0
@@ -422,8 +421,7 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
// 2. go from tangent to delta. We need again the dynamic model for this
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);
+ fti::tangent2delta(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/src/sensor/sensor_force_torque.cpp b/src/sensor/sensor_force_torque.cpp
index dce5dcff3ea1bb6d0e504c9d720d91cc6d447238..9c9d613b033cef03c3160c23924cebdc14a583c2 100644
--- a/src/sensor/sensor_force_torque.cpp
+++ b/src/sensor/sensor_force_torque.cpp
@@ -44,7 +44,7 @@
namespace wolf
{
-SensorForceTorque::SensorForceTorque(const YAML::Node &_params)
+SensorForceTorque::SensorForceTorque(const YAML::Node& _params)
: SensorBase("SensorForceTorque", TypeComposite{}, _params),
mass_(_params["mass"].as<double>()),
std_f_(_params["std_f"].as<double>()),
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index 2c33eac741a0d965dc9f618b52734c0c185ee50f..80449b4617a1c669200b4e3c50ec24c5a79b3705 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -46,7 +46,6 @@
namespace wolf
{
-
// TODO: remove this constructor after merging MR !448
SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd& _extrinsics,
ParamsSensorForceTorqueInertialPtr _params)
@@ -61,70 +60,73 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
params_fti_(_params)
{
// set IMU bias here (it's complicated to do in the constructor above)
- Vector6d imu_bias; imu_bias << params_fti_->acc_bias, params_fti_->gyro_bias;
+ Vector6d imu_bias;
+ imu_bias << params_fti_->acc_bias, params_fti_->gyro_bias;
getStateBlock('I')->setState(imu_bias);
- addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, params_fti_->com_fix)); // centre of mass
- addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, params_fti_->inertia_fix)); // inertial vector
- addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), params_fti_->mass_fix)); // total mass
+ addStateBlock(
+ 'C', FactoryStateBlock::create("StateParams3", params_fti_->com, params_fti_->com_fix)); // centre of mass
+ addStateBlock(
+ 'i',
+ FactoryStateBlock::create("StateParams3", params_fti_->inertia, params_fti_->inertia_fix)); // inertial vector
+ addStateBlock(
+ 'm',
+ FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), params_fti_->mass_fix)); // total mass
setStateBlockDynamic('I', false);
setStateBlockDynamic('C', false);
setStateBlockDynamic('i', false);
setStateBlockDynamic('m', false);
// set priors for regularization
- if(params_fti_->set_mass_prior)
+ if (params_fti_->set_mass_prior)
{
- std::cout<<"Setting prior for mass: "<<params_fti_->mass;
- std::cout<<"\n With std: "<<params_fti_->prior_mass_std;
+ std::cout << "Setting prior for mass: " << params_fti_->mass;
+ std::cout << "\n With std: " << params_fti_->prior_mass_std;
Vector1d prior_mass;
- prior_mass<<params_fti_->mass;
+ prior_mass << params_fti_->mass;
Matrix1d prior_mass_var;
- prior_mass_var<<params_fti_->prior_mass_std*params_fti_->prior_mass_std;
- std::cout<<"\n With cov:\n"<<prior_mass_var<<"\n";
- addPriorParameter('m',prior_mass,prior_mass_var);
- std::cout<<"Done\n";
+ prior_mass_var << params_fti_->prior_mass_std * params_fti_->prior_mass_std;
+ std::cout << "\n With cov:\n" << prior_mass_var << "\n";
+ addPriorParameter('m', prior_mass, prior_mass_var);
+ std::cout << "Done\n";
}
- if(params_fti_->set_com_prior)
+ if (params_fti_->set_com_prior)
{
- std::cout<<"Setting prior for center of mass: "<<params_fti_->com.transpose();
- std::cout<<"\n With std: "<<params_fti_->prior_com_std.transpose();
+ std::cout << "Setting prior for center of mass: " << params_fti_->com.transpose();
+ std::cout << "\n With std: " << params_fti_->prior_com_std.transpose();
Matrix3d prior_com_cov = params_fti_->prior_com_std.array().pow(2).matrix().asDiagonal();
- std::cout<<"\n With cov:\n"<<prior_com_cov<<"\n";
- addPriorParameter('C',params_fti_->com, prior_com_cov);
- std::cout<<"Done\n";
+ std::cout << "\n With cov:\n" << prior_com_cov << "\n";
+ addPriorParameter('C', params_fti_->com, prior_com_cov);
+ std::cout << "Done\n";
}
- if(params_fti_->set_inertia_prior)
+ if (params_fti_->set_inertia_prior)
{
- std::cout<<"Setting prior for inertia: "<<params_fti_->inertia.transpose();
+ std::cout << "Setting prior for inertia: " << params_fti_->inertia.transpose();
Matrix3d prior_inertia_cov = params_fti_->prior_inertia_std.array().pow(2).matrix().asDiagonal();
- std::cout<<"\n With std: "<<params_fti_->prior_inertia_std.transpose();
- std::cout<<"\n With cov: \n"<<prior_inertia_cov<<"\n";
- addPriorParameter('i',params_fti_->inertia, prior_inertia_cov);
- std::cout<<"Done\n";
+ std::cout << "\n With std: " << params_fti_->prior_inertia_std.transpose();
+ std::cout << "\n With cov: \n" << prior_inertia_cov << "\n";
+ addPriorParameter('i', params_fti_->inertia, prior_inertia_cov);
+ std::cout << "Done\n";
}
- if(params_fti_->set_bias_prior)
+ if (params_fti_->set_bias_prior)
{
- std::cout<<"Setting prior for bias: "<<Vector6d::Zero().transpose();
+ std::cout << "Setting prior for bias: " << Vector6d::Zero().transpose();
Matrix6d prior_bias_cov = params_fti_->prior_bias_std.array().pow(2).matrix().asDiagonal();
- std::cout<<"\n With std: "<<params_fti_->prior_bias_std.transpose();
- std::cout<<"\n With cov: \n"<<prior_bias_cov<<"\n";
- addPriorParameter('I',params_fti_->bias, prior_bias_cov);
- std::cout<<"Done\n";
+ std::cout << "\n With std: " << params_fti_->prior_bias_std.transpose();
+ std::cout << "\n With cov: \n" << prior_bias_cov << "\n";
+ addPriorParameter('I', params_fti_->bias, prior_bias_cov);
+ std::cout << "Done\n";
}
-
-
VectorXd noise_std_vector(12);
- Vector3d acc_noise_std_vector = Vector3d::Ones() * params_fti_->acc_noise_std;
- Vector3d gyro_noise_std_vector = Vector3d::Ones() * params_fti_->gyro_noise_std;
- Vector3d force_noise_std_vector = Vector3d::Ones() * params_fti_->force_noise_std;
- Vector3d torque_noise_std_vector =Vector3d::Ones() * params_fti_->torque_noise_std;
+ Vector3d acc_noise_std_vector = Vector3d::Ones() * params_fti_->acc_noise_std;
+ Vector3d gyro_noise_std_vector = Vector3d::Ones() * params_fti_->gyro_noise_std;
+ Vector3d force_noise_std_vector = Vector3d::Ones() * params_fti_->force_noise_std;
+ Vector3d torque_noise_std_vector = Vector3d::Ones() * params_fti_->torque_noise_std;
noise_std_vector << acc_noise_std_vector, gyro_noise_std_vector, force_noise_std_vector, torque_noise_std_vector;
setNoiseStd(noise_std_vector);
}
-
// TODO: Adapt this API to that of MR !448
SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite& _states,
ParamsSensorForceTorqueInertialPtr _params)
diff --git a/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
index 88d1296f8ae4990edc0a866197c2068bd9d16748..00fbba8de8f9b18bb572275b8458790e12f1e530 100644
--- a/src/sensor/sensor_inertial_kinematics.cpp
+++ b/src/sensor/sensor_inertial_kinematics.cpp
@@ -41,7 +41,7 @@
namespace wolf
{
-SensorInertialKinematics::SensorInertialKinematics(const YAML::Node &_params)
+SensorInertialKinematics::SensorInertialKinematics(const YAML::Node& _params)
: SensorBase("SensorInertialKinematics", TypeComposite{'I', "StateParams3"}, _params),
std_pbc_(_params["std_pbc"].as<double>()),
std_vbc_(_params["std_vbc"].as<double>())
diff --git a/test/gtest_factor_force_torque_inertial.cpp b/test/gtest_factor_force_torque_inertial.cpp
index 9b8233afcc3d10350761bb193750dd5b8c50dad8..57c0903934fb7f4900f54a0cfa1d06693f046ad8 100644
--- a/test/gtest_factor_force_torque_inertial.cpp
+++ b/test/gtest_factor_force_torque_inertial.cpp
@@ -63,13 +63,13 @@ WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
class Test_FactorForceTorqueInertialPreint : public testing::Test
{
public:
- ProblemPtr problem;
- SensorForceTorqueInertialPtr sensor;
+ ProblemPtr problem;
+ SensorForceTorqueInertialPtr sensor;
ProcessorForceTorqueInertialPtr processor;
- FrameBasePtr frame_origin, frame_last, frame;
- CaptureMotionPtr capture_origin, capture_last, capture;
- FeatureMotionPtr feature;
- FactorForceTorqueInertialPtr factor;
+ FrameBasePtr frame_origin, frame_last, frame;
+ CaptureMotionPtr capture_origin, capture_last, capture;
+ FeatureMotionPtr feature;
+ FactorForceTorqueInertialPtr factor;
VectorXd data;
MatrixXd data_cov;
@@ -125,8 +125,8 @@ TEST(Dummy, force_load_libwolfbodydynamics_so_at_launch_time)
TEST_F(Test_FactorForceTorqueInertialPreint, constructor)
{
- feature = FeatureMotion::emplace<FeatureMotion>(capture_last, "FeatureFTI", VectorXd(19),
- MatrixXd::Identity(18, 18), VectorXd(6), MatrixXd(18, 6));
+ feature = FeatureMotion::emplace<FeatureMotion>(
+ capture_last, "FeatureFTI", VectorXd(19), MatrixXd::Identity(18, 18), VectorXd(6), MatrixXd(18, 6));
FactorForceTorqueInertial f(feature, capture_origin, processor, false);
WOLF_INFO("f id: ", f.id());
@@ -136,12 +136,12 @@ TEST_F(Test_FactorForceTorqueInertialPreint, constructor)
TEST_F(Test_FactorForceTorqueInertialPreint, emplace)
{
- feature = FeatureMotion::emplace<FeatureMotion>(capture_last, "FeatureFTI", VectorXd(19),
- MatrixXd::Identity(18, 18), VectorXd(6), MatrixXd(18, 6));
- factor = FactorBase::emplace<FactorForceTorqueInertial>(feature, feature, capture_origin, processor, false);
+ feature = FeatureMotion::emplace<FeatureMotion>(
+ capture_last, "FeatureFTI", VectorXd(19), MatrixXd::Identity(18, 18), VectorXd(6), MatrixXd(18, 6));
+ factor = FactorBase::emplace<FactorForceTorqueInertial>(feature, feature, capture_origin, processor, false);
// this ^^^ will create a warning "ADDING FACTOR id TO FEATURE id THAT IS NOT CONNECTED WITH PROBLEM."
- problem->print(4, 1, 1, 1); // feature and factor not in the tree since they belong to last_ptr with no KF.
+ problem->print(4, 1, 1, 1); // feature and factor not in the tree since they belong to last_ptr with no KF.
ASSERT_EQ(factor->getCaptureOther(), capture_origin);
ASSERT_EQ(factor->getCapture(), capture_last);
diff --git a/test/gtest_factor_force_torque_inertial_dynamics.cpp b/test/gtest_factor_force_torque_inertial_dynamics.cpp
index 96048d8aae4f3c1b5f03aa5e3342fb6c869a1de4..24baec1a7d33880a9ddcfe795fcff2566474b075 100644
--- a/test/gtest_factor_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_factor_force_torque_inertial_dynamics.cpp
@@ -64,13 +64,13 @@ WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
{
public:
- ProblemPtr P;
- SensorForceTorqueInertialPtr S;
- ProcessorForceTorqueInertialDynamicsPtr p;
- FrameBasePtr F0, F1, F;
- CaptureMotionPtr C0, C1, C;
- FeatureMotionPtr f1;
- FactorForceTorqueInertialDynamicsPtr c1;
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialDynamicsPtr p;
+ FrameBasePtr F0, F1, F;
+ CaptureMotionPtr C0, C1, C;
+ FeatureMotionPtr f1;
+ FactorForceTorqueInertialDynamicsPtr c1;
VectorXd data;
MatrixXd data_cov;
@@ -92,8 +92,8 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
data = VectorXd::Zero(12); // [ a, w, f, t ]
data_cov = MatrixXd::Identity(12, 12);
- C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
-
+ C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+
C->process();
C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -111,39 +111,36 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
F1->link(P); // by the face
VectorXd delta_preint(VectorXd::Zero(19));
- delta_preint.head<3>() = -0.5*gravity();
+ delta_preint.head<3>() = -0.5 * gravity();
delta_preint.segment<3>(3) = -gravity();
- delta_preint.segment<3>(6) = -0.5*gravity();
+ delta_preint.segment<3>(6) = -0.5 * gravity();
delta_preint.segment<3>(9) = -gravity();
- delta_preint(18) = 1;
+ delta_preint(18) = 1;
MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
- f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,
- jacobian_calib);
+ f1 = FeatureBase::emplace<FeatureMotion>(
+ C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
- F1->getStateBlock('P')->setState(Vector3d(0,0,0));
- F1->getStateBlock('V')->setState(Vector3d(0,0,0));
- F1->getStateBlock('O')->setState(Vector4d(0,0,0,1));
- F1->getStateBlock('L')->setState(Vector3d(0,0,0));
-
+ F1->getStateBlock('P')->setState(Vector3d(0, 0, 0));
+ F1->getStateBlock('V')->setState(Vector3d(0, 0, 0));
+ F1->getStateBlock('O')->setState(Vector4d(0, 0, 0, 1));
+ F1->getStateBlock('L')->setState(Vector3d(0, 0, 0));
}
-
};
-
class Test_FactorForceTorqueInertialDynamics : public testing::Test
{
public:
- ProblemPtr P;
- SensorForceTorqueInertialPtr S;
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialDynamicsPtr p;
- FrameBasePtr F0, F1;
- CaptureMotionPtr C0, C1;
- FeatureMotionPtr f1;
- FactorForceTorqueInertialDynamicsPtr c1;
+ FrameBasePtr F0, F1;
+ CaptureMotionPtr C0, C1;
+ FeatureMotionPtr f1;
+ FactorForceTorqueInertialDynamicsPtr c1;
VectorXd data;
MatrixXd data_cov;
@@ -170,7 +167,7 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
// crear processador
auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialDynamics>();
- p = ProcessorBase::emplace<ProcessorForceTorqueInertialDynamics>(S, params_processor);
+ p = ProcessorBase::emplace<ProcessorForceTorqueInertialDynamics>(S, params_processor);
// crear dos frame
VectorXd state(13);
@@ -184,23 +181,23 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
MatrixXd data_cov(MatrixXd::Identity(12, 12));
Vector6d bias(Vector6d::Zero());
auto sb_bias = std::make_shared<StateParams6>(bias);
- C0 = CaptureBase::emplace<CaptureMotion>(F0, "CaptureMotion", 0.0, S, data, data_cov, nullptr, nullptr,
- nullptr, sb_bias);
- C1 = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1.0, S, data, data_cov, C0, nullptr, nullptr,
- sb_bias);
+ C0 = CaptureBase::emplace<CaptureMotion>(
+ F0, "CaptureMotion", 0.0, S, data, data_cov, nullptr, nullptr, nullptr, sb_bias);
+ C1 = CaptureBase::emplace<CaptureMotion>(
+ F1, "CaptureMotion", 1.0, S, data, data_cov, C0, nullptr, nullptr, sb_bias);
// crear un feature
VectorXd delta_preint(VectorXd::Zero(19));
- delta_preint.head<3>() = -0.5*gravity();
+ delta_preint.head<3>() = -0.5 * gravity();
delta_preint.segment<3>(3) = -gravity();
- delta_preint.segment<3>(6) = -0.5*gravity();
+ delta_preint.segment<3>(6) = -0.5 * gravity();
delta_preint.segment<3>(9) = -gravity();
- delta_preint(18) = 1;
+ delta_preint(18) = 1;
MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
- f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,
- jacobian_calib);
+ f1 = FeatureBase::emplace<FeatureMotion>(
+ C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
// crear un factor
c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
@@ -212,28 +209,28 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
// FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
// }
-//Test to see if the constructor works (not yaml files)
+// Test to see if the constructor works (not yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics, constructor)
{
ASSERT_EQ(c1->getCapture(), C1);
ASSERT_EQ(c1->getCalibPreint().size(), 13);
}
-//Test to see if the constructor works (yaml files)
+// Test to see if the constructor works (yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor)
{
ASSERT_EQ(c1->getCapture(), C1);
ASSERT_EQ(c1->getCalibPreint().size(), 13);
}
-//Test en el que no hi ha moviment (not yaml files)
+// Test en el que no hi ha moviment (not yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
{
VectorXd res_exp = VectorXd::Zero(18);
Matrix<double, 18, 1> res;
VectorXd bias = VectorXd::Zero(6);
- P->print(4,1,1,1);
+ P->print(4, 1, 1, 1);
c1->residual(F0->getStateBlock('P')->getState(), // p0
Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
@@ -252,7 +249,7 @@ TEST_F(Test_FactorForceTorqueInertialDynamics, residual)
ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
}
-//Test en el que no hi ha moviment (yaml files)
+// Test en el que no hi ha moviment (yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
{
VectorXd res_exp = VectorXd::Zero(18);
@@ -276,33 +273,33 @@ TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residual)
ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
}
-
-//Test en el que s'avança 1m en direcció x (not yaml files)
+// Test en el que s'avança 1m en direcció x (not yaml files)
TEST_F(Test_FactorForceTorqueInertialDynamics, residualx)
{
VectorXd res_exp = VectorXd::Zero(18);
Matrix<double, 18, 1> res;
VectorXd bias = VectorXd::Zero(6);
- //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
- F1->getStateBlock('P')->setState(Vector3d (1,0,0));
+ // provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
+ F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
- //Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
+ // Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
VectorXd delta_preint(VectorXd::Zero(19));
- delta_preint.head<3>() = -0.5*gravity();
- delta_preint(0) = 1;
+ delta_preint.head<3>() = -0.5 * gravity();
+ delta_preint(0) = 1;
delta_preint.segment<3>(3) = -gravity();
- delta_preint.segment<3>(6) = -0.5*gravity();
- delta_preint(6) = 1;
+ delta_preint.segment<3>(6) = -0.5 * gravity();
+ delta_preint(6) = 1;
delta_preint.segment<3>(9) = -gravity();
- delta_preint(18) = 1;
- MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
- VectorXd calib_preint(VectorXd::Zero(13));
- MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
- FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib);
-
- FactorForceTorqueInertialDynamicsPtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+ delta_preint(18) = 1;
+ MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+ VectorXd calib_preint(VectorXd::Zero(13));
+ MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+ FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
+ C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
+ FactorForceTorqueInertialDynamicsPtr c2 =
+ FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
c2->residual(F0->getStateBlock('P')->getState(), // p0
Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
@@ -321,7 +318,7 @@ TEST_F(Test_FactorForceTorqueInertialDynamics, residualx)
ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
}
-//Test en el que s'avança 1m en direcció x (fitxers yaml)
+// Test en el que s'avança 1m en direcció x (fitxers yaml)
TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residualx)
{
@@ -329,25 +326,26 @@ TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residualx)
Matrix<double, 18, 1> res;
VectorXd bias = VectorXd::Zero(6);
- //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
- F1->getStateBlock('P')->setState(Vector3d (1,0,0));
+ // provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual
+ F1->getStateBlock('P')->setState(Vector3d(1, 0, 0));
- //Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
+ // Hem de crear un nou feature i un nou factor ja que la delta preint canvia.
VectorXd delta_preint(VectorXd::Zero(19));
- delta_preint.head<3>() = -0.5*gravity();
- delta_preint(0) = 1;
+ delta_preint.head<3>() = -0.5 * gravity();
+ delta_preint(0) = 1;
delta_preint.segment<3>(3) = -gravity();
- delta_preint.segment<3>(6) = -0.5*gravity();
- delta_preint(6) = 1;
+ delta_preint.segment<3>(6) = -0.5 * gravity();
+ delta_preint(6) = 1;
delta_preint.segment<3>(9) = -gravity();
- delta_preint(18) = 1;
- MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
- VectorXd calib_preint(VectorXd::Zero(13));
- MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
- FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib);
-
- FactorForceTorqueInertialDynamicsPtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
+ delta_preint(18) = 1;
+ MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
+ VectorXd calib_preint(VectorXd::Zero(13));
+ MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
+ FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(
+ C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
+ FactorForceTorqueInertialDynamicsPtr c2 =
+ FactorBase::emplace<FactorForceTorqueInertialDynamics>(f2, f2, C0, p, false);
c2->residual(F0->getStateBlock('P')->getState(), // p0
Quaterniond(F0->getStateBlock('O')->getState().data()), // q0
@@ -366,8 +364,6 @@ TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, residualx)
ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
}
-
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_force_torque_inertial_delta_tools.cpp b/test/gtest_force_torque_inertial_delta_tools.cpp
index a2874ff2a916b5acaba95d51b2fc45320bf589dc..1ee8b34c438c047195c769f432788900e3720aec 100644
--- a/test/gtest_force_torque_inertial_delta_tools.cpp
+++ b/test/gtest_force_torque_inertial_delta_tools.cpp
@@ -201,13 +201,13 @@ TEST(FTI_tools, compose_between_with_state_composite)
{
double dt = 0.1;
- VectorComposite x(VectorXd::Random(13), "PVLO", {3,3,3,4});
- x.at('O').normalize();
- VectorComposite d(VectorXd::Random(19), "PVpvLO", {3,3,3,3,3,4});
- d.at('O').normalize();
+ VectorComposite x(VectorXd::Random(13), "PVLO", {3, 3, 3, 4});
+ x.at('O').normalize();
+ VectorComposite d(VectorXd::Random(19), "PVpvLO", {3, 3, 3, 3, 3, 4});
+ d.at('O').normalize();
- VectorComposite xi = composeOverState(x, d, dt, fti::IMU);
- VectorComposite xf = composeOverState(x, d, dt, fti::FT);
+ VectorComposite xi = composeOverState(x, d, dt, fti::IMU);
+ VectorComposite xf = composeOverState(x, d, dt, fti::FT);
ASSERT_MATRIX_APPROX(xi.at('L'), xf.at('L'), 1e-10);
ASSERT_MATRIX_APPROX(xi.at('O'), xf.at('O'), 1e-10);
@@ -481,13 +481,22 @@ TEST(FTI_tools, forces2acc_jacobians_matlab)
double mass = 2;
Vector3d acc;
- Matrix3d J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com, J_acc_inertia;
+ Matrix3d J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com, J_acc_inertia;
Vector3d J_acc_mass;
- forces2acc(force, torque, angvel, com, inertia, mass,
- acc,
- J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com,
- J_acc_inertia, J_acc_mass);
+ forces2acc(force,
+ torque,
+ angvel,
+ com,
+ inertia,
+ mass,
+ acc,
+ J_acc_force,
+ J_acc_torque,
+ J_acc_angvel,
+ J_acc_com,
+ J_acc_inertia,
+ J_acc_mass);
// Matlab results using symbolic Jacobians:
//
@@ -535,11 +544,12 @@ TEST(FTI_tools, forces2acc_jacobians)
double mass;
Vector3d acc;
- force .setRandom().normalize();
- torque .setRandom().normalize();
- angvel .setRandom().normalize();
- com .setRandom().normalize();
- inertia .setRandom().normalize(); inertia *= 100;
+ force.setRandom().normalize();
+ torque.setRandom().normalize();
+ angvel.setRandom().normalize();
+ com.setRandom().normalize();
+ inertia.setRandom().normalize();
+ inertia *= 100;
inertia = (inertia.array() * inertia.array()).matrix(); // make positive
mass = 2.0;
@@ -547,8 +557,19 @@ TEST(FTI_tools, forces2acc_jacobians)
MatrixXd J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com, J_acc_inertia, J_acc_mass;
// analytical jacs
- forces2acc(force, torque, angvel, com, inertia, mass, acc, J_acc_force, J_acc_torque, J_acc_angvel, J_acc_com,
- J_acc_inertia, J_acc_mass);
+ forces2acc(force,
+ torque,
+ angvel,
+ com,
+ inertia,
+ mass,
+ acc,
+ J_acc_force,
+ J_acc_torque,
+ J_acc_angvel,
+ J_acc_com,
+ J_acc_inertia,
+ J_acc_mass);
// linear approximations
Vector3d pert, acc_pert, acc_pert_approx;
@@ -600,7 +621,7 @@ TEST(FTI_tools, tangent2delta_jacobians)
tangent.setRandom();
model.setRandom();
- model.tail(4) *= 10; // augment inertia and mass
+ model.tail(4) *= 10; // augment inertia and mass
tangent2delta(tangent, model, dt, delta, J_delta_tangent, J_delta_model);
@@ -701,7 +722,6 @@ TEST(FTI_tools, data2delta_jacobians)
ASSERT_MATRIX_APPROX(delta_pert, delta_pert_approx, 1e-8);
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_force_torque.cpp b/test/gtest_processor_force_torque.cpp
index 1b949c4335500fdf303407f1518fac5780f2d2bd..2ab4c491f2d6cf4493fb8b3b3dca996e1eaf6baf 100644
--- a/test/gtest_processor_force_torque.cpp
+++ b/test/gtest_processor_force_torque.cpp
@@ -90,7 +90,6 @@ const Vector3d PBCX = {-0.1, 0, 0}; // X axis offset
const Vector3d PBCY = {0, -0.1, 0}; // Y axis offset
const Vector3d PBCZ = {0, 0, -0.1}; // Z axis offset
-
class ProcessorForceTorqueImuOdom3dIkin2KF : public testing::Test
{
public:
@@ -102,8 +101,8 @@ class ProcessorForceTorqueImuOdom3dIkin2KF : public testing::Test
SensorForceTorquePtr sen_ft_;
ProcessorImuPtr proc_imu_;
ProcessorInertialKinematicsPtr proc_inkin_;
- ProcessorForceTorquePtr proc_ft_;
- FrameBasePtr KF1_;
+ ProcessorForceTorquePtr proc_ft_;
+ FrameBasePtr KF1_;
void SetUp() override
{
@@ -153,29 +152,31 @@ class ProcessorForceTorqueImuOdom3dIkin2KF : public testing::Test
intr_ft->std_tau = 0.001;
intr_ft->mass = MASS;
SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
- // SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
- sen_ft_ = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
+ // SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0),
+ // bodydynamics_root_dir + "/demos/sensor_ft.yaml");
+ sen_ft_ = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
ParamsProcessorForceTorquePtr params_sen_ft = std::make_shared<ParamsProcessorForceTorque>();
- params_sen_ft->sensor_ikin_name = "SenIK";
- params_sen_ft->sensor_angvel_name = "SenImu";
- params_sen_ft->nbc = 2;
- params_sen_ft->dimc = 6;
- params_sen_ft->time_tolerance = 0.0005;
- params_sen_ft->unmeasured_perturbation_std = 1e-4;
- params_sen_ft->max_time_span = 1000;
- params_sen_ft->max_buff_length = 500;
- params_sen_ft->dist_traveled = 20000.0;
- params_sen_ft->angle_turned = 1000;
- params_sen_ft->voting_active = true;
- ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", sen_ft_, params_sen_ft);
- // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
+ params_sen_ft->sensor_ikin_name = "SenIK";
+ params_sen_ft->sensor_angvel_name = "SenImu";
+ params_sen_ft->nbc = 2;
+ params_sen_ft->dimc = 6;
+ params_sen_ft->time_tolerance = 0.0005;
+ params_sen_ft->unmeasured_perturbation_std = 1e-4;
+ params_sen_ft->max_time_span = 1000;
+ params_sen_ft->max_buff_length = 500;
+ params_sen_ft->dist_traveled = 20000.0;
+ params_sen_ft->angle_turned = 1000;
+ params_sen_ft->voting_active = true;
+ ProcessorBasePtr proc_ft_base =
+ problem_->installProcessor("ProcessorForceTorque", "PFT", sen_ft_, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", "SenFT",
+ // bodydynamics_root_dir + "/demos/processor_ft_preint");
proc_ft_ = std::static_pointer_cast<ProcessorForceTorque>(proc_ft_base);
}
void TearDown() override {}
};
-
class Cst2KFZeroMotion : public ProcessorForceTorqueImuOdom3dIkin2KF
{
public:
diff --git a/test/gtest_processor_force_torque_inertial.cpp b/test/gtest_processor_force_torque_inertial.cpp
index 4dbdb2bd22be1398c8b4e2982224fd86940c8507..1f75c1edd1cde1f17816d52c7df9995bdfa6b72b 100644
--- a/test/gtest_processor_force_torque_inertial.cpp
+++ b/test/gtest_processor_force_torque_inertial.cpp
@@ -84,7 +84,7 @@ TEST(TestGroup, testName)
// individual tests
}
-int main (int argc, char** argv)
+int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_force_torque_inertial_dynamics.cpp b/test/gtest_processor_force_torque_inertial_dynamics.cpp
index 4ed912485cd56fe329f75673f4064db03a76e091..f290aec03e7e499c9d3fe462dbb438c482e05c67 100644
--- a/test/gtest_processor_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_processor_force_torque_inertial_dynamics.cpp
@@ -64,17 +64,18 @@ WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
class Test_ProcessorForceTorqueInertialDynamics_yaml : public testing::Test
{
public:
- ProblemPtr P;
- SensorForceTorqueInertialPtr S;
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialDynamicsPtr p;
protected:
void SetUp() override
{
- std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
- ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics_processor_test.yaml", wolf_root + "/test/yaml/");
- ParamsServer server = ParamsServer(parser.getParams());
- P = Problem::autoSetup(server);
+ std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser =
+ ParserYaml("problem_force_torque_inertial_dynamics_processor_test.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
@@ -89,17 +90,17 @@ TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, force_registraion)
// Test to see if the processor works (yaml files). Here the dron is not moving
TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, not_moving_test)
{
- VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
- data.segment<3>(0) = - gravity();
- data.segment<3>(6) = - 1.952*gravity();
- MatrixXd data_cov = MatrixXd::Identity(12, 12);
-
- CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
- CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
- CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
- CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -1.952 * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -112,66 +113,67 @@ TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, not_moving_test)
CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
-
- //We check that, effectively, the dron has not moved
- ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('P')->getState(), C_KF0->getFrame()->getStateBlock('P')->getState(), 1e-8);
- ASSERT_QUATERNION_APPROX(Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data()), Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data()), 1e-8);
- ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('V')->getState(), C_KF0->getFrame()->getStateBlock('V')->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('L')->getState(), C_KF0->getFrame()->getStateBlock('L')->getState(), 1e-8);
-
-
- //Checking that the captures we have taken are the correct ones by looking both time stands
+ // We check that, effectively, the dron has not moved
+ ASSERT_MATRIX_APPROX(
+ C_KF1->getFrame()->getStateBlock('P')->getState(), C_KF0->getFrame()->getStateBlock('P')->getState(), 1e-8);
+ ASSERT_QUATERNION_APPROX(Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data()),
+ Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data()),
+ 1e-8);
+ ASSERT_MATRIX_APPROX(
+ C_KF1->getFrame()->getStateBlock('V')->getState(), C_KF0->getFrame()->getStateBlock('V')->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(
+ C_KF1->getFrame()->getStateBlock('L')->getState(), C_KF0->getFrame()->getStateBlock('L')->getState(), 1e-8);
+
+ // Checking that the captures we have taken are the correct ones by looking both time stands
ASSERT_EQ(C_KF0->getTimeStamp(), C0_0->getTimeStamp());
ASSERT_EQ(C_KF1->getTimeStamp(), C5_1->getTimeStamp());
-
- Vector3d p0 = C_KF0->getFrame()->getStateBlock('P')->getState();
+ Vector3d p0 = C_KF0->getFrame()->getStateBlock('P')->getState();
Quaterniond q0 = Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data());
- Vector3d v0 = C_KF0->getFrame()->getStateBlock('V')->getState();
- Vector3d L0 = C_KF0->getFrame()->getStateBlock('L')->getState();
+ Vector3d v0 = C_KF0->getFrame()->getStateBlock('V')->getState();
+ Vector3d L0 = C_KF0->getFrame()->getStateBlock('L')->getState();
- Vector3d p1 = C_KF1->getFrame()->getStateBlock('P')->getState();
+ Vector3d p1 = C_KF1->getFrame()->getStateBlock('P')->getState();
Quaterniond q1 = Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data());
- Vector3d v1 = C_KF1->getFrame()->getStateBlock('V')->getState();
- Vector3d L1 = C_KF1->getFrame()->getStateBlock('L')->getState();
-
- Vector3d dpi;
- Vector3d dvi;
- Vector3d dpd;
- Vector3d dvd;
- Vector3d dL;
+ Vector3d v1 = C_KF1->getFrame()->getStateBlock('V')->getState();
+ Vector3d L1 = C_KF1->getFrame()->getStateBlock('L')->getState();
+
+ Vector3d dpi;
+ Vector3d dvi;
+ Vector3d dpd;
+ Vector3d dvd;
+ Vector3d dL;
Quaterniond dq;
- double dt = 1.0;
- VectorXd betw_states(19);
+ double dt = 1.0;
+ VectorXd betw_states(19);
+
+ betweenStates(p0, v0, L0, q0, p1, v1, L1, q1, dt, dpi, dvi, dpd, dvd, dL, dq);
- betweenStates(p0,v0,L0,q0,p1,v1,L1,q1,dt,dpi,dvi,dpd,dvd,dL,dq);
+ betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
- betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
-
VectorXd delta_preintegrated = C_KF1->getFeatureList().front()->getMeasurement();
- //Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
+ // Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);
-
}
-//Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction
+// Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction
TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, 1m_x_moving_test__com_zero)
{
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = -gravity();
data(0) = 2;
- data.segment<3>(6) = - 1.952*gravity();
- data(6) = 1.952*2;
- MatrixXd data_cov = MatrixXd::Identity(12, 12);
+ data.segment<3>(6) = -1.952 * gravity();
+ data(6) = 1.952 * 2;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
// Set CoM to zero so that an acceleration on X does not produce a torque, thereby producing non-null 'L' at KF1
S->getStateBlock('C')->setState(Vector3d::Zero());
- CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
- CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
C1_0->process();
@@ -180,102 +182,99 @@ TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, 1m_x_moving_test__com_zer
CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
- P->print(4,1,1,1);
-
- C_KF1->getBuffer().print(1,1,1,0,0);
+ P->print(4, 1, 1, 1);
- //We check that, effectively, the drone has moved 1m in the x direction, the x component of the
- //velocity is now 2m/s and nothing else has changed from the original state
- ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('P')->getState(), Vector3d(1,0,0), 1e-8);
- ASSERT_QUATERNION_APPROX(Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data()), Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data()), 1e-8);
- ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('V')->getState(), Vector3d(2,0,0), 1e-8);
- ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('L')->getState(), C_KF0->getFrame()->getStateBlock('L')->getState(), 1e-8);
+ C_KF1->getBuffer().print(1, 1, 1, 0, 0);
+ // We check that, effectively, the drone has moved 1m in the x direction, the x component of the
+ // velocity is now 2m/s and nothing else has changed from the original state
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('P')->getState(), Vector3d(1, 0, 0), 1e-8);
+ ASSERT_QUATERNION_APPROX(Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data()),
+ Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data()),
+ 1e-8);
+ ASSERT_MATRIX_APPROX(C_KF1->getFrame()->getStateBlock('V')->getState(), Vector3d(2, 0, 0), 1e-8);
+ ASSERT_MATRIX_APPROX(
+ C_KF1->getFrame()->getStateBlock('L')->getState(), C_KF0->getFrame()->getStateBlock('L')->getState(), 1e-8);
-
- Vector3d p0 = C_KF0->getFrame()->getStateBlock('P')->getState();
+ Vector3d p0 = C_KF0->getFrame()->getStateBlock('P')->getState();
Quaterniond q0 = Quaterniond(C_KF0->getFrame()->getStateBlock('O')->getState().data());
- Vector3d v0 = C_KF0->getFrame()->getStateBlock('V')->getState();
- Vector3d L0 = C_KF0->getFrame()->getStateBlock('L')->getState();
+ Vector3d v0 = C_KF0->getFrame()->getStateBlock('V')->getState();
+ Vector3d L0 = C_KF0->getFrame()->getStateBlock('L')->getState();
- Vector3d p1 = C_KF1->getFrame()->getStateBlock('P')->getState();
+ Vector3d p1 = C_KF1->getFrame()->getStateBlock('P')->getState();
Quaterniond q1 = Quaterniond(C_KF1->getFrame()->getStateBlock('O')->getState().data());
- Vector3d v1 = C_KF1->getFrame()->getStateBlock('V')->getState();
- Vector3d L1 = C_KF1->getFrame()->getStateBlock('L')->getState();
-
- Vector3d dpi;
- Vector3d dvi;
- Vector3d dpd;
- Vector3d dvd;
- Vector3d dL;
+ Vector3d v1 = C_KF1->getFrame()->getStateBlock('V')->getState();
+ Vector3d L1 = C_KF1->getFrame()->getStateBlock('L')->getState();
+
+ Vector3d dpi;
+ Vector3d dvi;
+ Vector3d dpd;
+ Vector3d dvd;
+ Vector3d dL;
Quaterniond dq;
- double dt = 1.0;
- VectorXd betw_states(19);
+ double dt = 1.0;
+ VectorXd betw_states(19);
+
+ betweenStates(p0, v0, L0, q0, p1, v1, L1, q1, dt, dpi, dvi, dpd, dvd, dL, dq);
- betweenStates(p0,v0,L0,q0,p1,v1,L1,q1,dt,dpi,dvi,dpd,dvd,dL,dq);
+ betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
- betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs();
-
VectorXd delta_preintegrated = C_KF1->getFeatureList().front()->getMeasurement();
- //Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
+ // Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);
-
}
-//Test to see if the voteForKeyFrame works (distance traveled)
+// Test to see if the voteForKeyFrame works (distance traveled)
TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_dist)
{
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = -gravity();
data(0) = 2;
- data.segment<3>(6) = - 1.952*gravity();
- data(6) = 1.952*2;
- MatrixXd data_cov = MatrixXd::Identity(12, 12);
+ data.segment<3>(6) = -1.952 * gravity();
+ data(6) = 1.952 * 2;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
p->setMaxTimeSpan(999);
p->setDistTraveled(0.995);
- CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
- CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
C0_0->process();
C1_0->process();
C2_0->process();
C3_0->process();
- P->print(4,1,1,1);
-
+ P->print(4, 1, 1, 1);
}
-//Test to see if the voteForKeyFrame works (buffer)
+// Test to see if the voteForKeyFrame works (buffer)
TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_buffer)
{
VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
data.segment<3>(0) = -gravity();
data(0) = 2;
- data.segment<3>(6) = - 1.952*gravity();
- data(6) = 1.952*2;
- MatrixXd data_cov = MatrixXd::Identity(12, 12);
+ data.segment<3>(6) = -1.952 * gravity();
+ data(6) = 1.952 * 2;
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
p->setMaxTimeSpan(999);
p->setMaxBuffLength(3);
- CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
- CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
C0_0->process();
C1_0->process();
C2_0->process();
C3_0->process();
- P->print(4,1,1,1);
-
+ P->print(4, 1, 1, 1);
}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index 597f8715b963cc3b0029cd8175cdc8abc0247167..9e7b8be41b281b43e286fc3b49e945ef12ca58f4 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -59,7 +59,6 @@
#include <core/tree_manager/tree_manager_sliding_window.h>
#include <core/capture/capture_pose.h>
-
#include <Eigen/Dense>
#include <Eigen/Geometry>
@@ -76,9 +75,9 @@ WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::Test
{
public:
- ProblemPtr P;
- SolverCeresPtr solver;
-
+ ProblemPtr P;
+ SolverCeresPtr solver;
+
// FTI sensor
SensorForceTorqueInertialPtr S;
ProcessorForceTorqueInertialDynamicsPtr p;
@@ -87,17 +86,17 @@ class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::T
Vector3d inertia_true;
double mass_true;
Matrix<double, 12, 1> data_fti;
-
+
// Pose sensor
- SensorBasePtr SP;
- Vector7d data_pose;
+ SensorBasePtr SP;
+ Vector7d data_pose;
- double dt;
+ double dt;
// Reading csv data_fti
- std::vector<double> time_stamp;
- std::vector<Vector3d> position, vlin, vang, force, torque, a_meas;
- std::vector<Quaterniond> quaternion;
+ std::vector<double> time_stamp;
+ std::vector<Vector3d> position, vlin, vang, force, torque, a_meas;
+ std::vector<Quaterniond> quaternion;
protected:
void extractAndCompleteData(const std::string& file_path_name)
@@ -215,10 +214,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_i = force_i / mass_true;
a_meas.push_back(a_meas_i);
-
+
counter++;
}
}
@@ -233,7 +232,6 @@ 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/");
@@ -261,14 +259,14 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, force_registratio
FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
}
-//this test checks the pre-integration evolution along the time
+// 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
std::fstream fout;
fout.open(wolf_bodydynamics_root + "/test/dades_simulacio_pep_estimador.csv",
- std::fstream::out | std::fstream::trunc);
+ std::fstream::out | std::fstream::trunc);
S->getStateBlock('I')->setState(bias_true);
S->getStateBlock('C')->setState(cdm_true);
@@ -283,7 +281,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
S->getStateBlock('m')->fix();
S->setStateBlockDynamic('I', false);
- //Writing the first lines to know how the data will be distributed in the csv
+ // Writing the first lines to know how the data will be distributed in the csv
fout << "time stamp"
<< ","
<< "position_x_true"
@@ -334,7 +332,6 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
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_fti
{
// SIMULATOR
@@ -357,9 +354,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, preintegration_an
<< 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";
-
+ << quaternion_coeffs_est(2) << "," << quaternion_coeffs_est(3) << "\n";
}
fout.close();
}
@@ -380,24 +375,23 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
S->getStateBlock('m')->unfix();
S->setStateBlockDynamic('I', false);
-
// add regularization, uncomment if the parameter is not fixed
- S->addPriorParameter('I', // cdm
- bias_guess, // cdm
- 0.1*0.1*Matrix6d::Identity()); // cov
+ S->addPriorParameter('I', // cdm
+ bias_guess, // cdm
+ 0.1 * 0.1 * Matrix6d::Identity()); // cov
- S->addPriorParameter('C', // cdm
- cdm_guess, // cdm
- 0.1*0.1*Matrix3d::Identity()); // cov
+ S->addPriorParameter('C', // cdm
+ cdm_guess, // cdm
+ 0.1 * 0.1 * Matrix3d::Identity()); // cov
- S->addPriorParameter('i', // inertia
- inertia_guess, // inertia
- 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
+ S->addPriorParameter('m', // mass
+ Vector1d(mass_guess), // mass
+ 0.1 * 0.1 * Matrix1d::Identity()); // cov
std::string report;
@@ -409,7 +403,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
int i_init = 0;
// Pose
- data_pose << position[i_init] , quaternion[i_init].coeffs();
+ data_pose << position[i_init], quaternion[i_init].coeffs();
CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, data_pose, SP->getNoiseCov());
CP->process();
@@ -422,8 +416,8 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
TimeStamp t = time_stamp[i];
// Data
- data_fti << a_meas[i], vang[i], force[i], torque[i];
- data_pose << position[i] , quaternion[i].coeffs();
+ data_fti << a_meas[i], vang[i], force[i], torque[i];
+ data_pose << position[i], quaternion[i].coeffs();
// ESTIMATOR
@@ -440,7 +434,7 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
// solve!
report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
+
// results of this iteration
WOLF_INFO("Time : ", t, " s.");
WOLF_INFO(report);
@@ -449,7 +443,6 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
-
}
}
@@ -468,10 +461,10 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_online
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
- ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.11);
- ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 1e-2);
+ ASSERT_MATRIX_APPROX(bias_true, S->getStateBlock('I')->getState(), 0.11);
+ ASSERT_MATRIX_APPROX(cdm_true, S->getStateBlock('C')->getState(), 1e-2);
ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
- ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 2e-2);
+ ASSERT_NEAR(mass_true, S->getStateBlock('m')->getState()(0), 2e-2);
}
TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
@@ -490,7 +483,6 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
S->getStateBlock('m')->unfix();
S->setStateBlockDynamic('I', false);
-
// // add regularization, uncomment if the parameter is not fixed
// S->addPriorParameter('I', // bias
@@ -510,14 +502,14 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
// 0.01 * 0.01 * Matrix1d::Identity()); // cov
int i_init = 0;
-
+
// Force FTI processor to make a KF at t=0
CaptureMotionPtr C0 =
std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), S->getNoiseCov(), nullptr);
C0->process();
// Pose
- data_pose << position[i_init] , quaternion[i_init].coeffs();
+ data_pose << position[i_init], quaternion[i_init].coeffs();
CaptureBasePtr CP = std::make_shared<CapturePose>(time_stamp[i_init], SP, data_pose, SP->getNoiseCov());
CP->process();
@@ -527,8 +519,8 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
TimeStamp t = time_stamp[i];
// Data
- data_fti << a_meas[i], vang[i], force[i], torque[i];
- data_pose << position[i] , quaternion[i].coeffs();
+ data_fti << a_meas[i], vang[i], force[i], torque[i];
+ data_pose << position[i], quaternion[i].coeffs();
// ESTIMATOR
@@ -543,7 +535,6 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
// FINAL RESULTS
WOLF_INFO("-----------------------------");
WOLF_INFO(report);
@@ -561,19 +552,18 @@ TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation_batch)
WOLF_INFO("Estimated mass : ", S->getStateBlock('m')->getState()(0), " Kg.");
WOLF_INFO("-----------------------------");
- ASSERT_MATRIX_APPROX(bias_true , S->getStateBlock('I')->getState(), 0.21);
- ASSERT_MATRIX_APPROX(cdm_true , S->getStateBlock('C')->getState(), 2e-3);
+ ASSERT_MATRIX_APPROX(bias_true, S->getStateBlock('I')->getState(), 0.21);
+ ASSERT_MATRIX_APPROX(cdm_true, S->getStateBlock('C')->getState(), 2e-3);
ASSERT_MATRIX_APPROX(inertia_true, S->getStateBlock('i')->getState(), 1e-2);
- ASSERT_NEAR (mass_true , S->getStateBlock('m')->getState()(0), 4e-2);
+ ASSERT_NEAR(mass_true, S->getStateBlock('m')->getState()(0), 4e-2);
}
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*";
-
+ // "Test_SimulationProblemForceTorqueInertialDynamics_yaml.preintegration_and_csv";
+ ::testing::GTEST_FLAG(filter) = "Test_SimulationProblemForceTorqueInertialDynamics_yaml.simulation*";
+
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 0e71fe996a862cc83d7aec8744708ab38ded88e6..909c97b25cd8d9384c8912c7b13af2700d06a3a7 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -1111,7 +1111,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();
+ data_cov = S->getNoiseCov();
// Pose measurements (simulate motion capture)
position_data = position_true;