modified lwr inertia to make it positive definite. Now it seems to be working