Add params to YAML system

include/bodydynamics/sensor/sensor_force_torque_inertial.h

@@ -43,6 +43,7 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
     double   gyro_drift_std;
     double   force_noise_std;
     double   torque_noise_std;
+    Vector3d acc_bias, gyro_bias;
     Vector3d com;
     Vector3d inertia;
     double   mass;
@@ -58,15 +59,27 @@ 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");
-        com               = _server.getParam<Vector3d>(prefix + _unique_name + "/com");
-        inertia           = _server.getParam<Vector3d>(prefix + _unique_name + "/inertia");
-        mass              = _server.getParam<double>(prefix + _unique_name + "/mass");
-        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");
-        mass_fix          = _server.getParam<bool>  (prefix + _unique_name + "/mass_fix");
+        
+        if (_server.hasParam(prefix + _unique_name + "/acc_bias"))
+            acc_bias = _server.getParam<Vector3d>(prefix + _unique_name + "/acc_bias");
+        else
+            acc_bias.setZero();
+        if (_server.hasParam(prefix + _unique_name + "/gyro_bias"))
+            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");
+        
+        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");
+        mass_fix     = _server.getParam<bool>(prefix + _unique_name + "/mass_fix");
     }
     ~ParamsSensorForceTorqueInertial() override = default;
     std::string print() const override

src/sensor/sensor_force_torque_inertial.cpp

@@ -41,6 +41,10 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
                  false),
       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;
+    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
@@ -58,6 +62,7 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
     setNoiseStd(noise_std_vector);
 }
 
+
 // TODO: Adapt this API to that of MR !448
 SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite&             _states,
                                                      ParamsSensorForceTorqueInertialPtr _params)