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

comment out pure integration tests for force torque preint

parent e4090cbe
No related branches found
No related tags found
2 merge requests!18Release after RAL,!17After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
test/gtest_processor_force_torque_preint.cpp
+ 52
− 52
View file @ 7022b8c8
......@@ -541,20 +541,20 @@ public:
TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
{
// TEST FIRST PURE INTEGRATION
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// // TEST FIRST PURE INTEGRATION
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// THEN SOLVE
problem_->perturb();
......@@ -614,19 +614,19 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
// // COM position on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState(), posi_diff, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
problem_->perturb();
......@@ -659,19 +659,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2), 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_diff, 1e-6);
// // COM position on Z axis is not observable with this problem
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("C")->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_diff, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
problem_->perturb();
......@@ -760,20 +760,20 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
}
TEST_F(Cst2KFXaxisRotationPureTorque, RotationOnlyNoSolve)
{
// TEST_F(Cst2KFXaxisRotationPureTorque, RotationOnlyNoSolve)
// {
Vector3d Lc_diff; Lc_diff << M_PI, 0, 0;
Vector4d q_diff; q_diff << 1,0,0,0;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// Vector3d Lc_diff; Lc_diff << M_PI, 0, 0;
// Vector4d q_diff; q_diff << 1,0,0,0;
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("C")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("O")->getState(), q_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), Lc_diff, 1e-6);
}
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("O")->getState(), q_diff, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), Lc_diff, 1e-6);
// }
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