added quadratic objective (to not move the other joints).  it's not fast, but it finally behaves as expected