Added cartesian path planning