Add a bench to ForwardDynamics.
Optimize ForwardDynamics::computeH.