Position and force control loop together