About to draw a rrt
Finished forward kinematics test