Quasistatic contact-rich manipulation via
linear complementarity quadratic programming

IROS 2022
1OMRON SINIC X Corporation2Kyoto University* work done as an intern at OMRON SINIC X.


Contact-rich manipulation is challenging due to dynamically-changing physical constraints by the contact-mode changes undergone during manipulation. This paper proposes a versatile local planning and control framework for contact-rich manipulation that determines the continuous control action under variable contact modes online. We model the physical characteristics of contact-rich manipulation by quasistatic dynamics and complementarity constraints. We then propose a linear complementarity quadratic program (LCQP) to efficiently determine the control action that implicitly includes the decisions on the contact modes under these constraints. In the LCQP, we relax the complementarity constraints to alleviate ill-conditioned problems that are typically caused by measure noises or model miss-matches. We conduct dynamical simulations on a 3D physical simulator and demonstrate that the proposed method can achieve various contact-rich manipulation tasks by determining the control action including the contact modes in real-time.


Unlike conventional manipulation performed by strictly grasping objects, contact-rich manupulation leverages various contacts between objects and environments to perform manipulation tasks. In the above figure, a gripper is raising a box by leveraging contacts between the box and the gripper as well as those between the box and the environment (the ground and a wall). Contact-rich manipulation is challenging, because it involves many possible discrete states (contact modes) and continuous states (position, velocity, and force) depending on each other and changing dynamically during manipulation. Such dynamical systems, called hybrid systems, make planning and control problems difficult, especially for online methods.

Contact complementarity

In this study, we aim to develop a planning and control method for contact-rich manipulation that allows real-time execution with online reasoning about contact modes. Our key idea is to utilize complementarity constraints to formulate the physical characteristics of the contacts in a principle fashion. The complementarity constraint on two scalar variables, a and b, is expressed as

0 ≤ a ⊥ b ≥ 0,

denoting that a, b satisfy a, b ≥ 0 and ab = 0. In the above figure, the distance between the gripper and the box is complementary to the contact force between them during manipulation, as a physical constraint. Complementarity is satisfied for other contacts including those between the box and the environment. With complementarity constraints, we can express discrete contact modes of a hybrid system by continuous variables without explicit mode enumeration as in mixed integer programming-based approaches, thereby reducing computational burdens.

LCQP controller

Our method performs a manipulation task by optimizing a non-linear objective function with contact complementarity constraints, solving for a state variable q concatenating the system coordinates and the concact forces. The objective function is formulated to be minimal given a desired goal state. In each control step during manupulation, we observe a current state and then approximate the objective function around it as linear complementarity quadratic programming (LCQP). The LCQP approximation allows fast optimization for online execution while ensuring physical constraints of contacts for resulting solutions. We solve the LCQP to plan a desired next state. With a fixed control frequency, the difference from the current to the next state (Δq) represents the desired veclotity and we thus command the robot to follow it. By repeating these steps, the system will eventually reach the desired goal state as the global minimizer of the objective function.


Non-grasp pivoting to raise a box

Pivoting to raise a cylinder

Pivoting to lay a cylinder

Pivoting a box by using two fingers without strict grasping

Pivoting a box by leveraging contacts with another box


  # IROS version
    title     = {Quasistatic contact-rich manipulation via linear complementarity quadratic programming},
    author    = {Sotaro Katayama and 
                 Tatsunori Taniai and 
                 Kazutoshi Tanaka},
    booktitle = {2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2022)},
    year      = {2022},
    pages     = {203--210}
  # arXiv version
    title     = {Quasistatic contact-rich manipulation via linear complementarity quadratic programming},
    author    = {Sotaro Katayama and 
                 Tatsunori Taniai and 
                 Kazutoshi Tanaka},
    journal   = {arXiv preprint arXiv:2210.13908},
    year      = {2022}
© 2022 OMRON SINIC X Corporation, all rights reserved.