chomp optimizer bug fix
collision-aware cartesian trajectory planning