main inverse dynamics algorithm calling rigidBodyInvDyn.m