diff --git a/CMakeLists.txt b/CMakeLists.txt index eba12ca4379a314de77f457308cb5e2ff20876d8..84505a3442495401472555f5de562c45f8743e6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,30 +103,29 @@ include/${PROJECT_NAME}/math/force_torque_delta_tools.h include/${PROJECT_NAME}/math/force_torque_inertial_delta_tools.h ) SET(HDRS_CAPTURE -include/${PROJECT_NAME}/capture/capture_force_torque_preint.h +include/${PROJECT_NAME}/capture/capture_force_torque.h +include/${PROJECT_NAME}/capture/capture_force_torque_inertial.h include/${PROJECT_NAME}/capture/capture_inertial_kinematics.h include/${PROJECT_NAME}/capture/capture_leg_odom.h include/${PROJECT_NAME}/capture/capture_point_feet_nomove.h ) SET(HDRS_FACTOR include/${PROJECT_NAME}/factor/factor_angular_momentum.h -include/${PROJECT_NAME}/factor/factor_force_torque.h include/${PROJECT_NAME}/factor/factor_force_torque_inertial.h include/${PROJECT_NAME}/factor/factor_force_torque_inertial_dynamics.h -include/${PROJECT_NAME}/factor/factor_force_torque_preint.h +include/${PROJECT_NAME}/factor/factor_force_torque.h include/${PROJECT_NAME}/factor/factor_inertial_kinematics.h include/${PROJECT_NAME}/factor/factor_point_feet_nomove.h include/${PROJECT_NAME}/factor/factor_point_feet_altitude.h ) SET(HDRS_FEATURE include/${PROJECT_NAME}/feature/feature_force_torque.h -include/${PROJECT_NAME}/feature/feature_force_torque_preint.h include/${PROJECT_NAME}/feature/feature_inertial_kinematics.h ) SET(HDRS_PROCESSOR -include/${PROJECT_NAME}/processor/processor_force_torque_inertial_preint.h -include/${PROJECT_NAME}/processor/processor_force_torque_inertial_preint_dynamics.h -include/${PROJECT_NAME}/processor/processor_force_torque_preint.h +include/${PROJECT_NAME}/processor/processor_force_torque_inertial.h +include/${PROJECT_NAME}/processor/processor_force_torque_inertial_dynamics.h +include/${PROJECT_NAME}/processor/processor_force_torque.h include/${PROJECT_NAME}/processor/processor_inertial_kinematics.h include/${PROJECT_NAME}/processor/processor_point_feet_nomove.h ) @@ -139,20 +138,20 @@ include/${PROJECT_NAME}/sensor/sensor_point_feet_nomove.h # ============ SOURCES ============ SET(SRCS_CAPTURE -src/capture/capture_force_torque_preint.cpp +src/capture/capture_force_torque.cpp +src/capture/capture_force_torque_inertial.cpp src/capture/capture_inertial_kinematics.cpp src/capture/capture_leg_odom.cpp src/capture/capture_point_feet_nomove.cpp ) SET(SRCS_FEATURE src/feature/feature_force_torque.cpp -src/feature/feature_force_torque_preint.cpp src/feature/feature_inertial_kinematics.cpp ) SET(SRCS_PROCESSOR -src/processor/processor_force_torque_inertial_preint.cpp -src/processor/processor_force_torque_inertial_preint_dynamics.cpp -src/processor/processor_force_torque_preint.cpp +src/processor/processor_force_torque_inertial.cpp +src/processor/processor_force_torque_inertial_dynamics.cpp +src/processor/processor_force_torque.cpp src/processor/processor_inertial_kinematics.cpp src/processor/processor_point_feet_nomove.cpp ) diff --git a/demos/processor_ft_preint.yaml b/demos/processor_ft_preint.yaml index d9b49d70b860fe2e3029a4cc9bd62ffcc2fdc6c8..e9d8c3b37000e40a58ac253eaec04a96f0ae4232 100644 --- a/demos/processor_ft_preint.yaml +++ b/demos/processor_ft_preint.yaml @@ -1,5 +1,5 @@ -type: "ProcessorForceTorquePreint" # This must match the KEY used in the SensorFactory. Otherwise it is an error. -name: "PFTPreint" # This is ignored. The name provided to the SensorFactory prevails +type: "ProcessorForceTorque" # This must match the KEY used in the SensorFactory. Otherwise it is an error. +name: "PFT" # This is ignored. The name provided to the SensorFactory prevails time_tolerance: 0.0005 # Time tolerance for joining KFs unmeasured_perturbation_std: 0.0000000 sensor_ikin_name: "SenIK" diff --git a/demos/solo_imu_kine.cpp b/demos/solo_imu_kine.cpp index a9f52035066348852f9a296504e414520669656e..02a07f6c138bab362d7a2357aa23dc63d2b6826e 100644 --- a/demos/solo_imu_kine.cpp +++ b/demos/solo_imu_kine.cpp @@ -68,12 +68,12 @@ #include "bodydynamics/sensor/sensor_inertial_kinematics.h" #include "bodydynamics/sensor/sensor_force_torque.h" #include "bodydynamics/capture/capture_inertial_kinematics.h" -#include "bodydynamics/capture/capture_force_torque_preint.h" +#include "bodydynamics/capture/capture_force_torque_.h" #include "bodydynamics/capture/capture_leg_odom.h" #include "bodydynamics/processor/processor_inertial_kinematics.h" -#include "bodydynamics/processor/processor_force_torque_preint.h" +#include "bodydynamics/processor/processor_force_torque_.h" #include "bodydynamics/factor/factor_inertial_kinematics.h" -#include "bodydynamics/factor/factor_force_torque_preint.h" +#include "bodydynamics/factor/factor_force_torque_.h" #include "bodydynamics/sensor/sensor_point_feet_nomove.h" #include "bodydynamics/processor/processor_point_feet_nomove.h" diff --git a/demos/solo_imu_kine_mocap.cpp b/demos/solo_imu_kine_mocap.cpp index 82d2df1c42da66a66e6894fe49e037b4a04259cd..8ae580418ec8691b984c8a5c3a11e1c3ea81aab5 100644 --- a/demos/solo_imu_kine_mocap.cpp +++ b/demos/solo_imu_kine_mocap.cpp @@ -68,12 +68,12 @@ #include "bodydynamics/sensor/sensor_inertial_kinematics.h" #include "bodydynamics/sensor/sensor_force_torque.h" #include "bodydynamics/capture/capture_inertial_kinematics.h" -#include "bodydynamics/capture/capture_force_torque_preint.h" +#include "bodydynamics/capture/capture_force_torque_.h" #include "bodydynamics/capture/capture_leg_odom.h" #include "bodydynamics/processor/processor_inertial_kinematics.h" -#include "bodydynamics/processor/processor_force_torque_preint.h" +#include "bodydynamics/processor/processor_force_torque_.h" #include "bodydynamics/factor/factor_inertial_kinematics.h" -#include "bodydynamics/factor/factor_force_torque_preint.h" +#include "bodydynamics/factor/factor_force_torque_.h" #include "bodydynamics/capture/capture_point_feet_nomove.h" #include "bodydynamics/sensor/sensor_point_feet_nomove.h" diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp index 9953db41444d0d1b1da732f5460c3bd398cd3a63..181fc3d44d83f1d89755f34abb5a24f4a806e4f3 100644 --- a/demos/solo_real_povcdl_estimation.cpp +++ b/demos/solo_real_povcdl_estimation.cpp @@ -58,12 +58,12 @@ #include "bodydynamics/sensor/sensor_inertial_kinematics.h" #include "bodydynamics/sensor/sensor_force_torque.h" #include "bodydynamics/capture/capture_inertial_kinematics.h" -#include "bodydynamics/capture/capture_force_torque_preint.h" +#include "bodydynamics/capture/capture_force_torque_.h" #include "bodydynamics/capture/capture_leg_odom.h" #include "bodydynamics/processor/processor_inertial_kinematics.h" -#include "bodydynamics/processor/processor_force_torque_preint.h" +#include "bodydynamics/processor/processor_force_torque_.h" #include "bodydynamics/factor/factor_inertial_kinematics.h" -#include "bodydynamics/factor/factor_force_torque_preint.h" +#include "bodydynamics/factor/factor_force_torque_.h" #include "bodydynamics/capture/capture_point_feet_nomove.h" #include "bodydynamics/sensor/sensor_point_feet_nomove.h" @@ -284,7 +284,7 @@ int main (int argc, char **argv) { SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft); // SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml"); SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base); - ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>(); + ParamsProcessorForceTorquePtr params_sen_ft = std::make_shared<ParamsProcessorForceTorque>(); params_sen_ft->sensor_ikin_name = "SenIK"; params_sen_ft->sensor_angvel_name = "SenImu"; params_sen_ft->nbc = nbc; @@ -296,9 +296,9 @@ int main (int argc, char **argv) { params_sen_ft->dist_traveled = 20000.0; params_sen_ft->angle_turned = 1000; params_sen_ft->voting_active = false; - ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft); - // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml"); - ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base); + ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorque", "PFT", sen_ft, params_sen_ft); + // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorque", "PFT", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml"); + ProcessorForceTorquePtr proc_ft = std::static_pointer_cast<ProcessorForceTorque>(proc_ft_base); // SENSOR + PROCESSOR POINT FEET NOMOVE ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>(); @@ -355,7 +355,7 @@ int main (int argc, char **argv) { VectorXd meas_ikin(9); meas_ikin << i_p_ic, i_v_ic, i_omg_oi; auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti); CIKin0->process(); - proc_ftpreint->setOrigin(KF1); + proc_ft->setOrigin(KF1); //////////////////////////////////////////// // INITIAL BIAS FACTORS @@ -557,7 +557,7 @@ int main (int argc, char **argv) { CImu->process(); auto CIKin = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti); CIKin->process(); - auto CFTpreint = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp); + auto CFTpreint = std::make_shared<CaptureForceTorque>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp); CFTpreint->process(); diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque.h similarity index 94% rename from include/bodydynamics/capture/capture_force_torque_preint.h rename to include/bodydynamics/capture/capture_force_torque.h index 81db7bf05f90fc9048a907253b477b73f8c748a5..fab1ca4951a7ec09fef965b2176a5d8a848aee66 100644 --- a/include/bodydynamics/capture/capture_force_torque_preint.h +++ b/include/bodydynamics/capture/capture_force_torque.h @@ -32,9 +32,9 @@ namespace wolf { -WOLF_PTR_TYPEDEFS(CaptureForceTorquePreint); +WOLF_PTR_TYPEDEFS(CaptureForceTorque); -class CaptureForceTorquePreint : public CaptureMotion +class CaptureForceTorque : public CaptureMotion { public: /* @@ -51,7 +51,7 @@ class CaptureForceTorquePreint : public CaptureMotion qbl1, 4 : orientation of foot 1 in base frame qbl2, 4 : orientation of foot 2 in base frame */ - CaptureForceTorquePreint( + CaptureForceTorque( const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias @@ -60,7 +60,7 @@ class CaptureForceTorquePreint : public CaptureMotion const Eigen::MatrixXd& _data_cov, CaptureBasePtr _capture_origin_ptr = nullptr); - ~CaptureForceTorquePreint() override; + ~CaptureForceTorque() override; CaptureBaseConstPtr getIkinCaptureOther() const { return cap_ikin_other_;} CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_;} diff --git a/src/feature/feature_force_torque_preint.cpp b/include/bodydynamics/capture/capture_force_torque_inertial.h similarity index 56% rename from src/feature/feature_force_torque_preint.cpp rename to include/bodydynamics/capture/capture_force_torque_inertial.h index 1a21081d827843645a62c75508b3ceda10794c71..b6ba6d7752cd7c4427df706e84ffedd07ab9873e 100644 --- a/src/feature/feature_force_torque_preint.cpp +++ b/include/bodydynamics/capture/capture_force_torque_inertial.h @@ -19,18 +19,27 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -#include "bodydynamics/feature/feature_force_torque_preint.h" -namespace wolf { +#ifndef CAPTURE_FORCE_TORQUE_INERTIAL_H +#define CAPTURE_FORCE_TORQUE_INERTIAL_H -FeatureForceTorquePreint::FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated, - const Eigen::MatrixXd& _delta_preintegrated_covariance, - const Eigen::Vector6d& _biases_preint, - const Eigen::Matrix<double,12,6>& _J_delta_biases) : - FeatureBase("FeatureForceTorquePreint", _delta_preintegrated, _delta_preintegrated_covariance), - pbc_bias_preint_(_biases_preint.head<3>()), - gyro_bias_preint_(_biases_preint.tail<3>()), - J_delta_bias_(_J_delta_biases) +#include <core/capture/capture_motion.h> + +namespace wolf { -} -} // namespace wolf +WOLF_PTR_TYPEDEFS(CaptureForceTorqueInertial); + +class CaptureForceTorqueInertial : public CaptureMotion +{ + public: + CaptureForceTorqueInertial(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + CaptureBasePtr _capture_origin_ptr = nullptr); + virtual ~CaptureForceTorqueInertial(); +}; + +} // namespace wolf + +#endif // CAPTURE_FORCE_TORQUE_INERTIAL_H \ No newline at end of file diff --git a/include/bodydynamics/factor/factor_angular_momentum.h b/include/bodydynamics/factor/factor_angular_momentum.h new file mode 100644 index 0000000000000000000000000000000000000000..5c120ae2eb369c9c7e373b66f5d55dbd2996f0ba --- /dev/null +++ b/include/bodydynamics/factor/factor_angular_momentum.h @@ -0,0 +1,172 @@ +//--------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-------- +#ifndef FACTOR_ANGULAR_MOMENTUM_H +#define FACTOR_ANGULAR_MOMENTUM_H + +#include "bodydynamics/math/force_torque_inertial_delta_tools.h" +#include <core/feature/feature_motion.h> +#include <core/factor/factor_autodiff.h> + +namespace wolf +{ +using namespace Eigen; +using namespace bodydynamics; + +WOLF_PTR_TYPEDEFS(FactorAngularMomentum); + +/** + * @brief + * + * This factor evaluates the real angular velocity against the one obtained with the pre-integrated angular momentum + * and the moment of inertia. + * + * State blocks considered: + * - Frame + * 'L' angular momentum + * - Capture + * 'I' IMU biases + * - Sensor Force Torque Inertial + * 'i' inertia matrix components (we are assuming that it is a diagonal matrix) + * + * The residual computed has the following components, in this order + * - angular velocity error according to FT preintegration + */ +class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3> // State Blocks: L, I, i +{ + public: + FactorAngularMomentum(const FeatureMotionPtr& _ftr, // gets measurement and access to parents + const ProcessorBasePtr& _processor, // gets access to processor and sensor + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE); + + ~FactorAngularMomentum() override = default; + + template <typename T> + bool operator()(const T* const _L, // ang momentum + const T* const _I, // IMU bias (acc and gyro) + const T* const _i, // inertia matrix + T* _res) const; // residual + + template <typename D1, typename D2, typename D3, typename D4> + bool residual(const MatrixBase<D1>& _L, + const MatrixBase<D2>& _I, + const MatrixBase<D3>& _i, + MatrixBase<D4>& _res) const; + + Vector3d residual() const; + + private: + Matrix<double, 12, 1> data; + Matrix<double, 3, 3> sqrt_info_upper_; +}; + +inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr, + const ProcessorBasePtr& _processor, + bool _apply_loss_function, + FactorStatus _status) + : FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3>( + "FactorAngularMomentum", + TOP_MOTION, + _ftr, // parent feature + nullptr, // frame other + nullptr, // capture other + nullptr, // feature other + nullptr, // landmark other + _processor, // processor + _apply_loss_function, + _status, + _ftr->getFrame()->getStateBlock('L'), // previous frame L + _capture_origin->getStateBlock('I'), // previous frame IMU bias + _processor->getSensor()->getStateBlock('i'), // sensor i + data(ftr->getMeasurement()), + sqrt_info_upper_(_processor->getSensor()->getNoiseCov().block<3,3>(3,3))) +{ + // +} + +template <typename D1, typename D2, typename D3, typename D4> +inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L, + const MatrixBase<D2>& _I, + const MatrixBase<D3>& _i, + MatrixBase<D4>& _res) const +{ + MatrixSizeCheck<3, 1>::check(_res); + + /* Will do the following: + * + * Compute the real angular velocity through the measurement and the IMU bias + * w_real = w_m - w_b + * + * Compute the angular velocity obtained by the relation between L pre-integrated and the i + * w_est = i^(-1)*L + * + * Compute error between them + * err = w_m - w_b - i^(-1)*L + * + * Compute residual + * res = W * err , with W the sqrt of the info matrix. + */ + + Matrix<T, 3, 1> w_real = data.segment<3>(3) - _I.segment<3>(3); + double Lx = _L(0); + double Ly = _L(1); + double Lz = _L(2); + double ixx = _i(0); + double iyy = _i(1); + double izz = _i(2); + Matrix<T, 3, 1> w_est = [Lx/ixx, Ly/iyy, Lz/izz]; + Matrix<T, 3, 1> err = w_real - w_est; + _res = sqrt_info_upper_ * err; + + return true; +} + +inline Vector3d FactorAngularMomentum::residual() const +{ + Vector3d res(3); + Vector3d L = getFrame()->getStateBlock('L')->getState(); + Vector6d I = getFeature()->getCapture()->getSensor()->getState(); + Vector3d i = getFeature()->getSensor()->getStateBlock('i')->getState(); + + residual(L, I, i, res); + return res; +} + +template <typename T> +inline bool FactorAngularMomentum::operator()(const T* const _L, + const T* const _I, + const T* const _i, + T* _res) const +{ + Map<const Matrix<T, 3, 1>> L(_L); + Map<const Matrix<T, 6, 1>> I(_I); + Map<const Matrix<T, 3, 1>> i(_i); + Map<Matrix<T, 3, 1>> res(_res); + + residual(L, I, i, res); + + return true; +} + +} // namespace wolf + +#endif // FACTOR_ANGULAR_MOMENTUM_H \ No newline at end of file diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h index 958398a325c0e6c77fd5b70a2cabb4497fec746b..b9ff1b4c45b3e4108c0ed1e755c7a326044a3f76 100644 --- a/include/bodydynamics/factor/factor_force_torque.h +++ b/include/bodydynamics/factor/factor_force_torque.h @@ -19,264 +19,325 @@ // 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> - +//Wolf includes +#include "bodydynamics/capture/capture_force_torque.h" #include "bodydynamics/feature/feature_force_torque.h" +#include "bodydynamics/sensor/sensor_force_torque.h" +#include "core/factor/factor_autodiff.h" +#include "core/math/rotations.h" -namespace wolf -{ +//Eigen include +namespace wolf { + WOLF_PTR_TYPEDEFS(FactorForceTorque); -class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3,3,3,4> +//class +class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 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 + FactorForceTorque(const FeatureForceTorquePtr& _ftr_ptr, + const CaptureForceTorquePtr& _cap_origin_ptr, // gets to bp1, bw1 + const ProcessorBasePtr& _processor_ptr, + const CaptureBasePtr& _cap_ikin_other, + const CaptureBasePtr& _cap_gyro_other, + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE); + + ~FactorForceTorque() override = default; + + /** \brief : compute the residual from the state blocks being iterated by the solver. + -> computes the expected measurement + -> corrects actual measurement with new bias + -> compares the corrected measurement with the expected one + -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;) */ 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_; + bool operator ()(const T* const _c1, + const T* const _cd1, + const T* const _Lc1, + const T* const _q1, + const T* const _bp1, + const T* const _bw1, + const T* const _c2, + const T* const _cd2, + const T* const _Lc2, + const T* const _q2, + T* _res) const; + + /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator()) + -> computes the expected measurement + -> corrects actual measurement with new bias + -> compares the corrected measurement with the expected one + -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;) + * params : + * _c1 : COM position at time t1 in world frame + * _cd1: COM velocity at time t1 in world frame + * _Lc1: Angular momentum at time t1 in world frame + * _q1 : Base orientation at time t1 + * _bp1: COM position measurement at time t1 in body frame + * _bw1: gyroscope bias at time t1 in body frame + * _c2 : COM position at time t2 in world frame + * _cd2: COM velocity at time t2 in world frame + * _Lc2: Angular momentum at time t2 in world frame + * _q2 : Base orientation at time t2 + * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION + */ + template<typename D1, typename D2, typename D3, typename D4> + bool residual(const MatrixBase<D1> & _c1, + const MatrixBase<D1> & _cd1, + const MatrixBase<D1> & _Lc1, + const QuaternionBase<D2> & _q1, + const MatrixBase<D1> & _bp1, + const MatrixBase<D1> & _bw1, + const MatrixBase<D1> & _c2, + const MatrixBase<D1> & _cd2, + const MatrixBase<D1> & _Lc2, + const QuaternionBase<D3> & _q2, + MatrixBase<D4> & _res) const; + + // Matrix<double,12,1> residual() const; + // double cost() const; + + private: + /// Preintegrated delta + Vector3d dc_preint_; + Vector3d dcd_preint_; + Vector3d dLc_preint_; + Quaterniond dq_preint_; + + // Biases used during preintegration + Vector3d pbc_bias_preint_; + Vector3d gyro_bias_preint_; + + // Jacobians of preintegrated deltas wrt biases + Matrix3d J_dLc_pb_; + Matrix3d J_dc_wb_; + Matrix3d J_dcd_wb_; + Matrix3d J_dLc_wb_; + Matrix3d J_dq_wb_; + + /// Metrics + double dt_; ///< delta-time and delta-time-squared between keyframes + double mass_; ///< constant total robot mass + }; -} /* 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) +///////////////////// IMPLEMENTATION //////////////////////////// + +inline FactorForceTorque::FactorForceTorque( + const FeatureForceTorquePtr& _ftr_ptr, + const CaptureForceTorquePtr& _cap_origin_ptr, + const ProcessorBasePtr& _processor_ptr, + const CaptureBasePtr& _cap_ikin_other, + const CaptureBasePtr& _cap_gyro_other, + bool _apply_loss_function, + FactorStatus _status) : + FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 3,3,3,4>( + "FactorForceTorque", + TOP_MOTION, + _ftr_ptr, + _cap_origin_ptr->getFrame(), + _cap_origin_ptr, + nullptr, + nullptr, + _processor_ptr, + _apply_loss_function, + _status, + _cap_origin_ptr->getFrame()->getStateBlock('C'), + _cap_origin_ptr->getFrame()->getStateBlock('D'), + _cap_origin_ptr->getFrame()->getStateBlock('L'), + _cap_origin_ptr->getFrame()->getO(), + _cap_ikin_other->getSensorIntrinsic(), + _cap_gyro_other->getSensorIntrinsic(), + _ftr_ptr->getFrame()->getStateBlock('C'), + _ftr_ptr->getFrame()->getStateBlock('D'), + _ftr_ptr->getFrame()->getStateBlock('L'), + _ftr_ptr->getFrame()->getO() + ), + dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time + dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)), + dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)), + dq_preint_(_ftr_ptr->getMeasurement().data()+9), + pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorque>(_ftr_ptr)->getPbcBiasPreint()), + gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorque>(_ftr_ptr)->getGyroBiasPreint()), + J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias + J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias + J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias + J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias + J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias + dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()), + mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass()) { - 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) +template<typename T> +inline bool FactorForceTorque::operator ()(const T* const _c1, + const T* const _cd1, + const T* const _Lc1, + const T* const _q1, + const T* const _bp1, + const T* const _bw1, + const T* const _c2, + const T* const _cd2, + const T* const _Lc2, + const T* const _q2, + T* _res) const { - 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(); -} + Map<const Matrix<T,3,1> > c1(_c1); + Map<const Matrix<T,3,1> > cd1(_cd1); + Map<const Matrix<T,3,1> > Lc1(_Lc1); + Map<const Quaternion<T> > q1(_q1); + Map<const Matrix<T,3,1> > bp1(_bp1); + Map<const Matrix<T,3,1> > bw1(_bw1 + 3); // bw1 = bimu = [ba, bw] + Map<const Matrix<T,3,1> > c2(_c2); + Map<const Matrix<T,3,1> > cd2(_cd2); + Map<const Matrix<T,3,1> > Lc2(_Lc2); + Map<const Quaternion<T> > q2(_q2); + Map<Matrix<T,12,1> > res(_res); + + residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res); + return true; +} -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 +template<typename D1, typename D2, typename D3, typename D4> +inline bool FactorForceTorque::residual(const MatrixBase<D1> & _c1, + const MatrixBase<D1> & _cd1, + const MatrixBase<D1> & _Lc1, + const QuaternionBase<D2> & _q1, + const MatrixBase<D1> & _bp1, + const MatrixBase<D1> & _bw1, + const MatrixBase<D1> & _c2, + const MatrixBase<D1> & _cd2, + const MatrixBase<D1> & _Lc2, + const QuaternionBase<D3> & _q2, + MatrixBase<D4> & _res) 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; + /* Help for the Imu residual function + * + * Notations: + * D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv] + * T_* : a motion difference in the Tangent space to the manifold -- implemented as 9-vectors with [Dp, Do, Dv] + * b* : a bias + * J* : a Jacobian matrix + * + * We use the following functions from imu_tools.h: + * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1 + * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T + * T = diff(D1,D2) : minus operator, T = D2 (-) D1 + * + * Two methods are possible (select with #define below this note) : + * + * Computations common to methods 1 and 2: + * D_exp = betweenStates(x1,x2,dt) // Predict delta from states + * T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change + * + * Method 1: + * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change + * T_err = diff(D_exp, D_corr) // compare against expected delta + * res = W.sqrt * T_err + * + * results in: + * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) ) + * + * Method 2: + * T_diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta + * T_err = T_diff - T_step // the difference should match the correction step due to bias change + * res = W.sqrt * err + * + * results in : + * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) ) + * + * NOTE: See optimization report at the end of this file for comparisons of both methods. + */ + + //needed typedefs + typedef typename D1::Scalar T; + + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12); + + // 1. Expected delta from states + Matrix<T, 3, 1 > dc_exp; + Matrix<T, 3, 1 > dcd_exp; + Matrix<T, 3, 1 > dLc_exp; + Quaternion<T> dq_exp; + + bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp); + + // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint) + + // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint) + Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_); + Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_); + Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_); + Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_); + + // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step + Matrix<T,3,1> dc_correct; + Matrix<T,3,1> dcd_correct; + Matrix<T,3,1> dLc_correct; + Quaternion<T> dq_correct; + + bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(), + dc_step, dcd_step, dLc_step, do_step, + dc_correct, dcd_correct, dLc_correct, dq_correct); + + // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr) + // Note the Dt here is zero because it's the delta-time between the same time stamps! + Matrix<T, 12, 1> d_error; + Map<Matrix<T, 3, 1> > dc_error (d_error.data() ); + Map<Matrix<T, 3, 1> > dcd_error(d_error.data() + 3); + Map<Matrix<T, 3, 1> > dLc_error(d_error.data() + 6); + Map<Matrix<T, 3, 1> > do_error (d_error.data() + 9); + + + bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp, + dc_correct, dcd_correct, dLc_correct, dq_correct, + dc_error, dcd_error, dLc_error, do_error); + + _res = getMeasurementSquareRootInformationUpper() * d_error; + + return true; } -// void FactorForceTorque::recomputeJac(const FeatureForceTorquePtr& _feat, -// const double& _dt, -// const Eigen::VectorXd& _bp, -// Eigen::Matrix<double, 9, 15>& _J_ny_nz) +// Matrix<double,12,1> FactorForceTorque::residual() const // { -// 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; -// } +// Matrix<double,12,1> res; -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; +// FrameBasePtr frm_prev = getFrameOther(); +// FrameBasePtr frm_curr = getFeature()->getFrame(); - return true; -} +// CaptureBaseWPtrList cap_lst = getCaptureOtherList(); // !! not retrieving the rigt captures... +// auto cap_ikin_other = cap_lst.front().lock(); +// auto cap_gyro_other = cap_lst.back().lock(); + +// Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data()); +// Map<const Matrix<double,3,1> > cd1(frm_prev->getStateBlock('D')->getState().data()); +// Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data()); +// Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data()); +// Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data()); +// Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3); // bw1 = bimu = [ba, bw] +// Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data()); +// Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data()); +// Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data()); +// Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data()); + +// residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res); + +// return res; +// } + +// double FactorForceTorque::cost() const +// { +// Matrix<double,12,1> toto = residual(); +// } } // namespace wolf -#endif /* FACTOR_FORCE_TORQUE_H_ */ +#endif diff --git a/include/bodydynamics/factor/factor_force_torque_inertial.h b/include/bodydynamics/factor/factor_force_torque_inertial.h index 51c60184de4a56d64da4f20992a7ade895baea72..cfea278e30816f355353fbc833d4647c66cf6726 100644 --- a/include/bodydynamics/factor/factor_force_torque_inertial.h +++ b/include/bodydynamics/factor/factor_force_torque_inertial.h @@ -35,9 +35,9 @@ WOLF_PTR_TYPEDEFS(FactorForceTorqueInertial); /** * @brief Pre-integrated factor taking IMU and force-torque measurements (FTI) - * + * * This factor evaluates the preintegrated FTI against pose, linear velocity, IMU bias and angular momentum. - * + * * State blocks considered: * - Frame 1 (origin) * 'P' position @@ -51,7 +51,7 @@ WOLF_PTR_TYPEDEFS(FactorForceTorqueInertial); * 'V' velocity * 'O' orientation * 'L' angular momentum - * + * * The residual computed has the following components, in this order * - position delta error according to IMU preintegration * - velocity delta error according to IMU preintegration @@ -191,10 +191,11 @@ inline bool FactorForceTorqueInertial::residual(const MatrixBase<D1>& _p1, fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq); Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>(); - Matrix<T, 18, 1> delta_step = J_delta_bias_ * (_b1 - bias_preint_); // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1> - Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step); - Matrix<T, 18, 1> delta_err = fti::diff(delta_est, delta_corr); - _res = sqrt_info_upper_ * delta_err; + Matrix<T, 18, 1> delta_step = + J_delta_bias_ * (_b1 - bias_preint_); // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1> + Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step); + Matrix<T, 18, 1> delta_err = fti::diff(delta_est, delta_corr); + _res = sqrt_info_upper_ * delta_err; return true; } diff --git a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h index f33b5d10d34d1bd659fbe5aca49e92dfac05a7b8..abece6e0d706e25da604a03d8c13f70dafdb2ed6 100644 --- a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h +++ b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h @@ -149,9 +149,9 @@ inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(cons _ftr->getFrame()->getStateBlock('O'), // current frame O _ftr->getFrame()->getStateBlock('V'), // current frame V _ftr->getFrame()->getStateBlock('L'), // current frame L - _capture_origin->getSensor()->getStateBlock('C'), // sensor C - _capture_origin->getSensor()->getStateBlock('i'), // sensor i - _capture_origin->getSensor()->getStateBlock('m')), // sensor m + _ftr->getCapture()->getStateBlock('C'), // sensor C + _ftr->getCapture()->getStateBlock('i'), // sensor i + _ftr->getCapture()->getStateBlock('m')), // sensor m dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()), delta_preint_(_ftr->getDeltaPreint()), calib_preint_(_ftr->getCalibrationPreint()), @@ -173,7 +173,7 @@ inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>& const MatrixBase<D3>& _L2, const MatrixBase<D6>& _C, const MatrixBase<D6>& _i, - const D7& _m, + const D7& _m, MatrixBase<D8>& _res) const { MatrixSizeCheck<18, 1>::check(_res); @@ -228,21 +228,21 @@ inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>& inline VectorXd FactorForceTorqueInertialDynamics::residual() const { - VectorXd res(18); - Vector3d p0 = getFrameOther()->getStateBlock('P')->getState(); // previous frame P - Quaterniond q0 = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data()); - Vector3d v0 = getFrameOther()->getStateBlock('V')->getState(); - Vector3d L0 = getFrameOther()->getStateBlock('L')->getState(); - Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState(); - Vector3d p1 = getFrame()->getStateBlock('P')->getState(); // previous frame P - Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data()); - Vector3d v1 = getFrame()->getStateBlock('V')->getState(); - Vector3d L1 = getFrame()->getStateBlock('L')->getState(); - Vector3d C = getCapture()->getSensor()->getStateBlock('C')->getState(); - Vector3d i = getCapture()->getSensor()->getStateBlock('i')->getState(); - double m = getCapture()->getSensor()->getStateBlock('m')->getState()(0); + VectorXd res(18); + Vector3d p0 = getFrameOther()->getStateBlock('P')->getState(); // previous frame P + Quaterniond q0 = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data()); + Vector3d v0 = getFrameOther()->getStateBlock('V')->getState(); + Vector3d L0 = getFrameOther()->getStateBlock('L')->getState(); + Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState(); + Vector3d p1 = getFrame()->getStateBlock('P')->getState(); // previous frame P + Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data()); + Vector3d v1 = getFrame()->getStateBlock('V')->getState(); + Vector3d L1 = getFrame()->getStateBlock('L')->getState(); + Vector3d C = getCapture()->getSensor()->getStateBlockDynamic('C')->getState(); + Vector3d i = getCapture()->getSensor()->getStateBlockDynamic('i')->getState(); + double m = getCapture()->getSensor()->getStateBlockDynamic('m')->getState()(0); - residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res); + residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res); return res; } @@ -259,7 +259,7 @@ inline bool FactorForceTorqueInertialDynamics::operator()(const T* const _p1, const T* const _C, const T* const _i, const T* const _m, - T* _res) const + T* _res) const { Map<const Matrix<T, 3, 1>> p1(_p1); Map<const Quaternion<T>> q1(_q1); diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h deleted file mode 100644 index 841bb63d6f94ffa9dc4ec115e5f4a3c3dd06d20a..0000000000000000000000000000000000000000 --- a/include/bodydynamics/factor/factor_force_torque_preint.h +++ /dev/null @@ -1,343 +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-------- -#ifndef FACTOR_FORCE_TORQUE_PREINT_THETA_H_ -#define FACTOR_FORCE_TORQUE_PREINT_THETA_H_ - -//Wolf includes -#include "bodydynamics/capture/capture_force_torque_preint.h" -#include "bodydynamics/feature/feature_force_torque_preint.h" -#include "bodydynamics/sensor/sensor_force_torque.h" -#include "core/factor/factor_autodiff.h" -#include "core/math/rotations.h" - -//Eigen include - -namespace wolf { - -WOLF_PTR_TYPEDEFS(FactorForceTorquePreint); - -//class -class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4> -{ - public: - FactorForceTorquePreint(const FeatureForceTorquePreintPtr& _ftr_ptr, - const CaptureForceTorquePreintPtr& _cap_origin_ptr, // gets to bp1, bw1 - const ProcessorBasePtr& _processor_ptr, - const CaptureBasePtr& _cap_ikin_other, - const CaptureBasePtr& _cap_gyro_other, - bool _apply_loss_function, - FactorStatus _status = FAC_ACTIVE); - - ~FactorForceTorquePreint() override = default; - - /** \brief : compute the residual from the state blocks being iterated by the solver. - -> computes the expected measurement - -> corrects actual measurement with new bias - -> compares the corrected measurement with the expected one - -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;) - */ - template<typename T> - bool operator ()(const T* const _c1, - const T* const _cd1, - const T* const _Lc1, - const T* const _q1, - const T* const _bp1, - const T* const _bw1, - const T* const _c2, - const T* const _cd2, - const T* const _Lc2, - const T* const _q2, - T* _res) const; - - /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator()) - -> computes the expected measurement - -> corrects actual measurement with new bias - -> compares the corrected measurement with the expected one - -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;) - * params : - * _c1 : COM position at time t1 in world frame - * _cd1: COM velocity at time t1 in world frame - * _Lc1: Angular momentum at time t1 in world frame - * _q1 : Base orientation at time t1 - * _bp1: COM position measurement at time t1 in body frame - * _bw1: gyroscope bias at time t1 in body frame - * _c2 : COM position at time t2 in world frame - * _cd2: COM velocity at time t2 in world frame - * _Lc2: Angular momentum at time t2 in world frame - * _q2 : Base orientation at time t2 - * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION - */ - template<typename D1, typename D2, typename D3, typename D4> - bool residual(const MatrixBase<D1> & _c1, - const MatrixBase<D1> & _cd1, - const MatrixBase<D1> & _Lc1, - const QuaternionBase<D2> & _q1, - const MatrixBase<D1> & _bp1, - const MatrixBase<D1> & _bw1, - const MatrixBase<D1> & _c2, - const MatrixBase<D1> & _cd2, - const MatrixBase<D1> & _Lc2, - const QuaternionBase<D3> & _q2, - MatrixBase<D4> & _res) const; - - // Matrix<double,12,1> residual() const; - // double cost() const; - - private: - /// Preintegrated delta - Vector3d dc_preint_; - Vector3d dcd_preint_; - Vector3d dLc_preint_; - Quaterniond dq_preint_; - - // Biases used during preintegration - Vector3d pbc_bias_preint_; - Vector3d gyro_bias_preint_; - - // Jacobians of preintegrated deltas wrt biases - Matrix3d J_dLc_pb_; - Matrix3d J_dc_wb_; - Matrix3d J_dcd_wb_; - Matrix3d J_dLc_wb_; - Matrix3d J_dq_wb_; - - /// Metrics - double dt_; ///< delta-time and delta-time-squared between keyframes - double mass_; ///< constant total robot mass - -}; - -///////////////////// IMPLEMENTATION //////////////////////////// - -inline FactorForceTorquePreint::FactorForceTorquePreint( - const FeatureForceTorquePreintPtr& _ftr_ptr, - const CaptureForceTorquePreintPtr& _cap_origin_ptr, - const ProcessorBasePtr& _processor_ptr, - const CaptureBasePtr& _cap_ikin_other, - const CaptureBasePtr& _cap_gyro_other, - bool _apply_loss_function, - FactorStatus _status) : - FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>( - "FactorForceTorquePreint", - TOP_MOTION, - _ftr_ptr, - _cap_origin_ptr->getFrame(), - _cap_origin_ptr, - nullptr, - nullptr, - _processor_ptr, - _apply_loss_function, - _status, - _cap_origin_ptr->getFrame()->getStateBlock('C'), - _cap_origin_ptr->getFrame()->getStateBlock('D'), - _cap_origin_ptr->getFrame()->getStateBlock('L'), - _cap_origin_ptr->getFrame()->getO(), - _cap_ikin_other->getSensorIntrinsic(), - _cap_gyro_other->getSensorIntrinsic(), - _ftr_ptr->getFrame()->getStateBlock('C'), - _ftr_ptr->getFrame()->getStateBlock('D'), - _ftr_ptr->getFrame()->getStateBlock('L'), - _ftr_ptr->getFrame()->getO() - ), - dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time - dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)), - dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)), - dq_preint_(_ftr_ptr->getMeasurement().data()+9), - pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getPbcBiasPreint()), - gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getGyroBiasPreint()), - J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias - J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias - J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias - J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias - J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias - dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()), - mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass()) -{ -// -} - -template<typename T> -inline bool FactorForceTorquePreint::operator ()(const T* const _c1, - const T* const _cd1, - const T* const _Lc1, - const T* const _q1, - const T* const _bp1, - const T* const _bw1, - const T* const _c2, - const T* const _cd2, - const T* const _Lc2, - const T* const _q2, - T* _res) const -{ - Map<const Matrix<T,3,1> > c1(_c1); - Map<const Matrix<T,3,1> > cd1(_cd1); - Map<const Matrix<T,3,1> > Lc1(_Lc1); - Map<const Quaternion<T> > q1(_q1); - Map<const Matrix<T,3,1> > bp1(_bp1); - Map<const Matrix<T,3,1> > bw1(_bw1 + 3); // bw1 = bimu = [ba, bw] - Map<const Matrix<T,3,1> > c2(_c2); - Map<const Matrix<T,3,1> > cd2(_cd2); - Map<const Matrix<T,3,1> > Lc2(_Lc2); - Map<const Quaternion<T> > q2(_q2); - Map<Matrix<T,12,1> > res(_res); - - residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res); - - return true; -} - -template<typename D1, typename D2, typename D3, typename D4> -inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1, - const MatrixBase<D1> & _cd1, - const MatrixBase<D1> & _Lc1, - const QuaternionBase<D2> & _q1, - const MatrixBase<D1> & _bp1, - const MatrixBase<D1> & _bw1, - const MatrixBase<D1> & _c2, - const MatrixBase<D1> & _cd2, - const MatrixBase<D1> & _Lc2, - const QuaternionBase<D3> & _q2, - MatrixBase<D4> & _res) const -{ - /* Help for the Imu residual function - * - * Notations: - * D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv] - * T_* : a motion difference in the Tangent space to the manifold -- implemented as 9-vectors with [Dp, Do, Dv] - * b* : a bias - * J* : a Jacobian matrix - * - * We use the following functions from imu_tools.h: - * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1 - * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T - * T = diff(D1,D2) : minus operator, T = D2 (-) D1 - * - * Two methods are possible (select with #define below this note) : - * - * Computations common to methods 1 and 2: - * D_exp = betweenStates(x1,x2,dt) // Predict delta from states - * T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change - * - * Method 1: - * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change - * T_err = diff(D_exp, D_corr) // compare against expected delta - * res = W.sqrt * T_err - * - * results in: - * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) ) - * - * Method 2: - * T_diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta - * T_err = T_diff - T_step // the difference should match the correction step due to bias change - * res = W.sqrt * err - * - * results in : - * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) ) - * - * NOTE: See optimization report at the end of this file for comparisons of both methods. - */ - - //needed typedefs - typedef typename D1::Scalar T; - - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12); - - // 1. Expected delta from states - Matrix<T, 3, 1 > dc_exp; - Matrix<T, 3, 1 > dcd_exp; - Matrix<T, 3, 1 > dLc_exp; - Quaternion<T> dq_exp; - - bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp); - - // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint) - - // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint) - Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_); - Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_); - Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_); - Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_); - - // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step - Matrix<T,3,1> dc_correct; - Matrix<T,3,1> dcd_correct; - Matrix<T,3,1> dLc_correct; - Quaternion<T> dq_correct; - - bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(), - dc_step, dcd_step, dLc_step, do_step, - dc_correct, dcd_correct, dLc_correct, dq_correct); - - // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr) - // Note the Dt here is zero because it's the delta-time between the same time stamps! - Matrix<T, 12, 1> d_error; - Map<Matrix<T, 3, 1> > dc_error (d_error.data() ); - Map<Matrix<T, 3, 1> > dcd_error(d_error.data() + 3); - Map<Matrix<T, 3, 1> > dLc_error(d_error.data() + 6); - Map<Matrix<T, 3, 1> > do_error (d_error.data() + 9); - - - bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp, - dc_correct, dcd_correct, dLc_correct, dq_correct, - dc_error, dcd_error, dLc_error, do_error); - - _res = getMeasurementSquareRootInformationUpper() * d_error; - - return true; -} - -// Matrix<double,12,1> FactorForceTorquePreint::residual() const -// { -// Matrix<double,12,1> res; - - -// FrameBasePtr frm_prev = getFrameOther(); -// FrameBasePtr frm_curr = getFeature()->getFrame(); - -// CaptureBaseWPtrList cap_lst = getCaptureOtherList(); // !! not retrieving the rigt captures... -// auto cap_ikin_other = cap_lst.front().lock(); -// auto cap_gyro_other = cap_lst.back().lock(); - -// Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data()); -// Map<const Matrix<double,3,1> > cd1(frm_prev->getStateBlock('D')->getState().data()); -// Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data()); -// Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data()); -// Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data()); -// Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3); // bw1 = bimu = [ba, bw] -// Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data()); -// Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data()); -// Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data()); -// Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data()); - -// residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res); - -// return res; -// } - -// double FactorForceTorquePreint::cost() const -// { -// Matrix<double,12,1> toto = residual(); -// } - -} // namespace wolf - -#endif diff --git a/include/bodydynamics/feature/feature_force_torque.h b/include/bodydynamics/feature/feature_force_torque.h index 14594821f3171f7628199e4b3b5f76026f4f7f63..c9fdd98d49b1619db1bf1d424cddfb59ae84a3b9 100644 --- a/include/bodydynamics/feature/feature_force_torque.h +++ b/include/bodydynamics/feature/feature_force_torque.h @@ -23,70 +23,73 @@ #define FEATURE_FORCE_TORQUE_H_ //Wolf includes -#include "core/feature/feature_base.h" +#include <core/feature/feature_base.h> +#include <core/common/wolf.h> //std includes + namespace wolf { - + +//WOLF_PTR_TYPEDEFS(CaptureImu); WOLF_PTR_TYPEDEFS(FeatureForceTorque); -//class FeatureApriltag class FeatureForceTorque : public FeatureBase { public: - FeatureForceTorque( - const double& _dt, - const double& _mass, - const Eigen::Vector6d& _forces_meas, - const Eigen::Vector6d& _torques_meas, - const Eigen::Vector3d& _pbc_meas, - const Eigen::Matrix<double,14,1>& _kin_meas, - const Eigen::Matrix6d& _cov_forces_meas, - const Eigen::Matrix6d& _cov_torques_meas, - const Eigen::Matrix3d& _cov_pbc_meas, - const Eigen::Matrix<double,14,14>& _cov_kin_meas = Eigen::Matrix<double,14,14>::Zero() - ); - - ~FeatureForceTorque() override; - - const double & getDt() const {return dt_;} - const double & getMass() const {return mass_;} - void setDt(const double & _dt){dt_ = _dt;} - void setMass(const double & _mass){mass_ = _mass;} - - const Eigen::Vector6d& getForcesMeas() const {return forces_meas_;} - const Eigen::Vector6d& getTorquesMeas() const {return torques_meas_;} - const Eigen::Vector3d& getPbcMeas() const {return pbc_meas_;} - const Eigen::Matrix<double,14,1>& getKinMeas() const {return kin_meas_;} - const Eigen::Matrix6d& getCovForcesMeas() const {return cov_forces_meas_;} - const Eigen::Matrix6d& getCovTorquesMeas() const {return cov_torques_meas_;} - const Eigen::Matrix3d& getCovPbcMeas() const {return cov_pbc_meas_;} - const Eigen::Matrix<double,14,14>& getCovKinMeas() const {return cov_kin_meas_;} - - void setForcesMeas(const Eigen::Vector6d& _forces_meas){forces_meas_ = _forces_meas;} - void setTorquesMeas(const Eigen::Vector6d& _torques_meas){torques_meas_ = _torques_meas;} - void setKinMeas(const Eigen::Matrix<double,14,1>& _kin_meas){kin_meas_ = _kin_meas;} - void setPbcMeas(const Eigen::Vector3d& _pbc_meas){pbc_meas_ = _pbc_meas;} - void setCovForcesMeas(const Eigen::Matrix6d& _cov_forces_meas){cov_forces_meas_ = _cov_forces_meas;} - void setCovTorquesMeas(const Eigen::Matrix6d& _cov_torques_meas){cov_torques_meas_ = _cov_torques_meas;} - void setCovPbcMeas(const Eigen::Matrix3d& _cov_pbc_meas){cov_pbc_meas_ = _cov_pbc_meas;} - void setCovKinMeas(const Eigen::Matrix<double,14,14>& _cov_kin_meas){cov_kin_meas_ = _cov_kin_meas;} - + /** \brief Constructor from and measures + * + * \param _measurement the measurement + * \param _meas_covariance the noise of the measurement + * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases + * \param _pbc_bias COM position relative to bias bias of origin frame time + * \param _gyro_bias gyroscope bias of origin frame time + * \param _cap_ft_ptr pointer to parent CaptureMotion (CaptureForceTorque) + */ + FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated, + const Eigen::MatrixXd& _delta_preintegrated_covariance, + const Eigen::Vector6d& _biases_preint, + const Eigen::Matrix<double,12,6>& _J_delta_biases); + +// /** \brief Constructor from capture pointer +// * +// * \param _cap_imu_ptr pointer to parent CaptureMotion +// */ +// FeatureForceTorque(CaptureMotionPtr _cap_imu_ptr); + + ~FeatureForceTorque() override = default; + + const Eigen::Vector3d& getPbcBiasPreint() const; + const Eigen::Vector3d& getGyroBiasPreint() const; + const Eigen::Matrix<double, 12, 6>& getJacobianBias() const; + private: - double dt_; - double mass_; - Eigen::Vector6d forces_meas_; - Eigen::Vector6d torques_meas_; - Eigen::Vector3d pbc_meas_; - Eigen::Matrix<double,14,1> kin_meas_; - Eigen::Matrix6d cov_forces_meas_; - Eigen::Matrix6d cov_torques_meas_; - Eigen::Matrix3d cov_pbc_meas_; - Eigen::Matrix<double,14,14> cov_kin_meas_; + // Used biases + Eigen::Vector3d pbc_bias_preint_; ///< Acceleration bias used for delta preintegration + Eigen::Vector3d gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration + + Eigen::Matrix<double, 12, 6> J_delta_bias_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; }; +inline const Eigen::Vector3d& FeatureForceTorque::getPbcBiasPreint() const +{ + return pbc_bias_preint_; +} + +inline const Eigen::Vector3d& FeatureForceTorque::getGyroBiasPreint() const +{ + return gyro_bias_preint_; +} + +inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorque::getJacobianBias() const +{ + return J_delta_bias_; +} + } // namespace wolf -#endif +#endif // FEATURE_FORCE_TORQUE_H_ diff --git a/include/bodydynamics/feature/feature_force_torque_preint.h b/include/bodydynamics/feature/feature_force_torque_preint.h deleted file mode 100644 index 9a03d2a73e668123b29dd948c7988795ea45f9cf..0000000000000000000000000000000000000000 --- a/include/bodydynamics/feature/feature_force_torque_preint.h +++ /dev/null @@ -1,95 +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-------- -#ifndef FEATURE_FORCE_TORQUE_PREINT_H_ -#define FEATURE_FORCE_TORQUE_PREINT_H_ - -//Wolf includes -#include <core/feature/feature_base.h> -#include <core/common/wolf.h> - -//std includes - -namespace wolf { - -//WOLF_PTR_TYPEDEFS(CaptureImu); -WOLF_PTR_TYPEDEFS(FeatureForceTorquePreint); - -class FeatureForceTorquePreint : public FeatureBase -{ - public: - - /** \brief Constructor from and measures - * - * \param _measurement the measurement - * \param _meas_covariance the noise of the measurement - * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases - * \param _pbc_bias COM position relative to bias bias of origin frame time - * \param _gyro_bias gyroscope bias of origin frame time - * \param _cap_ftpreint_ptr pointer to parent CaptureMotion (CaptureForceTorquePreint) - */ - FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated, - const Eigen::MatrixXd& _delta_preintegrated_covariance, - const Eigen::Vector6d& _biases_preint, - const Eigen::Matrix<double,12,6>& _J_delta_biases); - -// /** \brief Constructor from capture pointer -// * -// * \param _cap_imu_ptr pointer to parent CaptureMotion -// */ -// FeatureForceTorquePreint(CaptureMotionPtr _cap_imu_ptr); - - ~FeatureForceTorquePreint() override = default; - - const Eigen::Vector3d& getPbcBiasPreint() const; - const Eigen::Vector3d& getGyroBiasPreint() const; - const Eigen::Matrix<double, 12, 6>& getJacobianBias() const; - - private: - - // Used biases - Eigen::Vector3d pbc_bias_preint_; ///< Acceleration bias used for delta preintegration - Eigen::Vector3d gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration - - Eigen::Matrix<double, 12, 6> J_delta_bias_; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; -}; - -inline const Eigen::Vector3d& FeatureForceTorquePreint::getPbcBiasPreint() const -{ - return pbc_bias_preint_; -} - -inline const Eigen::Vector3d& FeatureForceTorquePreint::getGyroBiasPreint() const -{ - return gyro_bias_preint_; -} - -inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorquePreint::getJacobianBias() const -{ - return J_delta_bias_; -} - -} // namespace wolf - -#endif // FEATURE_FORCE_TORQUE_PREINT_H_ diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque.h similarity index 83% rename from include/bodydynamics/processor/processor_force_torque_preint.h rename to include/bodydynamics/processor/processor_force_torque.h index 6cd089107cfebd55fad2a51ff1b6cf5eddb0b036..8c9a8d4ab0a8bf944a86f88c8d7ad44bfb330d33 100644 --- a/include/bodydynamics/processor/processor_force_torque_preint.h +++ b/include/bodydynamics/processor/processor_force_torque.h @@ -19,24 +19,24 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -#ifndef PROCESSOR_FORCE_TORQUE_PREINT_H -#define PROCESSOR_FORCE_TORQUE_PREINT_H +#ifndef PROCESSOR_FORCE_TORQUE_H +#define PROCESSOR_FORCE_TORQUE_H // Wolf core #include <core/processor/processor_motion.h> namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorquePreint); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorque); -struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion +struct ParamsProcessorForceTorque : public ParamsProcessorMotion { std::string sensor_ikin_name; std::string sensor_angvel_name; int nbc; // Number of contacts int dimc; // Dimension of the contact: 3D == point feet = 3D force, 6D = humanoid = wrench - ParamsProcessorForceTorquePreint() = default; - ParamsProcessorForceTorquePreint(std::string _unique_name, const ParamsServer& _server): + ParamsProcessorForceTorque() = default; + ParamsProcessorForceTorque(std::string _unique_name, const ParamsServer& _server): ParamsProcessorMotion(_unique_name, _server) { sensor_ikin_name = _server.getParam<std::string>(_unique_name + "/sensor_ikin_name"); @@ -44,7 +44,7 @@ struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion nbc = _server.getParam<int>(_unique_name + "/nbc_"); dimc = _server.getParam<int>(_unique_name + "/dimc_"); } - ~ParamsProcessorForceTorquePreint() override = default; + ~ParamsProcessorForceTorque() override = default; std::string print() const override { return ParamsProcessorMotion::print() + "\n" @@ -56,16 +56,16 @@ struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion } }; -WOLF_PTR_TYPEDEFS(ProcessorForceTorquePreint); +WOLF_PTR_TYPEDEFS(ProcessorForceTorque); //class -class ProcessorForceTorquePreint : public ProcessorMotion{ +class ProcessorForceTorque : public ProcessorMotion{ public: - ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint); - ~ProcessorForceTorquePreint() override; + ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque); + ~ProcessorForceTorque() override; void configure(SensorBasePtr _sensor) override; - WOLF_PROCESSOR_CREATE(ProcessorForceTorquePreint, ParamsProcessorForceTorquePreint); + WOLF_PROCESSOR_CREATE(ProcessorForceTorque, ParamsProcessorForceTorque); protected: void computeCurrentDelta(const Eigen::VectorXd& _data, @@ -109,7 +109,7 @@ class ProcessorForceTorquePreint : public ProcessorMotion{ CaptureBasePtr _capture_origin) override; private: - ParamsProcessorForceTorquePreintPtr params_motion_force_torque_preint_; + ParamsProcessorForceTorquePtr params_motion_force_torque_; SensorBasePtr sensor_ikin_; SensorBasePtr sensor_angvel_; int nbc_; @@ -124,17 +124,17 @@ class ProcessorForceTorquePreint : public ProcessorMotion{ namespace wolf{ -inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor) +inline void ProcessorForceTorque::configure(SensorBasePtr _sensor) { - sensor_ikin_ = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_ikin_name); - sensor_angvel_ = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_angvel_name); + sensor_ikin_ = _sensor->getProblem()->findSensor(params_motion_force_torque_->sensor_ikin_name); + sensor_angvel_ = _sensor->getProblem()->findSensor(params_motion_force_torque_->sensor_angvel_name); }; -inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const +inline Eigen::VectorXd ProcessorForceTorque::deltaZero() const { return (Eigen::VectorXd(13) << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1 ).finished(); // com, com vel, ang momentum, orientation } } // namespace wolf -#endif // PROCESSOR_FORCE_TORQUE_PREINT_H +#endif // PROCESSOR_FORCE_TORQUE_H diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint.h b/include/bodydynamics/processor/processor_force_torque_inertial.h similarity index 89% rename from include/bodydynamics/processor/processor_force_torque_inertial_preint.h rename to include/bodydynamics/processor/processor_force_torque_inertial.h index 503247053b9f77094b4000d5ad5ecbfa54f50baa..616411f510f74ff6fa12d26d5270ce0252bb5dee 100644 --- a/include/bodydynamics/processor/processor_force_torque_inertial_preint.h +++ b/include/bodydynamics/processor/processor_force_torque_inertial.h @@ -20,14 +20,14 @@ // //--------LICENSE_END-------- /* - * processor_preintegrator_force_torque_inertial.h + * processor_force_torque_inertial.h * * Created on: Aug 19, 2021 * Author: jsola */ -#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_ -#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_ +#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_ +#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_ #include "bodydynamics/math/force_torque_inertial_delta_tools.h" #include "bodydynamics/sensor/sensor_force_torque_inertial.h" @@ -37,12 +37,12 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialPreint); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertial); -struct ParamsProcessorForceTorqueInertialPreint : public ParamsProcessorMotion +struct ParamsProcessorForceTorqueInertial : public ParamsProcessorMotion { - ParamsProcessorForceTorqueInertialPreint() = default; - ParamsProcessorForceTorqueInertialPreint(std::string _unique_name, const ParamsServer& _server) + ParamsProcessorForceTorqueInertial() = default; + ParamsProcessorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server) : ParamsProcessorMotion(_unique_name, _server) { n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers"); @@ -57,19 +57,19 @@ struct ParamsProcessorForceTorqueInertialPreint : public ParamsProcessorMotion int n_propellers; }; -WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialPreint); -class ProcessorForceTorqueInertialPreint : public ProcessorMotion +WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertial); +class ProcessorForceTorqueInertial : public ProcessorMotion { public: - ProcessorForceTorqueInertialPreint( - ParamsProcessorForceTorqueInertialPreintPtr _params_force_torque_inertial_preint); - ~ProcessorForceTorqueInertialPreint() override; + ProcessorForceTorqueInertial( + ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial); + ~ProcessorForceTorqueInertial() override; void configure(SensorBasePtr _sensor) override; - WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreint, ParamsProcessorForceTorqueInertialPreint); + WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertial, ParamsProcessorForceTorqueInertial); protected: - ParamsProcessorForceTorqueInertialPreintPtr params_force_torque_inertial_preint_; - SensorForceTorqueInertialPtr sensor_fti_; + ParamsProcessorForceTorqueInertialPtr params_force_torque_inertial_; + SensorForceTorqueInertialPtr sensor_fti_; public: /** \brief convert raw CaptureMotion data to the delta-state format. @@ -242,11 +242,11 @@ class ProcessorForceTorqueInertialPreint : public ProcessorMotion virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override; }; -inline Eigen::VectorXd ProcessorForceTorqueInertialPreint::deltaZero() const +inline Eigen::VectorXd ProcessorForceTorqueInertial::deltaZero() const { return bodydynamics::fti::identity(); } } /* namespace wolf */ -#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_ */ +#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_ */ diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h similarity index 89% rename from include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h rename to include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h index 8bcc1608659f2a373282c323099bdd38dc8d0666..c05887e9614252f80afd8861e4b7fc16fe45243c 100644 --- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h +++ b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h @@ -20,14 +20,14 @@ // //--------LICENSE_END-------- /* - * processor_preintegrator_force_torque_inertial.h + * processor_force_torque_inertial_dynamics.h * * Created on: Aug 19, 2021 * Author: jsola */ -#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_ -#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_ +#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_ +#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_ #include "bodydynamics/math/force_torque_inertial_delta_tools.h" #include "bodydynamics/sensor/sensor_force_torque_inertial.h" @@ -37,12 +37,12 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialPreintDynamics); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialDynamics); -struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessorMotion +struct ParamsProcessorForceTorqueInertialDynamics : public ParamsProcessorMotion { - ParamsProcessorForceTorqueInertialPreintDynamics() = default; - ParamsProcessorForceTorqueInertialPreintDynamics(std::string _unique_name, const ParamsServer& _server) + ParamsProcessorForceTorqueInertialDynamics() = default; + ParamsProcessorForceTorqueInertialDynamics(std::string _unique_name, const ParamsServer& _server) : ParamsProcessorMotion(_unique_name, _server) { // n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers"); @@ -57,20 +57,20 @@ struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessor // int n_propellers; }; -WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialPreintDynamics); -class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion +WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialDynamics); +class ProcessorForceTorqueInertialDynamics : public ProcessorMotion { public: - ProcessorForceTorqueInertialPreintDynamics( - ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics_); - ~ProcessorForceTorqueInertialPreintDynamics() override; + ProcessorForceTorqueInertialDynamics( + ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics_); + ~ProcessorForceTorqueInertialDynamics() override; void configure(SensorBasePtr _sensor) override; - WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics, - ParamsProcessorForceTorqueInertialPreintDynamics); + WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialDynamics, + ParamsProcessorForceTorqueInertialDynamics); protected: - ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_dynamics_; - SensorForceTorqueInertialPtr sensor_fti_; + ParamsProcessorForceTorqueInertialDynamicsPtr params_force_torque_inertial_dynamics_; + SensorForceTorqueInertialPtr sensor_fti_; Matrix6d imu_drift_cov_; Matrix3d gyro_noise_cov_; @@ -268,11 +268,11 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion Eigen::MatrixXd& _J_tangent_model) const; }; -inline Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::deltaZero() const +inline Eigen::VectorXd ProcessorForceTorqueInertialDynamics::deltaZero() const { return bodydynamics::fti::identity(); } } /* namespace wolf */ -#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_ */ +#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_ */ diff --git a/src/capture/capture_force_torque_preint.cpp b/src/capture/capture_force_torque.cpp similarity index 87% rename from src/capture/capture_force_torque_preint.cpp rename to src/capture/capture_force_torque.cpp index 83e145e4b85a4895a81dce2251fcddd10681460d..3b09bdda9e821497e4e7cc77300a8e06cb656e71 100644 --- a/src/capture/capture_force_torque_preint.cpp +++ b/src/capture/capture_force_torque.cpp @@ -20,13 +20,13 @@ // //--------LICENSE_END-------- #include "bodydynamics/capture/capture_inertial_kinematics.h" -#include "bodydynamics/capture/capture_force_torque_preint.h" +#include "bodydynamics/capture/capture_force_torque.h" #include "bodydynamics/sensor/sensor_force_torque.h" #include "core/state_block/state_quaternion.h" namespace wolf { -CaptureForceTorquePreint::CaptureForceTorquePreint( +CaptureForceTorque::CaptureForceTorque( const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias @@ -34,14 +34,14 @@ CaptureForceTorquePreint::CaptureForceTorquePreint( const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov, // TODO: no uncertainty from kinematics CaptureBasePtr _capture_origin_ptr) : - CaptureMotion("CaptureForceTorquePreint", _init_ts, _sensor_ptr, _data, _data_cov, + CaptureMotion("CaptureForceTorque", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr, nullptr, nullptr, nullptr), cap_ikin_other_(_capture_IK_ptr), cap_gyro_other_(_capture_motion_ptr) { } -CaptureForceTorquePreint::~CaptureForceTorquePreint() +CaptureForceTorque::~CaptureForceTorque() { } diff --git a/src/capture/capture_force_torque_inertial.cpp b/src/capture/capture_force_torque_inertial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..85a7f3603e24aaa04de9bfaafa5b33f32aa58da4 --- /dev/null +++ b/src/capture/capture_force_torque_inertial.cpp @@ -0,0 +1,50 @@ +//--------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/capture/capture_force_torque_inertial.h" + +#include <core/state_block/state_block_derived.h> + +namespace wolf +{ +CaptureForceTorqueInertial::CaptureForceTorqueInertial(const TimeStamp& _init_ts, + SensorBasePtr _sensor_ptr, + const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + CaptureBasePtr _capture_origin_ptr) + : CaptureMotion("CaptureForceTorqueInertial", + _init_ts, + _sensor_ptr, + _data, + _data_cov, + _capture_origin_ptr, + nullptr, // position is static + nullptr, // orientation is static + (_sensor_ptr->isStateBlockDynamic('I') // dynamic intrinsics are IMU bias + ? std::make_shared<StateParams6>(_sensor_ptr->getStateBlockDynamic('I')->getState(), false) + : nullptr)) +{ + // +} + +CaptureForceTorqueInertial::~CaptureForceTorqueInertial() {} + +} // namespace wolf \ No newline at end of file diff --git a/src/feature/feature_force_torque.cpp b/src/feature/feature_force_torque.cpp index 83c12548fd923e7621b56465ecedbcc12b673500..04eacfa7e8c4c02944bb2fd6a151f05551030c47 100644 --- a/src/feature/feature_force_torque.cpp +++ b/src/feature/feature_force_torque.cpp @@ -20,35 +20,18 @@ // //--------LICENSE_END-------- #include "bodydynamics/feature/feature_force_torque.h" +namespace wolf +{ -namespace wolf { - -WOLF_PTR_TYPEDEFS(FeatureForceTorque); +FeatureForceTorque::FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated, + const Eigen::MatrixXd& _delta_preintegrated_covariance, + const Eigen::Vector6d& _biases_preint, + const Eigen::Matrix<double, 12, 6>& _J_delta_biases) + : FeatureBase("FeatureForceTorque", _delta_preintegrated, _delta_preintegrated_covariance), + pbc_bias_preint_(_biases_preint.head<3>()), + gyro_bias_preint_(_biases_preint.tail<3>()), + J_delta_bias_(_J_delta_biases) +{ +} -FeatureForceTorque::FeatureForceTorque( - const double& _dt, - const double& _mass, - const Eigen::Vector6d& _forces_meas, - const Eigen::Vector6d& _torques_meas, - const Eigen::Vector3d& _pbc_meas, - const Eigen::Matrix<double,14,1>& _kin_meas, - const Eigen::Matrix6d& _cov_forces_meas, - const Eigen::Matrix6d& _cov_torques_meas, - const Eigen::Matrix3d& _cov_pbc_meas, - const Eigen::Matrix<double,14,14>& _cov_kin_meas) : - FeatureBase("FORCETORQUE", 42*Eigen::Matrix1d::Identity(), 42*Eigen::Matrix1d::Identity(), UNCERTAINTY_IS_COVARIANCE), // Pass dummmy values -> we are bypassing the way meas and cov is usually stored because too many of them == vector is too big -> error prone - dt_(_dt), - mass_(_mass), - forces_meas_(_forces_meas), - torques_meas_(_torques_meas), - pbc_meas_(_pbc_meas), - kin_meas_(_kin_meas), - cov_forces_meas_(_cov_forces_meas), - cov_torques_meas_(_cov_torques_meas), - cov_pbc_meas_(_cov_pbc_meas), - cov_kin_meas_(_cov_kin_meas) -{} - -FeatureForceTorque::~FeatureForceTorque(){} - -} // namespace wolf +} // namespace wolf diff --git a/src/processor/processor_force_torque.cpp b/src/processor/processor_force_torque.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d9bd77e5474df95514c60c72c5f3f138e92415b4 --- /dev/null +++ b/src/processor/processor_force_torque.cpp @@ -0,0 +1,294 @@ +//--------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-------- +// wolf +#include "bodydynamics/math/force_torque_delta_tools.h" +#include "bodydynamics/capture/capture_force_torque.h" +#include "bodydynamics/feature/feature_force_torque.h" +#include "bodydynamics/processor/processor_force_torque.h" +#include "bodydynamics/factor/factor_force_torque.h" + +namespace wolf +{ + +int compute_data_size(int nbc, int dimc) +{ + // compute the size of the data/body vectors used by processorMotion API depending + // on the number of contacts (nbc) and contact dimension (dimc) + // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations + return nbc * dimc + 3 + 3 + nbc * 3 + nbc * 4; +} + +using namespace Eigen; + +ProcessorForceTorque::ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque) + : ProcessorMotion("ProcessorForceTorque", + "CDLO", + 3, + 13, + 13, + 12, + compute_data_size(_params_motion_force_torque->nbc, _params_motion_force_torque->dimc), + 6, + _params_motion_force_torque), + params_motion_force_torque_(std::make_shared<ParamsProcessorForceTorque>(*_params_motion_force_torque)), + nbc_(_params_motion_force_torque->nbc), + dimc_(_params_motion_force_torque->dimc) + +{ + // +} + +ProcessorForceTorque::~ProcessorForceTorque() +{ + // +} + +bool ProcessorForceTorque::voteForKeyFrame() const +{ + // time span + if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_->max_time_span) + { + WOLF_DEBUG("PM: vote: time span"); + return true; + } + // buffer length + if (getBuffer().size() > params_motion_force_torque_->max_buff_length) + { + WOLF_DEBUG("PM: vote: buffer length"); + return true; + } + + // Some other measure of movement? + + // WOLF_DEBUG( "PM: do not vote" ); + return false; +} + +CaptureMotionPtr ProcessorForceTorque::emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin) +{ + // Here we have to retrieve the origin capture no + // !! PROBLEM: + // when doing setOrigin, emplaceCapture is called 2 times + // - first on the first KF (usually created by setPrior), this one contains the biases + // - secondly on the inner frame (last) which does not contains other captures + auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_); + auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_); + if ((capture_ikin == nullptr) || (capture_angvel == nullptr)) + { + // We have to retrieve the bias from a former Keyframe: origin + capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_); + capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_); + } + auto cap = + CaptureBase::emplace<CaptureForceTorque>(_frame_own, + _ts, + _sensor, + std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin), + std::static_pointer_cast<CaptureMotion>(capture_angvel), + _data, + _data_cov, + _capture_origin); + + // Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_ + // which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures + + auto cap_ft = std::static_pointer_cast<CaptureForceTorque>(cap); + Vector6d calib = getCalibration(cap_ft); + setCalibration(cap_ft, calib); + cap_ft->setCalibrationPreint(calib); + + return cap; +} + +FeatureBasePtr ProcessorForceTorque::emplaceFeature(CaptureMotionPtr _capture_motion) +{ + // Retrieving the origin capture and getting the bias from here should be done here? + auto feature = FeatureBase::emplace<FeatureForceTorque>( + _capture_motion, + _capture_motion->getBuffer().back().delta_integr_, + _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_, + _capture_motion->getCalibrationPreint(), + _capture_motion->getBuffer().back().jacobian_calib_); + return feature; +} + +Eigen::VectorXd ProcessorForceTorque::correctDelta(const Eigen::VectorXd& delta_preint, + const Eigen::VectorXd& delta_step) const +{ + return bodydynamics::plus(delta_preint, delta_step); +} + +VectorXd ProcessorForceTorque::getCalibration(const CaptureBaseConstPtr _capture) const +{ + VectorXd bias_vec(6); + + if (_capture) // access from capture is quicker + { + auto cap_ft(std::static_pointer_cast<const CaptureForceTorque>(_capture)); + + // get calib part from Ikin capture + bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState(); + + // get calib part from IMU capture + bias_vec.segment<3>(3) = + cap_ft->getGyroCaptureOther() + ->getSensorIntrinsic() + ->getState() + .tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias] + } + else // access from sensor is slower + { + // get calib part from Ikin capture + bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState(); + + // get calib part from IMU capture + bias_vec.segment<3>(3) = + sensor_angvel_->getIntrinsic() + ->getState() + .tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias] + } + return bias_vec; +} + +void ProcessorForceTorque::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) +{ + CaptureForceTorquePtr cap_ft(std::static_pointer_cast<CaptureForceTorque>(_capture)); + + // set calib part in Ikin capture + cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->setState(_calibration.head<3>()); + + // set calib part in IMU capture + Vector6d calib_imu = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState(); + calib_imu.tail<3>() = _calibration.tail<3>(); + + cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu); +} + +FactorBasePtr ProcessorForceTorque::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) +{ + CaptureForceTorquePtr cap_ft_origin = std::static_pointer_cast<CaptureForceTorque>(_capture_origin); + FeatureForceTorquePtr ftr_ft = std::static_pointer_cast<FeatureForceTorque>(_feature_motion); + CaptureForceTorquePtr cap_ft_new = std::static_pointer_cast<CaptureForceTorque>(ftr_ft->getCapture()); + + auto fac_ft = FactorBase::emplace<FactorForceTorque>(ftr_ft, + ftr_ft, + cap_ft_origin, + shared_from_this(), + cap_ft_origin->getIkinCaptureOther(), // to retrieve sb_bp1 + cap_ft_origin->getGyroCaptureOther(), // to retrieve sb_w1 + false); + + return fac_ft; +} + +void ProcessorForceTorque::computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jac_delta_calib) const +{ + assert(_data.size() == data_size_ && "Wrong data size!"); + + // create delta + MatrixXd jac_body_bias(data_size_ - nbc_, 6); + VectorXd body(data_size_); + bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias); + + MatrixXd jac_delta_body(12, data_size_ - nbc_); + bodydynamics::body2delta(body, + _dt, + std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(), + nbc_, + dimc_, + _delta, + jac_delta_body); // Jacobians tested in bodydynamics_tools + + // [f], [tau], pbc, wb + MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_ * dimc_ + 6); + + // compute delta_cov (using uncertainty on f,tau,pbc) + _delta_cov = jac_delta_body_reduced * _data_cov * + jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity + // _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose(); // trivially: jac_body_delta = Identity + + // compute jacobian_calib + _jac_delta_calib = jac_delta_body * jac_body_bias; +} + +void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta, + const double _dt, + Eigen::VectorXd& _delta_preint_plus_delta) const +{ + _delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt); +} + +void ProcessorForceTorque::statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _dt, + VectorComposite& _x_plus_delta) const +{ + assert(_delta.size() == 13 && "Wrong _delta vector size"); + assert(_dt >= 0 && "Time interval _dt is negative!"); + + // verbose way : create Eigen vectors, then compute, then convert back to Composite + + auto x = _x.vector("CDLO"); + + VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt); + + _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3, 3, 3, 4}); +} + +void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta, + const double _dt, + Eigen::VectorXd& _delta_preint_plus_delta, + Eigen::MatrixXd& _jacobian_delta_preint, + Eigen::MatrixXd& _jacobian_delta) const +{ + bodydynamics::compose(_delta_preint, + _delta, + _dt, + _delta_preint_plus_delta, + _jacobian_delta_preint, + _jacobian_delta); // jacobians tested in bodydynamics_tools +} + +} // namespace wolf + +// Register in the FactorySensor +#include "core/processor/factory_processor.h" + +namespace wolf +{ +WOLF_REGISTER_PROCESSOR(ProcessorForceTorque) +} diff --git a/src/processor/processor_force_torque_inertial.cpp b/src/processor/processor_force_torque_inertial.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8b54a8cb543393a0bd4198d95e214fc9c65f0d0a --- /dev/null +++ b/src/processor/processor_force_torque_inertial.cpp @@ -0,0 +1,176 @@ +//--------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-------- +/* + * processor_force_torque_inertial.cpp + * + * Created on: Aug 19, 2021 + * Author: jsola + */ + +// bodydynamics +#include "bodydynamics/processor/processor_force_torque_inertial.h" +#include "bodydynamics/factor/factor_force_torque_inertial.h" +#include "bodydynamics/math/force_torque_inertial_delta_tools.h" + +// wolf +#include <core/state_block/state_composite.h> +#include <core/capture/capture_motion.h> + +namespace wolf +{ + +ProcessorForceTorqueInertial::ProcessorForceTorqueInertial( + ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial) + : ProcessorMotion("ProcessorForceTorqueInertial", + "POVL", + 3, // dim + 13, // state size [p, q, v, L] + 19, // delta size [pi, vi, pd, vd, L, q] + 18, // delta cov size + 12, // data size [a, w, f, t] + 6, // calib size [ab, wb] + _params_force_torque_inertial), + params_force_torque_inertial_(_params_force_torque_inertial) +{ + // TODO Auto-generated constructor stub +} + +ProcessorForceTorqueInertial::~ProcessorForceTorqueInertial() +{ + // TODO Auto-generated destructor stub +} + +CaptureMotionPtr ProcessorForceTorqueInertial::emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin_ptr) +{ + CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>( + _frame_own, "CaptureForceTorqueInertial", _ts, _sensor, _data, _data_cov, _capture_origin_ptr); + setCalibration(capture, _calib); + capture->setCalibrationPreint(_calib_preint); + return capture; +} + +FeatureBasePtr ProcessorForceTorqueInertial::emplaceFeature(CaptureMotionPtr _capture_own) +{ + FeatureBasePtr returnValue; + return returnValue; +} + +FactorBasePtr ProcessorForceTorqueInertial::emplaceFactor(FeatureBasePtr _feature_motion, + CaptureBasePtr _capture_origin) +{ + FactorBasePtr returnValue; + return returnValue; +} + +void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) +{ + _capture->getStateBlock('I')->setState(_calibration); // Set IMU bias +} + +void ProcessorForceTorqueInertial::configure(SensorBasePtr _sensor) +{ + sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor); +} + +void ProcessorForceTorqueInertial::computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jacobian_calib) const +{ + // compute tangent by removing IMU bias + Matrix<double, 12, 1> tangent = _data; + tangent.head<6>() -= _calib; // J_tangent_data = Identity(12,12) + Matrix<double, 12, 6> J_tangent_calib; // J_tangent_calib = [-Identity(6,6) ; Zero(6,6)] + J_tangent_calib.topRows<6>() = -Matrix6d::Identity(); + J_tangent_calib.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 + 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(); + _jacobian_calib = J_delta_tangent * J_tangent_calib; +} + +void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _dt2, + Eigen::VectorXd& _delta1_plus_delta2) const +{ + bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2); +} + +void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _dt2, + Eigen::VectorXd& _delta1_plus_delta2, + Eigen::MatrixXd& _jacobian1, + Eigen::MatrixXd& _jacobian2) const +{ + bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2); +} + +void ProcessorForceTorqueInertial::statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _dt, + VectorComposite& _x_plus_delta) const +{ + auto x = _x.vector("PVLO"); + VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt); + _x_plus_delta = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4}); +} + +Eigen::VectorXd ProcessorForceTorqueInertial::correctDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta_step) const +{ + return fti::plus(_delta_preint, _delta_step); +} + +VectorXd ProcessorForceTorqueInertial::getCalibration(const CaptureBaseConstPtr _capture) const +{ + if (_capture) + return _capture->getStateBlock('I')->getState(); // IMU bias + else + return getSensor()->getStateBlock('I')->getState(); // IMU bias +} +} /* namespace wolf */ + +#include <core/processor/factory_processor.h> +namespace wolf +{ +WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertial); +WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertial); +} // namespace wolf \ No newline at end of file diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp similarity index 74% rename from src/processor/processor_force_torque_inertial_preint_dynamics.cpp rename to src/processor/processor_force_torque_inertial_dynamics.cpp index 4e8d1b278e302ebe7db8867296432754e893ece5..8becfc0a198f2c4169067d9ceae7b81ecc0ed4bd 100644 --- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp +++ b/src/processor/processor_force_torque_inertial_dynamics.cpp @@ -20,16 +20,17 @@ // //--------LICENSE_END-------- /* - * processor_preintegrator_force_torque_inertial_dynamics.cpp + * processor_force_torque_inertial_dynamics.cpp * * Created on: Aug 1, 2022 * Author: asanjuan */ // bodydynamics -#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h" +#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> @@ -39,9 +40,9 @@ namespace wolf { -ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDynamics( - ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics) - : ProcessorMotion("ProcessorForceTorqueInertialPreintDynamics", +ProcessorForceTorqueInertialDynamics::ProcessorForceTorqueInertialDynamics( + ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics) + : ProcessorMotion("ProcessorForceTorqueInertialDynamics", "POVL", 3, // dim 13, // state size [p, q, v, L] @@ -49,37 +50,35 @@ ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDy 18, // delta cov size 12, // data size [a, w, f, t] 13, // calib size [ab, wb, c, i, m] - _params_force_torque_inertial_preint_dynamics), - params_force_torque_inertial_preint_dynamics_(_params_force_torque_inertial_preint_dynamics) + _params_force_torque_inertial_dynamics), + params_force_torque_inertial_dynamics_(_params_force_torque_inertial_dynamics) { // } -ProcessorForceTorqueInertialPreintDynamics::~ProcessorForceTorqueInertialPreintDynamics() +ProcessorForceTorqueInertialDynamics::~ProcessorForceTorqueInertialDynamics() { // } -CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin_ptr) +CaptureMotionPtr ProcessorForceTorqueInertialDynamics::emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + 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; } - -FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(CaptureMotionPtr _capture_own) +FeatureBasePtr ProcessorForceTorqueInertialDynamics::emplaceFeature(CaptureMotionPtr _capture_own) { - auto motion = _capture_own->getBuffer().back(); FeatureBasePtr ftr = FeatureBase::emplace<FeatureMotion>(_capture_own, "FeatureMotion", @@ -90,8 +89,8 @@ FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(Captur return ftr; } -FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureBasePtr _feature_motion, - CaptureBasePtr _capture_origin) +FactorBasePtr ProcessorForceTorqueInertialDynamics::emplaceFactor(FeatureBasePtr _feature_motion, + CaptureBasePtr _capture_origin) { FeatureMotionPtr ftr_ftipd = std::static_pointer_cast<FeatureMotion>(_feature_motion); @@ -100,18 +99,18 @@ FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureB return fac_ftipd; } -void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, - CaptureMotionPtr _capture_own) +void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, + CaptureMotionPtr _capture_own) { // 1. Feature and factor FTI -- force-torque-inertial //---------------------------------------------------- auto motion = _capture_own->getBuffer().back(); auto ftr_fti = FeatureBase::emplace<FeatureMotion>(_capture_own, - "FeatureMotion", - motion.delta_integr_, - motion.delta_integr_cov_ + unmeasured_perturbation_cov_, - _capture_own->getCalibrationPreint(), - motion.jacobian_calib_); + "FeatureMotion", + motion.delta_integr_, + motion.delta_integr_cov_ + unmeasured_perturbation_cov_, + _capture_own->getCalibrationPreint(), + motion.jacobian_calib_); FactorBase::emplace<FactorForceTorqueInertialDynamics>( ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function); @@ -120,11 +119,10 @@ void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(Captu //--------------------------------------------- // - feature has the current gyro measurement // - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor) - + // auto measurement_gyro = motion.data_.segment<3>(3); // auto meas_cov = sensor_fti_->getNoiseCov().block<3,3>(3,3); // ?? - // 3. Feature and factor bias -- IMU bias drift for acc and gyro //--------------------------------------------------------------- // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I')) @@ -160,8 +158,7 @@ void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(Captu } } -void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture, - const VectorXd& _calibration) +void ProcessorForceTorqueInertialDynamics::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 @@ -169,7 +166,7 @@ void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBas _capture->getStateBlock('m')->setState(_calibration.segment<1>(12)); // Set m } -void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor) +void ProcessorForceTorqueInertialDynamics::configure(SensorBasePtr _sensor) { sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor); @@ -183,21 +180,21 @@ void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor gyro_noise_cov_ = Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal(); } -void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data, - const Eigen::VectorXd& _bias, - const Eigen::VectorXd& _model, - Eigen::VectorXd& _tangent, - Eigen::MatrixXd& _J_tangent_data, - Eigen::MatrixXd& _J_tangent_bias, - Eigen::MatrixXd& _J_tangent_model) const +void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _data, + const Eigen::VectorXd& _bias, + const Eigen::VectorXd& _model, + Eigen::VectorXd& _tangent, + Eigen::MatrixXd& _J_tangent_data, + Eigen::MatrixXd& _J_tangent_bias, + Eigen::MatrixXd& _J_tangent_model) const { - const Vector6d& awd = _data.segment<6>(0); // this is (a,w) of data in a 6-vector - const Vector3d& fd = _data.segment<3>(6); - const Vector3d& td = _data.segment<3>(9); - const Vector3d& c = _model.segment<3>(0); + const Vector6d& awd = _data.segment<6>(0); // this is (a,w) of data in a 6-vector + const Vector3d& fd = _data.segment<3>(6); + const Vector3d& td = _data.segment<3>(9); + const Vector3d& c = _model.segment<3>(0); const Matrix3d& fd_cross = skew(fd); - const Matrix3d& c_cross = skew(c); + const Matrix3d& c_cross = skew(c); // tangent(a,w) = data(a,w) - bias(a,w) // tangent(f) = data(f) @@ -215,17 +212,17 @@ void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::Vecto _J_tangent_bias.bottomRows<6>() = Matrix6d::Zero(); // J_tangent_model - _J_tangent_model.setZero(12,7); + _J_tangent_model.setZero(12, 7); _J_tangent_model.block<3, 3>(9, 0) = fd_cross; // J_tt_c = fd_cross } -void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jacobian_calib) const +void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jacobian_calib) const { // compute tangent by removing IMU bias @@ -424,41 +421,41 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen ////////////////////////////////////////////////////////////////////////////////////////////////////////////// } -void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _dt2, - Eigen::VectorXd& _delta1_plus_delta2) const +void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _dt2, + Eigen::VectorXd& _delta1_plus_delta2) const { bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2); } -void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _dt2, - Eigen::VectorXd& _delta1_plus_delta2, - Eigen::MatrixXd& _jacobian1, - Eigen::MatrixXd& _jacobian2) const +void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1, + const Eigen::VectorXd& _delta2, + const double _dt2, + Eigen::VectorXd& _delta1_plus_delta2, + Eigen::MatrixXd& _jacobian1, + Eigen::MatrixXd& _jacobian2) const { bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2); } -void ProcessorForceTorqueInertialPreintDynamics::statePlusDelta(const VectorComposite& _x, - const Eigen::VectorXd& _delta, - const double _dt, - VectorComposite& _x_plus_delta) const +void ProcessorForceTorqueInertialDynamics::statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _dt, + VectorComposite& _x_plus_delta) const { auto x = _x.vector("PVLO"); VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt); _x_plus_delta = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4}); } -Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::correctDelta(const Eigen::VectorXd& _delta_preint, - const Eigen::VectorXd& _delta_step) const +Eigen::VectorXd ProcessorForceTorqueInertialDynamics::correctDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta_step) const { return fti::plus(_delta_preint, _delta_step); } -VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const CaptureBaseConstPtr _capture) const +VectorXd ProcessorForceTorqueInertialDynamics::getCalibration(const CaptureBaseConstPtr _capture) const { if (_capture) { @@ -473,26 +470,25 @@ VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const Captur else { 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(); - const Vector1d& m_calib = getSensor()->getStateBlock('m')->getState(); + const Vector6d& I_calib = getSensor()->getStateBlockDynamic('I')->getState(); + const Vector3d& C_calib = getSensor()->getStateBlockDynamic('C')->getState(); + const Vector3d& i_calib = getSensor()->getStateBlockDynamic('i')->getState(); + const Vector1d& m_calib = getSensor()->getStateBlockDynamic('m')->getState(); calibration << I_calib, C_calib, i_calib, m_calib; return calibration; } } -bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const +bool ProcessorForceTorqueInertialDynamics::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_dynamics_->max_time_span) { WOLF_DEBUG("PM: vote: time span"); return true; } // buffer length - if (getBuffer().size() > params_force_torque_inertial_preint_dynamics_->max_buff_length) + if (getBuffer().size() > params_force_torque_inertial_dynamics_->max_buff_length) { WOLF_DEBUG("PM: vote: buffer length"); return true; @@ -503,14 +499,14 @@ bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const 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) + if (dist > params_force_torque_inertial_dynamics_->dist_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) + if (angle > params_force_torque_inertial_dynamics_->angle_turned) { WOLF_DEBUG("PM: vote: angle turned"); return true; @@ -524,6 +520,6 @@ bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const #include <core/processor/factory_processor.h> namespace wolf { -WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialPreintDynamics); -WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialPreintDynamics); +WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialDynamics); +WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialDynamics); } // namespace wolf \ No newline at end of file diff --git a/src/processor/processor_force_torque_inertial_preint.cpp b/src/processor/processor_force_torque_inertial_preint.cpp deleted file mode 100644 index 508be9dd558e32b11ff6ec151659562731bbb367..0000000000000000000000000000000000000000 --- a/src/processor/processor_force_torque_inertial_preint.cpp +++ /dev/null @@ -1,176 +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-------- -/* - * processor_preintegrator_force_torque_inertial.cpp - * - * Created on: Aug 19, 2021 - * Author: jsola - */ - -// bodydynamics -#include "bodydynamics/processor/processor_force_torque_inertial_preint.h" -#include "bodydynamics/factor/factor_force_torque_inertial.h" -#include "bodydynamics/math/force_torque_inertial_delta_tools.h" - -// wolf -#include <core/state_block/state_composite.h> -#include <core/capture/capture_motion.h> - -namespace wolf -{ - -ProcessorForceTorqueInertialPreint::ProcessorForceTorqueInertialPreint( - ParamsProcessorForceTorqueInertialPreintPtr _params_force_torque_inertial_preint) - : ProcessorMotion("ProcessorForceTorqueInertialPreint", - "POVL", - 3, // dim - 13, // state size [p, q, v, L] - 19, // delta size [pi, vi, pd, vd, L, q] - 18, // delta cov size - 12, // data size [a, w, f, t] - 6, // calib size [ab, wb] - _params_force_torque_inertial_preint), - params_force_torque_inertial_preint_(_params_force_torque_inertial_preint) -{ - // TODO Auto-generated constructor stub -} - -ProcessorForceTorqueInertialPreint::~ProcessorForceTorqueInertialPreint() -{ - // TODO Auto-generated destructor stub -} - -CaptureMotionPtr ProcessorForceTorqueInertialPreint::emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin_ptr) -{ - CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(_frame_own, "CaptureForceTorqueInertial", _ts, - _sensor, _data, _data_cov, _capture_origin_ptr); - setCalibration(capture, _calib); - capture->setCalibrationPreint(_calib_preint); - return capture; -} - -FeatureBasePtr ProcessorForceTorqueInertialPreint::emplaceFeature(CaptureMotionPtr _capture_own) -{ - FeatureBasePtr returnValue; - return returnValue; -} - -FactorBasePtr ProcessorForceTorqueInertialPreint::emplaceFactor(FeatureBasePtr _feature_motion, - CaptureBasePtr _capture_origin) -{ - FactorBasePtr returnValue; - return returnValue; -} - -void ProcessorForceTorqueInertialPreint::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) -{ - _capture->getStateBlock('I')->setState(_calibration); // Set IMU bias -} - -void ProcessorForceTorqueInertialPreint::configure(SensorBasePtr _sensor) -{ - sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor); -} - -void ProcessorForceTorqueInertialPreint::computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jacobian_calib) const -{ - // compute tangent by removing IMU bias - Matrix<double, 12, 1> tangent = _data; - tangent.head<6>() -= _calib; // J_tangent_data = Identity(12,12) - Matrix<double, 12, 6> J_tangent_calib; // J_tangent_calib = [-Identity(6,6) ; Zero(6,6)] - J_tangent_calib.topRows<6>() = -Matrix6d::Identity(); - J_tangent_calib.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 - 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(); - _jacobian_calib = J_delta_tangent * J_tangent_calib; -} - -void ProcessorForceTorqueInertialPreint::deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _dt2, - Eigen::VectorXd& _delta1_plus_delta2) const -{ - bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2); -} - -void ProcessorForceTorqueInertialPreint::deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _dt2, - Eigen::VectorXd& _delta1_plus_delta2, - Eigen::MatrixXd& _jacobian1, - Eigen::MatrixXd& _jacobian2) const -{ - bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2); -} - -void ProcessorForceTorqueInertialPreint::statePlusDelta(const VectorComposite& _x, - const Eigen::VectorXd& _delta, - const double _dt, - VectorComposite& _x_plus_delta) const -{ - auto x = _x.vector("PVLO"); - VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt); - _x_plus_delta = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4}); -} - -Eigen::VectorXd ProcessorForceTorqueInertialPreint::correctDelta(const Eigen::VectorXd& _delta_preint, - const Eigen::VectorXd& _delta_step) const -{ - return fti::plus(_delta_preint, _delta_step); -} - -VectorXd ProcessorForceTorqueInertialPreint::getCalibration(const CaptureBaseConstPtr _capture) const -{ - if (_capture) - return _capture->getStateBlock('I')->getState(); // IMU bias - else - return getSensor()->getStateBlock('I')->getState(); // IMU bias -} -} /* namespace wolf */ - -#include <core/processor/factory_processor.h> -namespace wolf -{ -WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialPreint); -WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialPreint); -} // namespace wolf \ No newline at end of file diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp deleted file mode 100644 index 1b84f786077a0f65ee3cc944bf8ebe4f54511499..0000000000000000000000000000000000000000 --- a/src/processor/processor_force_torque_preint.cpp +++ /dev/null @@ -1,272 +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-------- -// wolf -#include "bodydynamics/math/force_torque_delta_tools.h" -#include "bodydynamics/capture/capture_force_torque_preint.h" -#include "bodydynamics/feature/feature_force_torque_preint.h" -#include "bodydynamics/processor/processor_force_torque_preint.h" -#include "bodydynamics/factor/factor_force_torque_preint.h" - -namespace wolf { - -int compute_data_size(int nbc, int dimc){ - // compute the size of the data/body vectors used by processorMotion API depending - // on the number of contacts (nbc) and contact dimension (dimc) - // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations - return nbc*dimc + 3 + 3 + nbc*3 + nbc*4; -} - -using namespace Eigen; - -ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint) : - ProcessorMotion("ProcessorForceTorquePreint", "CDLO", 3, 13, 13, 12, - compute_data_size(_params_motion_force_torque_preint->nbc, _params_motion_force_torque_preint->dimc), - 6, _params_motion_force_torque_preint), - params_motion_force_torque_preint_(std::make_shared<ParamsProcessorForceTorquePreint>(*_params_motion_force_torque_preint)), - nbc_(_params_motion_force_torque_preint->nbc), - dimc_(_params_motion_force_torque_preint->dimc) - -{ - // -} - -ProcessorForceTorquePreint::~ProcessorForceTorquePreint() -{ - // -} - -bool ProcessorForceTorquePreint::voteForKeyFrame() const -{ - // time span - if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_preint_->max_time_span) - { - WOLF_DEBUG( "PM: vote: time span" ); - return true; - } - // buffer length - if (getBuffer().size() > params_motion_force_torque_preint_->max_buff_length) - { - WOLF_DEBUG( "PM: vote: buffer length" ); - return true; - } - - // Some other measure of movement? - - //WOLF_DEBUG( "PM: do not vote" ); - return false; -} - - -CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin) -{ - - // Here we have to retrieve the origin capture no - // !! PROBLEM: - // when doing setOrigin, emplaceCapture is called 2 times - // - first on the first KF (usually created by setPrior), this one contains the biases - // - secondly on the inner frame (last) which does not contains other captures - auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_); - auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_); - if ((capture_ikin == nullptr) || (capture_angvel == nullptr)){ - // We have to retrieve the bias from a former Keyframe: origin - capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_); - capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_); - } - auto cap = CaptureBase::emplace<CaptureForceTorquePreint>( - _frame_own, - _ts, - _sensor, - std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin), - std::static_pointer_cast<CaptureMotion>(capture_angvel), - _data, - _data_cov, - _capture_origin); - - // Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_ - // which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures - - auto cap_ftpreint = std::static_pointer_cast<CaptureForceTorquePreint>(cap); - Vector6d calib = getCalibration(cap_ftpreint); - setCalibration(cap_ftpreint, calib); - cap_ftpreint->setCalibrationPreint(calib); - - return cap; -} - -FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capture_motion) -{ - // Retrieving the origin capture and getting the bias from here should be done here? - auto feature = FeatureBase::emplace<FeatureForceTorquePreint>(_capture_motion, - _capture_motion->getBuffer().back().delta_integr_, - _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_, - _capture_motion->getCalibrationPreint(), - _capture_motion->getBuffer().back().jacobian_calib_); - return feature; -} - -Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd& delta_preint, - const Eigen::VectorXd& delta_step) const -{ - return bodydynamics::plus(delta_preint, delta_step); -} - -VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBaseConstPtr _capture) const -{ - - VectorXd bias_vec(6); - - if (_capture) // access from capture is quicker - { - auto cap_ft(std::static_pointer_cast<const CaptureForceTorquePreint>(_capture)); - - // get calib part from Ikin capture - bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState(); - - // get calib part from IMU capture - bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias] - } - else // access from sensor is slower - { - // get calib part from Ikin capture - bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState(); - - // get calib part from IMU capture - bias_vec.segment<3>(3) = sensor_angvel_->getIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias] - } - return bias_vec; -} - -void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration) -{ - CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture)); - - // set calib part in Ikin capture - cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->setState(_calibration.head<3>()); - - // set calib part in IMU capture - Vector6d calib_imu = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState(); - calib_imu.tail<3>() = _calibration.tail<3>(); - - cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu); -} - -FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) -{ - CaptureForceTorquePreintPtr cap_ftpreint_origin = std::static_pointer_cast<CaptureForceTorquePreint>(_capture_origin); - FeatureForceTorquePreintPtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorquePreint>(_feature_motion); - CaptureForceTorquePreintPtr cap_ftpreint_new = std::static_pointer_cast<CaptureForceTorquePreint>(ftr_ftpreint->getCapture()); - - auto fac_ftpreint = FactorBase::emplace<FactorForceTorquePreint>( - ftr_ftpreint, - ftr_ftpreint, - cap_ftpreint_origin, - shared_from_this(), - cap_ftpreint_origin->getIkinCaptureOther(), // to retrieve sb_bp1 - cap_ftpreint_origin->getGyroCaptureOther(), // to retrieve sb_w1 - false); - - return fac_ftpreint; -} - -void ProcessorForceTorquePreint::computeCurrentDelta( - const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jac_delta_calib) const -{ - assert(_data.size() == data_size_ && "Wrong data size!"); - - // create delta - MatrixXd jac_body_bias(data_size_-nbc_,6); - VectorXd body(data_size_); - bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias); - - MatrixXd jac_delta_body(12,data_size_-nbc_); - bodydynamics::body2delta(body, _dt, std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(), - nbc_, dimc_, - _delta, jac_delta_body); // Jacobians tested in bodydynamics_tools - - // [f], [tau], pbc, wb - MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_*dimc_+6); - - // compute delta_cov (using uncertainty on f,tau,pbc) - _delta_cov = jac_delta_body_reduced * _data_cov * jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity - // _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose(); // trivially: jac_body_delta = Identity - - // compute jacobian_calib - _jac_delta_calib = jac_delta_body * jac_body_bias; -} - -void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, - const Eigen::VectorXd& _delta, - const double _dt, - Eigen::VectorXd& _delta_preint_plus_delta) const -{ - _delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt); -} - -void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x, - const Eigen::VectorXd& _delta, - const double _dt, - VectorComposite& _x_plus_delta) const -{ - assert(_delta.size() == 13 && "Wrong _delta vector size"); - assert(_dt >= 0 && "Time interval _dt is negative!"); - - // verbose way : create Eigen vectors, then compute, then convert back to Composite - - auto x = _x.vector("CDLO"); - - VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt); - - _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3,3,3,4}); -} - -void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, - const Eigen::VectorXd& _delta, - const double _dt, - Eigen::VectorXd& _delta_preint_plus_delta, - Eigen::MatrixXd& _jacobian_delta_preint, - Eigen::MatrixXd& _jacobian_delta) const -{ - bodydynamics::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in bodydynamics_tools -} - -} // namespace wolf - -// Register in the FactorySensor -#include "core/processor/factory_processor.h" - -namespace wolf -{ -WOLF_REGISTER_PROCESSOR(ProcessorForceTorquePreint) -} diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp index 89fac3f9749e224738807467384d8aa837571638..aa38e8ab62d4bd0bdc0fee48bd15c227ea52fc8d 100644 --- a/src/sensor/sensor_force_torque_inertial.cpp +++ b/src/sensor/sensor_force_torque_inertial.cpp @@ -41,14 +41,13 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd& false), params_fti_(_params) { - addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true)); // centre of mass + 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 @@ -76,7 +75,8 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite& } // namespace wolf #include <core/sensor/factory_sensor.h> -namespace wolf{ +namespace wolf +{ WOLF_REGISTER_SENSOR(SensorForceTorqueInertial); WOLF_REGISTER_SENSOR_AUTO(SensorForceTorqueInertial); -} \ No newline at end of file +} // namespace wolf \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 425d6f1291d02b80f53ced0bcbe614bc88c402c1..52e1fd976f3619fc9b303b7c9ef601c740d10677 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) @@ -27,11 +25,11 @@ wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp wolf_add_gtest(gtest_force_torque_inertial_delta_tools gtest_force_torque_inertial_delta_tools.cpp) # TODO: revive those -# wolf_add_gtest(gtest_feature_force_torque_preint gtest_feature_force_torque_preint.cpp) +# wolf_add_gtest(gtest_feature_force_torque gtest_feature_force_torque.cpp) # wolf_add_gtest(gtest_processor_inertial_kinematics gtest_processor_inertial_kinematics.cpp) -# wolf_add_gtest(gtest_processor_force_torque_preint gtest_processor_force_torque_preint.cpp) +# wolf_add_gtest(gtest_processor_force_torque gtest_processor_force_torque.cpp) -wolf_add_gtest(gtest_processor_force_torque_inertial_preint_dynamics gtest_processor_force_torque_inertial_preint_dynamics.cpp) +wolf_add_gtest(gtest_processor_force_torque_inertial_dynamics gtest_processor_force_torque_inertial_dynamics.cpp) wolf_add_gtest(gtest_processor_point_feet_nomove gtest_processor_point_feet_nomove.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(); -} diff --git a/test/gtest_factor_force_torque_inertial.cpp b/test/gtest_factor_force_torque_inertial.cpp index 489cb9df7ccde81c0e3f24731b260cc53d88dda6..72e5f59675df274e4387876ae644c081a61798ab 100644 --- a/test/gtest_factor_force_torque_inertial.cpp +++ b/test/gtest_factor_force_torque_inertial.cpp @@ -21,7 +21,7 @@ //--------LICENSE_END-------- #include "bodydynamics/factor/factor_force_torque_inertial.h" -#include "bodydynamics/processor/processor_force_torque_inertial_preint.h" +#include "bodydynamics/processor/processor_force_torque_inertial.h" #include "bodydynamics/sensor/sensor_force_torque_inertial.h" #include "bodydynamics/internal/config.h" @@ -46,7 +46,7 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test public: ProblemPtr problem; SensorForceTorqueInertialPtr sensor; - ProcessorForceTorqueInertialPreintPtr processor; + ProcessorForceTorqueInertialPtr processor; FrameBasePtr frame_origin, frame_last, frame; CaptureMotionPtr capture_origin, capture_last, capture; FeatureMotionPtr feature; @@ -71,7 +71,7 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test problem = Problem::autoSetup(server); sensor = std::static_pointer_cast<SensorForceTorqueInertial>(problem->getHardware()->getSensorList().front()); - processor = std::static_pointer_cast<ProcessorForceTorqueInertialPreint>(sensor->getProcessorList().front()); + processor = std::static_pointer_cast<ProcessorForceTorqueInertial>(sensor->getProcessorList().front()); problem->print(4, 1, 1, 1); diff --git a/test/gtest_factor_force_torque_inertial_dynamics.cpp b/test/gtest_factor_force_torque_inertial_dynamics.cpp index a0a659fa636a0337a570c93f57d819b22f5c9ce5..846807390641df274b1b0d9cf8542183e6948067 100644 --- a/test/gtest_factor_force_torque_inertial_dynamics.cpp +++ b/test/gtest_factor_force_torque_inertial_dynamics.cpp @@ -21,7 +21,7 @@ //--------LICENSE_END-------- #include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h" -#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h" +#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h" #include "bodydynamics/sensor/sensor_force_torque_inertial.h" #include "bodydynamics/internal/config.h" @@ -47,7 +47,7 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test public: ProblemPtr P; SensorForceTorqueInertialPtr S; - ProcessorForceTorqueInertialPreintDynamicsPtr p; + ProcessorForceTorqueInertialDynamicsPtr p; FrameBasePtr F0, F1, F; CaptureMotionPtr C0, C1, C; FeatureMotionPtr f1; @@ -69,7 +69,7 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test P = Problem::autoSetup(server); S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front()); - p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front()); + p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front()); data = VectorXd::Zero(12); // [ a, w, f, t ] data_cov = MatrixXd::Identity(12, 12); @@ -120,7 +120,7 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test public: ProblemPtr P; SensorForceTorqueInertialPtr S; - ProcessorForceTorqueInertialPreintDynamicsPtr p; + ProcessorForceTorqueInertialDynamicsPtr p; FrameBasePtr F0, F1; CaptureMotionPtr C0, C1; FeatureMotionPtr f1; @@ -150,8 +150,8 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test S->setStateBlockDynamic('I', true); // crear processador - auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialPreintDynamics>(); - p = ProcessorBase::emplace<ProcessorForceTorqueInertialPreintDynamics>(S, params_processor); + auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialDynamics>(); + p = ProcessorBase::emplace<ProcessorForceTorqueInertialDynamics>(S, params_processor); // crear dos frame VectorXd state(13); diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque.cpp similarity index 96% rename from test/gtest_processor_force_torque_preint.cpp rename to test/gtest_processor_force_torque.cpp index 3465ea6187358cffe667af6a2dd326d279e31e47..b00181b9bd356dbabce6e2d45c0d36a8626ac497 100644 --- a/test/gtest_processor_force_torque_preint.cpp +++ b/test/gtest_processor_force_torque.cpp @@ -21,7 +21,7 @@ //--------LICENSE_END-------- /** - * \file gtest_factor_force_torque_preint.cpp + * \file gtest_factor_force_torque.cpp * * Created on: March 20, 2020 * \author: Mederic Fourmy @@ -57,9 +57,9 @@ #include "bodydynamics/sensor/sensor_inertial_kinematics.h" #include "bodydynamics/sensor/sensor_force_torque.h" #include "bodydynamics/capture/capture_inertial_kinematics.h" -#include "bodydynamics/capture/capture_force_torque_preint.h" +#include "bodydynamics/capture/capture_force_torque.h" #include "bodydynamics/processor/processor_inertial_kinematics.h" -#include "bodydynamics/processor/processor_force_torque_preint.h" +#include "bodydynamics/processor/processor_force_torque.h" #include <Eigen/Eigenvalues> @@ -81,7 +81,7 @@ const Vector3d PBCY = {0, -0.1, 0}; // Y axis offset const Vector3d PBCZ = {0, 0, -0.1}; // Z axis offset -class ProcessorForceTorquePreintImuOdom3dIkin2KF : public testing::Test +class ProcessorForceTorqueImuOdom3dIkin2KF : public testing::Test { public: wolf::TimeStamp t0_; @@ -92,7 +92,7 @@ public: SensorForceTorquePtr sen_ft_; ProcessorImuPtr proc_imu_; ProcessorInertialKinematicsPtr proc_inkin_; - ProcessorForceTorquePreintPtr proc_ftpreint_; + ProcessorForceTorquePtr proc_ft_; FrameBasePtr KF1_; void SetUp() override @@ -138,7 +138,7 @@ public: SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft); // SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml"); sen_ft_ = std::static_pointer_cast<SensorForceTorque>(sen_ft_base); - ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>(); + ParamsProcessorForceTorquePtr params_sen_ft = std::make_shared<ParamsProcessorForceTorque>(); params_sen_ft->sensor_ikin_name = "SenIK"; params_sen_ft->sensor_angvel_name = "SenImu"; params_sen_ft->nbc = 2; @@ -150,16 +150,16 @@ public: params_sen_ft->dist_traveled = 20000.0; params_sen_ft->angle_turned = 1000; params_sen_ft->voting_active = true; - ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft_, params_sen_ft); - // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint"); - proc_ftpreint_ = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base); + ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", sen_ft_, params_sen_ft); + // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint"); + proc_ft_ = std::static_pointer_cast<ProcessorForceTorque>(proc_ft_base); } void TearDown() override {} }; -class Cst2KFZeroMotion : public ProcessorForceTorquePreintImuOdom3dIkin2KF +class Cst2KFZeroMotion : public ProcessorForceTorqueImuOdom3dIkin2KF { public: FrameBasePtr KF2_; @@ -212,7 +212,7 @@ public: void SetUp() override { - ProcessorForceTorquePreintImuOdom3dIkin2KF::SetUp(); + ProcessorForceTorqueImuOdom3dIkin2KF::SetUp(); t0_.set(0.0); TimeStamp t1; t1.set(1*DT); TimeStamp t2; t2.set(2*DT); @@ -246,14 +246,14 @@ public: proc_imu_->setOrigin(KF1_); auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0_, sen_ikin_, meas_ikin_, Iq_, Lq_); CIKin0->process(); - proc_ftpreint_->setOrigin(KF1_); + proc_ft_->setOrigin(KF1_); // T1 CaptureImuPtr CImu1 = std::make_shared<CaptureImu>(t1, sen_imu_, acc_gyr_meas_, acc_gyr_cov_); CImu1->process(); auto CIKin1 = std::make_shared<CaptureInertialKinematics>(t1, sen_ikin_, meas_ikin_, Iq_, Lq_); CIKin1->process(); - auto CFTpreint1 = std::make_shared<CaptureForceTorquePreint>(t1, sen_ft_, CIKin1, CImu1, cap_ftp_data_, Qftp_); + auto CFTpreint1 = std::make_shared<CaptureForceTorque>(t1, sen_ft_, CIKin1, CImu1, cap_ftp_data_, Qftp_); CFTpreint1->process(); @@ -265,7 +265,7 @@ public: CImu2->process(); auto CIKin2 = std::make_shared<CaptureInertialKinematics>(t2, sen_ikin_, meas_ikin_, Iq_, Lq_); CIKin2->process(); - auto CFTpreint2 = std::make_shared<CaptureForceTorquePreint>(t2, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_); + auto CFTpreint2 = std::make_shared<CaptureForceTorque>(t2, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_); CFTpreint2->process(); changeForData3(); @@ -275,7 +275,7 @@ public: CImu3->process(); CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin_, Iq_, Lq_); CIKin3->process(); - auto CFTpreint3 = std::make_shared<CaptureForceTorquePreint>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_); + auto CFTpreint3 = std::make_shared<CaptureForceTorque>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_); CFTpreint3->process(); // T4, just here to make sure the KF at t3 is created @@ -283,7 +283,7 @@ public: CImu4->process(); CaptureInertialKinematicsPtr CIKin4 = std::make_shared<CaptureInertialKinematics>(t4, sen_ikin_, meas_ikin_, Iq_, Lq_); CIKin4->process(); - auto CFTpreint4 = std::make_shared<CaptureForceTorquePreint>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_); + auto CFTpreint4 = std::make_shared<CaptureForceTorque>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_); CFTpreint4->process(); ///////////////////////////////////////////// diff --git a/test/gtest_processor_force_torque_inertial_preint.cpp b/test/gtest_processor_force_torque_inertial.cpp similarity index 96% rename from test/gtest_processor_force_torque_inertial_preint.cpp rename to test/gtest_processor_force_torque_inertial.cpp index 38910f16f75a89f70432c4d67ec1e9e0d60403a9..b8be3e23b05e5c64f1a63ad0e36c9c3559f81e2c 100644 --- a/test/gtest_processor_force_torque_inertial_preint.cpp +++ b/test/gtest_processor_force_torque_inertial.cpp @@ -20,13 +20,13 @@ // //--------LICENSE_END-------- /* - * gtest_processor_preintegrator_force_torque_inertial.cpp + * gtest_processor_force_torque_inertial.cpp * * Created on: Aug 19, 2021 * Author: jsola */ -#include "bodydynamics/processor/processor_force_torque_inertial_preint.h" +#include "bodydynamics/processor/processor_force_torque_inertial.h" #include "core/utils/utils_gtest.h" #include "core/utils/logging.h" diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_dynamics.cpp similarity index 94% rename from test/gtest_processor_force_torque_inertial_preint_dynamics.cpp rename to test/gtest_processor_force_torque_inertial_dynamics.cpp index daa10f8db883230ad842e915b7f2882e02f59dc0..25df6c6d1d00683e5a77bc16f1a90511f41d991a 100644 --- a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp +++ b/test/gtest_processor_force_torque_inertial_dynamics.cpp @@ -21,7 +21,7 @@ //--------LICENSE_END-------- #include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h" -#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h" +#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h" #include "bodydynamics/sensor/sensor_force_torque_inertial.h" #include "bodydynamics/internal/config.h" @@ -42,12 +42,12 @@ using namespace bodydynamics::fti; WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d); -class Test_ProcessorForceTorqueInertialPreintDynamics_yaml : public testing::Test +class Test_ProcessorForceTorqueInertialDynamics_yaml : public testing::Test { public: ProblemPtr P; SensorForceTorqueInertialPtr S; - ProcessorForceTorqueInertialPreintDynamicsPtr p; + ProcessorForceTorqueInertialDynamicsPtr p; protected: void SetUp() override @@ -58,17 +58,17 @@ class Test_ProcessorForceTorqueInertialPreintDynamics_yaml : public testing::Tes P = Problem::autoSetup(server); S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front()); - p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front()); + p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front()); } }; -TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, force_registraion) +TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, force_registraion) { FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create); } // Test to see if the processor works (yaml files). Here the dron is not moving -TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test) +TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, not_moving_test) { VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ] data.segment<3>(0) = - gravity(); @@ -137,7 +137,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test) } //Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction -TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test__com_zero) +TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, 1m_x_moving_test__com_zero) { VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ] data.segment<3>(0) = -gravity(); @@ -205,7 +205,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test__c } //Test to see if the voteForKeyFrame works (distance traveled) -TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_dist) +TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_dist) { VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ] data.segment<3>(0) = -gravity(); @@ -231,7 +231,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_dis } //Test to see if the voteForKeyFrame works (buffer) -TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_buffer) +TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_buffer) { VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ] data.segment<3>(0) = -gravity(); @@ -260,6 +260,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.not_moving_test"; + // ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialDynamics_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 index 3c9ef6531c8608f91cca6818deed2fed6e64ad9b..20e26c06fdee4f8fd03e49b74c201107b1583e36 100644 --- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp +++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp @@ -21,9 +21,10 @@ //--------LICENSE_END-------- #include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h" -#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h" +#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> @@ -43,16 +44,16 @@ using namespace bodydynamics::fti; WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d); -class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::Test +class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test { public: - ProblemPtr P; - SensorForceTorqueInertialPtr S; - ProcessorForceTorqueInertialPreintDynamicsPtr p; - SolverCeresPtr solver; - Vector3d cdm_true; - Vector3d inertia_true; - double mass_true; + ProblemPtr P; + SensorForceTorqueInertialPtr S; + ProcessorForceTorqueInertialDynamicsPtr p; + SolverCeresPtr solver; + Vector3d cdm_true; + Vector3d inertia_true; + double mass_true; protected: void SetUp() override @@ -70,7 +71,7 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing:: // solver->getSolverOptions().parameter_tolerance = 1e-15; S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front()); - p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front()); + p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front()); cdm_true = {0, 0, 0.0341}; inertia_true = {0.017598, 0.017957, 0.029599}; @@ -78,13 +79,13 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing:: } }; -TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, force_registraion) +TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, force_registraion) { FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create); } // Hover test. -TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hovering) +TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering) { double mass_guess = S->getStateBlock('m')->getState()(0); @@ -101,14 +102,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri 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_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri 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); @@ -152,7 +152,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3); } -TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hovering) +TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering) { S->getStateBlock('C')->setState(Vector3d(0.01, 0.01, 0.033)); S->getStateBlock('m')->setState(Vector1d(mass_true)); @@ -172,12 +172,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin 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_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin 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(); @@ -220,7 +219,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin ASSERT_MATRIX_APPROX(S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering } -TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering) +TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering) { S->getStateBlock('C')->setState(Vector3d(0.005, 0.005, 0.05)); S->getStateBlock('m')->setState(Vector1d(1.900)); @@ -240,13 +239,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov 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_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov // 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 @@ -307,6 +307,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.cdm_only_hovering"; + // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.cdm_only_hovering"; return RUN_ALL_TESTS(); } diff --git a/test/yaml/problem_force_torque_inertial.yaml b/test/yaml/problem_force_torque_inertial.yaml index 2082a65ad6580b58d0e234a9f9594fd615646010..e2ae117aee579fb6f444cce6f378b2f1c673e739 100644 --- a/test/yaml/problem_force_torque_inertial.yaml +++ b/test/yaml/problem_force_torque_inertial.yaml @@ -32,8 +32,8 @@ config: processors: - name: "proc FTI" - type: "ProcessorForceTorqueInertialPreint" + type: "ProcessorForceTorqueInertial" sensor_name: "sensor FTI" plugin: "bodydynamics" - follow: "processor_force_torque_inertial_preint.yaml" + follow: "processor_force_torque_inertial.yaml" \ No newline at end of file diff --git a/test/yaml/problem_force_torque_inertial_dynamics.yaml b/test/yaml/problem_force_torque_inertial_dynamics.yaml index 2d9b19cc19d0519737336367906b1ee1e2621f88..207b69bba5597a63cc41167fbe7a3106ffa4606a 100644 --- a/test/yaml/problem_force_torque_inertial_dynamics.yaml +++ b/test/yaml/problem_force_torque_inertial_dynamics.yaml @@ -32,8 +32,8 @@ config: processors: - name: "proc FTID" - type: "ProcessorForceTorqueInertialPreintDynamics" + type: "ProcessorForceTorqueInertialDynamics" sensor_name: "sensor FTI" plugin: "bodydynamics" - follow: "processor_force_torque_inertial_preint_dynamics.yaml" + follow: "processor_force_torque_inertial_dynamics.yaml" \ No newline at end of file diff --git a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml index 5ef4ecbe3e2c57c493d962ef0d80a72b5d94ff2e..233368f38accb87f4bbd4500e37bede35dd1378a 100644 --- a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml +++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml @@ -41,7 +41,7 @@ config: processors: - name: "proc FTID" - type: "ProcessorForceTorqueInertialPreintDynamics" + type: "ProcessorForceTorqueInertialDynamics" sensor_name: "sensor FTI" plugin: "bodydynamics" time_tolerance: 0.1 diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml index 182966b1bea83384e4f0fd498e98f5970944cade..8eb8204126f94bc008bcb01587ae2fa5482fec6b 100644 --- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml +++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml @@ -48,7 +48,7 @@ config: processors: - name: "proc FTID" - type: "ProcessorForceTorqueInertialPreintDynamics" + type: "ProcessorForceTorqueInertialDynamics" sensor_name: "sensor FTI" plugin: "bodydynamics" time_tolerance: 0.1 diff --git a/test/yaml/processor_force_torque_inertial_preint.yaml b/test/yaml/processor_force_torque_inertial.yaml similarity index 100% rename from test/yaml/processor_force_torque_inertial_preint.yaml rename to test/yaml/processor_force_torque_inertial.yaml diff --git a/test/yaml/processor_force_torque_inertial_preint_dynamics.yaml b/test/yaml/processor_force_torque_inertial_dynamics.yaml similarity index 100% rename from test/yaml/processor_force_torque_inertial_preint_dynamics.yaml rename to test/yaml/processor_force_torque_inertial_dynamics.yaml