Integration of planning and execution in force controlled compliant motion