diff --git a/include/bodydynamics/capture/capture_force_torque_inertial.h b/include/bodydynamics/capture/capture_force_torque_inertial.h
index 9bef3980c4850136192e2af8612b28d1f0a0bd5c..3bc96c2b95f5f8ff1ce4f2d94a5df11aa9c7006c 100644
--- a/include/bodydynamics/capture/capture_force_torque_inertial.h
+++ b/include/bodydynamics/capture/capture_force_torque_inertial.h
@@ -10,6 +10,7 @@ WOLF_PTR_TYPEDEFS(CaptureForceTorqueInertial);
class CaptureForceTorqueInertial : public CaptureMotion
{
+ public:
CaptureForceTorqueInertial(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index 77bc249abbec9a59a048e9a5c7faae94beef0447..8becfc0a198f2c4169067d9ceae7b81ecc0ed4bd 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -30,6 +30,7 @@
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include "bodydynamics/capture/capture_force_torque_inertial.h"
// wolf
#include <core/state_block/state_composite.h>
@@ -69,8 +70,8 @@ CaptureMotionPtr ProcessorForceTorqueInertialDynamics::emplaceCapture(const Fram
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr)
{
- CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(
- _frame_own, "CaptureMotion", _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
+ CaptureMotionPtr capture = CaptureBase::emplace<CaptureForceTorqueInertial>(
+ _frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
setCalibration(capture, _calib);
capture->setCalibrationPreint(_calib_preint);
return capture;
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 099a536aa1fb1a4617241af6e0bb6adb2ddeddd9..20e26c06fdee4f8fd03e49b74c201107b1583e36 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -24,6 +24,7 @@
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
#include "bodydynamics/internal/config.h"
+#include "bodydynamics/capture/capture_force_torque_inertial.h"
#include <core/sensor/factory_sensor.h>
@@ -101,14 +102,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
S->getStateBlock('C')->fix();
S->getStateBlock('i')->fix();
S->getStateBlock('m')->unfix();
- // S->setStateBlockDynamic('I');
+ S->setStateBlockDynamic('I');
- CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
- CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
- CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
- CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -135,10 +136,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
WOLF_INFO("-----------------------------");
- CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
{
- C->setTimeStamp(t);
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->process();
p->getOrigin()->getFrame()->fix();
report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
@@ -172,12 +172,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
S->getStateBlock('i')->fix();
S->getStateBlock('m')->fix();
- CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
- CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
- CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
- CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>( 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>( 0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>( 0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>( 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>( 0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>( 1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -204,10 +204,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
WOLF_INFO("-----------------------------");
- CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 11.0 ; t += 1.0)
{
- C->setTimeStamp(t);
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
C->process();
p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
p->getOrigin()->getFrame()->fix();
@@ -240,13 +239,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
S->getStateBlock('C')->unfix();
S->getStateBlock('i')->fix();
S->getStateBlock('m')->unfix();
+ S->setStateBlockDynamic('I');
- CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
- CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
- CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
- CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
- CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
- CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
C0_0->process();
CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -283,9 +283,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
// which if not eliminated contaminate the overall solution.
// This is due to these first factors relying on the linearization Jacobian to correct the
// poorly preintegrated delta.
- CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
{
+ CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
C->setTimeStamp(t);
C->process();
p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques