Why do you define your inertia about the origin? Isn't the better way to define your inertia about the center of mass of each link.