Skip to content
Snippets Groups Projects
Commit cee942bb authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Added constant side force tests for FT factor

parent 5b1abbf6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
test/gtest_factor_force_torque.cpp
+ 123
− 76
View file @ cee942bb
......@@ -16,10 +16,10 @@ solve is done with a perturbated system.
Tests list:
FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt:
FactorInertialKinematics_1KF_1p_bpfix,ZeroMvt:
FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt:
FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
FactorInertialKinematics_2KF_Meas0_bpfix,ZeroMvt:
FactorInertialKinematics_2KF_1p_bpfix,ZeroMvt:
FactorInertialKinematics_2KF_m1p_pfix,ZeroMvt:
FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
*/
......@@ -58,6 +58,11 @@ using namespace Eigen;
using namespace std;
// SOME CONSTANTS
const double Acc1 = 1.0;
const double Acc2 = 2.5;
Matrix9d computeKinJac(const Vector3d& w_unb,
const Vector3d& p_unb,
const Matrix3d& Iq)
......@@ -115,14 +120,21 @@ void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
}
else
{
Eigen::Vector3d pert = Eigen::Vector3d::Random();
Vector3d pert = Vector3d::Random();
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);
}
}
class FactorInertialKinematics_1KF : public testing::Test
class FactorInertialKinematics_2KF : public testing::Test
{
public:
double mass_;
......@@ -134,10 +146,11 @@ class FactorInertialKinematics_1KF : public testing::Test
MatrixXd P_origin_;
FrameBasePtr KFA_;
FrameBasePtr KFB_;
Eigen::Matrix3d Qp_, Qv_, Qw_;
Eigen::Vector3d bias_p_, bias_imu_;
Matrix3d Qp_, Qv_, Qw_;
Vector3d bias_p_, bias_imu_;
FeatureInertialKinematicsPtr feat_ikinA_;
FeatureInertialKinematicsPtr feat_ikinB_;
FeatureForceTorquePtr feat_ftAB_;
// SensorIMUPtr sen_imu_;
// ProcessorIMUPtr processor_imu_;
......@@ -159,19 +172,19 @@ class FactorInertialKinematics_1KF : public testing::Test
ceres_manager_ = new CeresManager(problem_, ceres_options);
// INSTALL IMU SENSOR
// Eigen::Vector7d extrinsics; extrinsics << 0,0,0,0,0,0,1;
// 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");
// Eigen::Vector6d data = Eigen::Vector6d::Zero();
// wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(0, sen_imu_, data, sen_imu_->getNoiseCov(), Eigen::Vector6d::Zero());
// Vector6d data = Vector6d::Zero();
// wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(0, sen_imu_, data, sen_imu_->getNoiseCov(), Vector6d::Zero());
// // sen_imu_->process(imu_ptr);
// ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
tA_.set(0);
x_origin_.resize(10);
x_origin_ << 0,0,0, 0,0,0,1, 0,0,0;
P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
P_origin_ = pow(1e-3, 2) * Matrix9d::Identity();
// Set origin of the problem
KFA_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), tA_, 0.005);
......@@ -210,9 +223,9 @@ class FactorInertialKinematics_1KF : public testing::Test
Matrix3d Iq; Iq.setIdentity();
Vector3d Lq = zero3;
Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
Qw_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
Qp_ = pow(1e-2, 2)*Matrix3d::Identity();
Qv_ = pow(1e-2, 2)*Matrix3d::Identity();
Qw_ = pow(1e-2, 2)*Matrix3d::Identity();
Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_measA-bias_p_, w_measA-bias_imu_.tail(3), Iq);
// FACTOR INERTIAL KINEMATICS ON KFA
......@@ -241,7 +254,7 @@ class FactorInertialKinematics_1KF : public testing::Test
// // ================ PROCESS IMU DATA
// Eigen::Vector6d data_imu;
// 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
......@@ -259,64 +272,59 @@ class FactorInertialKinematics_1KF : public testing::Test
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
Eigen::Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
Eigen::Vector3d tau1; tau1 << 0,0,0;
Eigen::Vector3d pbl1; pbl1 << 0,0,0;
Eigen::Vector4d bql1; bql1 << 0,0,0,1;
Eigen::Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
Eigen::Vector3d tau2; tau2 << 0,0,0;
Eigen::Vector3d pbl2; pbl2 << 0,0,0;
Eigen::Vector4d bql2; bql2 << 0,0,0,1;
Eigen::Vector3d pbc; pbc << 0,0,0;
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
Eigen::Vector6d f_meas; f_meas << f1, f2;
Eigen::Vector6d tau_meas; tau_meas << tau1, tau2;
Eigen::Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
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;
Eigen::Matrix6d cov_f = 1e-4 * Eigen::Matrix6d::Identity();
Eigen::Matrix6d cov_tau = 1e-4 * Eigen::Matrix6d::Identity();
Eigen::Matrix3d cov_pbc = 1e-4 * Eigen::Matrix3d::Identity();
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);
auto feat_ftAB = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
feat_ftAB_ = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
tB_ - tA_, mass_,
f_meas, tau_meas, pbc, kin_meas,
cov_f, cov_tau, cov_pbc);
FactorForceTorquePtr fac_ftB = FactorBase::emplace<FactorForceTorque>(feat_ftAB, feat_ftAB, KFA_, nullptr, false);
FactorForceTorquePtr fac_ftB = FactorBase::emplace<FactorForceTorque>(feat_ftAB_, feat_ftAB_, KFA_, nullptr, false);
}
virtual void TearDown(){}
};
class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics_1KF
class FactorInertialKinematics_2KF_FX : public FactorInertialKinematics_2KF
{
virtual void SetUp() override
{
FactorInertialKinematics_1KF::SetUp();
// Fix the bp bias
KFA_->getStateBlock("B")->fix();
KFA_->getStateBlock("C")->unfix();
Eigen::Vector3d zero3 = Eigen::Vector3d::Zero();
Eigen::Vector3d pBC_measA = zero3;
Eigen::Vector3d vBC_measA = zero3;
Eigen::Vector3d w_measA = zero3;
Eigen::Vector9d meas; meas << pBC_measA, vBC_measA, w_measA;
// momentum parameters
Eigen::Matrix3d Iq; Iq.setIdentity();
Eigen::Vector3d Lq = zero3;
Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_measA-bias_p_, w_measA-bias_imu_.tail(3), Iq);
feat_ikinB_->setMeasurement(meas);
feat_ikinB_->setMeasurementCovariance(Qkin);
feat_ikinB_->setBIq(Iq);
feat_ikinB_->setBLcm(Lq);
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;
feat_ftAB_->setForcesMeas(fmeas);
}
};
class FactorInertialKinematics_2KF_FY : public FactorInertialKinematics_2KF
{
virtual 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;
feat_ftAB_->setForcesMeas(fmeas);
}
};
////////////////////////////////////////////////////////
......@@ -324,37 +332,26 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics
////////////////////////////////////////////////////////
TEST_F(FactorInertialKinematics_1KF,ZeroMvt)
TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
{
Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
Vector3d c_exp = Vector3d::Zero();
Vector3d cd_exp = Vector3d::Zero();
Vector3d Lc_exp = Vector3d::Zero();
Vector3d bp_exp = Vector3d::Zero();
string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateIfUnFixed(KFA_, "P");
perturbateIfUnFixed(KFA_, "O");
perturbateIfUnFixed(KFA_, "V");
perturbateIfUnFixed(KFA_, "C");
perturbateIfUnFixed(KFA_, "D");
perturbateIfUnFixed(KFA_, "L");
perturbateIfUnFixed(KFA_, "B");
perturbateIfUnFixed(KFB_, "P");
perturbateIfUnFixed(KFB_, "O");
perturbateIfUnFixed(KFB_, "V");
perturbateIfUnFixed(KFB_, "C");
perturbateIfUnFixed(KFB_, "D");
perturbateIfUnFixed(KFB_, "L");
perturbateIfUnFixed(KFB_, "B");
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
problem_->print(4,true,true,true);
report = ceres_manager_->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_.head(3), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
// ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5); // due to opposite
ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 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);
......@@ -366,6 +363,56 @@ TEST_F(FactorInertialKinematics_1KF,ZeroMvt)
}
TEST_F(FactorInertialKinematics_2KF_FX,ZeroMvt)
{
string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
problem_->print(4,true,true,true);
report = ceres_manager_->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_.head<3>(), 1e-5);
// ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2, 0, 0).finished(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
}
TEST_F(FactorInertialKinematics_2KF_FY,ZeroMvt)
{
string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
problem_->print(4,true,true,true);
report = ceres_manager_->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_.head<3>(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, Acc2/2, 0).finished(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, Acc2, 0).finished(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
}
int main(int argc, char **argv)
{
......
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