Skip to content
Snippets Groups Projects
Commit 8435bff3 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Remove FactorForceTorque

parent f981d593
No related branches found
No related tags found
3 merge requests!31devel->main,!29Add functionality for UAV,!27Fix test with acc_x = 2 m/s2
Expand all Hide whitespace changes
Inline Side-by-side
include/bodydynamics/factor/factor_force_torque.h deleted 100644 → 0
+ 0
− 282
View file @ f981d593
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file factor_force_torque.h
*
* Created on: Feb 19, 2020
* \author: mfourmy
*
* Works only for 2 limbs
*/
#ifndef FACTOR_FORCE_TORQUE_H_
#define FACTOR_FORCE_TORQUE_H_
#include <core/math/rotations.h>
#include <core/math/covariance.h>
#include <core/factor/factor_autodiff.h>
#include <core/feature/feature_base.h>
#include "bodydynamics/feature/feature_force_torque.h"
namespace wolf
{
WOLF_PTR_TYPEDEFS(FactorForceTorque);
class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3,3,3,4>
{
public:
FactorForceTorque(const FeatureBasePtr& _feat,
const FrameBasePtr& _frame_other,
const StateBlockPtr& _sb_bp_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorForceTorque() override { /* nothing */ }
/*
_ck : COM position in world frame, time k
_cdk : COM velocity in world frame, time k
_Lck : Angular momentum in world frame, time k
_ckm : COM position in world frame, time k-1
_cdkm : COM velocity in world frame, time k-1
_Lckm : Angular momentum in world frame, time k-1
_bpkm : COM position measurement bias, time k-1
_qkm : Base orientation in world frame, time k-1
*/
template<typename T>
bool operator () (
const T* const _ck,
const T* const _cdk,
const T* const _Lck,
const T* const _ckm,
const T* const _cdkm,
const T* const _Lckm,
const T* const _bpkm,
const T* const _qkm,
T* _res) const;
void computeJac(const FeatureForceTorqueConstPtr& _feat,
const double& _mass,
const double& _dt,
const Eigen::VectorXd& _bp,
Eigen::Matrix<double, 9, 15> & _J_ny_nz) const;
// void recomputeJac(const FeatureForceTorquePtr& _feat,
// const double& _dt,
// const Eigen::VectorXd& _bp,
// Eigen::Matrix<double, 9, 15>& _J_ny_nz) const;
void retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov);
StateBlockPtr sb_bp_other_;
double mass_;
double dt_;
Eigen::Matrix<double,15,15> cov_meas_;
// Eigen::Matrix<double, 9, 15> J_ny_nz_; // cannot be modified in operator() since const function
// Eigen::Matrix9d errCov_;
};
} /* namespace wolf */
namespace wolf {
FactorForceTorque::FactorForceTorque(
const FeatureBasePtr& _feat,
const FrameBasePtr& _frame_other,
const StateBlockPtr& _sb_bp_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
FactorAutodiff("FactorForceTorque",
TOP_GEOM,
_feat,
_frame_other, // _frame_other_ptr
nullptr, // _capture_other_ptr
nullptr, // _feature_other_ptr
nullptr, // _landmark_other_ptr
_processor_ptr,
_apply_loss_function,
_status,
_feat->getFrame()->getStateBlock('C'), // COM position, current
_feat->getFrame()->getStateBlock('D'), // COM velocity (bad name), current
_feat->getFrame()->getStateBlock('L'), // Angular momentum, current
_frame_other->getStateBlock('C'), // COM position, previous
_frame_other->getStateBlock('D'), // COM velocity (bad name), previous
_frame_other->getStateBlock('L'), // Angular momentum, previous
_sb_bp_other, // BC relative position bias, previous
_frame_other->getStateBlock('O') // Base orientation, previous
),
sb_bp_other_(_sb_bp_other)
{
FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
mass_ = feat->getMass();
dt_ = feat->getDt();
retrieveMeasurementCovariance(feat, cov_meas_);
}
void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov)
{
cov.setZero();
cov.block<6,6>(0,0) = feat->getCovForcesMeas(); // reset some extra zeros
cov.block<6,6>(6,6) = feat->getCovTorquesMeas(); // reset some extra zeros
cov.block<3,3>(12,12) = feat->getCovPbcMeas();
}
void FactorForceTorque::computeJac(const FeatureForceTorqueConstPtr& _feat,
const double& _mass,
const double& _dt,
const Eigen::VectorXd& _bp,
Eigen::Matrix<double, 9, 15>& _J_ny_nz) const
{
_J_ny_nz.setZero();
// Measurements retrieval
Eigen::Map<const Eigen::Vector3d> f1 (_feat->getForcesMeas().data());
Eigen::Map<const Eigen::Vector3d> f2 (_feat->getForcesMeas().data() + 3 );
Eigen::Map<const Eigen::Vector3d> tau1(_feat->getTorquesMeas().data());
Eigen::Map<const Eigen::Vector3d> tau2(_feat->getTorquesMeas().data() + 3 );
Eigen::Map<const Eigen::Vector3d> pbl1(_feat->getKinMeas().data());
Eigen::Map<const Eigen::Vector3d> pbl2(_feat->getKinMeas().data() + 3 );
Eigen::Map<const Eigen::Quaterniond> bql1(_feat->getKinMeas().data() + 6);
Eigen::Map<const Eigen::Quaterniond> bql2(_feat->getKinMeas().data() + 10);
Eigen::Map<const Eigen::Vector3d> pbc (_feat->getPbcMeas().data());
Eigen::Matrix3d bRl1 = q2R(bql1);
Eigen::Matrix3d bRl2 = q2R(bql2);
_J_ny_nz.block<3,3>(0,0) = 0.5*bRl1*pow(_dt,2)/_mass;
_J_ny_nz.block<3,3>(0,3) = 0.5*bRl2*pow(_dt,2)/_mass;
_J_ny_nz.block<3,3>(3,0) = bRl1*_dt/_mass;
_J_ny_nz.block<3,3>(3,3) = bRl2*_dt/_mass;
_J_ny_nz.block<3,3>(6,0) = skew(pbl1 - pbc + _bp)*bRl1*_dt;
_J_ny_nz.block<3,3>(6,3) = skew(pbl2 - pbc + _bp)*bRl2*_dt;
_J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
_J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
_J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
}
// void FactorForceTorque::recomputeJac(const FeatureForceTorquePtr& _feat,
// const double& _dt,
// const Eigen::VectorXd& _bp,
// Eigen::Matrix<double, 9, 15>& _J_ny_nz)
// {
// FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
// // Measurements retrieval
// // Eigen::Map<const Eigen::Vector3d> f1 (feat->getForcesMeas().data());
// // Eigen::Map<const Eigen::Vector3d> f2 (feat->getForcesMeas().data() + 3 );
// // Eigen::Map<const Eigen::Vector3d> tau1(feat->getTorquesMeas().data());
// // Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3 );
// Eigen::Map<const Eigen::Vector3d> pbl1(feat->getKinMeas().data());
// Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3 );
// Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
// Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
// Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
// Eigen::Matrix3d bRl1 = q2R(bql1);
// Eigen::Matrix3d bRl2 = q2R(bql2);
// // _J_ny_nz.block<3,3>(0,0) = 0.5*bRl1*pow(_dt,2)/_mass;
// // _J_ny_nz.block<3,3>(0,3) = 0.5*bRl2*pow(_dt,2)/_mass;
// // _J_ny_nz.block<3,3>(3,0) = bRl1*_dt/_mass;
// // _J_ny_nz.block<3,3>(3,3) = bRl2*_dt/_mass;
// _J_ny_nz.block<3,3>(6,0) = skew(pbl1 - pbc + _bp)*bRl1*_dt;
// _J_ny_nz.block<3,3>(6,3) = skew(pbl2 - pbc + _bp)*bRl2*_dt;
// // _J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
// // _J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
// // _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
// }
template<typename T>
bool FactorForceTorque::operator () (
const T* const _ck,
const T* const _cdk,
const T* const _Lck,
const T* const _ckm,
const T* const _cdkm,
const T* const _Lckm,
const T* const _bpkm,
const T* const _qkm,
T* _res) const
{
auto feat = std::static_pointer_cast<const FeatureForceTorque>(getFeature());
// State variables instanciation
Eigen::Map<const Eigen::Matrix<T,3,1> > ck(_ck);
Eigen::Map<const Eigen::Matrix<T,3,1> > cdk(_cdk);
Eigen::Map<const Eigen::Matrix<T,3,1> > Lck(_Lck);
Eigen::Map<const Eigen::Matrix<T,3,1> > ckm(_ckm);
Eigen::Map<const Eigen::Matrix<T,3,1> > cdkm(_cdkm);
Eigen::Map<const Eigen::Matrix<T,3,1> > Lckm(_Lckm);
Eigen::Map<const Eigen::Matrix<T,3,1> > bpkm(_bpkm);
Eigen::Map<const Eigen::Quaternion<T> > qkm(_qkm);
Eigen::Map<Eigen::Matrix<T,9,1> > res(_res);
// Retrieve all measurements
Eigen::Map<const Eigen::Vector3d> f1 (feat->getForcesMeas().data());
Eigen::Map<const Eigen::Vector3d> f2 (feat->getForcesMeas().data() + 3 );
Eigen::Map<const Eigen::Vector3d> tau1(feat->getTorquesMeas().data());
Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3 );
Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
Eigen::Map<const Eigen::Vector3d> pbl1(feat->getKinMeas().data());
Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3 );
Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
// Recompute the error variable covariance according to the new bias bp estimation
Eigen::Matrix<double, 9, 15> J_ny_nz;
computeJac(feat, mass_, dt_, sb_bp_other_->getState(), J_ny_nz); // bp
Eigen::Matrix9d errCov = J_ny_nz * cov_meas_ * J_ny_nz.transpose();
errCov.block<6,6>(0,0) = errCov.block<6,6>(0,0) + 1e-12 * Eigen::Matrix6d::Identity();
// Error variable instanciation
Eigen::Matrix<T, 9, 1> err;
Eigen::Map<Eigen::Matrix<T, 3, 1> > err_c (err.data());
Eigen::Map<Eigen::Matrix<T, 3, 1> > err_cd(err.data() + 3);
Eigen::Map<Eigen::Matrix<T, 3, 1> > err_Lc(err.data() + 6);
err_c = qkm.conjugate()*(ck - ckm - cdkm * dt_ - 0.5*wolf::gravity()*pow(dt_, 2))
- (0.5/mass_) * (bql1 * f1 * pow(dt_,2) + bql2 * f2 * pow(dt_,2));
err_cd = qkm.conjugate()*(cdk - cdkm - wolf::gravity()*dt_)
- (1/mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_);
err_Lc = qkm.conjugate()*(Lck - Lckm)
- ( bql1 * tau1 + (pbl1 - pbc + bpkm).cross(bql1 * f1)
+ bql2 * tau2 + (pbl2 - pbc + bpkm).cross(bql2 * f2));
res = wolf::computeSqrtUpper(errCov.inverse()) * err;
return true;
}
} // namespace wolf
#endif /* FACTOR_FORCE_TORQUE_H_ */
test/CMakeLists.txt
+ 0
− 2
View file @ 8435bff3
......@@ -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)
......
test/gtest_factor_force_torque.cpp deleted 100644 → 0
+ 0
− 865
View file @ f981d593
This diff is collapsed.
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment