Impact dynamics
I added an impact dynamics, with nu=0. The dynamics is such that: M(vnext-v) = J^T impulse J vnext = 0 The derivatives are computed from drnea_dq.
I would like to handle the case where there is a contact model in addition to the impulse model. I guess it should be:
M(v_{next}-v) = J_{impulse}^T
J_{impulse} v_{next} = 0
J_{contact} v_{next} = J_{contact} v
The current work is in the impulse.py file, with unittest.