Add yanml params to fix calibration states

include/bodydynamics/sensor/sensor_force_torque_inertial.h

@@ -46,6 +46,7 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
     Vector3d com;
     Vector3d inertia;
     double   mass;
+    bool     imu_bias_fix, com_fix, inertia_fix, mass_fix;
 
     ParamsSensorForceTorqueInertial() = default;
     ParamsSensorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
@@ -62,6 +63,10 @@ struct ParamsSensorForceTorqueInertial : public ParamsSensorBase
         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

@@ -34,16 +34,16 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
     : SensorBase("SensorForceTorqueInertial",
                  FactoryStateBlock::create("StatePoint3d", _extrinsics.head(3), true),
                  FactoryStateBlock::create("StateQuaternion", _extrinsics.tail(4), true),
-                 FactoryStateBlock::create("StateParams6", Vector6d::Zero(), false),  // IMU bias
+                 FactoryStateBlock::create("StateParams6", Vector6d::Zero(), _params->imu_bias_fix),  // IMU bias
                  12,
                  false,
                  false,
                  false),
       params_fti_(_params)
 {
-    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
+    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
     setStateBlockDynamic('I', false);
     setStateBlockDynamic('C', false);
     setStateBlockDynamic('i', false);