An internal model based approach for robot control and diagnosis