new data2tangent function in the processor ftipd

include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h

@@ -50,8 +50,8 @@ struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessor
 
     std::string print() const override
     {
-        return ParamsProcessorMotion::print() + "\n";               //
-              //  + "n_propellers: " + std::to_string(n_propellers);  //
+        return ParamsProcessorMotion::print() + "\n";  //
+                                                       //  + "n_propellers: " + std::to_string(n_propellers);  //
     }
 
     // int n_propellers;
@@ -65,11 +65,12 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
         ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics_);
     ~ProcessorForceTorqueInertialPreintDynamics() override;
     void configure(SensorBasePtr _sensor) override;
-    WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics, ParamsProcessorForceTorqueInertialPreintDynamics);
+    WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics,
+                          ParamsProcessorForceTorqueInertialPreintDynamics);
 
   protected:
     ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_dynamics_;
-    SensorForceTorqueInertialPtr                sensor_fti_;
+    SensorForceTorqueInertialPtr                        sensor_fti_;
 
   public:
     /** \brief convert raw CaptureMotion data to the delta-state format.
@@ -132,6 +133,12 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
                              Eigen::MatrixXd&       _delta_cov,
                              Eigen::MatrixXd&       _jacobian_calib) const override;
 
+    void data2tangent(const Eigen::VectorXd&        _data,
+                       const Eigen::VectorXd&        _calib,
+                       Eigen::VectorXd& _tangent,
+                       Eigen::MatrixXd& _J_tangent_data,
+                       Eigen::MatrixXd& _J_tangent_bias) const;
+
     /** \brief composes a delta-state on top of another delta-state
      * \param _delta1 the first delta-state
      * \param _delta2 the second delta-state

src/processor/processor_force_torque_inertial_preint_dynamics.cpp

@@ -111,6 +111,38 @@ void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor
     sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
 }
 
+void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd&         _data,
+                                                               const Eigen::VectorXd&         _calib,
+                                                               Eigen::VectorXd&  _tangent,
+                                                               Eigen::MatrixXd& _J_tangent_data,
+                                                               Eigen::MatrixXd&  _J_tangent_bias) const
+
+{
+    Vector6d        bias  = _calib.segment<6>(0);
+    Vector7d        model = _calib.segment<7>(6);
+    const Vector6d& awd   = _data.segment<6>(0);  // this is (a,w) of data in a 6-vector
+    const Vector3d& fd    = _data.segment<3>(6);
+    const Vector3d& td    = _data.segment<3>(9);
+    const Vector3d& c     = model.segment<3>(0);
+    // const Matrix3d& fd_cross = skew(fd);
+    const Matrix3d& c_cross = skew(c);
+
+    // tangent(a,w) = data(a,w) - bias(a,w)
+    // tangent(f)   = data(f)
+    // tangent(t)   = data(t) - model(c) x data(f)
+    _tangent.segment<6>(0) = awd - bias;
+    _tangent.segment<3>(6) = fd;
+    _tangent.segment<3>(9) = td - c.cross(fd);  // c x fd
+
+    // J_tangent_data
+    _J_tangent_data                   = MatrixXd::Identity(12, 12);
+    _J_tangent_data.block<3, 3>(9, 6) = -c_cross;  // J_tt_fd = -c_cross
+
+    // J_tangent_bias = [-Identity(6,6) ; Zero(6,6)]
+    _J_tangent_bias.topRows<6>()    = -Matrix6d::Identity();
+    _J_tangent_bias.bottomRows<6>() = Matrix6d::Zero();
+}
+
 void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
                                                                      const Eigen::MatrixXd& _data_cov,
                                                                      const Eigen::VectorXd& _calib,
@@ -290,60 +322,28 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
      * tangent  = [at,wt,ft,tt]
      */
 
-    Vector6d        bias     = _calib.segment<6>(0);
-    Vector7d        model    = _calib.segment<7>(6);
-    const Vector6d& awd      = _data.segment<6>(0);  // this is (a,w) of data in a 6-vector
-    const Vector3d& fd       = _data.segment<3>(6);
-    const Vector3d& td       = _data.segment<3>(9);
-    const Vector3d& c        = model.segment<3>(0);
-    const Matrix3d& fd_cross = skew(fd);
-    const Matrix3d& c_cross  = skew(c);
+    VectorXd  tangent(12); 
+    MatrixXd J_tangent_data(12, 12);
+    MatrixXd  J_tangent_bias(12, 6);
 
-    // tangent(a,w) = data(a,w) - bias(a,w)
-    // tangent(f)   = data(f)
-    // tangent(t)   = data(t) - model(c) x data(f)
-    Matrix<double, 12, 1> tangent;
-    tangent.segment<6>(0) = awd - bias;
-    tangent.segment<3>(6) = fd;
-    tangent.segment<3>(9) = td - c.cross(fd);  // c x fd
-
-    // J_tangent_data
-    Matrix<double, 12, 12> J_tangent_data = MatrixXd::Identity(12, 12);
-    J_tangent_data.block<3, 3>(9, 6)      = -c_cross;  // J_tt_fd = -c_cross
-
-    // J_tangent_bias = [-Identity(6,6) ; Zero(6,6)]
-    Matrix<double, 12, 6> J_tangent_bias;
-    J_tangent_bias.topRows<6>()    = -Matrix6d::Identity();
-    J_tangent_bias.bottomRows<6>() = Matrix6d::Zero();
-
-    // J_tangent_model
-    Matrix<double, 12, 7> J_tangent_model = MatrixXd::Zero(12, 7);
-    J_tangent_model.block<3, 3>(9, 0)     = fd_cross;  // J_tt_c = fd_cross
-
-    WOLF_INFO("J_tangent_data\n", J_tangent_data);
-    WOLF_INFO("J_tangent_bias\n", J_tangent_bias);
-    WOLF_INFO("J_tangent_model\n", J_tangent_model);
+    data2tangent(_data, _calib, tangent, J_tangent_data, J_tangent_bias);
 
     // 2. go from tangent to delta. We need again the dynamic model for this
-    // const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
+    Vector7d               model = _calib.segment<7>(6);
     Matrix<double, 18, 12> J_delta_tangent;
     Matrix<double, 18, 7>  J_delta_model;
     fti::tangent2delta(
         tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);  // Aquí ja farà bé ell sol el J_delta_model?
 
-    WOLF_INFO("J_delta_tangent\n", J_delta_tangent);
-    WOLF_INFO("J_delta_model\n", J_delta_model);
-
     // 3. Compose jacobians
     Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias;
-    // J_delta_model += J_delta_tangent * J_tangent_model;
+    // !!!!!! Now J_delta_model seems to be calculated in function tangent2delta as J_delta_tangent * J_tangent_model !!!!!!!!!!!
+    // TODO: Check the calculation od the J_delta_model
+    // J_delta_model += J_delta_tangent * J_tangent_model; 
     _jacobian_calib << J_delta_bias,
         J_delta_model;  // J_delta_calib = _jacobian_calib = [J_delta_bias ; J_delta_model]
     const auto& J_delta_data = J_delta_tangent * J_tangent_data;
 
-    WOLF_INFO("J_delta_data\n", J_delta_data);
-    WOLF_INFO("J_delta_calib\n", _jacobian_calib);
-
     // 4. compute covariance
     _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
 

test/gtest_solve_problem_force_torque_inertial_dynamics.cpp

@@ -67,9 +67,9 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
         // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION;  // ceres::TRUST_REGION;ceres::LINE_SEARCH
         // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
         // solver_->getSolverOptions().max_num_iterations               = 1e4;
-        // solver_->getSolverOptions().function_tolerance               = 1e-15;
-        // solver_->getSolverOptions().gradient_tolerance               = 1e-15;
-        // solver_->getSolverOptions().parameter_tolerance              = 1e-15;
+        solver_->getSolverOptions().function_tolerance               = 1e-15;
+        solver_->getSolverOptions().gradient_tolerance               = 1e-15;
+        solver_->getSolverOptions().parameter_tolerance              = 1e-15;
 
         S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
         p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
@@ -192,8 +192,8 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
 
 TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
 {
-    S->getStateBlock('C')->setState(Vector3d(0.1, 0.1, 0.2));
-    S->getStateBlock('m')->setState(Vector1d(1.9));
+    S->getStateBlock('C')->setState(Vector3d(0.0, 0.0, 0.02));
+    S->getStateBlock('m')->setState(Vector1d(1.91));
     Vector3d cdm_guess  = S->getStateBlock('C')->getState();
     double   mass_guess = S->getStateBlock('m')->getState()(0);
 
@@ -250,6 +250,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.cdm_only_hovering";
+    ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.mass_and_cdm_hovering";
     return RUN_ALL_TESTS();
 }
\ No newline at end of file