Expansive Space Trees with Reality Warping Actions for Simultaneous Design and Kinodyanmic Motion Planning

Peg-in-hole is an extensively studied robotics task as it is representative of assembly-type tasks, which are highly common in practical applications (connector insertion, general assembly). These tasks incorporate highly discontinuous dynamics that result in high requirements for manipulator precision unless compliant strategies can be found. Despite the canonical nature of this problem, consideration of use of explicitly designed geometric structures to provide caging and compliance is uncommon, and frameworks that automatically design such structures while planning the assembly action are nonexistent. In this paper, we shed some light on this underexplored approach and introduce a belief-space kinodynamic planning framework that incorporates object design into state-space while providing actions that allow the motion planner to perform edits to the object’s design as part of the planning process. We use this framework to plan open-loop force-control trajectories while designing connector geometry to match, and evaluate them under significant variance in dynamics parameters against hand-designed caging geometry as well as a bare peg.