Made some changes to position solving it contact solver. (consider

revisiting)