Fixed ugly code in RRT and OncillaKinematics