Integrating Combined Task and Motion Planning with Compliant Control

Planning a motion for inserting pegs remains an open problem. The difficulty lies in both the inevitable errors in the grasps of a robotic hand and absolute precision problems in robot joint motors. This paper proposes an integral method to solve the problem. The method uses combined task and motion planning to plan the grasps and motion for a dual-arm robot to pick up the objects and move them to assembly poses. Then, it controls the dual-arm robot using a compliant strategy (a combination of linear search, spiral search, and impedance control) to finish up the insertion. The method is implemented on a dual-arm Universal Robots 3 robot. Six objects, including a connector with fifteen peg-in-hole pairs for detailed analysis and other five objects with different contours of pegs and holes for additional validation, were tested by the robot. Experimental results show reasonable force-torque signal changes and end-effector position changes. The proposed method exhibits high robustness and high fidelity in successfully conducting planned peg-in-hole tasks.