Fix MultipleTimeKinematicConstraint & testKinCnst
Fix testQuasiStaticConstraint
Merge pull request #3 from avalenzu/trajopt

Fix failing RigidBodyConstraint unit tests