Add setQnom to InverseKinematics
Add arc-length costs to LeggedRobotPlanningProblem

The arc-length traveled by each hand is penalized
Add excludeCollisionGroups method to MinDistanceConstraint
Add 'sense' option for snopt in NonlinearProgram

Valid values are 'Minimize' (default), 'Maximize', and 'Feasible point'.
Add start_from_rest and end_from_rest to LeggedRobotPlanningProblem
Add excluded_collision_groups to LeggedRobotPlanningProblem
Use speye in MinDistanceConstraint/scaleDistance

This is much faster when the number of collision-pairs is large.
Modify default behavior of addRigidBodyConstraint

In SDFKP, the previous default behavior of addRigidBodyConstraint in the case
where the time_index was a numeric array, was to apply the constraint to each
knot individually:

```matlab
time_index = num2cell(reshape(time_index,1,[]));
```

This is a bad default for MultipleTimeKinematicConstraint which are meant to be
applied accross multiple knots. Now, if the constraint is a
MultipleTimeKinematicConstraint and time_index is a numeric vector, we apply
the constraint to all of the knots together:

```matlab
time_index = {reshape(time_index,1,[])};
```
Update KinTrajOptimization/addRigidBodyConstraint

Bring changes over from SDFKP in anticipation of making
KinematicTrajectoryOptimization a parent of SDFKP and removing the
addRigidBodyConstraint method from SDFKP.
Don't constrain more than 3 points in addSupportOnFlatGround
Homogenize LeggedRobPlanProb generate methods

Now generateComDynamicsFullKinematicsPlanner and generateQuasiStaticPlanner can
work with the same input arguments.