HybridTrajectoryOptimization and minor updates

Added HybridTrajectoryOptimization and Compass Gait example. Minor changes to ContactImplicitTrajectoryOptimization, particularly the initialization of slack variables.
Yes, since MATLAB switch is different from C switch. Only mode 2 has slack variables, so only it needs a slack function.