Add IMU drift params to SensorFTI

include/bodydynamics/sensor/sensor_force_torque_inertial.h

@@ -37,6 +37,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
     // noise std dev
     double   acc_noise_std;
     double   gyro_noise_std;
+    double   acc_drift_std;
+    double   gyro_drift_std;
     double   force_noise_std;
     double   torque_noise_std;
     Vector3d com;
@@ -49,6 +51,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
     {
         acc_noise_std    = _server.getParam<double>(prefix + _unique_name + "/acc_noise_std");
         gyro_noise_std   = _server.getParam<double>(prefix + _unique_name + "/gyro_noise_std");
+        acc_drift_std    = _server.getParam<double>(prefix + _unique_name + "/acc_drift_std");
+        gyro_drift_std   = _server.getParam<double>(prefix + _unique_name + "/gyro_drift_std");
         force_noise_std  = _server.getParam<double>(prefix + _unique_name + "/force_noise_std");
         torque_noise_std = _server.getParam<double>(prefix + _unique_name + "/torque_noise_std");
         com              = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
@@ -61,6 +65,8 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
         return ParamsSensorBase::print() + "\n"                                   //
                + "acc_noise_std:    " + std::to_string(acc_noise_std) + "\n"      //
                + "gyro_noise_std:   " + std::to_string(gyro_noise_std) + "\n"     //
+               + "acc_drift_std:    " + std::to_string(acc_drift_std) + "\n"      //
+               + "gyro_drift_std:   " + std::to_string(gyro_drift_std) + "\n"     //
                + "force_noise_std:  " + std::to_string(force_noise_std) + "\n"    //
                + "torque_noise_std: " + std::to_string(torque_noise_std) + "\n"   //
                + "com: print not implemented." + "\n"                             //
@@ -91,6 +97,7 @@ class SensorForceTorqueInertial : public SensorBase
     Vector3d getInertia() const;   // Inertia vector (diagonal of inertia matrix)
     Vector3d getCom() const;       // centre of mass
     Vector7d getModel() const;     // dynamic model [com, inertia, mass]
+    ParamsSensorForceTorqueInertialPtr getParamsSensorForceTorqueInertial(){return params_fti_;}
 };
 
 inline double SensorForceTorqueInertial::getMass() const

test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml

@@ -31,6 +31,8 @@ config:
     torque_noise_std:             0.1         # std dev of torque noise in N/m
     acc_noise_std:                0.01        # std dev of acc noise in m/s2
     gyro_noise_std:               0.01        # std dev of gyro noise in rad/s
+    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]

test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml

@@ -31,11 +31,16 @@ config:
     plugin: "bodydynamics"
     extrinsic:
       pose: [0,0,0, 0,0,0,1]
+    
+    # IMU
     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
-
+    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)
+    
+    # Dynamics
     com:                          [0,0,0.0341]                      # center of mass [m]
     inertia:                      [0.017598,0.017957,0.029599]      # moments of inertia i_xx, i_yy, i_zz [kg m2]
     mass:                         1.9                               # mass [kg]

test/yaml/sensor_force_torque_inertial.yaml

@@ -2,6 +2,8 @@ force_noise_std:              0.1         # std dev of force noise in N
 torque_noise_std:             0.1         # std dev of torque noise in N/m
 acc_noise_std:                0.01        # std dev of acc noise in m/s2
 gyro_noise_std:               0.01        # std dev of gyro noise in rad/s
+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]