add the test for bodyContactPosition in testRigidBodyContactWrench
RigidBodyContactWrench passes the gradient test for tangentialDisplacement function