Fast interpolation and time-optimization with contact

This paper presents a method for generating dynamically feasible, keyframe-interpolating motions for robots undergoing contact, such as in legged locomotion and manipulation. The first stage generates a twice-differentiable interpolating path that obeys kinematic contact constraints up to a user-specified tolerance. The second stage optimizes speeds along the path to minimize time while satisfying dynamic constraints. The method supports velocity, acceleration, and torque constraints, and polyhedral contact friction constraints at an arbitrary number of contact points. The method is numerically stable, and empirical running time is weakly linear in the number of degrees of freedom and polynomial in the time-domain grid resolution. Experiments demonstrate that full-body motions for robots with 100 degrees of freedom and dozens of contact points are calculated in seconds.