From f3126acd196d0c8b8bad969da90f79e4282122a5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 8 Aug 2022 17:38:55 +0200
Subject: [PATCH] Use the new CaptureForceTorqueInertial

This helps handling dynamic sensor params because it creates the dynamic state blocks automatically if required.
---
 .../capture/capture_force_torque_inertial.h   |  1 +
 ...ocessor_force_torque_inertial_dynamics.cpp |  5 +-
 ...problem_force_torque_inertial_dynamics.cpp | 48 +++++++++----------
 3 files changed, 28 insertions(+), 26 deletions(-)

diff --git a/include/bodydynamics/capture/capture_force_torque_inertial.h b/include/bodydynamics/capture/capture_force_torque_inertial.h
index 9bef398..3bc96c2 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 77bc249..8becfc0 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 099a536..20e26c0 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
-- 
GitLab