From 389e25880ca0b5659e3cec07f6c010b46429eef9 Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Thu, 4 Aug 2022 17:30:15 +0200
Subject: [PATCH] processor_ftipd modification (fixing errors) and new gtests
---
.clang-format | 2 +-
..._force_torque_inertial_preint_dynamics.cpp | 130 ++++++---
src/sensor/sensor_force_torque_inertial.cpp | 7 +-
test/CMakeLists.txt | 2 +
..._force_torque_inertial_preint_dynamics.cpp | 2 +-
...problem_force_torque_inertial_dynamics.cpp | 255 ++++++++++++++++++
...e_torque_inertial_dynamics_solve_test.yaml | 56 ++++
7 files changed, 415 insertions(+), 39 deletions(-)
create mode 100644 test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
create mode 100644 test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
diff --git a/.clang-format b/.clang-format
index d7ec3ad..8f284dd 100644
--- a/.clang-format
+++ b/.clang-format
@@ -19,7 +19,7 @@ AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
-BinPackArguments: true
+BinPackArguments: false
BinPackParameters: false
BreakBeforeBraces: Custom
BraceWrapping:
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index 81156be..3a73a02 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -68,12 +68,8 @@ CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(cons
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<CaptureMotion>(
+ _frame_own, "CaptureMotion", _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
setCalibration(capture, _calib);
capture->setCalibrationPreint(_calib_preint);
return capture;
@@ -81,33 +77,32 @@ CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(cons
FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
{
- auto motion = _capture_own->getBuffer().back();
- FeatureBasePtr ftr = FeatureBase::emplace<FeatureMotion>(
- _capture_own,
- "FeatureMotion",
- motion.delta_integr_,
- motion.delta_integr_cov_,
- _capture_own->getCalibrationPreint(),
- motion.jacobian_calib_);
+ auto motion = _capture_own->getBuffer().back();
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureMotion>(_capture_own,
+ "FeatureMotion",
+ motion.delta_integr_,
+ motion.delta_integr_cov_,
+ _capture_own->getCalibrationPreint(),
+ motion.jacobian_calib_);
return ftr;
}
FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin)
{
-
FeatureMotionPtr ftr_ftipd = std::static_pointer_cast<FeatureMotion>(_feature_motion);
- auto fac_ftipd = FactorBase::emplace<FactorForceTorqueInertialDynamics>(_feature_motion, ftr_ftipd, _capture_origin, shared_from_this(), params_->apply_loss_function);
+ auto fac_ftipd = FactorBase::emplace<FactorForceTorqueInertialDynamics>(
+ _feature_motion, ftr_ftipd, _capture_origin, shared_from_this(), params_->apply_loss_function);
return fac_ftipd;
}
void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture,
const VectorXd& _calibration)
{
- _capture->getStateBlock('I')->setState(_calibration.segment<6>(0)); // Set IMU bias
- _capture->getStateBlock('C')->setState(_calibration.segment<3>(6)); // Set C
- _capture->getStateBlock('i')->setState(_calibration.segment<3>(9)); // Set i
+ _capture->getStateBlock('I')->setState(_calibration.segment<6>(0)); // Set IMU bias
+ _capture->getStateBlock('C')->setState(_calibration.segment<3>(6)); // Set C
+ _capture->getStateBlock('i')->setState(_calibration.segment<3>(9)); // Set i
_capture->getStateBlock('m')->setState(_calibration.segment<1>(12)); // Set m
}
@@ -125,23 +120,87 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
Eigen::MatrixXd& _jacobian_calib) const
{
// compute tangent by removing IMU bias
+
+ /**
+ * @brief tangent and data
+ *
+ * tangent = [at,wt,ft,tt] wrt CoM
+ * data = [ad,wd,fd,td] measured
+ * calib = [I,C,i,m]
+ * I = [ab,wb]
+ *
+ * at = ad - ab
+ * wt = wd - wb
+ * ft = fd
+ * tt = td +- C x fd
+ *
+ * cross product and Jacobians
+ *
+ * a, b in R3
+ *
+ * a_x, b_x, skew-symmetric matrices
+ *
+ * a_x = [0 -az ay ; az 0 -ax ; -ay ax 0]
+ *
+ * a x b = a_x b
+ *
+ * a x b = - b x a = - b_x a
+ *
+ * J_(axb)_a = -b_x
+ *
+ * J_(axb)_b = a_x
+ */
+
+ // Matrix<double, 12, 1> tangent = _data;
+ // tangent.head<6>() -= _calib.head<6>(); // J_tangent_data = Identity(12,12)
+ // Matrix<double, 12, 6> J_tangent_I; // J_tangent_I = [-Identity(6,6) ; Zero(6,6)]
+ // J_tangent_I.topRows<6>() = -Matrix6d::Identity();
+ // J_tangent_I.bottomRows<6>() = Matrix6d::Zero();
+
+ // // go from tangent to delta. We need to dynamic model for this
+ // const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
+ // Matrix<double, 18, 12> J_delta_tangent;
+ // Matrix<double, 18, 7> J_delta_model;
+ // fti::tangent2delta(tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);
+
+ // // Compute cov and compose jacobians
+ // Matrix<double, 18, 6> J_delta_I = J_delta_tangent * J_tangent_I;
+ // _jacobian_calib << J_delta_I, J_delta_model; // J_delta_calib = _jacobian_calib = [J_delta_I ; J_delta_model]
+ // const auto& J_delta_data = J_delta_tangent; // * J_tangent_data, which is the Identity;
+ // _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
+
+ ////////////////////// NOU CODI QUE HAURE DE REVISAR I CANVIAR PEL QUE HI HA ADALT //////////////////////////
+
+ Vector3d C = _calib.segment<3>(6);
+ Matrix3d C_cross = skew(C);
+ Vector3d fd = _data.segment<3>(6);
+ Matrix3d fd_cross = skew(fd);
+
Matrix<double, 12, 1> tangent = _data;
- tangent.head<6>() -= _calib.head<6>(); // J_tangent_data = Identity(12,12)
- Matrix<double, 12, 6> J_tangent_I; // J_tangent_I = [-Identity(6,6) ; Zero(6,6)]
- J_tangent_I.topRows<6>() = -Matrix6d::Identity();
- J_tangent_I.bottomRows<6>() = Matrix6d::Zero();
+ tangent.segment<3>(9) += _calib.segment<3>(6).cross(_data.segment<3>(6)); // C x fd
+ Matrix<double, 12, 12> J_tangent_data = MatrixXd::Identity(12, 12);
+ J_tangent_data.bottomRows(3).middleCols(6, 3) = C_cross;
+ Matrix<double, 12, 6> J_tangent_I; // J_tangent_I = [-Identity(6,6) ; Zero(6,6)]
+ J_tangent_I.topRows<6>() = -Matrix6d::Identity();
+ J_tangent_I.bottomRows<6>() = Matrix6d::Zero();
+ Matrix<double, 12, 7> J_tangent_model = MatrixXd::Zero(12, 7);
+ J_tangent_model.bottomRows(3).leftCols(3) = -fd_cross; // J_tangent_model = [Zero(9,7); -fd_cross, Zero(3,3), 0]
// go from tangent to delta. We need to dynamic model for this
const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
Matrix<double, 18, 12> J_delta_tangent;
Matrix<double, 18, 7> J_delta_model;
- fti::tangent2delta(tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);
+ fti::tangent2delta(
+ tangent, model, _dt, _delta, J_delta_tangent, J_delta_model); // Aquà ja farà bé ell sol el J_delta_model?
// Compute cov and compose jacobians
Matrix<double, 18, 6> J_delta_I = J_delta_tangent * J_tangent_I;
+ J_delta_model += J_delta_tangent * J_tangent_model;
_jacobian_calib << J_delta_I, J_delta_model; // J_delta_calib = _jacobian_calib = [J_delta_I ; J_delta_model]
- const auto& J_delta_data = J_delta_tangent; // * J_tangent_data, which is the Identity;
- _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
+ const auto& J_delta_data = J_delta_tangent * J_tangent_data;
+ _delta_cov = J_delta_data * _data_cov * J_delta_data.transpose();
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
@@ -181,8 +240,8 @@ Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::correctDelta(const E
VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
{
if (_capture)
- {
- VectorXd calibration(13);
+ {
+ VectorXd calibration(13);
const Vector6d& I_calib = _capture->getStateBlock('I')->getState();
const Vector3d& C_calib = _capture->getStateBlock('C')->getState();
const Vector3d& i_calib = _capture->getStateBlock('i')->getState();
@@ -192,7 +251,7 @@ VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const Captur
}
else
{
- VectorXd calibration(13);
+ VectorXd calibration(13);
const Vector6d& I_calib = getSensor()->getStateBlock('I')->getState();
const Vector3d& C_calib = getSensor()->getStateBlock('C')->getState();
const Vector3d& i_calib = getSensor()->getStateBlock('i')->getState();
@@ -205,33 +264,34 @@ VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const Captur
bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
{
// time span
- if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_force_torque_inertial_preint_dynamics_->max_time_span)
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ >
+ params_force_torque_inertial_preint_dynamics_->max_time_span)
{
- WOLF_DEBUG( "PM: vote: time span" );
+ WOLF_DEBUG("PM: vote: time span");
return true;
}
// buffer length
if (getBuffer().size() > params_force_torque_inertial_preint_dynamics_->max_buff_length)
{
- WOLF_DEBUG( "PM: vote: buffer length" );
+ WOLF_DEBUG("PM: vote: buffer length");
return true;
}
// dist_traveled
VectorComposite X0 = getOrigin()->getFrame()->getState();
VectorComposite X1;
- double dt = getBuffer().back().ts_ - getOrigin()->getFrame()->getTimeStamp();
+ double dt = getBuffer().back().ts_ - getOrigin()->getFrame()->getTimeStamp();
statePlusDelta(X0, getBuffer().back().delta_integr_, dt, X1);
double dist = (X1.at('P') - X0.at('P')).norm();
if (dist > params_force_torque_inertial_preint_dynamics_->dist_traveled)
{
- WOLF_DEBUG( "PM: vote: distance traveled" );
+ WOLF_DEBUG("PM: vote: distance traveled");
return true;
}
// angle turned
double angle = 2.0 * acos(getMotion().delta_integr_(15));
if (angle > params_force_torque_inertial_preint_dynamics_->angle_turned)
{
- WOLF_DEBUG( "PM: vote: angle turned" );
+ WOLF_DEBUG("PM: vote: angle turned");
return true;
}
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index 886ef10..89fac3f 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -32,8 +32,8 @@ namespace wolf
SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd& _extrinsics,
ParamsSensorForceTorqueInertialPtr _params)
: SensorBase("SensorForceTorqueInertial",
- FactoryStateBlock::create("StatePoint3d", _extrinsics.head(3), false),
- FactoryStateBlock::create("StateQuaternion", _extrinsics.tail(4), false),
+ FactoryStateBlock::create("StatePoint3d", _extrinsics.head(3), true),
+ FactoryStateBlock::create("StateQuaternion", _extrinsics.tail(4), true),
FactoryStateBlock::create("StateParams6", Vector6d::Zero(), false), // IMU bias
12,
false,
@@ -44,9 +44,11 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
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
+ setStateBlockDynamic('I', false);
setStateBlockDynamic('C', false);
setStateBlockDynamic('i', false);
setStateBlockDynamic('m', false);
+
}
// TODO: Adapt this API to that of MR !448
@@ -65,6 +67,7 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite&
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('I', false);
setStateBlockDynamic('C', false);
setStateBlockDynamic('i', false);
setStateBlockDynamic('m', false);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index e2ad0be..425d6f1 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -39,3 +39,5 @@ wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
wolf_add_gtest(gtest_sensor_inertial_kinematics gtest_sensor_inertial_kinematics.cpp)
+wolf_add_gtest(gtest_solve_problem_force_torque_inertial_dynamics gtest_solve_problem_force_torque_inertial_dynamics.cpp)
+
diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
index f18a414..d9f1e87 100644
--- a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
+++ b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
@@ -254,6 +254,6 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_buf
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.VoteForKeyFrame_buffer";
+ ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.not_moving_test";
return RUN_ALL_TESTS();
}
\ No newline at end of file
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
new file mode 100644
index 0000000..619dd4b
--- /dev/null
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -0,0 +1,255 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+
+#include <core/sensor/factory_sensor.h>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialPreintDynamicsPtr p;
+ SolverCeresPtr solver_;
+ Vector3d cdm_true;
+ Vector3d inertia_true;
+ double mass_true;
+
+ protected:
+ void SetUp() override
+ {
+ std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser =
+ ParserYaml("problem_force_torque_inertial_dynamics_solve_test.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+ // CERES WRAPPER
+ solver_ = std::make_shared<SolverCeres>(P);
+ solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; // ceres::TRUST_REGION;ceres::LINE_SEARCH
+ solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
+ solver_->getSolverOptions().max_num_iterations = 1e4;
+
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+
+ cdm_true = {0,0,0.0341};
+ inertia_true = {0.017598,0.017957,0.029599};
+ mass_true = 1.952;
+ }
+};
+
+TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, force_registraion)
+{
+ FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+}
+
+// Hover test.
+TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hovering)
+{
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the mass
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->fix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->unfix();
+
+ 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);
+
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->fix();
+ C_KF1->getFrame()->fix();
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("======== SOLVE PROBLEM =======")
+ std::string report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report); // should show a very low iteration number (possibly 1)
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+}
+
+
+TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hovering)
+{
+ S->getStateBlock('C')->setState(Vector3d(0.005,0.005,0.33));
+ S->getStateBlock('m')->setState(Vector1d(mass_true));
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the cdm
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->unfix();
+ 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);
+
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->fix();
+ C_KF1->getFrame()->fix();
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("======== SOLVE PROBLEM =======")
+ std::string report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report); // should show a very low iteration number (possibly 1)
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
+}
+
+
+TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
+{
+ S->getStateBlock('C')->setState(Vector3d(0.1,0.1,0.2));
+ S->getStateBlock('m')->setState(Vector1d(1.9));
+ Vector3d cdm_guess = S->getStateBlock('C')->getState();
+ double mass_guess = S->getStateBlock('m')->getState()(0);
+
+ // Data
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = -gravity();
+ data.segment<3>(6) = -mass_true * gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+ // We fix the parameters of the sensor except for the cdm and the mass
+ S->getStateBlock('P')->fix();
+ S->getStateBlock('O')->fix();
+ S->getStateBlock('I')->fix();
+ S->getStateBlock('C')->unfix();
+ S->getStateBlock('i')->fix();
+ S->getStateBlock('m')->unfix();
+
+ 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);
+
+ C0_0->process();
+ CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+ C1_0->process();
+ C2_0->process();
+ C3_0->process();
+ C4_0->process();
+ C5_1->process();
+
+ CaptureMotionPtr C_KF1 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C_KF0->getFrame()->fix();
+ C_KF1->getFrame()->fix();
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("======== SOLVE PROBLEM =======")
+ std::string report = solver_->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report); // should show a very low iteration number (possibly 1)
+
+ P->print(4, 1, 1, 1);
+
+ WOLF_INFO("True mass : ", mass_true, " Kg.");
+ WOLF_INFO("Guess mass : ", mass_guess, " Kg.");
+ WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState(), " Kg.");
+ WOLF_INFO("True cdm : ", cdm_true.transpose(), " m.");
+ WOLF_INFO("Guess cdm : ", cdm_guess.transpose(), " m.");
+ WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.cdm_only_hovering";
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
new file mode 100644
index 0000000..ed94232
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -0,0 +1,56 @@
+config:
+ problem:
+ frame_structure: "POVL"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ L: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [1,1,1]
+ L: [1,1,1]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ force_noise_std: 0.1 # std dev of force noise in N
+ torque_noise_std: 0.1 # std dev of torque noise in N/m
+ acc_noise_std: 0.01 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+
+ com: [0,0,0.0341] # center of mass [m]
+ inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2]
+ mass: 1.9 # mass [kg]
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialPreintDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ time_tolerance: 0.1
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.00000001
+ state_getter: true
+ state_priority: 1
+ n_propellers: 3
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.995
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
--
GitLab