Jacobian Planner

Moves in a straight line
Path not yet generated
Way points planned and set on robot and checked for collision