fn for chomp to optimize constructed trajectory
Merge pull request #7 from personalrobotics/feature/OptimizeTraj

Added OptimizeTrajectory to CHOMP.