A Robust Time-Stepping Scheme for Quasistatic Rigid Multibody Systems

Abstract

An effective scheme to simulate low-speed, contact-rich manipulation tasks is to assume quasistatic physics and advance system states by solving linear complementarity problems (LCPs). However, the existing LCP-based quasistatic time-stepping scheme fails to simulate grasping-an essential motion primitive in manipulation-due to two drawbacks specific to quasistatic systems. Firstly, inputs to quasistatic systems are velocity commands instead of torques. This can lead to penetration, and thus an infeasible LCP, when two rigid bodies in contact are commanded to push against each other. Secondly, as multiple force solutions exist for a given velocity command, a grasping velocity command is not guaranteed to generate sufficient grasping forces. In this paper, we reformulate the quasistatic time-stepping scheme as an optimization problem with complementarity constraints and a quadratic objective. By minimizing the difference between actual and commanded velocities, linearized non-penetration constraints can always be satisfied. Moreover, undesirable solutions with insufficient normal forces can be removed by considering elasticity, which is modeled by comparing actual and commanded velocities. The resulting optimization problem is a mixed-integer quadratic program, which can be solved reasonably quickly for small-to-medium-sized systems. The effectiveness of the proposed reformulation is validated by simulation results of systems with different levels of complexity.