Abstract:

Contact-implicit trajectory optimization is an effective method to plan complex trajectories for various contact- rich systems including manipulation and locomotion. These methods formulate contact as complementarity constraints and require solving a mathematical program with complementarity constraints (MPCC). However, MPCC solve times increase steeply with the number of variables and complementarity con- straints, which limits their applicability to problems with low geometric complexity. This paper introduces the simultaneous trajectory optimization and contact selection (STOCS) method that embeds the detection of salient contact points and contact times inside trajectory optimization. Because the number of active contact points is usually small, this approach minimize the number of MPCC variables and constraints, which makes solving manipulation trajectories for objects with complex, non-convex geometry computationally tractable. The proposed approach is validated on a pivoting problem in simulation and on a 6 DoF manipulator arm.