ENH Add logic to use many RHS on the ODE solvers

Using a single rhs is limiting in the case we wan to add other RHS values. Like is the case of contact forces.

Clean some of the interfaces as well. Add a few more matrices to the solvers test.