2A1-E19 Motion Estimation for Environment-Contact Task with Position Controlled Manipulator