diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp index 25ac26708a510aaef28a5da6466ef6b3924983ae..5e55b6171b199d9b4228ce5495d910f248f42290 100644 --- a/test/gtest_factor_force_torque.cpp +++ b/test/gtest_factor_force_torque.cpp @@ -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) {