Modified existing functions to deal with time varying orientation matrix for a simplified inverse dynamics