Real-Time Multi-Contact Model Predictive Control via ADMM

In this work, we utilize complementarity problems to represent the contact forces. These models are common in modeling multi-contact robotics problems [26] and can also be efficiently learned from data [27]. The theory of linear complementarity problems (LCP) is well established [28].

Here, the vector $\lambda$ typically represents the contact forces and slack variables ([26]), $y$ represents the gap function and orthogonality constraint embeds the hybrid structure.

Linear complementarity systems (LCS) are useful because they formulate the discrete-time dynamics of a multi-contact system. We define an LCS as a difference equation coupled with a variable that is the solution of an LCP.

For a given $k$, $x_{k}$ and $u_{k}$, the corresponding complementarity variable $\lambda_{k}$ can be found by solving $\text{LCP}(Ex_{k}+Hu_{k}+c,F)$ (see Definition 1). Similarly, $x_{k+1}$ can be computed using the first equation in (1) when $x_{k},u_{k}$ and $\lambda_{k}$ are known. We focus on cases where $(x_{k})_{k\in\mathbb{N}_{0}}$ is unique, and note that this holds for the examples in the paper. While non-uniqueness can arise due to rigidity [29], control in these pathological settings is outside the scope of this paper.

In this work, we want to solve the following mathematical optimization problem:

where $N$ is the planning horizon, $Q_{k},Q_{N}$ are positive semidefinite matrices, $R_{k}$ are positive definite matrices and $\mathcal{C}$ is a convex set (e.g. input bounds, safety constraints, or goal conditions) and $\boldsymbol{x}^{T}=[x_{1}^{T},\ldots,x_{N}^{T}]$, $\boldsymbol{\lambda}^{T}=[\lambda_{0}^{T},\lambda_{1}^{T},\ldots,\lambda_{N-1}^{T}]$, $\boldsymbol{u}^{T}=[u_{0}^{T},u_{1}^{T},\ldots,u_{N-1}^{T}]$.

Given $x_{0}$, one solves the optimization (2) and applies $u_{0}$ to the plant and repeats the process in every time step in a receding horizon manner.

One straightforward, but computationally expensive, approach to solving (2) is via a mixed integer formulation which exchanges the non-convex orthogonality constraints for binary variables:

where $\mathbf{1}$ is a vector of ones, $M$ is a scalar used for the big M method and $s_{k}$ are the binary variables. This approach requires solving $2^{Nn_{\lambda}}$ quadratic programs as a worst case. This method is popular because, in practice, it is much faster than this worst case analysis. Even still, for the multi-contact problems explored in this work, directly solving (3) via state-of-the-art commercial solvers remains too slow for real-time control. Methods that learn the MIQP problem offline are promising, but this line of work requires large-scale training on every problem instance [30], [14].

Utilizing ADMM, we will solve (2) more quickly than with mixed integer formulations. Since the problem we are addressing is non-convex, our method is not guaranteed to find the global solution or converge unlike MIQP-based approaches, but is significantly faster. We rewrite (2), equivalently, in the consensus form [24] where we create copies (named $\delta_{k}$) of variables $z_{k}^{T}=[x_{k}^{T},\lambda_{k}^{T},u_{k}^{T}]$ and move the constraints into the objective function using $0-\infty$ indicator functions:

where $z^{T}=[z_{0}^{T},z_{1}^{T},\ldots,z_{N}^{T}]$, $c(z)$ is the cost function in (2), the set $\mathcal{D}$ includes the dynamics constraints:

the sets $\mathcal{H}_{k}$ represent the LCP (contact) constraints:

Note that we leverage the time-dependent structure in the complementarity constraints to separate $\delta_{k}$’s.

The general augmented Lagrangian ([21], Section 3.4.2.) for the problem in consensus form (4) is:

where $w^{T}=[w_{0}^{T},w_{1}^{T},\ldots,w_{N-1}^{T}]$, $w_{k}$ are scaled dual variables, $r_{k}=z_{k}-\delta_{k}+w_{k}$, $G_{k}$ is a positive definite matrix, and $\delta^{T}=[\delta_{0}^{T},\delta_{1}^{T},\ldots,\delta_{N-1}^{T}]$. Observe that the standard augmented Lagrangian [24] is recovered for $G_{k}=\rho_{k}I$.

In order to solve (4), we apply the classical ADMM algorithm, consisting of the following operations:

Here, (5) requires solving a quadratic program, (6) is a projection onto the LCP constraints and (7) is a dual variable update. Next, we analyze these operations in the given order.

We consider a cart-pole that can interact with soft walls as in [18, 35]. This is a benchmark in contact-based control algorithms. Here, $x^{(1)}$ represents the position of the cart, $x^{(2)}$ represents the position of the pole and $x^{(3)},x^{(4)}$ represent their velocities respectively. The forces that affect the pole are described by $\lambda^{(1)}$ and $\lambda^{(2)}$ for right and left walls respectively.

The model is linearized around $x^{(2)}=0$ where $m_{c}=0.978$ is the mass of the cart, $m_{p}=0.411$ is the combined mass of the pole and the rod, $l_{p}=0.6$ is the length of the pole, $l_{c}=0.4267$ is the length of the center of mass position, $k_{1}=k_{2}=50$ are the stiffness parameter of the walls, $d=0.35$ is the distance between the origin and the soft walls. Then, we discretize the dynamics using the explicit Euler method with time step $T_{s}=0.01$ to obtain the system matrices and use the model in [35]. We note that the MIQP formulation (as in (3)) runs at approximately $10$ Hz.

We design a controller where $\theta=10$, $G=0.1I$, $\rho_{k}=2$, and $N=10$. There is a clear trade-off between solve time and planning horizon [36]. We test all three projection methods described in Section III and report run-times (single solve of (10)) on Table I averaged for 1000 solves. We also report the average of cost-to-go value assuming all of the methods can run at $100$ Hz (only the MIQP projection cannot). Even though the MIQP projection is usually better at exploring new contacts, it does not always result in a better cost. We note that the controller can run slightly faster than $240$ Hz if LCP-based projection is used. For this example, the QP in (8) has no inequality constraints and can therefore the KKT conditions can be directly solved.

Next, we lift a rigid object upwards using four fingers. The setup for this problem is illustrated in Figure 1. The red circles indicate where the grippers interact the object and we assume that the grippers are always near the surface of the object and the force they apply on the object can be controlled. This force affects the friction between the object and grippers. Since the grippers never leave the surface, we assume that there is no rotation. The goal of this task is to lift the object vertically, while the fingers are constrained to stay close to their original locations (soft constraints shown in yellow). This task, therefore, requires finger gaiting to achieve large vertical motion of the object.

We use the formulation in [26] for modeling the system and denote the positions of the grippers as $g^{(1)}$, $g^{(2)}$ respectively and position of the object as $o$. We choose $g=9.81$ as gravitational acceleration and $\mu=1$ is the coefficient of friction for both grippers.

We design a two different controllers based on Algorithm 1 where $G=I$, $N=10$. Both controllers use the MIQP projection method since other two projection methods failed to find stable motions. For this example, we use $\theta=10$, $\rho_{k}=1.2$. Parallelization leads to $\approx 2.5$x speedup for this particular example and we are able to run at $10$ Hz. We also enforce limits:

We performed 100 trials starting from different initial conditions where $o(0)\sim U[-6,-8]$, $g^{(1)}(0)\sim U[2,3]$ , $g^{(2)}(0)\sim U[3,4]$ are uniformly distributed and both the grippers and the object have zero initial speed. The controller managed to stabilize the system in all cases. As seen in Figure 2, the grippers first throw the object into the air and then catch it followed by some finger gaiting.

In this example, we consider pivoting a rigid object that can make and break contact with the ground inspired by Hogan et. al [37]. Two fingers (indicated via blue) interact with the object as in Figure 3. The goal is to balance the rigid-object at the midpoint.

The positions of the fingers with respect to the object are described via $f_{1}$, $f_{2}$ respectively. The normal force that the fingers exert onto the box can be controlled. The center of mass position is denoted by $x$ and $y$ respectively, $\alpha$ denotes the angle with the ground and $w=1$, $h=1$ are the dimensions of the object. The coefficient of friction for the fingers are $\mu_{1}=\mu_{2}=0.1$, and the coefficient of friction with the ground is $\mu_{3}=1$. We take the gravitational acceleration as $g=9.81$ and mass of the object as $m=1$. The fingers start close to the pivot point where $f_{1}=-0.3$, $f_{2}=-0.7$ and the objects configuration is given by $x=0$, $y=1.36$ and $\alpha=0.2$. The goal is to balance the object at the midpoint ($x=0$, $y=\sqrt{2}$, $\alpha=\pi/4$) while simultaneously moving the fingers towards the end of the object $(f_{1}=f_{2}=0.9)$.

We model the system using an implicit time-stepping scheme [26]. The system has 3 contacts, where the finger contacts are represented by $3$ complementarity variables each, and the ground contact is modeled via $4$ complementarity variables. The system has ten states ($n_{x}=10$), 10 complementarity variables ($n_{\lambda}=10$) and 4 inputs ($n_{u}=4$). More concretely, it has $36$ hybrid modes.

For this example, as the LCS-representation is only an approximation, we compute a new local LCS approximation at every time step $k$. We pick $\theta=5$, $N=10$, $\rho_{k}=1.1$ and use the local LCS approximation given at time-step $k$ while planning. Parallelization leads to $\approx 4$x speedup for this particular example and controller can run around $16$ Hz.

Figure 4 demonstrates the robustness of the controller for different Gaussian disturbances (added to dynamics) with standard deviations (for $\sigma=0.1,0.5$). Note that at every time step, all positions and velocities (including angular) are affected by the process noise. The object approximately reaches the desired configuration (midpoint where $\alpha=\frac{\pi}{4}$) for $\sigma=0,0.05,0.1$ and starts failing to get close to the desired configuration for $\sigma=0.5$. Plots with $\sigma=0$ and $\sigma=0.05$ are omitted as those were similar to the one with $\sigma=0.1$. We emphasize that the process noise causes unplanned mode changes and the controller seamlessly reacts. This example demonstrates that our method works well with successive linearizations as many multi-contact systems can not be captured via a single LCS approximation.