Optimize quasi-static pushing cost function's Jacobians

[mhmukadam]

Current implementation uses a few intermediate matrix multiplication that can be removed and the final results just hardcoded using indexing or basic torch operations.

[aymuos15]

Can I pick this up if still relevant?

[luisenp]

Hi @aymuos15. Definitely, we welcome contributions. This is the relevant class. It's been a few years, so I don't quite remember which particular matrix products could be simplified, so you may have to look at the math in this paper.

[aymuos15]

Awesome, thanks! Are these tests enough? (haven't gone through any material yet): https://github.com/facebookresearch/theseus/blob/main/tests/theseus_tests/embodied/motionmodel/test_quasi_static_pushing_planar.py

[luisenp]

Assuming you only change jacobians computation and not error, the second test should give good coverage. When testing for the first time we can increase the batch size and number of reps just to make sure we get more variety of cases.

[aymuos15]

Okay, sounds good! I will get to this soon. Going through the math for now.

Thank you!