Fix failing RigidBodyConstraint unit tests

Fixes `testKinCnst` and `testQuasiStaticConstraint`. `testIK` and `testIKtraj` still pass.