Robot and Multibody Dynamics:Analysis and Algorithms