Testing 5 DOF arm with KDL