Compute CoM and CoM velocity, CoP, Ground Reaction Forces, etc.

[jerryepratt]

Would be nice to have SCS2 automatically compute the CoM location and velocity. Maybe require a call to enable this, or an update call each time you want to compute it.

Also, for each set of contact points, would be nice to compute a total force and maybe CoP for those points. Though that might only be relevant on flat ground.