Merged Alessandro changes. Added small changes in position control