Fix wrong matrix types

I found those wrong types when trying to use a HarmonicEnergyStiffness
with a RigidBodyMotion.