Needs collision avoidance in the scalar function, and x and z are mysteriously flipped, but otherwise it is touching the points correctly
It works about half the time. The other half, the robot spins off like crazy