modify existing classes

include/bodydynamics/factor/factor_force_torque_inertial.h

@@ -191,7 +191,7 @@ inline bool FactorForceTorqueInertial::residual(const MatrixBase<D1>&     _p1,
     fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq);
 
     Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
-    Matrix<T, 18, 1> delta_step   = J_delta_bias_ * (_b1 - bias_preint_);
+    Matrix<T, 18, 1> delta_step   = J_delta_bias_ * (_b1 - bias_preint_); // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1>
     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;

include/bodydynamics/sensor/sensor_force_torque_inertial.h

@@ -35,10 +35,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorForceTorqueInertial);
 struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
 {
     // noise std dev
-    double acc_noise_std;
-    double gyro_noise_std;
-    double force_noise_std;
-    double torque_noise_std;
+    double   acc_noise_std;
+    double   gyro_noise_std;
+    double   force_noise_std;
+    double   torque_noise_std;
+    Vector3d com;
+    Vector3d inertia;
+    double   mass;
 
     ParamsSensorForceTorqueInertial() = default;
     ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
@@ -48,6 +51,9 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
         gyro_noise_std   = _server.getParam<double>(prefix + _unique_name + "/gyro_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");
+        com              = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
+        inertia          = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
+        mass             = _server.getParam<double>(prefix + _unique_name + "/mass");
     }
     ~ParamsSensorForceTorqueInertial() override = default;
     std::string print() const override
@@ -56,7 +62,10 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
                + "acc_noise_std:    " + std::to_string(acc_noise_std) + "\n"      //
                + "gyro_noise_std:   " + std::to_string(gyro_noise_std) + "\n"     //
                + "force_noise_std:  " + std::to_string(force_noise_std) + "\n"    //
-               + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n";  //
+               + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n"   //
+               + "com: print not implemented." + "\n"                             //
+               + "inertia: print not implemented. \n"                             //
+               + "mass:             " + std::to_string(mass) + "\n";              //
     }
 };
 
@@ -81,7 +90,7 @@ class SensorForceTorqueInertial : public SensorBase
     double   getMass() const;      // Total mass
     Vector3d getInertia() const;   // Inertia vector (diagonal of inertia matrix)
     Vector3d getCom() const;       // centre of mass
-    Vector7d getModel() const;      // dynamic model [com, inertia, mass]
+    Vector7d getModel() const;     // dynamic model [com, inertia, mass]
 };
 
 inline double SensorForceTorqueInertial::getMass() const
@@ -111,13 +120,13 @@ inline Vector3d SensorForceTorqueInertial::getInertia() const
 
 inline Vector3d SensorForceTorqueInertial::getCom() const
 {
-    return getStateBlock('c')->getState();
+    return getStateBlock('C')->getState();
 }
 
 inline Vector7d SensorForceTorqueInertial::getModel() const
 {
     Vector7d model;
-    model << getCom() , getInertia() , getMass();
+    model << getCom(), getInertia(), getMass();
     return model;
 }
 

src/sensor/sensor_force_torque_inertial.cpp

@@ -41,9 +41,12 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
                  false),
       params_fti_(_params)
 {
-    addStateBlock('c', FactoryStateBlock::create("StateParams3", Vector3d::Ones(), true));  // centre of mass
-    addStateBlock('i', FactoryStateBlock::create("StateParams3", Vector3d::Ones(), true));  // inertial vector
-    addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d::Ones(), true));  // total mass
+    addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true));  // centre of mass
+    addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, true));  // inertial vector
+    addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), true));  // total mass
+    setStateBlockDynamic('C', false);
+    setStateBlockDynamic('i', false);
+    setStateBlockDynamic('m', false);
 }
 
 // TODO: Adapt this API to that of MR !448
@@ -59,10 +62,10 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite&
                  false),
       params_fti_(_params)
 {
-    auto sbc = emplaceStateBlock<StateParams3>('c', getProblem(), _states.at('c'), true);  // centre of mass
+    auto sbc = emplaceStateBlock<StateParams3>('C', getProblem(), _states.at('C'), true);  // centre of mass
     auto sbi = emplaceStateBlock<StateParams3>('i', getProblem(), _states.at('i'), true);  // inertial vector
     auto sbm = emplaceStateBlock<StateParams1>('m', getProblem(), _states.at('m'), true);  // total mass
-    setStateBlockDynamic('c', false);
+    setStateBlockDynamic('C', false);
     setStateBlockDynamic('i', false);
     setStateBlockDynamic('m', false);
 }

test/CMakeLists.txt

@@ -20,6 +20,8 @@ wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
 
 wolf_add_gtest(gtest_factor_force_torque_inertial gtest_factor_force_torque_inertial.cpp)
 
+wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics gtest_factor_force_torque_inertial_dynamics.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/yaml/sensor_force_torque_inertial.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
+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
\ No newline at end of file
+
+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]
\ No newline at end of file