new factor fti externalforce & gtest (not working)

CMakeLists.txt

@@ -113,6 +113,7 @@ SET(HDRS_FACTOR
 include/${PROJECT_NAME}/factor/factor_angular_momentum.h
 include/${PROJECT_NAME}/factor/factor_force_torque_inertial.h
 include/${PROJECT_NAME}/factor/factor_force_torque_inertial_dynamics.h
+include/${PROJECT_NAME}/factor/factor_force_torque_inertial_dynamics_external_force.h
 include/${PROJECT_NAME}/factor/factor_force_torque.h
 include/${PROJECT_NAME}/factor/factor_inertial_kinematics.h
 include/${PROJECT_NAME}/factor/factor_point_feet_nomove.h

include/bodydynamics/factor/factor_force_torque_inertial_dynamics_external_force.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include <core/feature/feature_motion.h>
+#include <core/factor/factor_autodiff.h>
+
+namespace wolf
+{
+using namespace Eigen;
+using namespace bodydynamics;
+
+WOLF_PTR_TYPEDEFS(FactorForceTorqueInertialDynamicsExternalForce);
+
+/**
+ * @brief Pre-integrated factor taking IMU and force-torque measurements (FTI)
+ *
+ * This factor evaluates the preintegrated FTI against pose, linear velocity, IMU bias and angular momentum.
+ *
+ * State blocks considered:
+ *  - Frame 1 (origin)
+ *    'P' position
+ *    'V' velocity
+ *    'O' orientation
+ *    'L' angular momentum
+ *  - Capture 1 (origin)
+ *    'I' IMU biases
+ *  - Frame 2
+ *    'P' position
+ *    'V' velocity
+ *    'O' orientation
+ *    'L' angular momentum
+ *  - Sensor Force Torque Inertial
+ *    'C' position of the center of mass
+ *    'i' inertia matrix components (we are assuming that it is a diagonal matrix)
+ *    'm' mass
+ *    'f' external force (global coordinates) applied to the center of mass
+ *
+ * The residual computed has the following components, in this order
+ *  - position delta error according to IMU preintegration
+ *  - velocity delta error according to IMU preintegration
+ *  - position delta error according to FT preintegration
+ *  - velocity delta error according to FT preintegration
+ *  - angular momentum delta error according to FT preintegration
+ *  - orientation delta error according to IMU preintegration
+ */
+class FactorForceTorqueInertialDynamicsExternalForce
+    : public FactorAutodiff<FactorForceTorqueInertialDynamicsExternalForce, 18,             // residual
+                                                                            3, 4, 3, 3, 6,  // POVLB
+                                                                            3, 4, 3, 3,     // POVL
+                                                                            3, 3, 1, 3>     // Cimf
+{
+  public:
+    FactorForceTorqueInertialDynamicsExternalForce(const FeatureMotionPtr& _ftr,  // gets measurement and access to parents
+                                                   const CaptureBasePtr&   _capture_origin,  // gets biases
+                                                   const ProcessorBasePtr& _processor,       // gets access to processor and sensor
+                                                   bool                    _apply_loss_function,
+                                                   FactorStatus            _status = FAC_ACTIVE);
+
+    ~FactorForceTorqueInertialDynamicsExternalForce() override = default;
+
+    template <typename T>
+    bool operator()(const T* const _p1,  // position 1
+                    const T* const _q1,  // orientation
+                    const T* const _v1,  // velocity
+                    const T* const _L1,  // ang momentum
+                    const T* const _b1,  // IMU bias (acc and gyro)
+                    const T* const _p2,  // position 2
+                    const T* const _q2,  // orientation
+                    const T* const _v2,  // velocity
+                    const T* const _L2,  // ang momentum
+                    const T* const _C,   // center of mass
+                    const T* const _i,   // inertia matrix
+                    const T* const _m,   // mass
+                    const T* const _f,   // external force
+                    T*             _res) const;      // residual
+
+    template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
+    bool residual(const MatrixBase<D1>&     _p1,
+                  const QuaternionBase<D4>& _q1,
+                  const MatrixBase<D1>&     _v1,
+                  const MatrixBase<D1>&     _L1,
+                  const MatrixBase<D2>&     _b1,
+                  const MatrixBase<D3>&     _p2,
+                  const QuaternionBase<D5>& _q2,
+                  const MatrixBase<D3>&     _v2,
+                  const MatrixBase<D3>&     _L2,
+                  const MatrixBase<D6>&     _C,
+                  const MatrixBase<D6>&     _i,
+                  const D7&                 _m,
+                  const MatrixBase<D8>&     _f,
+                  MatrixBase<D9>&           _res) const;
+
+    VectorXd residual() const;
+
+    VectorXd getCalibPreint() const
+    {
+        return calib_preint_;
+    }
+
+  private:
+    double                 dt_;
+    Matrix<double, 19, 1>  delta_preint_;
+    Matrix<double, 13, 1>  calib_preint_;
+    Matrix<double, 18, 13> J_delta_calib_;
+    Matrix<double, 18, 18> sqrt_info_upper_;
+};
+
+inline FactorForceTorqueInertialDynamicsExternalForce::FactorForceTorqueInertialDynamicsExternalForce(const FeatureMotionPtr& _ftr,
+                                                                                                      const CaptureBasePtr&   _capture_origin,
+                                                                                                      const ProcessorBasePtr& _processor,
+                                                                                                      bool         _apply_loss_function,
+                                                                                                      FactorStatus _status)
+    : FactorAutodiff<FactorForceTorqueInertialDynamicsExternalForce, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3, 3, 3, 1, 3>(
+          "FactorForceTorqueInertialDynamicsExternalForce",
+          TOP_MOTION,
+          _ftr,                         // parent feature
+          _capture_origin->getFrame(),  // frame other
+          _capture_origin,              // capture other
+          nullptr,                      // feature other
+          nullptr,                      // landmark other
+          _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
+          _ftr->getCapture()->getStateBlock('f')),            // sensor f
+      dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()),
+      delta_preint_(_ftr->getDeltaPreint()),
+      calib_preint_(_ftr->getCalibrationPreint()),
+      J_delta_calib_(_ftr->getJacobianCalibration()),
+      sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper())
+{
+    //
+}
+
+template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
+inline bool FactorForceTorqueInertialDynamicsExternalForce::residual(const MatrixBase<D1>&     _p1,
+                                                                     const QuaternionBase<D4>& _q1,
+                                                                     const MatrixBase<D1>&     _v1,
+                                                                     const MatrixBase<D1>&     _L1,
+                                                                     const MatrixBase<D2>&     _b1,
+                                                                     const MatrixBase<D3>&     _p2,
+                                                                     const QuaternionBase<D5>& _q2,
+                                                                     const MatrixBase<D3>&     _v2,
+                                                                     const MatrixBase<D3>&     _L2,
+                                                                     const MatrixBase<D6>&     _C,
+                                                                     const MatrixBase<D6>&     _i,
+                                                                     const D7&                 _m,
+                                                                     const MatrixBase<D8>&     _f,
+                                                                     MatrixBase<D9>&           _res) const
+{
+    MatrixSizeCheck<18, 1>::check(_res);
+
+    typedef typename D4::Scalar  T;
+    typedef Map<Matrix<T, 3, 1>> Matrix3t;
+
+    /* Will do the following:
+     *
+     * Compute estimated delta, using betweenStatesGlobalAcceleration()
+     *    d_est = x2 (-) x1
+     *
+     * Compute correction step according to bias change, this is a linear operation
+     *    d_step = J_delta_calib * (calib - bias_preint)
+     *
+     * Correct preintegrated delta with new bias, using plus()
+     *    d_corr = d_preint (+) d_step
+     *
+     * Compute error, using diff()
+     *    d_err = d_corr (-) d_est
+     *
+     * Compute residual
+     *    res = W * d_err , with W the sqrt of the info matrix.
+     */
+
+    /* WARNING -- order of state blocks might be confusing!!
+     * Frame blocks are ordered as follows:  "POVL"         -- pos, ori, vel, ang_momentum
+     * Factor blocks are ordered as follows: "POVL-B-POVL"  -- B is IMU bias
+     * Delta blocks are ordered as follows:  "PVpvLO"       -- where "P,V,O" are from IMU, and "p,v,L" from FT
+     */
+    Matrix<T, 19, 1>     delta_est;           // delta_est = x2 (-) x1, computed with betweenStates(x1,x2,dt)
+    Matrix3t             dpi(&delta_est(0));  // 'P'
+    Matrix3t             dvi(&delta_est(3));  // 'V'
+    Matrix3t             dpd(&delta_est(6));  // 'p'
+    Matrix3t             dvd(&delta_est(9));  // 'v'
+    Matrix3t             dL(&delta_est(12));  // 'L'
+    Map<Quaternion<T>>   dq(&delta_est(15));  // 'O'
+    fti::betweenStatesGlobalAcceleration(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, _m * _f - gravity(), dt_, dpi, dvi, dpd, dvd, dL, dq);
+
+    Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
+    Matrix<T, 13, 1> calib;
+    calib << _b1, _C, _i, _m;
+    Matrix<T, 18, 1> delta_step = J_delta_calib_ * (calib - calib_preint_);
+    Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
+    Matrix<T, 18, 1> delta_err  = fti::diff(delta_est, delta_corr);
+    _res                        = sqrt_info_upper_ * delta_err;
+
+    return true;
+}
+
+inline VectorXd FactorForceTorqueInertialDynamicsExternalForce::residual() const
+{
+    VectorXd    res(18);
+    Vector3d    p0   = getFrameOther()->getStateBlock('P')->getState();  // previous frame P
+    Quaterniond q0   = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data());
+    Vector3d    v0   = getFrameOther()->getStateBlock('V')->getState();
+    Vector3d    L0   = getFrameOther()->getStateBlock('L')->getState();
+    Vector6d    bias = getCaptureOther()->getSensorIntrinsic()->getState();
+    Vector3d    p1   = getFrame()->getStateBlock('P')->getState();  // previous frame P
+    Quaterniond q1   = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
+    Vector3d    v1   = getFrame()->getStateBlock('V')->getState();
+    Vector3d    L1   = getFrame()->getStateBlock('L')->getState();
+    Vector3d    C    = getCapture()->getStateBlock('C')->getState();
+    Vector3d    i    = getCapture()->getStateBlock('i')->getState();
+    double      m    = getCapture()->getStateBlock('m')->getState()(0);
+    Vector3d    f    = getCapture()->getStateBlock('f')->getState();
+
+    residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, f, res);
+    return res;
+}
+
+template <typename T>
+inline bool FactorForceTorqueInertialDynamicsExternalForce::operator()(const T* const _p1,
+                                                                       const T* const _q1,
+                                                                       const T* const _v1,
+                                                                       const T* const _L1,
+                                                                       const T* const _b1,
+                                                                       const T* const _p2,
+                                                                       const T* const _q2,
+                                                                       const T* const _v2,
+                                                                       const T* const _L2,
+                                                                       const T* const _C,
+                                                                       const T* const _i,
+                                                                       const T* const _m,
+                                                                       const T* const _f,
+                                                                       T*             _res) const
+{
+    Map<const Matrix<T, 3, 1>> p1(_p1);
+    Map<const Quaternion<T>>   q1(_q1);
+    Map<const Matrix<T, 3, 1>> v1(_v1);
+    Map<const Matrix<T, 3, 1>> L1(_L1);
+    Map<const Matrix<T, 6, 1>> b1(_b1);
+    Map<const Matrix<T, 3, 1>> p2(_p2);
+    Map<const Quaternion<T>>   q2(_q2);
+    Map<const Matrix<T, 3, 1>> v2(_v2);
+    Map<const Matrix<T, 3, 1>> L2(_L2);
+    Map<const Matrix<T, 3, 1>> C(_C);
+    Map<const Matrix<T, 3, 1>> i(_i);
+    const T&                   m = *_m;
+    Map<const Matrix<T, 3, 1>> f(_f);
+    Map<Matrix<T, 18, 1>>      res(_res);
+
+    residual(p1, q1, v1, L1, b1, p2, q2, v2, L2, C, i, m, f, res);
+
+    return true;
+}
+
+}  // namespace wolf
\ No newline at end of file

test/CMakeLists.txt

@@ -20,6 +20,8 @@ wolf_add_gtest(gtest_factor_force_torque_inertial gtest_factor_force_torque_iner
 
 wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics gtest_factor_force_torque_inertial_dynamics.cpp)
 
+wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics_external_force gtest_factor_force_torque_inertial_dynamics_external_force.cpp)
+
 wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp)
 
 wolf_add_gtest(gtest_force_torque_inertial_delta_tools gtest_force_torque_inertial_delta_tools.cpp)

test/gtest_factor_force_torque_inertial_dynamics_external_force.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics_external_force.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+
+#include <core/sensor/factory_sensor.h>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_FactorForceTorqueInertialDynamicsExternalForce_yaml : public testing::Test
+{
+  public:
+    ProblemPtr                                          P;
+    SensorForceTorqueInertialPtr                        S;
+    ProcessorForceTorqueInertialDynamicsPtr             p;
+    FrameBasePtr                                        F0, F1, F;
+    CaptureMotionPtr                                    C0, C1, C;
+    FeatureMotionPtr                                    f1;
+    FactorForceTorqueInertialDynamicsExternalForcePtr   c1;
+
+    VectorXd              data;
+    MatrixXd              data_cov;
+    VectorXd              delta;
+    MatrixXd              delta_cov;
+    Matrix<double, 13, 1> calib;
+    MatrixXd              J_delta_calib;
+
+  protected:
+    void SetUp() override
+    {
+        ParserYaml   parser = ParserYaml("problem_force_torque_inertial_dynamics_external_force.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());
+
+        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->process();
+
+        C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+        F0 = C0->getFrame();
+
+        ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp());
+        ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp());
+
+        C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+        C->process();
+
+        C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
+        F1 = C1->getFrame();
+
+        F1->link(P);  // by the face
+
+        VectorXd delta_preint(VectorXd::Zero(19));
+        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>(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));
+        f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,
+                                                 jacobian_calib);
+
+        c1 = FactorBase::emplace<FactorForceTorqueInertialDynamicsExternalForce>(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));
+    }
+};
+
+
+class Test_FactorForceTorqueInertialDynamicsExternalForce : public testing::Test
+{
+  public:
+    ProblemPtr                                          P;
+    SensorForceTorqueInertialPtr                        S;
+    ProcessorForceTorqueInertialDynamicsPtr             p;
+    FrameBasePtr                                        F0, F1;
+    CaptureMotionPtr                                    C0, C1;
+    FeatureMotionPtr                                    f1;
+    FactorForceTorqueInertialDynamicsExternalForcePtr   c1;
+
+    VectorXd              data;
+    MatrixXd              data_cov;
+    VectorXd              delta;
+    MatrixXd              delta_cov;
+    Matrix<double, 13, 1> calib;
+    MatrixXd              J_delta_calib;
+
+  protected:
+    void SetUp() override
+    {
+        data     = VectorXd::Zero(12);  // [ a, w, f, t ]
+        data_cov = MatrixXd::Identity(12, 12);
+
+        // crear un problem
+        P = Problem::create("POVL", 3);
+
+        // crear un sensor
+        auto     params_sensor = std::make_shared<ParamsSensorForceTorqueInertial>();
+        Vector7d extrinsics;
+        extrinsics << 0, 0, 0, 0, 0, 0, 1;
+        params_sensor->add_external_force = true;
+        S = SensorBase::emplace<SensorForceTorqueInertial>(P->getHardware(), extrinsics, params_sensor);
+        S->setStateBlockDynamic('I', true);
+
+        // crear processador
+        auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialDynamics>();
+        p = ProcessorBase::emplace<ProcessorForceTorqueInertialDynamics>(S, params_processor);
+
+        // crear dos frame
+        VectorXd state(13);
+        state << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;
+        VectorComposite state_c(state, "POVL", {3, 4, 3, 3});
+        F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 0.0, "POVL", state_c);
+        F1 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 1.0, "POVL", state_c);
+
+        // crear dues capture
+        VectorXd data(VectorXd::Zero(12));
+        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);
+
+        // crear un feature
+        VectorXd delta_preint(VectorXd::Zero(19));
+        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>(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));
+        f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,
+                                                 jacobian_calib);
+
+        // crear un factor
+        c1 = FactorBase::emplace<FactorForceTorqueInertialDynamicsExternalForce>(f1, f1, C0, p, false);
+    }
+};
+
+// TEST(FactorForceTorqueInertialDynamics_yaml, force_registraion)
+// {
+//     FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+// }
+
+//Test to see if the constructor works (not yaml files)
+TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce, constructor)
+{
+    ASSERT_EQ(c1->getCapture(), C1);
+    ASSERT_EQ(c1->getCalibPreint().size(), 13);
+}
+
+//Test to see if the constructor works (yaml files)
+TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce_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_F(Test_FactorForceTorqueInertialDynamicsExternalForce, residual)
+{
+    VectorXd              res_exp = VectorXd::Zero(18);
+    Matrix<double, 18, 1> res;
+    VectorXd              bias = VectorXd::Zero(6);
+
+    P->print(4,1,1,1);
+
+    c1->residual(F0->getStateBlock('P')->getState(),                      // p0
+                 Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+                 F0->getStateBlock('V')->getState(),                      // v0
+                 F0->getStateBlock('L')->getState(),                      // L0
+                 bias,                                                    // I
+                 F1->getStateBlock('P')->getState(),                      // p1
+                 Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+                 F1->getStateBlock('V')->getState(),                      // v1
+                 F1->getStateBlock('L')->getState(),                      // L1
+                 S->getCom(),                                             // C
+                 S->getInertia(),                                         // i
+                 S->getMass(),                                            // m
+                 S->getExternalForce(),                                   // f
+                 res);
+
+    ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+}
+
+//Test en el que no hi ha moviment (yaml files)
+TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce_yaml, residual)
+{
+    VectorXd              res_exp = VectorXd::Zero(18);
+    Matrix<double, 18, 1> res;
+    VectorXd              bias = VectorXd::Zero(6);
+
+    c1->residual(F0->getStateBlock('P')->getState(),                      // p0
+                 Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+                 F0->getStateBlock('V')->getState(),                      // v0
+                 F0->getStateBlock('L')->getState(),                      // L0
+                 bias,                                                    // I
+                 F1->getStateBlock('P')->getState(),                      // p1
+                 Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+                 F1->getStateBlock('V')->getState(),                      // v1
+                 F1->getStateBlock('L')->getState(),                      // L1
+                 S->getCom(),                                             // C
+                 S->getInertia(),                                         // i
+                 S->getMass(),                                            // m
+                 S->getExternalForce(),                                   // f
+                 res);
+
+    ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+}
+
+
+//Test en el que s'avança 1m en direcció x (not yaml files) 
+TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce, 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));
+
+    //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.segment<3>(3) = -gravity();
+    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);
+
+    FactorForceTorqueInertialDynamicsExternalForcePtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamicsExternalForce>(f2, f2, C0, p, false);
+
+
+    c2->residual(F0->getStateBlock('P')->getState(),                      // p0
+                 Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+                 F0->getStateBlock('V')->getState(),                      // v0
+                 F0->getStateBlock('L')->getState(),                      // L0
+                 bias,                                                    // I
+                 F1->getStateBlock('P')->getState(),                      // p1
+                 Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+                 F1->getStateBlock('V')->getState(),                      // v1
+                 F1->getStateBlock('L')->getState(),                      // L1
+                 S->getCom(),                                             // C
+                 S->getInertia(),                                         // i
+                 S->getMass(),                                            // m
+                 S->getExternalForce(),                                   // f
+                 res);
+
+    ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+}
+
+//Test en el que s'avança 1m en direcció x (fitxers yaml) 
+
+TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce_yaml, 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));
+
+    //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.segment<3>(3) = -gravity();
+    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);
+
+    FactorForceTorqueInertialDynamicsExternalForcePtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamicsExternalForce>(f2, f2, C0, p, false);
+
+
+    c2->residual(F0->getStateBlock('P')->getState(),                      // p0
+                 Quaterniond(F0->getStateBlock('O')->getState().data()),  // q0
+                 F0->getStateBlock('V')->getState(),                      // v0
+                 F0->getStateBlock('L')->getState(),                      // L0
+                 bias,                                                    // I
+                 F1->getStateBlock('P')->getState(),                      // p1
+                 Quaterniond(F1->getStateBlock('O')->getState().data()),  // q1
+                 F1->getStateBlock('V')->getState(),                      // v1
+                 F1->getStateBlock('L')->getState(),                      // L1
+                 S->getCom(),                                             // C
+                 S->getInertia(),                                         // i
+                 S->getMass(),                                            // m
+                 S->getExternalForce(),                                   // f
+                 res);
+
+    ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
+}
+
+
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    //::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamicsExternalForce_yaml.residual";
+    return RUN_ALL_TESTS();
+}
\ No newline at end of file

test/yaml/problem_force_torque_inertial_dynamics_external_force.yaml

+config:
+  problem:
+    frame_structure: "POVL"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+        V: [0,0,0]
+        L: [0,0,0]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+        V: [1,1,1]
+        L: [1,1,1]
+      time_tolerance: 0.1
+    tree_manager: 
+      type: "None"
+  map:
+    type: "MapBase"
+    plugin: "core"
+  sensors:
+   -
+    name: "sensor FTI"
+    type: "SensorForceTorqueInertial"
+    plugin: "bodydynamics"
+    extrinsic:
+      pose: [0,0,0, 0,0,0,1]
+    follow: "./sensor_force_torque_inertial_external_force.yaml"
+
+  processors:
+   -
+    name: "proc FTID"
+    type: "ProcessorForceTorqueInertialDynamics"
+    sensor_name: "sensor FTI"
+    plugin: "bodydynamics"
+    follow: "processor_force_torque_inertial_dynamics.yaml"
+    
\ No newline at end of file

test/yaml/sensor_force_torque_inertial_external_force.yaml

+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)
+
+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]
+imu_bias_fix:                 true
+com_fix:                      true
+inertia_fix:                  true
+mass_fix:                     true
+
+add_external_force:           true
+external_force:               [0, 0, 1]
+external_force_fix:           true
\ No newline at end of file