1. Add a method contactPosition to ContactWrenchConstraint
2. Finish implementation of LinearFrictionConeWrenchConstraint. Add its test