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. ...@@ -16,10 +16,10 @@ solve is done with a perturbated system.
Tests list: Tests list:
FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt: FactorInertialKinematics_2KF_Meas0_bpfix,ZeroMvt:
FactorInertialKinematics_1KF_1p_bpfix,ZeroMvt: FactorInertialKinematics_2KF_1p_bpfix,ZeroMvt:
FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt: FactorInertialKinematics_2KF_m1p_pfix,ZeroMvt:
FactorInertialKinematics_1KF_1v_bfix,ZeroMvt: FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
*/ */
...@@ -58,6 +58,11 @@ using namespace Eigen; ...@@ -58,6 +58,11 @@ using namespace Eigen;
using namespace std; using namespace std;
// SOME CONSTANTS
const double Acc1 = 1.0;
const double Acc2 = 2.5;
Matrix9d computeKinJac(const Vector3d& w_unb, Matrix9d computeKinJac(const Vector3d& w_unb,
const Vector3d& p_unb, const Vector3d& p_unb,
const Matrix3d& Iq) const Matrix3d& Iq)
...@@ -115,14 +120,21 @@ void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){ ...@@ -115,14 +120,21 @@ void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
} }
else else
{ {
Eigen::Vector3d pert = Eigen::Vector3d::Random(); Vector3d pert = Vector3d::Random();
KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert); 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: public:
double mass_; double mass_;
...@@ -134,10 +146,11 @@ class FactorInertialKinematics_1KF : public testing::Test ...@@ -134,10 +146,11 @@ class FactorInertialKinematics_1KF : public testing::Test
MatrixXd P_origin_; MatrixXd P_origin_;
FrameBasePtr KFA_; FrameBasePtr KFA_;
FrameBasePtr KFB_; FrameBasePtr KFB_;
Eigen::Matrix3d Qp_, Qv_, Qw_; Matrix3d Qp_, Qv_, Qw_;
Eigen::Vector3d bias_p_, bias_imu_; Vector3d bias_p_, bias_imu_;
FeatureInertialKinematicsPtr feat_ikinA_; FeatureInertialKinematicsPtr feat_ikinA_;
FeatureInertialKinematicsPtr feat_ikinB_; FeatureInertialKinematicsPtr feat_ikinB_;
FeatureForceTorquePtr feat_ftAB_;
// SensorIMUPtr sen_imu_; // SensorIMUPtr sen_imu_;
// ProcessorIMUPtr processor_imu_; // ProcessorIMUPtr processor_imu_;
...@@ -159,19 +172,19 @@ class FactorInertialKinematics_1KF : public testing::Test ...@@ -159,19 +172,19 @@ class FactorInertialKinematics_1KF : public testing::Test
ceres_manager_ = new CeresManager(problem_, ceres_options); ceres_manager_ = new CeresManager(problem_, ceres_options);
// INSTALL IMU SENSOR // 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"); // 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); // 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"); // ProcessorBasePtr prc_imu = problem_->installProcessor("ProcessorIMU", "IMU PROC", "Main IMU Sensor", bodydynamics_root + "/demos/processor_imu.yaml");
// Eigen::Vector6d data = Eigen::Vector6d::Zero(); // Vector6d data = Vector6d::Zero();
// wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(0, sen_imu_, data, sen_imu_->getNoiseCov(), Eigen::Vector6d::Zero()); // wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(0, sen_imu_, data, sen_imu_->getNoiseCov(), Vector6d::Zero());
// // sen_imu_->process(imu_ptr); // // sen_imu_->process(imu_ptr);
// ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR // ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
tA_.set(0); tA_.set(0);
x_origin_.resize(10); x_origin_.resize(10);
x_origin_ << 0,0,0, 0,0,0,1, 0,0,0; 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 // Set origin of the problem
KFA_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), tA_, 0.005); KFA_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), tA_, 0.005);
...@@ -210,9 +223,9 @@ class FactorInertialKinematics_1KF : public testing::Test ...@@ -210,9 +223,9 @@ class FactorInertialKinematics_1KF : public testing::Test
Matrix3d Iq; Iq.setIdentity(); Matrix3d Iq; Iq.setIdentity();
Vector3d Lq = zero3; Vector3d Lq = zero3;
Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity(); Qp_ = pow(1e-2, 2)*Matrix3d::Identity();
Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity(); Qv_ = pow(1e-2, 2)*Matrix3d::Identity();
Qw_ = pow(1e-2, 2)*Eigen::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); Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_measA-bias_p_, w_measA-bias_imu_.tail(3), Iq);
// FACTOR INERTIAL KINEMATICS ON KFA // FACTOR INERTIAL KINEMATICS ON KFA
...@@ -241,7 +254,7 @@ class FactorInertialKinematics_1KF : public testing::Test ...@@ -241,7 +254,7 @@ class FactorInertialKinematics_1KF : public testing::Test
// // ================ PROCESS IMU DATA // // ================ PROCESS IMU DATA
// Eigen::Vector6d data_imu; // Vector6d data_imu;
// data_imu << -wolf::gravity(), 0,0,0; // 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) // 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 // // process data in capture
...@@ -259,64 +272,59 @@ class FactorInertialKinematics_1KF : public testing::Test ...@@ -259,64 +272,59 @@ class FactorInertialKinematics_1KF : public testing::Test
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
Eigen::Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
Eigen::Vector3d tau1; tau1 << 0,0,0; Vector3d tau1; tau1 << 0,0,0;
Eigen::Vector3d pbl1; pbl1 << 0,0,0; Vector3d pbl1; pbl1 << 0,0,0;
Eigen::Vector4d bql1; bql1 << 0,0,0,1; Vector4d bql1; bql1 << 0,0,0,1;
Eigen::Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
Eigen::Vector3d tau2; tau2 << 0,0,0; Vector3d tau2; tau2 << 0,0,0;
Eigen::Vector3d pbl2; pbl2 << 0,0,0; Vector3d pbl2; pbl2 << 0,0,0;
Eigen::Vector4d bql2; bql2 << 0,0,0,1; Vector4d bql2; bql2 << 0,0,0,1;
Eigen::Vector3d pbc; pbc << 0,0,0; Vector3d pbc; pbc << 0,0,0;
// aggregated meas // aggregated meas
Eigen::Vector6d f_meas; f_meas << f1, f2; Vector6d f_meas; f_meas << f1, f2;
Eigen::Vector6d tau_meas; tau_meas << tau1, tau2; Vector6d tau_meas; tau_meas << tau1, tau2;
Eigen::Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2; Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
Eigen::Matrix6d cov_f = 1e-4 * Eigen::Matrix6d::Identity(); Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
Eigen::Matrix6d cov_tau = 1e-4 * Eigen::Matrix6d::Identity(); Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
Eigen::Matrix3d cov_pbc = 1e-4 * Eigen::Matrix3d::Identity(); Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
CaptureBasePtr cap_ftB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureForceTorque", tB_, nullptr); 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_, tB_ - tA_, mass_,
f_meas, tau_meas, pbc, kin_meas, f_meas, tau_meas, pbc, kin_meas,
cov_f, cov_tau, cov_pbc); 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(){} virtual void TearDown(){}
}; };
class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics_1KF class FactorInertialKinematics_2KF_FX : public FactorInertialKinematics_2KF
{ {
virtual void SetUp() override virtual void SetUp() override
{ {
FactorInertialKinematics_1KF::SetUp(); FactorInertialKinematics_2KF::SetUp();
// Fix the bp bias Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
KFA_->getStateBlock("B")->fix(); Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
KFA_->getStateBlock("C")->unfix(); Vector6d fmeas; fmeas << f1, f2;
Eigen::Vector3d zero3 = Eigen::Vector3d::Zero(); feat_ftAB_->setForcesMeas(fmeas);
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);
} }
}; };
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 ...@@ -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(); Vector3d c_exp = Vector3d::Zero();
Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero(); Vector3d cd_exp = Vector3d::Zero();
Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero(); Vector3d Lc_exp = Vector3d::Zero();
Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero(); Vector3d bp_exp = Vector3d::Zero();
string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateIfUnFixed(KFA_, "P"); perturbateAllIfUnFixed(KFA_);
perturbateIfUnFixed(KFA_, "O"); perturbateAllIfUnFixed(KFB_);
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");
problem_->print(4,true,true,true); problem_->print(4,true,true,true);
report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
std::cout << report << std::endl; std::cout << report << std::endl;
problem_->print(4,true,true,true); 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("C")->getState(), c_exp, 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), cd_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(KFA_->getStateBlock("L")->getState(), Lc_exp, 1e-5);
...@@ -366,6 +363,56 @@ TEST_F(FactorInertialKinematics_1KF,ZeroMvt) ...@@ -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) 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