On the Solution of Inverse Dynamics and Trajectory Optimization Problems for Multibody Systems