Efficient planning for near-optimal contact-rich control under uncertainty

Abstract

Path planning classically focuses on avoiding environmental contact. However, some assembly tasks permit contact through compliance, and such contact may allow for more efficient and reliable solutions under action uncertainty. But optimal manipulation plans that leverage environmental contact are difficult to compute. Environmental contact produces complex kinematics that create difficulties for planning. This complexity is usually addressed by discretization over state and action space, but discretization quickly leads to computationally intractability if the optimal solution is desired. To overcome the challenge, we use the insight that only actions on configurations near the contact manifold are likely to involve complex kinematics, while segments of the plan through free space do not. Leveraging this structure can greatly reduce the number of states considered and scales much better with problem complexity. We develop the composite MDP algorithm based on this idea and show that it performs comparably to full MDP solutions at a fraction of the computational cost. However, the composite MDP still requires minutes to hours of computation, which is unsuitable for robots operating in novel environments. To overcome this limitation, we use the insight that environments are generally composed of a limited set of geometries. We can precompute the kinematic models of the dynamic object relative to these constituent geometries (constituent MDPs), and use them to assemble a kinematic model of the dynamic object relative to an environment with all constituent geometries present, by merging state spaces and transition functions. However, the straightforward assembly algorithm does not produce a sufficient computational speedup. Therefore, we introduce four assumptions to significantly reduce computation time. We demonstrate our algorithm to compute policies for novel environments on the order of seconds, without sacrificing solution quality.