Added basic smoothing to path planning, with all waypoints "fixed".