Implementation of RNEA for robot control