Inconsistent solver test results

MatthijsBurgh commented 4 years ago

https://travis-ci.org/github/orocos/orocos_kinematics_dynamics/jobs/686495037#L1271

FAIL: testFkPosAndIkPos (kinfamtest.KinfamTestFunctions)
1272----------------------------------------------------------------------
1273Traceback (most recent call last):
1274  File "/home/travis/build/orocos/orocos_kinematics_dynamics/python_orocos_kdl/tests/kinfamtest.py", line 224, in testFkPosAndIkPos
1275    self.assertEqual(q, q_solved)
1276AssertionError: [     1.60555     1.57944   -0.248299    -1.48487     3.10299    -1.20087] != [     1.60554     1.57944   -0.248286    -1.48487     3.10301    -1.20088]

It randomly happens that one of the travis jobs fails. It isn't always the same job. So unclear what is the cause for this inconsistency.

MatthijsBurgh commented 4 years ago

https://travis-ci.org/github/orocos/orocos_kinematics_dynamics/jobs/688782918

3: Test command: /home/travis/build/orocos/orocos_kinematics_dynamics/orocos_kdl/build/tests/solvertest
3: Test timeout computed to be: 10000000
3: ...square problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: underdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: overdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: overdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: .square problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: underdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: F.0.000000          0.000228      0.523108       0.045580     -0.098212       4.558028      -9.821186     0.410223      0.568421
3: 0.010000          0.000777      0.521924       0.064329     -0.138607       1.874883      -4.039482     0.267959      0.371448
3: 0.020000          0.001560      0.520237       0.092266     -0.198784       2.793640      -6.017780     0.317335      0.440332
3: 0.030000          0.002606      0.517985       0.116808     -0.251636       2.454215      -5.285122     0.300258      0.417227
3: 0.040000          0.003901      0.515194       0.142247     -0.306397       2.543890      -5.476142     0.306241      0.426347
3: 0.050000          0.005447      0.511864       0.167010     -0.359679       2.476367      -5.328224     0.304240      0.424554
3: 0.060000          0.007240      0.508003       0.191565     -0.412482       2.455505      -5.280259     0.305018      0.426829
3: 0.070000          0.009276      0.503619       0.215668     -0.464277       2.410306      -5.179493     0.304841      0.427962
3: 0.080000          0.011551      0.498723       0.239323     -0.515067       2.365461      -5.079057     0.305001      0.429758
3: 0.090000          0.014060      0.493324       0.262446     -0.564671       2.312321      -4.960401     0.305049      0.431584
3: 0.100000          0.016797      0.487436       0.284985     -0.612971       2.253869      -4.829973     0.305136      0.433650
3: .KDL-IK Solver Tests for Near Zero SVs
3: norminal case:  convergence
3: nonconvergence:  pseudoinverse singular
3: nonconvergence:  large displacement, low iterations
3: nonconvergence:  fully singular
3: .KDL-IK WDLS Vel Solver Tests for Near Zero SVs
3: smallest singular value is above threshold (no WDLS)
3: smallest singular value is below threshold (lambda is scaled)
3: smallest singular value is zero (lambda_scaled=lambda)
3: ...KDL FD Solver Development Test for Motoman SIA10
3: .KDL FD Solver Consistency Test for Motoman SIA10
3: .LDL Solver Test
3: .
3: 
3/8 Test #3: solvertest .......................***Failed    0.36 sec

Output of succesful test

3: Test command: /home/travis/build/orocos/orocos_kinematics_dynamics/orocos_kdl/build/tests/solvertest
3: Test timeout computed to be: 10000000
3: ...square problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: underdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: overdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: overdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: .square problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: underdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: overdetermined problem
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: underdetermined problem with WGs segment constructor
3: KDL-SVD-HouseHolder
3: KDL-SVD-Givens
3: .0.000000          0.000228      0.523108       0.045580     -0.098212       4.558028      -9.821186     0.410223      0.568421
3: 0.010000          0.000777      0.521924       0.064329     -0.138607       1.874883      -4.039482     0.267959      0.371448
3: 0.020000          0.001560      0.520237       0.092266     -0.198784       2.793640      -6.017780     0.317335      0.440332
3: 0.030000          0.002606      0.517985       0.116808     -0.251636       2.454215      -5.285122     0.300258      0.417227
3: 0.040000          0.003901      0.515194       0.142247     -0.306397       2.543890      -5.476142     0.306241      0.426347
3: 0.050000          0.005447      0.511864       0.167010     -0.359679       2.476367      -5.328224     0.304240      0.424554
3: 0.060000          0.007240      0.508003       0.191565     -0.412482       2.455505      -5.280259     0.305018      0.426829
3: 0.070000          0.009276      0.503619       0.215668     -0.464277       2.410306      -5.179493     0.304841      0.427962
3: 0.080000          0.011551      0.498723       0.239323     -0.515067       2.365461      -5.079057     0.305001      0.429758
3: 0.090000          0.014060      0.493324       0.262446     -0.564671       2.312321      -4.960401     0.305049      0.431584
3: 0.100000          0.016797      0.487436       0.284985     -0.612971       2.253869      -4.829973     0.305136      0.433650
3: .KDL-IK Solver Tests for Near Zero SVs
3: norminal case:  convergence
3: nonconvergence:  pseudoinverse singular
3: nonconvergence:  large displacement, low iterations
3: nonconvergence:  fully singular
3: .KDL-IK WDLS Vel Solver Tests for Near Zero SVs
3: smallest singular value is above threshold (no WDLS)
3: smallest singular value is below threshold (lambda is scaled)
3: smallest singular value is zero (lambda_scaled=lambda)
3: ...KDL FD Solver Development Test for Motoman SIA10
3: .KDL FD Solver Consistency Test for Motoman SIA10
3: .LDL Solver Test
3: .
3: 
3/8 Test #3: solvertest .......................   Passed    0.09 sec

MatthijsBurgh commented 4 years ago

Debugging:

3: underdetermined problem
3: KDL-SVD-HouseHolder
3: assert1
3: output1: 0
3: assert2
3: output2: -5

3: underdetermined problem with WGs segment constructor
3: KDL-SVD-HouseHolder
3: assert1
3: output1: 0
3: assert2
3: output2: 0
3: assert3
3: output3: 0
3: assert4
3: assert5
3: KDL-SVD-Givens
3: assert1
3: output1: 0
3: assert2
3: output2: -5

Numbers matching the Xth CPPUNIT_ASSERT in FkPosAndIkPosLocal

MatthijsBurgh commented 3 years ago

All problems seem to go back to ChainIkSolverPos_NR. In cpp it returns E_MAX_ITERATIONS_EXCEEDED in combination with ChainIkSolverVel_pinv and ChainIkSolverVel_pinv_givens on Chain4. In Python is also fails both in combination with ChainIkSolverVel_pinv and ChainIkSolverVel_pinv_givens. In Python it seems the problem that the result is a number of rotations off.

======================================================================
FAIL: testFkPosAndIkPosGivens (kinfamtest.KinfamTestFunctions)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "/home/amigo/ros/melodic/repos/github.com/orocos/orocos_kinematics_dynamics/python_orocos_kdl/tests/kinfamtest.py", line 258, in testFkPosAndIkPosGivens
    self.testFkPosAndIkPosImpl(self.fksolverpos, self.iksolverpos_givens, epsJ)
  File "/home/amigo/ros/melodic/repos/github.com/orocos/orocos_kinematics_dynamics/python_orocos_kdl/tests/kinfamtest.py", line 250, in testFkPosAndIkPosImpl
    self.assertTrue(Equal(q, q_solved, epsJ), "{} != {}".format(q, q_solved))
AssertionError: [  -0.0784393    -1.64079    0.574431   0.0575871     2.29737   -0.754842] != [    -3.22003     -1.5008    -2.56716       1.134    0.647722   -0.754842]

----------------------------------------------------------------------

The position does match, it is 1 rotations off.

-3.22003+math.pi = -0.07843734641020683

MatthijsBurgh commented 1 year ago

For now the wrench estimator is the problem. The third testcase is the problem. It converges to the solution, but then it starts to oscillate. I have created a branch which dumps the results, see wrench_estimator_test.

I have added the csv and the plot of a test run (This just contains two axes, the branch will dump all axes) q_wrench_estimator.csv wrench_estimator_test

@DjoleMNE you are the author of the estimator and the test, could you take a look at it?

orocos / orocos_kinematics_dynamics

Inconsistent solver test results #239