Initial dense waypoint trajectory
Initial Cartesian trajectories in C++
Cleaned up Cartesian parabolic blends