Use state derived

src/sensor/sensor_force_torque_inertial.cpp

@@ -23,6 +23,7 @@
 #include "bodydynamics/sensor/sensor_force_torque_inertial.h"
 
 #include <core/state_block/factory_state_block.h>
+#include <core/state_block/state_block_derived.h>
 
 namespace wolf
 {
@@ -31,36 +32,36 @@ namespace wolf
 SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&                    _extrinsics,
                                                      ParamsSensorForceTorqueInertialPtr _params)
     : SensorBase("SensorForceTorqueInertial",
-                 FactoryStateBlock::create("StateBlock", _extrinsics.head(3), false),
+                 FactoryStateBlock::create("StatePoint3d", _extrinsics.head(3), false),
                  FactoryStateBlock::create("StateQuaternion", _extrinsics.tail(4), false),
-                 FactoryStateBlock::create("StateBlock", Vector6d::Zero(), false),  // IMU bias
+                 FactoryStateBlock::create("StateParams6", Vector6d::Zero(), false),  // IMU bias
                  12,
                  false,
                  false,
                  false),
       params_fti_(_params)
 {
-    addStateBlock('c', FactoryStateBlock::create("StateBlock", Vector3d::Ones(), true));  // centre of mass
-    addStateBlock('i', FactoryStateBlock::create("StateBlock", Vector3d::Ones(), true));  // inertial vector
-    addStateBlock('m', FactoryStateBlock::create("StateBlock", Vector1d::Ones(), true));  // total mass
+    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
 }
 
 // TODO: Adapt this API to that of MR !448
 SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite&             _states,
                                                      ParamsSensorForceTorqueInertialPtr _params)
     : SensorBase("SensorForceTorqueInertial",
-                 FactoryStateBlock::create("StateBlock", _states.at('P'), false),
+                 FactoryStateBlock::create("StatePoint3d", _states.at('P'), false),
                  FactoryStateBlock::create("StateQuaternion", _states.at('O'), false),
-                 FactoryStateBlock::create("StateBlock", _states.at('I'), false),  // IMU bias
+                 FactoryStateBlock::create("StateParams6", _states.at('I'), false),  // IMU bias
                  12,
                  false,
                  false,
                  false),
       params_fti_(_params)
 {
-    emplaceStateBlock('c', getProblem(), _states.at('c'), true);  // centre of mass
-    emplaceStateBlock('i', getProblem(), _states.at('i'), true);  // inertial vector
-    emplaceStateBlock('m', getProblem(), _states.at('m'), true);  // total 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('i', false);
     setStateBlockDynamic('m', false);

test/gtest_factor_force_torque_inertial.cpp

@@ -36,6 +36,11 @@
 using namespace wolf;
 using namespace bodydynamics::fti;
 
+// Register StateBlock of type "L"
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
 class Test_FactorForceTorqueInertialPreint : public testing::Test
 {
   public: