ContactWrenchConstraint and RigidBodyConstraint

* Add `ContactWrenchConstraint` to enforce constraint on contact force parameters. This class also computes total wrench from contact forces. All `ContactWrenchConstraint` takes `kinsol` as one of the input arguments for `eval` and `wrench` function. This allows us to enforce kinematic constraint separately on `kinsol`.  The constraints in this class are the followings. Now I only finished MATLAB version
 * `FrictionConeWrenchConstraint`. Use nonlinear friction cone
 * `LinearFrictionConeWrenchConstraint`. Use linearized friction cone
 * `RailGraspWrenchConstraint`. Constrain the contact force and torque at the center of the grasping. The constraints are nonlinear friction cone and force magnitude.
* Restructure `RigidBodyConstraint`. Previously `RigidBodyConstraint`  is like an 'enum', that enumerates all the acceptable types of `RigidBodyConstraint`. Now it stores `robot`, `tspan` and `mex_ptr`. As every `RigidBodyConstraint` should have a `RigidBodyManipulator`. Also they are time-indexed.