1. Add a test file for FrictionConeWrenchConstraint
2. Fix some bugs in FrictionConeWrenchConstraint