Fix a bug in testIKtraj when computing the contact points on the foot