increment write MBS 2
correct LShy inertia matrix to solve forearm bug