diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
deleted file mode 100644
index 958398a325c0e6c77fd5b70a2cabb4497fec746b..0000000000000000000000000000000000000000
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ /dev/null
@@ -1,282 +0,0 @@
-//--------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--------
-/**
- * \file factor_force_torque.h
- *
- * Created on: Feb 19, 2020
- * \author: mfourmy
- *
- * Works only for 2 limbs
- */
-
-
-
-#ifndef FACTOR_FORCE_TORQUE_H_
-#define FACTOR_FORCE_TORQUE_H_
-
-#include <core/math/rotations.h>
-#include <core/math/covariance.h>
-#include <core/factor/factor_autodiff.h>
-#include <core/feature/feature_base.h>
-
-#include "bodydynamics/feature/feature_force_torque.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(FactorForceTorque);
-
-class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3,3,3,4>
-{
- public:
- FactorForceTorque(const FeatureBasePtr& _feat,
- const FrameBasePtr& _frame_other,
- const StateBlockPtr& _sb_bp_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorForceTorque() override { /* nothing */ }
-
- /*
- _ck : COM position in world frame, time k
- _cdk : COM velocity in world frame, time k
- _Lck : Angular momentum in world frame, time k
- _ckm : COM position in world frame, time k-1
- _cdkm : COM velocity in world frame, time k-1
- _Lckm : Angular momentum in world frame, time k-1
- _bpkm : COM position measurement bias, time k-1
- _qkm : Base orientation in world frame, time k-1
- */
- template<typename T>
- bool operator () (
- const T* const _ck,
- const T* const _cdk,
- const T* const _Lck,
- const T* const _ckm,
- const T* const _cdkm,
- const T* const _Lckm,
- const T* const _bpkm,
- const T* const _qkm,
- T* _res) const;
-
- void computeJac(const FeatureForceTorqueConstPtr& _feat,
- const double& _mass,
- const double& _dt,
- const Eigen::VectorXd& _bp,
- Eigen::Matrix<double, 9, 15> & _J_ny_nz) const;
-
- // void recomputeJac(const FeatureForceTorquePtr& _feat,
- // const double& _dt,
- // const Eigen::VectorXd& _bp,
- // Eigen::Matrix<double, 9, 15>& _J_ny_nz) const;
-
- void retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov);
-
- StateBlockPtr sb_bp_other_;
- double mass_;
- double dt_;
- Eigen::Matrix<double,15,15> cov_meas_;
- // Eigen::Matrix<double, 9, 15> J_ny_nz_; // cannot be modified in operator() since const function
- // Eigen::Matrix9d errCov_;
-};
-
-} /* namespace wolf */
-
-
-namespace wolf {
-
-FactorForceTorque::FactorForceTorque(
- const FeatureBasePtr& _feat,
- const FrameBasePtr& _frame_other,
- const StateBlockPtr& _sb_bp_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("FactorForceTorque",
- TOP_GEOM,
- _feat,
- _frame_other, // _frame_other_ptr
- nullptr, // _capture_other_ptr
- nullptr, // _feature_other_ptr
- nullptr, // _landmark_other_ptr
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feat->getFrame()->getStateBlock('C'), // COM position, current
- _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name), current
- _feat->getFrame()->getStateBlock('L'), // Angular momentum, current
- _frame_other->getStateBlock('C'), // COM position, previous
- _frame_other->getStateBlock('D'), // COM velocity (bad name), previous
- _frame_other->getStateBlock('L'), // Angular momentum, previous
- _sb_bp_other, // BC relative position bias, previous
- _frame_other->getStateBlock('O') // Base orientation, previous
- ),
- sb_bp_other_(_sb_bp_other)
-{
- FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
- mass_ = feat->getMass();
- dt_ = feat->getDt();
- retrieveMeasurementCovariance(feat, cov_meas_);
-}
-
-
-void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov)
-{
- cov.setZero();
- cov.block<6,6>(0,0) = feat->getCovForcesMeas(); // reset some extra zeros
- cov.block<6,6>(6,6) = feat->getCovTorquesMeas(); // reset some extra zeros
- cov.block<3,3>(12,12) = feat->getCovPbcMeas();
-}
-
-
-void FactorForceTorque::computeJac(const FeatureForceTorqueConstPtr& _feat,
- const double& _mass,
- const double& _dt,
- const Eigen::VectorXd& _bp,
- Eigen::Matrix<double, 9, 15>& _J_ny_nz) const
-{
- _J_ny_nz.setZero();
-
- // Measurements retrieval
- Eigen::Map<const Eigen::Vector3d> f1 (_feat->getForcesMeas().data());
- Eigen::Map<const Eigen::Vector3d> f2 (_feat->getForcesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> tau1(_feat->getTorquesMeas().data());
- Eigen::Map<const Eigen::Vector3d> tau2(_feat->getTorquesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> pbl1(_feat->getKinMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl2(_feat->getKinMeas().data() + 3 );
- Eigen::Map<const Eigen::Quaterniond> bql1(_feat->getKinMeas().data() + 6);
- Eigen::Map<const Eigen::Quaterniond> bql2(_feat->getKinMeas().data() + 10);
- Eigen::Map<const Eigen::Vector3d> pbc (_feat->getPbcMeas().data());
-
- Eigen::Matrix3d bRl1 = q2R(bql1);
- Eigen::Matrix3d bRl2 = q2R(bql2);
- _J_ny_nz.block<3,3>(0,0) = 0.5*bRl1*pow(_dt,2)/_mass;
- _J_ny_nz.block<3,3>(0,3) = 0.5*bRl2*pow(_dt,2)/_mass;
- _J_ny_nz.block<3,3>(3,0) = bRl1*_dt/_mass;
- _J_ny_nz.block<3,3>(3,3) = bRl2*_dt/_mass;
- _J_ny_nz.block<3,3>(6,0) = skew(pbl1 - pbc + _bp)*bRl1*_dt;
- _J_ny_nz.block<3,3>(6,3) = skew(pbl2 - pbc + _bp)*bRl2*_dt;
- _J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
- _J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
- _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
-}
-
-// void FactorForceTorque::recomputeJac(const FeatureForceTorquePtr& _feat,
-// const double& _dt,
-// const Eigen::VectorXd& _bp,
-// Eigen::Matrix<double, 9, 15>& _J_ny_nz)
-// {
-// FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
-
-// // Measurements retrieval
-// // Eigen::Map<const Eigen::Vector3d> f1 (feat->getForcesMeas().data());
-// // Eigen::Map<const Eigen::Vector3d> f2 (feat->getForcesMeas().data() + 3 );
-// // Eigen::Map<const Eigen::Vector3d> tau1(feat->getTorquesMeas().data());
-// // Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3 );
-// Eigen::Map<const Eigen::Vector3d> pbl1(feat->getKinMeas().data());
-// Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3 );
-// Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
-// Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
-// Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
-
-// Eigen::Matrix3d bRl1 = q2R(bql1);
-// Eigen::Matrix3d bRl2 = q2R(bql2);
-// // _J_ny_nz.block<3,3>(0,0) = 0.5*bRl1*pow(_dt,2)/_mass;
-// // _J_ny_nz.block<3,3>(0,3) = 0.5*bRl2*pow(_dt,2)/_mass;
-// // _J_ny_nz.block<3,3>(3,0) = bRl1*_dt/_mass;
-// // _J_ny_nz.block<3,3>(3,3) = bRl2*_dt/_mass;
-// _J_ny_nz.block<3,3>(6,0) = skew(pbl1 - pbc + _bp)*bRl1*_dt;
-// _J_ny_nz.block<3,3>(6,3) = skew(pbl2 - pbc + _bp)*bRl2*_dt;
-// // _J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
-// // _J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
-// // _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
-// }
-
-
-template<typename T>
-bool FactorForceTorque::operator () (
- const T* const _ck,
- const T* const _cdk,
- const T* const _Lck,
- const T* const _ckm,
- const T* const _cdkm,
- const T* const _Lckm,
- const T* const _bpkm,
- const T* const _qkm,
- T* _res) const
-{
- auto feat = std::static_pointer_cast<const FeatureForceTorque>(getFeature());
-
- // State variables instanciation
- Eigen::Map<const Eigen::Matrix<T,3,1> > ck(_ck);
- Eigen::Map<const Eigen::Matrix<T,3,1> > cdk(_cdk);
- Eigen::Map<const Eigen::Matrix<T,3,1> > Lck(_Lck);
- Eigen::Map<const Eigen::Matrix<T,3,1> > ckm(_ckm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > cdkm(_cdkm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > Lckm(_Lckm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > bpkm(_bpkm);
- Eigen::Map<const Eigen::Quaternion<T> > qkm(_qkm);
- Eigen::Map<Eigen::Matrix<T,9,1> > res(_res);
-
- // Retrieve all measurements
- Eigen::Map<const Eigen::Vector3d> f1 (feat->getForcesMeas().data());
- Eigen::Map<const Eigen::Vector3d> f2 (feat->getForcesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> tau1(feat->getTorquesMeas().data());
- Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl1(feat->getKinMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3 );
- Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
- Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
-
- // Recompute the error variable covariance according to the new bias bp estimation
-
- Eigen::Matrix<double, 9, 15> J_ny_nz;
- computeJac(feat, mass_, dt_, sb_bp_other_->getState(), J_ny_nz); // bp
- Eigen::Matrix9d errCov = J_ny_nz * cov_meas_ * J_ny_nz.transpose();
- errCov.block<6,6>(0,0) = errCov.block<6,6>(0,0) + 1e-12 * Eigen::Matrix6d::Identity();
-
- // Error variable instanciation
- Eigen::Matrix<T, 9, 1> err;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_c (err.data());
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_cd(err.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_Lc(err.data() + 6);
-
- err_c = qkm.conjugate()*(ck - ckm - cdkm * dt_ - 0.5*wolf::gravity()*pow(dt_, 2))
- - (0.5/mass_) * (bql1 * f1 * pow(dt_,2) + bql2 * f2 * pow(dt_,2));
-
- err_cd = qkm.conjugate()*(cdk - cdkm - wolf::gravity()*dt_)
- - (1/mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_);
-
- err_Lc = qkm.conjugate()*(Lck - Lckm)
- - ( bql1 * tau1 + (pbl1 - pbc + bpkm).cross(bql1 * f1)
- + bql2 * tau2 + (pbl2 - pbc + bpkm).cross(bql2 * f2));
-
- res = wolf::computeSqrtUpper(errCov.inverse()) * err;
-
- return true;
-}
-
-} // namespace wolf
-
-#endif /* FACTOR_FORCE_TORQUE_H_ */
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 425d6f1291d02b80f53ced0bcbe614bc88c402c1..93fddc94887955cdc786b13b1e1b8dccc5879e81 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -16,8 +16,6 @@ wolf_add_gtest(gtest_feature_inertial_kinematics gtest_feature_inertial_kinemati
wolf_add_gtest(gtest_factor_inertial_kinematics gtest_factor_inertial_kinematics.cpp)
-wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
-
wolf_add_gtest(gtest_factor_force_torque_inertial gtest_factor_force_torque_inertial.cpp)
wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics gtest_factor_force_torque_inertial_dynamics.cpp)
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
deleted file mode 100644
index 0ccf5135e3d1bbe45a2c07cdbd68cf530dc9c571..0000000000000000000000000000000000000000
--- a/test/gtest_factor_force_torque.cpp
+++ /dev/null
@@ -1,865 +0,0 @@
-//--------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--------
-
-/**
- * \file gtest_factor_inertial_kinematics.cpp
- *
- * Created on: Janv 29, 2020
- * \author: Mederic Fourmy
- */
-
-/*
-
-
-Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
-Then, each test case is derived in a child class which defines the data processed by the different sensors and expected estimated test values.
-Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
-solve is done with a perturbated system.
-
-Tests list:
-
-FactorInertialKinematics_2KF_Meas0_bpfix,ZeroMvt:
-FactorInertialKinematics_2KF_1p_bpfix,ZeroMvt:
-FactorInertialKinematics_2KF_m1p_pfix,ZeroMvt:
-FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
-*/
-
-
-// debug
-#include <iostream>
-#include <fstream>
-
-#include <core/utils/utils_gtest.h>
-#include <core/utils/logging.h>
-
-// WOLF
-#include <core/problem/problem.h>
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/math/rotations.h>
-#include <core/capture/capture_base.h>
-#include <core/feature/feature_base.h>
-#include <core/factor/factor_block_absolute.h>
-#include <core/state_block/state_block_derived.h>
-
-// Imu
-// #include "imu/internal/config.h"
-// #include "imu/capture/capture_imu.h"
-// #include "imu/processor/processor_imu.h"
-// #include "imu/sensor/sensor_imu.h"
-
-// BODYDYNAMICS
-#include "bodydynamics/internal/config.h"
-#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/feature/feature_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/feature/feature_force_torque.h"
-#include "bodydynamics/factor/factor_force_torque.h"
-
-// ODOM3d
-#include "core/capture/capture_pose.h"
-#include "core/feature/feature_pose.h"
-#include "core/factor/factor_relative_pose_3d.h"
-
-#include <Eigen/Eigenvalues>
-
-using namespace wolf;
-using namespace Eigen;
-using namespace std;
-
-
-// SOME CONSTANTS
-const double Acc1 = 1;
-const double Acc2 = 3;
-const Vector3d zero3 = Vector3d::Zero();
-const Vector6d zero6 = Vector6d::Zero();
-
-
-
-Matrix9d computeKinJac(const Vector3d& w_unb,
- const Vector3d& p_unb,
- const Matrix3d& Iq)
-{
- Matrix9d J; J.setZero();
-
- Matrix3d wx = skew(w_unb);
- Matrix3d px = skew(p_unb);
- // Starting from top left, to the right and then next row
- J.topLeftCorner<3,3>() = Matrix3d::Identity();
- // J.block<3,3>(0,3) = Matrix3d::Zero();
- // J.topRightCorner<3,3>() = Matrix3d::Zero();
- J.block<3,3>(3,0) = -wx;
- J.block<3,3>(3,3) = -Matrix3d::Identity();
- J.block<3,3>(3,6) = px;
- // J.bottomLeftCorner<3,3>() = Matrix3d::Zero();
- // J.block<3,3>(6,3) = Matrix3d::Zero();
- J.bottomRightCorner<3,3>() = -Iq;
-
- return J;
-}
-
-Matrix9d computeKinCov(const Matrix3d& Qp,
- const Matrix3d& Qv,
- const Matrix3d& Qw,
- const Vector3d& w_unb,
- const Vector3d& p_unb,
- const Matrix3d& Iq)
-{
- Matrix9d cov; cov.setZero();
-
- Matrix3d wx = skew(w_unb);
- Matrix3d px = skew(p_unb);
- // Starting from top left, to the right and then next row
- cov.topLeftCorner<3,3>() = Qp;
- cov.block<3,3>(0,3) = Qp * wx;
- // cov.topRightCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(3,0) = cov.block<3,3>(0,3).transpose();
- cov.block<3,3>(3,3) = -wx*Qp*wx + Qv - px*Qw*px;
- cov.block<3,3>(3,6) = -px*Qw*Iq;
- // cov.bottomLeftCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(6,3) = Iq*Qw*px;
- cov.bottomRightCorner<3,3>() = Iq*Qw*Iq;
-
- return cov;
-}
-
-
-void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
- if(!KF->getStateBlock(sb_name)->isFixed())
- {
- if (sb_name == 'O')
- {
- double max_rad_pert = M_PI / 3;
- Vector3d do_pert = max_rad_pert*Vector3d::Random();
- Quaterniond curr_state_q(KF->getO()->getState().data());
- KF->getStateBlock('O')->setState((curr_state_q * exp_q(do_pert)).coeffs());
- }
- else
- {
- VectorXd pert;
- pert.resize(KF->getStateBlock(sb_name)->getSize());
- pert.setRandom(); pert *= 0.5;
- KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert);
- }
- }
-}
-
-void perturbateAllIfUnFixed(const FrameBasePtr& KF)
-{
- for (auto it: KF->getStateBlockMap()){
- perturbateIfUnFixed(KF, it.first);
- }
-}
-
-FrameBasePtr createKFWithCDLI(const ProblemPtr& problem,
- const TimeStamp& t,
- VectorComposite x_origin,
- Vector3d c,
- Vector3d cd,
- Vector3d Lc,
- Vector6d bias_imu)
-{
- FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
- StateBlockPtr sbc = make_shared<StatePoint3d>(c);
- KF->addStateBlock('C', sbc, problem);
- StateBlockPtr sbd = make_shared<StateVector3d>(cd);
- KF->addStateBlock('D', sbd, problem);
- StateBlockPtr sbL = make_shared<StateVector3d>(Lc);
- KF->addStateBlock('L', sbL, problem);
- StateBlockPtr sbbimu = make_shared<StateParams6>(bias_imu);
- KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
- return KF;
-}
-
-class FactorInertialKinematics_2KF : public testing::Test
-{
- public:
- double mass_;
- wolf::TimeStamp tA_;
- wolf::TimeStamp tB_;
- ProblemPtr problem_;
- SolverCeresPtr solver_;
- VectorComposite x_origin_; // initial state
- VectorComposite s_origin_; // initial sigmas for prior
- SensorInertialKinematicsPtr SIK_;
- CaptureInertialKinematicsPtr CIKA_, CIKB_;
- FrameBasePtr KFA_;
- FrameBasePtr KFB_;
- Matrix3d Qp_, Qv_, Qw_;
- Vector3d bias_p_;
- Vector6d bias_imu_;
- FeatureInertialKinematicsPtr FIKA_;
- FeatureInertialKinematicsPtr FIKB_;
- FeatureForceTorquePtr FFTAB_;
- // SensorImuPtr sen_imu_;
- // ProcessorImuPtr processor_imu_;
-
- protected:
- void SetUp() override
- {
-
- std::string bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
-
- mass_ = 10.0; // Small 10 kg robot
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- problem_ = Problem::create("POV", 3);
-
- VectorXd extr_ik(0);
- ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = 0.1;
- intr_ik->std_vbc = 0.1;
- auto senik = problem_->installSensor("SensorInertialKinematics", "senIK", extr_ik, intr_ik);
- SIK_ = std::static_pointer_cast<SensorInertialKinematics>(senik);
-
- // CERES WRAPPER
- solver_ = std::make_shared<SolverCeres>(problem_);
- solver_->getSolverOptions().max_num_iterations = 1e3;
- // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
-
- // INSTALL Imu SENSOR
- // Vector7d extrinsics; extrinsics << 0,0,0,0,0,0,1;
- // SensorBasePtr sen_imu = problem_->installSensor("SensorImu", "Main Imu Sensor", extrinsics, bodydynamics_root + "/demos/sensor_imu.yaml");
- // sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu);
- // ProcessorBasePtr prc_imu = problem_->installProcessor("ProcessorImu", "Imu PROC", "Main Imu Sensor", bodydynamics_root + "/demos/processor_imu.yaml");
- // Vector6d data = zero6;
- // wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(0, sen_imu_, data, sen_imu_->getNoiseCov(), zero6);
- // // sen_imu_->process(imu_ptr);
-
- // ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
- tA_.set(0);
- x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
- s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
- KFA_ = problem_->setPriorFactor(x_origin_, s_origin_, tA_);
-
-
- // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
- bias_p_ = zero3;
- bias_imu_ = zero6;
- StateBlockPtr sbcA = make_shared<StatePoint3d>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
- StateBlockPtr sbdA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
- StateBlockPtr sbLA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
- StateBlockPtr sbbimuA = make_shared<StateParams6>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
-
- // Fix the one we cannot estimate
- // KFA_->getP()->fix();
- // KFA_->getO()->fix();
- // KFA_->getV()->fix();
- KFA_->getStateBlock('I')->fix(); // Imu
-
- // INERTIAL KINEMATICS FACTOR
- // Measurements
- Vector3d pBC_measA = zero3;
- Vector3d vBC_measA = zero3;
- Vector3d w_measA = zero3;
- Vector9d meas_ikinA; meas_ikinA << pBC_measA, vBC_measA, w_measA;
- // momentum parameters
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
-
- Qp_ = pow(1e-2, 2)*Matrix3d::Identity();
- Qv_ = pow(1e-2, 2)*Matrix3d::Identity();
- Qw_ = pow(1e-2, 2)*Matrix3d::Identity();
- Eigen::Matrix9d Q_ikin_err = computeKinCov(Qp_, Qv_, Qw_, meas_ikinA.head<3>()-bias_p_, meas_ikinA.tail<3>()-bias_imu_.tail<3>(), Iq);
-
- CIKA_ = CaptureBase::emplace<CaptureInertialKinematics>(KFA_, tA_, SIK_, meas_ikinA, Iq, Lq, bias_p_);
- CIKA_->getStateBlock('I')->fix(); // IK bias
- FIKA_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKA_, meas_ikinA, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- FactorBase::emplace<FactorInertialKinematics>(FIKA_, FIKA_, sbbimuA, nullptr, false);
-
-
- // =============== NEW KFB WITH CORRESPONDING STATEBLOCKS
- tB_.set(1);
-
- KFB_ = createKFWithCDLI(problem_, tB_, x_origin_,
- zero3, zero3, zero3, bias_imu_);
- // Fix the one we cannot estimate
- // KFB_->getP()->fix();
- KFB_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
- // KFB_->getV()->fix();
- KFB_->getStateBlock('I')->fix(); // Imu
-
-
- // // ================ PROCESS Imu DATA
- // Vector6d data_imu;
- // data_imu << -wolf::gravity(), 0,0,0;
- // CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(tB_, sen_imu_, data_imu, sen_imu_->getNoiseCov(), bias_imu_); //set data on Imu (measure only gravity here)
- // // process data in capture
- // // sen_imu_->process(cap_imu);
-
- // ================ FACTOR INERTIAL KINEMATICS ON KFB
- Vector3d pBC_measB = zero3;
- Vector3d vBC_measB = zero3;
- Vector3d w_measB = zero3;
- Vector9d meas_ikinB; meas_ikinB << pBC_measB, vBC_measB, w_measB;
- CIKB_ = CaptureBase::emplace<CaptureInertialKinematics>(KFB_, tB_, SIK_, meas_ikinB, Iq, Lq, bias_p_);
- CIKB_->getSensorIntrinsic()->fix(); // IK bias
- FIKB_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKB_, meas_ikinB, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock('I'), nullptr, false);
-
-
- // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
- Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau1; tau1 << 0,0,0;
- Vector3d pbl1; pbl1 << 0,0,0;
- Vector4d bql1; bql1 << 0,0,0,1;
- Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau2; tau2 << 0,0,0;
- Vector3d pbl2; pbl2 << 0,0,0;
- Vector4d bql2; bql2 << 0,0,0,1;
- Vector3d pbc; pbc << 0,0,0;
- // aggregated meas
- Vector6d f_meas; f_meas << f1, f2;
- Vector6d tau_meas; tau_meas << tau1, tau2;
- Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
-
- Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
- Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
- Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
-
- CaptureBasePtr cap_ftB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureForceTorque", tB_, nullptr);
- FFTAB_ = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
- tB_ - tA_, mass_,
- f_meas, tau_meas, pbc, kin_meas,
- cov_f, cov_tau, cov_pbc);
-
- FactorForceTorquePtr fac_ftAB = FactorBase::emplace<FactorForceTorque>(FFTAB_, FFTAB_, KFA_, CIKA_->getSensorIntrinsic(), nullptr, false);
-
- }
-};
-
-
-class FactorInertialKinematics_2KF_foot1turned : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- cout << "\n\n\nFactorInertialKinematics_2KF_foot1turned" << endl;
- FactorInertialKinematics_2KF::SetUp();
- problem_->print(3,false,true,true);
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
- Vector3d f1 = -mass_*wolf::gravity()/2;
- f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
- Vector3d f2 = -mass_*wolf::gravity()/2;
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setKinMeas(kin_meas);
- FFTAB_->setForcesMeas(fmeas);
- // problem_->print(3,false,true,true);
- }
-};
-
-class FactorInertialKinematics_2KF_foot2turned : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
- Vector3d f1 = -mass_*wolf::gravity()/2;
- Vector3d f2 = -mass_*wolf::gravity()/2;
- f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setKinMeas(kin_meas);
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_2KF_singleContactFoot2turned : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
- Vector3d f1 = -mass_*wolf::gravity();
- f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
- Vector3d f2 = zero3;
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setKinMeas(kin_meas);
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_2KF_singleContactFoot1turned : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
- Vector3d f1 = zero3;
- Vector3d f2 = -mass_*wolf::gravity();
- f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setKinMeas(kin_meas);
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-
-
-
-class FactorInertialKinematics_2KF_ForceX : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_2KF_ForceY : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_2KF_ForceZ : public FactorInertialKinematics_2KF
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
- Vector6d fmeas; fmeas << f1, f2;
- FFTAB_->setForcesMeas(fmeas);
- }
-};
-
-class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_ForceX
-{
- public:
- FrameBasePtr KFC_;
- CaptureInertialKinematicsPtr CIKC_;
- TimeStamp tC_;
-
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF_ForceX::SetUp();
- tC_.set(2);
-
- // KFB_->getStateBlock("O")->unfix();
-
- KFC_ = createKFWithCDLI(problem_, tC_, x_origin_,
- zero3, zero3, zero3, bias_imu_);
- // Fix the one we cannot estimate
- // KFB_->getP()->fix();
- // KFC_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
- // KFB_->getV()->fix();
- KFC_->getStateBlock('I')->fix();
-
- // ================ FACTOR INERTIAL KINEMATICS ON KFB
- Vector3d pBC_measC = zero3;
- Vector3d vBC_measC = zero3;
- Vector3d w_measC = zero3;
- Vector9d meas_ikinC; meas_ikinC << pBC_measC, vBC_measC, w_measC;
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
- Eigen::Matrix9d Q_ikin_errC = computeKinCov(Qp_, Qv_, Qw_, meas_ikinC.head<3>()-bias_p_, meas_ikinC.tail<3>()-bias_imu_.tail<3>(), Iq);
-
- CIKC_ = CaptureBase::emplace<CaptureInertialKinematics>(KFC_, tC_, SIK_, meas_ikinC, Iq, Lq, bias_p_);
- CIKC_->getStateBlock('I')->fix(); // IK bias
- auto feat_ikin_C = FeatureBase::emplace<FeatureInertialKinematics>(CIKC_, meas_ikinC, Q_ikin_errC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock('I'), nullptr, false);
-
-
- // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
- Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau1; tau1 << 0,0,0;
- Vector3d pbl1; pbl1 << 0,0,0;
- Vector4d bql1; bql1 << 0,0,0,1;
- Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau2; tau2 << 0,0,0;
- Vector3d pbl2; pbl2 << 0,0,0;
- Vector4d bql2; bql2 << 0,0,0,1;
- Vector3d pbc; pbc << 0,0,0;
- // aggregated meas
- Vector6d f_meas; f_meas << f1, f2;
- Vector6d tau_meas; tau_meas << tau1, tau2;
- Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
-
- Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
- Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
- Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
-
- CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC_, nullptr);
- auto feat_ftBC = FeatureBase::emplace<FeatureForceTorque>(cap_ftC,
- tC_ - tB_, mass_,
- f_meas, tau_meas, pbc, kin_meas,
- cov_f, cov_tau, cov_pbc);
- FactorForceTorquePtr fac_ftBC = FactorBase::emplace<FactorForceTorque>(feat_ftBC, feat_ftBC, KFB_, CIKB_->getSensorIntrinsic(), nullptr, false);
- }
-};
-
-class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinematics_2KF_ForceX
-{
- protected:
- void SetUp() override
- {
- FactorInertialKinematics_2KF_ForceX::SetUp();
-
- Vector7d pose_A_B; pose_A_B << (tB_-tA_)*Acc1/2,0,0, 0,0,0,1;
- Matrix6d rel_pose_cov = Matrix6d::Identity();
- rel_pose_cov.topLeftCorner(3, 3) *= pow(1e-3, 2);
- rel_pose_cov.bottomRightCorner(3, 3) *= pow(M_TORAD*1e-3, 2);
-
- CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KFB_, tB_, nullptr, pose_A_B, rel_pose_cov);
- FeatureBasePtr ftr_pose_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, pose_A_B, rel_pose_cov);
- FactorBase::emplace<FactorRelativePose3d>(ftr_pose_base, ftr_pose_base, KFA_, nullptr, false, TOP_MOTION);
-
- // With Odom3d the bias on relative base COM position is observable, we unfix the KFB state block
- CIKB_->getStateBlock('I')->unfix();
- // However this test is not sufficient to observe the bias at KFA
- // CIKA_->getStateBlock('I')->unfix(); // this is not observable
- }
-};
-
-
-
-
-
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-// =============== TEST FONCTIONS ======================
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-
-
-TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5); // due to opposite
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
-{
- Vector3d c_exp = zero3;
- Vector3d cd_exp = zero3;
- Vector3d Lc_exp = zero3;
- Vector3d bp_exp = zero3;
-
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
-{
- // problem_->print(4,true,true,true);
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
-{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
-{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-// TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
-// {
-// string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-// perturbateAllIfUnFixed(KFA_);
-// perturbateAllIfUnFixed(KFB_);
-// perturbateAllIfUnFixed(KFC_);
-
-// // problem_->print(4,true,true,true);
-// report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-// // std::cout << report << std::endl;
-// // problem_->print(4,true,true,true);
-
-// ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-// // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('C')->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('D')->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('L')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(CIKC_->getStateBlock('I')->getState(), zero3, 1e-5);
-// }
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d,ZeroMvt)
-{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- perturbateAllIfUnFixed(KFA_);
- perturbateAllIfUnFixed(KFB_);
-
- // problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
- // std::cout << report << std::endl;
- // problem_->print(4,true,true,true);
-
- ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
- // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
-
- return RUN_ALL_TESTS();
-}