Add COntactWrenchConstraint to deal with external force constraint
Almost finish FrictionConeWrenchConstraint. Now this constraint supports sparse gradient matrix
1. Compute total torque instead of wrench. Since torque is nonlinear, while total force is linear. It is better to separate them.
2. Add the function 'force' to return the linear transformation from force parameter to total force.
Add function 'wrench' in ContactWrenchConstraint that returns the wrench and its gradient
1. Add a test file for FrictionConeWrenchConstraint
2. Fix some bugs in FrictionConeWrenchConstraint
Change the methods of ContactWrenchConstraint. Now all inheritated constraints must supply with the evalSparse and evalSparseStructure function. Namely, the constraint gradient is supposed to be sparse.
For greater generality, eval function in ContactWrenchConstraint also takes kinsol as input argument. This would not hurt the efficiency in optimization since we have the evalSparsity function, which would return a sparse pattern if eval does not depend on kinsol
Add force_ub and force_lb to the ContactWrenchConstraint.
start to work on LinearFrictionConeWrenchConstraint
Add more contents to LinearFrictionConeWrenchConstaint
1. Add a method contactPosition to ContactWrenchConstraint
2. Finish implementation of LinearFrictionConeWrenchConstraint. Add its test
Start to work on a new constraint type, RailGraspWrenchConstraint
1. Finish implementing RailGraspWrenchConstraint in MATLAB
2. evalSparseStructure does not return number of rows or columns. These two quantities should be obtained through other functions