[FEATURE] Support Equality Constraint and Closed-loop robots

[YilingQiao]

A joint work with @zhenjia-xu to support equality constraint so that we can simulate closed-loop robots.

Try

python examples/rigid/closed_loop.py

equality.mp4

[Kashu7100]

Thanks for the awesome work, I have checked examples/rigid/closed_loop.py works on my end.

Quick question: how can we use this for a closed link gripper like robotiq85.zip? (Are there any restrictions on how equality is supposed to be defined in the XML?")

[YilingQiao]

I will try your file.