1. find some bugs in RigidBodyConstraint.cpp and inverseKinBackend.cpp
2. Add test for RigidBodyConstraint when time is not in the tspan