Add toleranced Cartesian waypoints to trajopt_ifopt

[rjoomen]

Toleranced cartesian waypoints added to Trajopt in this PR should also be added to trajopt_ifopt (as the latter is meant to be the eventual replacement of Trajopt).