Update to use transform error diff function for numerical jacobian

[Levi-Armstrong]

No description provided.

[rjoomen]

Want me to test this? What exactly does it solve?

[marrts]

Want me to test this? What exactly does it solve?

I believe this is to address the failing PuzzlePieceCppExampleUnit test in tesseract_planning, which I just tested locally and confirmed it does fix that.

[marrts]

Looks like this is failing the Ubuntu job because it's pointing to the 0.21 tag of tesseract and this calls functions in the latest comment which doesn't have a tag. It's also failing Unstable because of some lingering clang-tidy issues. Nominally this seems good to go though.

[Levi-Armstrong]

Want me to test this?

That would be great. This should also fix the IFOPT version for the puzzle piece but I have not tested.

What exactly does it solve?

There is a singularity at PI when computing angle axis which when numerically calculating the jacobian you get invalid results. The fix I had before to address worked for the unit tests but was not correct. The new function in tesseract_common does some additional checks to make sure things are correct before taking the difference to avoid the singularity issue when you want to calculate the difference for numerical jacobian.

[rjoomen]

I just tested all examples, and pick_and_place_example and puzzle_piece_example are working now, great! Only glass_upright_example does not work anymore, and I could not get it to work, by playing with coefficients etc. It does not produce errors, but simply fails to optimize, it stops at the penalty iteration limit.

Could you look into that maybe?

[marrts]

Only glass_upright_example does not work anymore

glass_upright_example passed on my machine when I ran it. I tried enabling the disabled ifopt test and that one appears to fail, is that what you're referring to?

[rjoomen]

Indeed, the ifopt version does not work anymore.

[rjoomen]

Indeed, the ifopt version does not work anymore.

Fixed by #387