search

tesseract-robotics / trajopt

Trajectory Optimization Motion Planner for ROS
373 stars 101 forks source link

Update to use transform error diff function for numerical jacobian #386

Closed Levi-Armstrong closed 5 months ago

rjoomen commented 5 months ago

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

marrts commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

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 commented 5 months ago

Indeed, the ifopt version does not work anymore.

rjoomen commented 5 months ago

Indeed, the ifopt version does not work anymore.

Fixed by #387