Added left/right arm qp inverse kinematics