Hoist constants out of loops.
more code factorisation
Merge pull request #2570 from rlamy/pydy_bench

KanesMethod optimisation