From 1e0ae19eb6b0c226b109af10d5456260059110c3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 24 Jun 2022 19:58:18 +0200
Subject: [PATCH] Use state derived
---
src/sensor/sensor_force_torque_inertial.cpp | 21 +++++++++++----------
test/gtest_factor_force_torque_inertial.cpp | 5 +++++
2 files changed, 16 insertions(+), 10 deletions(-)
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index 969073f..a183868 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/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);
diff --git a/test/gtest_factor_force_torque_inertial.cpp b/test/gtest_factor_force_torque_inertial.cpp
index 29ef132..489cb9d 100644
--- a/test/gtest_factor_force_torque_inertial.cpp
+++ b/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:
--
GitLab