Fix joint stiffness and damping setting via FRI
Use fixed matrix size for MassMatrix