This is a linear controller for 3D motion planning