diff --git a/CMakeLists.txt b/CMakeLists.txt index c2538cb479c738ed6becd5a484ed21ecd3c88e2f..28f0be854b2846e26cc22582a5bcd3c27a087f05 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -113,6 +113,7 @@ SET(HDRS_FACTOR include/${PROJECT_NAME}/factor/factor_angular_momentum.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_inertial_dynamics_external_force.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 diff --git a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics_external_force.h b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics_external_force.h new file mode 100644 index 0000000000000000000000000000000000000000..49e76cf3af23c2819c67f6f670a9fcf0d03cc578 --- /dev/null +++ b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics_external_force.h @@ -0,0 +1,288 @@ +//--------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-------- +#pragma once + +#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(FactorForceTorqueInertialDynamicsExternalForce); + +/** + * @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 + * 'V' velocity + * 'O' orientation + * 'L' angular momentum + * - Capture 1 (origin) + * 'I' IMU biases + * - Frame 2 + * 'P' position + * 'V' velocity + * 'O' orientation + * 'L' angular momentum + * - Sensor Force Torque Inertial + * 'C' position of the center of mass + * 'i' inertia matrix components (we are assuming that it is a diagonal matrix) + * 'm' mass + * 'f' external force (global coordinates) applied to the center of mass + * + * The residual computed has the following components, in this order + * - position delta error according to IMU preintegration + * - velocity delta error according to IMU preintegration + * - position delta error according to FT preintegration + * - velocity delta error according to FT preintegration + * - angular momentum delta error according to FT preintegration + * - orientation delta error according to IMU preintegration + */ +class FactorForceTorqueInertialDynamicsExternalForce + : public FactorAutodiff<FactorForceTorqueInertialDynamicsExternalForce, 18, // residual + 3, 4, 3, 3, 6, // POVLB + 3, 4, 3, 3, // POVL + 3, 3, 1, 3> // Cimf +{ + public: + FactorForceTorqueInertialDynamicsExternalForce(const FeatureMotionPtr& _ftr, // gets measurement and access to parents + const CaptureBasePtr& _capture_origin, // gets biases + const ProcessorBasePtr& _processor, // gets access to processor and sensor + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE); + + ~FactorForceTorqueInertialDynamicsExternalForce() override = default; + + template <typename T> + bool operator()(const T* const _p1, // position 1 + const T* const _q1, // orientation + const T* const _v1, // velocity + const T* const _L1, // ang momentum + const T* const _b1, // IMU bias (acc and gyro) + const T* const _p2, // position 2 + const T* const _q2, // orientation + const T* const _v2, // velocity + const T* const _L2, // ang momentum + const T* const _C, // center of mass + const T* const _i, // inertia matrix + const T* const _m, // mass + const T* const _f, // external force + T* _res) const; // residual + + template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9> + bool residual(const MatrixBase<D1>& _p1, + const QuaternionBase<D4>& _q1, + const MatrixBase<D1>& _v1, + const MatrixBase<D1>& _L1, + const MatrixBase<D2>& _b1, + const MatrixBase<D3>& _p2, + const QuaternionBase<D5>& _q2, + const MatrixBase<D3>& _v2, + const MatrixBase<D3>& _L2, + const MatrixBase<D6>& _C, + const MatrixBase<D6>& _i, + const D7& _m, + const MatrixBase<D8>& _f, + MatrixBase<D9>& _res) const; + + VectorXd residual() const; + + VectorXd getCalibPreint() const + { + return calib_preint_; + } + + private: + double dt_; + Matrix<double, 19, 1> delta_preint_; + Matrix<double, 13, 1> calib_preint_; + Matrix<double, 18, 13> J_delta_calib_; + Matrix<double, 18, 18> sqrt_info_upper_; +}; + +inline FactorForceTorqueInertialDynamicsExternalForce::FactorForceTorqueInertialDynamicsExternalForce(const FeatureMotionPtr& _ftr, + const CaptureBasePtr& _capture_origin, + const ProcessorBasePtr& _processor, + bool _apply_loss_function, + FactorStatus _status) + : FactorAutodiff<FactorForceTorqueInertialDynamicsExternalForce, 18, 3, 4, 3, 3, 6, 3, 4, 3, 3, 3, 3, 1, 3>( + "FactorForceTorqueInertialDynamicsExternalForce", + TOP_MOTION, + _ftr, // parent feature + _capture_origin->getFrame(), // frame other + _capture_origin, // capture other + nullptr, // feature other + nullptr, // landmark other + _processor, // processor + _apply_loss_function, + _status, + _capture_origin->getFrame()->getStateBlock('P'), // previous frame P + _capture_origin->getFrame()->getStateBlock('O'), // previous frame O + _capture_origin->getFrame()->getStateBlock('V'), // previous frame V + _capture_origin->getFrame()->getStateBlock('L'), // previous frame L + _capture_origin->getStateBlock('I'), // previous frame IMU bias + _ftr->getFrame()->getStateBlock('P'), // current frame P + _ftr->getFrame()->getStateBlock('O'), // current frame O + _ftr->getFrame()->getStateBlock('V'), // current frame V + _ftr->getFrame()->getStateBlock('L'), // current frame L + _ftr->getCapture()->getStateBlock('C'), // sensor C + _ftr->getCapture()->getStateBlock('i'), // sensor i + _ftr->getCapture()->getStateBlock('m'), // sensor m + _ftr->getCapture()->getStateBlock('f')), // sensor f + dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()), + delta_preint_(_ftr->getDeltaPreint()), + calib_preint_(_ftr->getCalibrationPreint()), + J_delta_calib_(_ftr->getJacobianCalibration()), + sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper()) +{ + // +} + +template <typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9> +inline bool FactorForceTorqueInertialDynamicsExternalForce::residual(const MatrixBase<D1>& _p1, + const QuaternionBase<D4>& _q1, + const MatrixBase<D1>& _v1, + const MatrixBase<D1>& _L1, + const MatrixBase<D2>& _b1, + const MatrixBase<D3>& _p2, + const QuaternionBase<D5>& _q2, + const MatrixBase<D3>& _v2, + const MatrixBase<D3>& _L2, + const MatrixBase<D6>& _C, + const MatrixBase<D6>& _i, + const D7& _m, + const MatrixBase<D8>& _f, + MatrixBase<D9>& _res) const +{ + MatrixSizeCheck<18, 1>::check(_res); + + typedef typename D4::Scalar T; + typedef Map<Matrix<T, 3, 1>> Matrix3t; + + /* Will do the following: + * + * Compute estimated delta, using betweenStatesGlobalAcceleration() + * d_est = x2 (-) x1 + * + * Compute correction step according to bias change, this is a linear operation + * d_step = J_delta_calib * (calib - bias_preint) + * + * Correct preintegrated delta with new bias, using plus() + * d_corr = d_preint (+) d_step + * + * Compute error, using diff() + * d_err = d_corr (-) d_est + * + * Compute residual + * res = W * d_err , with W the sqrt of the info matrix. + */ + + /* WARNING -- order of state blocks might be confusing!! + * Frame blocks are ordered as follows: "POVL" -- pos, ori, vel, ang_momentum + * Factor blocks are ordered as follows: "POVL-B-POVL" -- B is IMU bias + * Delta blocks are ordered as follows: "PVpvLO" -- where "P,V,O" are from IMU, and "p,v,L" from FT + */ + Matrix<T, 19, 1> delta_est; // delta_est = x2 (-) x1, computed with betweenStates(x1,x2,dt) + Matrix3t dpi(&delta_est(0)); // 'P' + Matrix3t dvi(&delta_est(3)); // 'V' + Matrix3t dpd(&delta_est(6)); // 'p' + Matrix3t dvd(&delta_est(9)); // 'v' + Matrix3t dL(&delta_est(12)); // 'L' + Map<Quaternion<T>> dq(&delta_est(15)); // 'O' + fti::betweenStatesGlobalAcceleration(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, _m * _f - gravity(), dt_, dpi, dvi, dpd, dvd, dL, dq); + + Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>(); + Matrix<T, 13, 1> calib; + calib << _b1, _C, _i, _m; + Matrix<T, 18, 1> delta_step = J_delta_calib_ * (calib - calib_preint_); + 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; +} + +inline VectorXd FactorForceTorqueInertialDynamicsExternalForce::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()->getStateBlock('C')->getState(); + Vector3d i = getCapture()->getStateBlock('i')->getState(); + double m = getCapture()->getStateBlock('m')->getState()(0); + Vector3d f = getCapture()->getStateBlock('f')->getState(); + + residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, f, res); + return res; +} + +template <typename T> +inline bool FactorForceTorqueInertialDynamicsExternalForce::operator()(const T* const _p1, + const T* const _q1, + const T* const _v1, + const T* const _L1, + const T* const _b1, + const T* const _p2, + const T* const _q2, + const T* const _v2, + const T* const _L2, + const T* const _C, + const T* const _i, + const T* const _m, + const T* const _f, + T* _res) const +{ + Map<const Matrix<T, 3, 1>> p1(_p1); + Map<const Quaternion<T>> q1(_q1); + Map<const Matrix<T, 3, 1>> v1(_v1); + Map<const Matrix<T, 3, 1>> L1(_L1); + Map<const Matrix<T, 6, 1>> b1(_b1); + Map<const Matrix<T, 3, 1>> p2(_p2); + Map<const Quaternion<T>> q2(_q2); + Map<const Matrix<T, 3, 1>> v2(_v2); + Map<const Matrix<T, 3, 1>> L2(_L2); + Map<const Matrix<T, 3, 1>> C(_C); + Map<const Matrix<T, 3, 1>> i(_i); + const T& m = *_m; + Map<const Matrix<T, 3, 1>> f(_f); + Map<Matrix<T, 18, 1>> res(_res); + + residual(p1, q1, v1, L1, b1, p2, q2, v2, L2, C, i, m, f, res); + + return true; +} + +} // namespace wolf \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index c6d84c19a4e3c5bf6140d5ae057524ad32cffd3c..737fbdf4ee284f8be5f86b4b33f19d19e9c510d9 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -20,6 +20,8 @@ wolf_add_gtest(gtest_factor_force_torque_inertial gtest_factor_force_torque_iner wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics gtest_factor_force_torque_inertial_dynamics.cpp) +wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics_external_force gtest_factor_force_torque_inertial_dynamics_external_force.cpp) + 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) diff --git a/test/gtest_factor_force_torque_inertial_dynamics_external_force.cpp b/test/gtest_factor_force_torque_inertial_dynamics_external_force.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d77b648e2cc6a9e52fdf5066d1eb381d7f5f96a0 --- /dev/null +++ b/test/gtest_factor_force_torque_inertial_dynamics_external_force.cpp @@ -0,0 +1,361 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- + +#include "bodydynamics/factor/factor_force_torque_inertial_dynamics_external_force.h" +#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h" +#include "bodydynamics/sensor/sensor_force_torque_inertial.h" +#include "bodydynamics/internal/config.h" + +#include <core/sensor/factory_sensor.h> + +#include <core/utils/utils_gtest.h> +#include <core/utils/logging.h> +#include <core/utils/params_server.h> +#include <core/yaml/parser_yaml.h> +#include <core/state_block/state_block_derived.h> +#include <core/state_block/factory_state_block.h> + +#include <Eigen/Dense> +#include <Eigen/Geometry> + +using namespace wolf; +using namespace bodydynamics::fti; + +std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR; + +WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d); + +class Test_FactorForceTorqueInertialDynamicsExternalForce_yaml : public testing::Test +{ + public: + ProblemPtr P; + SensorForceTorqueInertialPtr S; + ProcessorForceTorqueInertialDynamicsPtr p; + FrameBasePtr F0, F1, F; + CaptureMotionPtr C0, C1, C; + FeatureMotionPtr f1; + FactorForceTorqueInertialDynamicsExternalForcePtr c1; + + VectorXd data; + MatrixXd data_cov; + VectorXd delta; + MatrixXd delta_cov; + Matrix<double, 13, 1> calib; + MatrixXd J_delta_calib; + + protected: + void SetUp() override + { + ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics_external_force.yaml", wolf_root + "/test/yaml/"); + ParamsServer server = ParamsServer(parser.getParams()); + P = Problem::autoSetup(server); + + S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front()); + p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front()); + + data = VectorXd::Zero(12); // [ a, w, f, t ] + data_cov = MatrixXd::Identity(12, 12); + C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr); + + C->process(); + + C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin()); + F0 = C0->getFrame(); + + ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp()); + ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp()); + + C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr); + C->process(); + + C1 = std::static_pointer_cast<CaptureMotion>(p->getLast()); + F1 = C1->getFrame(); + + F1->link(P); // by the face + + VectorXd delta_preint(VectorXd::Zero(19)); + delta_preint.head<3>() = -0.5*gravity(); + delta_preint.segment<3>(3) = -gravity(); + delta_preint.segment<3>(6) = -0.5*gravity(); + delta_preint.segment<3>(9) = -gravity(); + delta_preint(18) = 1; + MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18)); + VectorXd calib_preint(VectorXd::Zero(13)); + MatrixXd jacobian_calib(MatrixXd::Zero(18, 13)); + f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, + jacobian_calib); + + c1 = FactorBase::emplace<FactorForceTorqueInertialDynamicsExternalForce>(f1, f1, C0, p, false); + + F1->getStateBlock('P')->setState(Vector3d(0,0,0)); + F1->getStateBlock('V')->setState(Vector3d(0,0,0)); + F1->getStateBlock('O')->setState(Vector4d(0,0,0,1)); + F1->getStateBlock('L')->setState(Vector3d(0,0,0)); + } +}; + + +class Test_FactorForceTorqueInertialDynamicsExternalForce : public testing::Test +{ + public: + ProblemPtr P; + SensorForceTorqueInertialPtr S; + ProcessorForceTorqueInertialDynamicsPtr p; + FrameBasePtr F0, F1; + CaptureMotionPtr C0, C1; + FeatureMotionPtr f1; + FactorForceTorqueInertialDynamicsExternalForcePtr c1; + + VectorXd data; + MatrixXd data_cov; + VectorXd delta; + MatrixXd delta_cov; + Matrix<double, 13, 1> calib; + MatrixXd J_delta_calib; + + protected: + void SetUp() override + { + data = VectorXd::Zero(12); // [ a, w, f, t ] + data_cov = MatrixXd::Identity(12, 12); + + // crear un problem + P = Problem::create("POVL", 3); + + // crear un sensor + auto params_sensor = std::make_shared<ParamsSensorForceTorqueInertial>(); + Vector7d extrinsics; + extrinsics << 0, 0, 0, 0, 0, 0, 1; + params_sensor->add_external_force = true; + S = SensorBase::emplace<SensorForceTorqueInertial>(P->getHardware(), extrinsics, params_sensor); + S->setStateBlockDynamic('I', true); + + // crear processador + auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialDynamics>(); + p = ProcessorBase::emplace<ProcessorForceTorqueInertialDynamics>(S, params_processor); + + // crear dos frame + VectorXd state(13); + state << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0; + VectorComposite state_c(state, "POVL", {3, 4, 3, 3}); + F0 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 0.0, "POVL", state_c); + F1 = FrameBase::emplace<FrameBase>(P->getTrajectory(), 1.0, "POVL", state_c); + + // crear dues capture + VectorXd data(VectorXd::Zero(12)); + MatrixXd data_cov(MatrixXd::Identity(12, 12)); + Vector6d bias(Vector6d::Zero()); + auto sb_bias = std::make_shared<StateParams6>(bias); + C0 = CaptureBase::emplace<CaptureMotion>(F0, "CaptureMotion", 0.0, S, data, data_cov, nullptr, nullptr, + nullptr, sb_bias); + C1 = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1.0, S, data, data_cov, C0, nullptr, nullptr, + sb_bias); + + // crear un feature + VectorXd delta_preint(VectorXd::Zero(19)); + delta_preint.head<3>() = -0.5*gravity(); + delta_preint.segment<3>(3) = -gravity(); + delta_preint.segment<3>(6) = -0.5*gravity(); + delta_preint.segment<3>(9) = -gravity(); + delta_preint(18) = 1; + MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18)); + VectorXd calib_preint(VectorXd::Zero(13)); + MatrixXd jacobian_calib(MatrixXd::Zero(18, 13)); + f1 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, + jacobian_calib); + + // crear un factor + c1 = FactorBase::emplace<FactorForceTorqueInertialDynamicsExternalForce>(f1, f1, C0, p, false); + } +}; + +// TEST(FactorForceTorqueInertialDynamics_yaml, force_registraion) +// { +// FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create); +// } + +//Test to see if the constructor works (not yaml files) +TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce, constructor) +{ + ASSERT_EQ(c1->getCapture(), C1); + ASSERT_EQ(c1->getCalibPreint().size(), 13); +} + +//Test to see if the constructor works (yaml files) +TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce_yaml, constructor) +{ + ASSERT_EQ(c1->getCapture(), C1); + ASSERT_EQ(c1->getCalibPreint().size(), 13); +} + +//Test en el que no hi ha moviment (not yaml files) +TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce, residual) +{ + VectorXd res_exp = VectorXd::Zero(18); + Matrix<double, 18, 1> res; + VectorXd bias = VectorXd::Zero(6); + + P->print(4,1,1,1); + + c1->residual(F0->getStateBlock('P')->getState(), // p0 + Quaterniond(F0->getStateBlock('O')->getState().data()), // q0 + F0->getStateBlock('V')->getState(), // v0 + F0->getStateBlock('L')->getState(), // L0 + bias, // I + F1->getStateBlock('P')->getState(), // p1 + Quaterniond(F1->getStateBlock('O')->getState().data()), // q1 + F1->getStateBlock('V')->getState(), // v1 + F1->getStateBlock('L')->getState(), // L1 + S->getCom(), // C + S->getInertia(), // i + S->getMass(), // m + S->getExternalForce(), // f + res); + + ASSERT_MATRIX_APPROX(res_exp, res, 1e-8); +} + +//Test en el que no hi ha moviment (yaml files) +TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce_yaml, residual) +{ + VectorXd res_exp = VectorXd::Zero(18); + Matrix<double, 18, 1> res; + VectorXd bias = VectorXd::Zero(6); + + c1->residual(F0->getStateBlock('P')->getState(), // p0 + Quaterniond(F0->getStateBlock('O')->getState().data()), // q0 + F0->getStateBlock('V')->getState(), // v0 + F0->getStateBlock('L')->getState(), // L0 + bias, // I + F1->getStateBlock('P')->getState(), // p1 + Quaterniond(F1->getStateBlock('O')->getState().data()), // q1 + F1->getStateBlock('V')->getState(), // v1 + F1->getStateBlock('L')->getState(), // L1 + S->getCom(), // C + S->getInertia(), // i + S->getMass(), // m + S->getExternalForce(), // f + res); + + ASSERT_MATRIX_APPROX(res_exp, res, 1e-8); +} + + +//Test en el que s'avança 1m en direcció x (not yaml files) +TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce, residualx) +{ + VectorXd res_exp = VectorXd::Zero(18); + Matrix<double, 18, 1> res; + VectorXd bias = VectorXd::Zero(6); + + //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual + F1->getStateBlock('P')->setState(Vector3d (1,0,0)); + + //Hem de crear un nou feature i un nou factor ja que la delta preint canvia. + VectorXd delta_preint(VectorXd::Zero(19)); + delta_preint.head<3>() = -0.5*gravity(); + delta_preint(0) = 1; + delta_preint.segment<3>(3) = -gravity(); + delta_preint.segment<3>(6) = -0.5*gravity(); + delta_preint(6) = 1; + delta_preint.segment<3>(9) = -gravity(); + delta_preint(18) = 1; + MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18)); + VectorXd calib_preint(VectorXd::Zero(13)); + MatrixXd jacobian_calib(MatrixXd::Zero(18, 13)); + FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib); + + FactorForceTorqueInertialDynamicsExternalForcePtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamicsExternalForce>(f2, f2, C0, p, false); + + + c2->residual(F0->getStateBlock('P')->getState(), // p0 + Quaterniond(F0->getStateBlock('O')->getState().data()), // q0 + F0->getStateBlock('V')->getState(), // v0 + F0->getStateBlock('L')->getState(), // L0 + bias, // I + F1->getStateBlock('P')->getState(), // p1 + Quaterniond(F1->getStateBlock('O')->getState().data()), // q1 + F1->getStateBlock('V')->getState(), // v1 + F1->getStateBlock('L')->getState(), // L1 + S->getCom(), // C + S->getInertia(), // i + S->getMass(), // m + S->getExternalForce(), // f + res); + + ASSERT_MATRIX_APPROX(res_exp, res, 1e-8); +} + +//Test en el que s'avança 1m en direcció x (fitxers yaml) + +TEST_F(Test_FactorForceTorqueInertialDynamicsExternalForce_yaml, residualx) +{ + VectorXd res_exp = VectorXd::Zero(18); + Matrix<double, 18, 1> res; + VectorXd bias = VectorXd::Zero(6); + + //provem el moviment de P=(0,0,0) a P=(1,0,0) i v, o i L queden igual + F1->getStateBlock('P')->setState(Vector3d (1,0,0)); + + //Hem de crear un nou feature i un nou factor ja que la delta preint canvia. + VectorXd delta_preint(VectorXd::Zero(19)); + delta_preint.head<3>() = -0.5*gravity(); + delta_preint(0) = 1; + delta_preint.segment<3>(3) = -gravity(); + delta_preint.segment<3>(6) = -0.5*gravity(); + delta_preint(6) = 1; + delta_preint.segment<3>(9) = -gravity(); + delta_preint(18) = 1; + MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18)); + VectorXd calib_preint(VectorXd::Zero(13)); + MatrixXd jacobian_calib(MatrixXd::Zero(18, 13)); + FeatureMotionPtr f2 = FeatureBase::emplace<FeatureMotion>(C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint,jacobian_calib); + + FactorForceTorqueInertialDynamicsExternalForcePtr c2 = FactorBase::emplace<FactorForceTorqueInertialDynamicsExternalForce>(f2, f2, C0, p, false); + + + c2->residual(F0->getStateBlock('P')->getState(), // p0 + Quaterniond(F0->getStateBlock('O')->getState().data()), // q0 + F0->getStateBlock('V')->getState(), // v0 + F0->getStateBlock('L')->getState(), // L0 + bias, // I + F1->getStateBlock('P')->getState(), // p1 + Quaterniond(F1->getStateBlock('O')->getState().data()), // q1 + F1->getStateBlock('V')->getState(), // v1 + F1->getStateBlock('L')->getState(), // L1 + S->getCom(), // C + S->getInertia(), // i + S->getMass(), // m + S->getExternalForce(), // f + res); + + ASSERT_MATRIX_APPROX(res_exp, res, 1e-8); +} + + + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + //::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamicsExternalForce_yaml.residual"; + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/test/yaml/problem_force_torque_inertial_dynamics_external_force.yaml b/test/yaml/problem_force_torque_inertial_dynamics_external_force.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e50afc0fdd63de14ea525e0884592173c20ea318 --- /dev/null +++ b/test/yaml/problem_force_torque_inertial_dynamics_external_force.yaml @@ -0,0 +1,39 @@ +config: + problem: + frame_structure: "POVL" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + V: [0,0,0] + L: [0,0,0] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + V: [1,1,1] + L: [1,1,1] + time_tolerance: 0.1 + tree_manager: + type: "None" + map: + type: "MapBase" + plugin: "core" + sensors: + - + name: "sensor FTI" + type: "SensorForceTorqueInertial" + plugin: "bodydynamics" + extrinsic: + pose: [0,0,0, 0,0,0,1] + follow: "./sensor_force_torque_inertial_external_force.yaml" + + processors: + - + name: "proc FTID" + type: "ProcessorForceTorqueInertialDynamics" + sensor_name: "sensor FTI" + plugin: "bodydynamics" + follow: "processor_force_torque_inertial_dynamics.yaml" + \ No newline at end of file diff --git a/test/yaml/sensor_force_torque_inertial_external_force.yaml b/test/yaml/sensor_force_torque_inertial_external_force.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ba7cb900e758e331b6dc8f452fbbe17cd304a01c --- /dev/null +++ b/test/yaml/sensor_force_torque_inertial_external_force.yaml @@ -0,0 +1,20 @@ +force_noise_std: 0.1 # std dev of force noise in N +torque_noise_std: 0.1 # std dev of torque noise in N/m +acc_noise_std: 0.01 # std dev of acc noise in m/s2 +gyro_noise_std: 0.01 # std dev of gyro noise in rad/s +accb_initial_std: 0 # m/s2 - initial bias +gyrob_initial_std: 0 # rad/sec - initial bias +acc_drift_std: 0.00001 # std dev of acc drift m/s2/sqrt(s) +gyro_drift_std: 0.00001 # std dev of gyro drift rad/s/sqrt(s) + +com: [0,0,0.0341] # center of mass [m] +inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2] +mass: 1.952 # mass [kg] +imu_bias_fix: true +com_fix: true +inertia_fix: true +mass_fix: true + +add_external_force: true +external_force: [0, 0, 1] +external_force_fix: true \ No newline at end of file