Stable force-position control for robots during constrained motion