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
...@@ -541,20 +541,20 @@ public: ...@@ -541,20 +541,20 @@ public:
TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias) TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
{ {
// TEST FIRST PURE INTEGRATION // // TEST FIRST PURE INTEGRATION
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6); // 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("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem // // 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("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->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(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6); // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->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 // // 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("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6); // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6); // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
// THEN SOLVE // THEN SOLVE
problem_->perturb(); problem_->perturb();
...@@ -614,19 +614,19 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias) ...@@ -614,19 +614,19 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2); posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3; Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT; vel_diff[0] = ACC * 3*DT;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6); // 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("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem // // 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("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->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(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6); // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("P")->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_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 // // 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("C")->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("D")->getState(), vel_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(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
problem_->perturb(); problem_->perturb();
...@@ -659,19 +659,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias) ...@@ -659,19 +659,19 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
posi_diff[0] = 0.5*ACC * pow(3*DT,2); posi_diff[0] = 0.5*ACC * pow(3*DT,2);
Vector3d vel_diff = ZERO3; Vector3d vel_diff = ZERO3;
vel_diff[0] = ACC * 3*DT; vel_diff[0] = ACC * 3*DT;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("P")->getState(), ZERO3, 1e-6); // 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("V")->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem // // 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("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("D")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("L")->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("P")->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("V")->getState(), vel_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 // // 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("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("D")->getState(), vel_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6); // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), ZERO3, 1e-6);
problem_->perturb(); problem_->perturb();
...@@ -760,20 +760,20 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias) ...@@ -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; // Vector3d Lc_diff; Lc_diff << M_PI, 0, 0;
Vector4d q_diff; q_diff << 1,0,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("P")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("V")->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("C")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock("D")->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(KF1_->getStateBlock("L")->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock("O")->getState(), q_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); // ASSERT_MATRIX_APPROX(KF2_->getStateBlock("L")->getState(), Lc_diff, 1e-6);
} // }
int main(int argc, char **argv) int main(int argc, char **argv)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment