A Quasi-static Model and Simulation Approach for Pushing, Grasping, and Jamming

TL;DR

This paper introduces a novel quasi-static model for planar robotic manipulation that accurately captures grasping and jamming behaviors by explicitly modeling manipulator limits, enabling efficient analysis and control of complex contact interactions.

Contribution

The work presents a new quasi-static model that directly maps manipulator velocities to object motion, explicitly models limiting behaviors, and proves solution existence, enhancing analysis of contact-rich manipulation.

Findings

Model captures grasping and jamming behaviors.

Solutions are formulated as Linear Complementarity Problems.

Proves existence of solutions for any manipulator motion.

Abstract

Quasi-static models of robotic motion with frictional contact provide a computationally efficient framework for analysis and have been widely used for planning and control of non-prehensile manipulation. In this work, we present a novel quasi-static model of planar manipulation that directly maps commanded manipulator velocities to object motion. While quasi-static models have traditionally been unable to capture grasping and jamming behaviors, our approach solves this issue by explicitly modeling the limiting behavior of a velocity-controlled manipulator. We retain the precise modeling of surface contact pressure distributions and efficient computation of contact-rich behaviors of previous methods and additionally prove existence of solutions for any desired manipulator motion. We derive continuous and time-stepping formulations, both posed as tractable Linear Complementarity Problems (LCPs).

Equations

$$\vec{E}=\left[\begin{array}{ccc}\vec{e}& \vec{0}& \cdots \\ \vec{0}& \vec{e}& \\ ⋮& & \ddots \end{array}\right]\in {\mathbb{R}}^{2k\times k}.$$

$$\vec{z}\in {\mathbb{R}}^n,$$

$${\vec{z}}^T(\vec{M}\vec{z}+\vec{w})=\vec{0},$$

$$\vec{z}\ge \vec{0},$$

$$\vec{M}\vec{z}+\vec{w}\ge \vec{0},$$

$$\vec{f}\in \arg \underset{{\vec{f}}^{\prime }\in \mathcal{F}}{\min }{\vec{f}}^{\prime }\cdot \vec{v}.$$

$$\vec{0}\le {\vec{J}}_{N,o}{\vec{\dot{q}}}_o+{\vec{J}}_{N,m}{\vec{\dot{q}}}_m\perp {\vec{\lambda }}_N\ge \vec{0}.$$

$$\vec{0}\le {\vec{J}}_{T,o}{\vec{\dot{q}}}_o+{\vec{J}}_{T,m}{\vec{\dot{q}}}_m+\vec{E}\vec{\gamma }\perp {\vec{\lambda }}_T\ge \vec{0},$$

$$\vec{0}\le \vec{\mu }{\vec{\lambda }}_N-{\vec{E}}^T{\vec{\lambda }}_T\perp \vec{\gamma }\ge \vec{0}.$$

$$\exists k\ge 0,\vec{V}=k\nabla H(\vec{F}).$$

$$\vec{V}=k\widetilde{\stackrel{⃗}{A}}\vec{F}=k\widetilde{\stackrel{⃗}{A}}\widetilde{\stackrel{⃗}{R}}(q_{\theta }{)}^T{\vec{F}}_o,$$

$${\vec{\dot{q}}}_o=\widetilde{\stackrel{⃗}{R}}(q_{\theta })\vec{V}=k\widetilde{\stackrel{⃗}{R}}(q_{\theta })\widetilde{\stackrel{⃗}{A}}\widetilde{\stackrel{⃗}{R}}(q_{\theta }{)}^T{\vec{F}}_o=k\vec{A}{\vec{F}}_o,$$

$$\vec{M}$$

$$\vec{w}(\vec{u})$$

$$-{\vec{F}}_m=\frac{1}{c}{\vec{B}}^{-1}(\vec{u}-{\vec{\dot{q}}}_m),$$

$${\vec{\dot{q}}}_m$$

$$\vec{M}(c\vec{B})$$

$$\vec{w}(\vec{u})$$

$${\vec{z}}^T\vec{M}(c\vec{B})\vec{z}={\vec{F}}_o^T\vec{A}{\vec{F}}_o+{\vec{F}}_m^{T}c\vec{B}{\vec{F}}_m+{\vec{\gamma }}^T\vec{\mu }{\vec{\lambda }}_N\ge 0,$$

$${\vec{F}}_o^T\vec{A}{\vec{F}}_o+{\vec{F}}_m^{T}c\vec{B}{\vec{F}}_m+{\vec{\gamma }}^T\vec{\mu }{\vec{\lambda }}_N=0,$$

$$\vec{z}$$

$${\vec{F}}_o^T\vec{A}{\vec{F}}_o+{\vec{F}}_m^{T}c\vec{B}{\vec{F}}_m+{\vec{\gamma }}^T\vec{\mu }{\vec{\lambda }}_N+{\vec{F}}_m^T\vec{u}$$

$$c{\vec{F}}_m^T\vec{B}{\vec{F}}_m+{\vec{F}}_m^T\vec{u}$$

$${\lambda }_{min}c{\Vert {\vec{F}}_m\Vert }_2^2+{\vec{F}}_m^T\vec{u}$$

$${\lambda }_{min}c{\Vert {\vec{F}}_m\Vert }_2^2$$

$$c{\Vert {\vec{F}}_m\Vert }_2$$

$$c_i{\vec{F}}_{o,i}^T\vec{A}{\vec{F}}_{o,i}{c}_i+c_i^3{\vec{F}}_{m,i}^T\vec{B}{\vec{F}}_{m,i}+c_i{\vec{\gamma }}_i^T\vec{\mu }{\vec{\lambda }}_{N_i}{c}_i+c_i^2{\vec{F}}_{m,i}^T\vec{u}=0.$$

$$c_i{\vec{F}}_{o,i}^T\vec{A}{\vec{F}}_{o,i}{c}_i+c_i{\vec{\gamma }}_i^T\vec{\mu }{\vec{\lambda }}_{N_i}{c}_i\to 0.$$

$${\vec{F}}_{o,i}^T\vec{A}{\vec{F}}_{o,i}+{\vec{\gamma }}_i\vec{\mu }{\vec{\lambda }}_{N_i}+{\vec{F}}_{m,i}^T{\vec{\dot{q}}}_i=\vec{0},$$

$$\frac{1}{{\Vert {\vec{\lambda }}_i\Vert }_2}{\vec{F}}_{o,i}^T\vec{A}{\vec{F}}_{o,i}\frac{1}{{\Vert {\vec{\lambda }}_i\Vert }_2}\to 0,$$

$$\vec{\Delta }\vec{\varphi }={\vec{\varphi }}^{+}-{\vec{\varphi }}^{-}\approx {\frac{\vec{\partial }\vec{\varphi }}{\vec{\partial }{\vec{q}}_o}}^{-}\vec{\Delta }{\vec{q}}_o+{\frac{\vec{\partial }\vec{\varphi }}{\vec{\partial }{\vec{q}}_m}}^{-}\vec{\Delta }{\vec{q}}_m={\vec{J}}_{N,o}^{-}\vec{\Delta }{\vec{q}}_o+{\vec{J}}_{N,m}^{-}\vec{\Delta }{\vec{q}}_m,$$

$$\vec{\Delta }{\vec{q}}_o={\vec{q}}_o^{+}-{\vec{q}}_o^{-}$$

$$\vec{\Delta }{\vec{q}}_m={\vec{q}}_m^{+}-{\vec{q}}_m^{-}$$

$$\vec{0}\le {\vec{\varphi }}^{+}\perp {\vec{\Lambda }}_N\ge \vec{0},$$

$$\vec{0}\le {\vec{J}}_{T,o}\vec{\Delta }{\vec{q}}_o+{\vec{J}}_{T,m}\vec{\Delta }{\vec{q}}_m+\vec{E}\vec{\gamma }\perp {\vec{\Lambda }}_T\ge \vec{0},$$

$$\vec{0}\le \vec{\mu }{\vec{\Lambda }}_N-{\vec{E}}^T{\vec{\Lambda }}_T\perp \vec{\gamma }\ge \vec{0}.$$

$$\widetilde{\stackrel{⃗}{M}}(c\vec{B})$$

Full text

1 Introduction

As frictional contact is the fundamental driving process by which many robots are able to interact with their surroundings, it is unsurprising that its behavior is central to a large body of robotic locomotion and manipulation research (e.g. [7, 11, 19, 20, 23, 27]). However, dynamical models of these systems are inherently complex and challenging to simulate and analyze. Impacts between rigid bodies induce instantaneous jumps in velocity states and a combinatorial explosion of hybrid modes that in conjunction render application of common tools from control theory and trajectory optimization difficult. While there has been notable progress in planning through unknown contact sequences with full dynamics [19, 15, 13], model complexity has thus far still inhibited real-time usage.

Many applications in robotics involving frictional contact exhibit structure that permits partial or full circumvention of these difficulties. Particularly, we examine planar tabletop manipulation, where a manipulator effects motion of an object that rests upon a flat, frictional surface. Several results in simulation, control, and planning for such systems have been enabled by quasi-static assumptions—that if manipulator accelerations and velocities are low, a force-balance equation can approximate Netwon’s second law. This assumption enables reduced-order modeling of the object’s movement in response to contact with a manipulator driven at a particular velocity; such models also often circumvent the complexity associated with state discontinuities in dynamical approaches. Furthermore, quasi-static models often eliminate numerical sensitivity induced by the stiff dynamics of manipulators with high feedback gain controllers.

The tractability of these models has enabled impressive results in formal control analysis, task planning, and learning (e.g. [5, 10, 12]). However, the range of motion that these methods are currently able to model is limited. They are often restricted to pushing and non-prehensile motions and are completely unable to usefully express grasping or jamming; in these cases, their associated mathematical programs often yield no solutions or ambiguous behavior. Grasping and jamming objects is crucial for a wide range of robot tasks, and much work has been devoted to planning and controlling action before and after grasping events (e.g.[22, 21, 9, 30, 18]); However, much of this work can only describe static grasp configurations, and is unable to depict grasp-like behavior with sliding contacts. The process of acquiring such a grasp itself often involves jamming (e.g. when reorienting an object by pushing it up against a wall), which neither the prehensile nor static grasping models can capture. We therefore find great value in the formulation of a unified quasi-static model that can smoothly capture a complete task involving the acquisition and use of a grasp. The ambiguity in traditional approaches arises from the inconsistent assumptions of rigid bodies and perfect control of manipulator velocity. Our key insight is that by appropriately representing the manipulator’s internal controller, physically-grounded motion of the object-manipulator system is guaranteed to exist. We contribute both instantaneous velocity and time-stepping position models that formulate this behavior as linear complementarity problems, and prove existence of solutions for each.

2 Related Work

There is a significant body of research that examines manipulation from a quasi-static perspective [14, 27, 11, 23, 17]. For systems in which the object experiences frictional support, relevant research typically examines the pressure distribution supporting the object. Some earlier works provide guaranteed properties of the object’s motion without full knowledge of this distribution. Mason [14] derived the voting theorem to construct a mapping from the center of pressure to the direction of the object’s angular velocity. Lynch and Mason [12] later performed stability and controllability analysis for a manipulator pushing an object with multiple fingers. Other works alternatively contribute models that directly map manipulator joint velocities to object motion. Trinkle [27] characterized vertical planar manipulation using a nonlinear mathematical program that explicitly solved for the contact forces between the object and the manipulator. A similar, more general model for arbitrary 3D rigid multibody systems was proposed in Trinkle et al. [28]. Neither formulation can model detailed pressure distributions between surfaces, as they model friction as acting at a finite set of points.

Efficient and expressive modeling of the complex behavior of these pressure distributions was enabled by Goyal et al. [8], who define the limit surface, the bounded convex set of friction loads that the surfaces in contact might exert on each other. Zhou et al. [31] approximate this set in high detail as a semialgebraic set fitted to experimental data, and from it derive a model for the motion of the manipulator-object system. This method, however, assumes that the manipulator follows a commanded velocity exactly, and therefore does not return a solution for infeasible commands. As such commands often result in grasping in a full dynamical model, valuable behaviors are not captured. Some planning methods (e.g [6]) introduce manipulator compliance through the generalized dampers of Whitney [29], though they have not been incorporated into models capable of initiating and releasing multiple contacts.

Pang and Tedrake [17] also devise a resolution for non-existence in velocity-controlled 3D rigid quasi-static systems. They model deviations from desired velocity as a result of local elastic deformation at point contacts, and preserve realism by minimizing them in a mixed-integer quadratic program (MIQP) formulation. While their work applies to a more general class of systems, our method has three key advantages for planar manipulation: we draw model behavior from a problem class that is far more tractable than MIQPs; our proof of existence makes possible formal guarantees for controller performance as well as simulation reliability; and our inclusion of a limit-surface model allows for more realistic modeling without introducing significant complexity.

3 Background

We now describe the behavior of a frictionally-supported object with center of mass $\vec{p}$ in contact with a manipulator, depicted in Figure 1. For a more detailed treatment of rigid-body contact dynamics, we refer the reader to [1]. Coordinates of the object and manipulator configurations are denoted $\vec{q}_{o}=\begin{bmatrix}\vec{p}^{W};&q_{\theta}\end{bmatrix}\in\mathbb{R}^{3}$ and $\vec{q}_{m}\in\mathbb{R}^{n}$, respectively. The manipulator is assumed to be controlled to track commanded velocity $\vec{u}$. Frictional contact with the manipulator causes the object to move with body-axis velocity $\vec{V}=\begin{bmatrix}v_{x,b}&v_{y,b}&\dot{q}_{\theta}\end{bmatrix}^{T}$, which can be converted to the world frame velocity $\vec{\dot{q}}_{o}$ with the full-rank transformation $\vec{\dot{q}}_{o}=\tilde{\vec{R}}(q_{\theta})\vec{V}$. Contact is modeled as normal and frictional forces $\vec{\lambda}_{N}\in\mathbb{R}^{k}$ and $\vec{\lambda}_{T}\in\mathbb{R}^{2k}$ at $k$ pairs of points on the boundaries of the object and manipulator. The coefficients of friction at these points are given by the diagonal matrix $\vec{\mu}\in\mathbb{R}^{k\times k}$. Each contact’s tangential force is split into two non-negative opposing forces $\vec{\lambda}_{T_{i}}^{+}$ and $\vec{\lambda}_{T_{i}}^{-}$ (as in [1]), such that the net tangential force is equal to $\vec{\lambda}_{T_{i}}^{+}-\vec{\lambda}_{T_{i}}^{-}$. The distances between each pair of points are represented by the vector $\vec{\phi}(\vec{q}_{o},\vec{q}_{m})\in\mathbb{R}^{k}$. When the points are in contact, their separating velocity can be calculated from the generalized velocities as $\dot{\vec{\phi}}=\vec{J}_{N,o}\vec{\dot{q}}_{o}+\vec{J}_{N,m}\vec{\dot{q}}_{m}$ using Jacobian matrices $\vec{J}_{N,o}=\frac{\vec{\partial}\vec{\phi}}{\vec{\partial}\vec{q}_{o}}$ and $\vec{J}_{N,m}=\frac{\vec{\partial}\vec{\phi}}{\vec{\partial}\vec{q}_{m}}$. Similarly, there exist Jacobian matrices $\vec{J}_{T,o}$ and $\vec{J}_{T,m}$ such that the velocity at which the bodies slide against each other is given by $\vec{J}_{T,o}\vec{\dot{q}}_{o}+\vec{J}_{T,m}\vec{\dot{q}}_{m}$. For notational convenience, we will often group the normal and tangential terms as $\vec{J}_{o}=[\vec{J}_{N,o};\vec{J}_{T,o}]$, $\vec{J}_{m}=[\vec{J}_{N,m};\vec{J}_{T,m}]$, and $\vec{\lambda}=\begin{bmatrix}\vec{\lambda}_{N};\vec{\lambda}_{T}\end{bmatrix}$. Using the relationship between the contact velocities and the generalized velocities, the contact forces can be used to determine the forces acting on the object and manipulator as $\vec{F}_{o}=\vec{J}_{o}^{T}\vec{\lambda}$ and $\vec{F}_{m}=\vec{J}_{m}^{T}\vec{\lambda}$, respectively.

We also make use of the constants $\vec{e}=\begin{bmatrix}1&1\end{bmatrix}^{T}$ and the block-diagonal matrix

[TABLE]

3.1 Linear Complementarity Problems

Throughout this work, we will make regular use of linear complementarity problems (LCPs). An LCP is a particular type of mathematical program for which solutions can be efficiently computed. LCPs have been widely used by the dynamics and robotics communities for describing the effects of contact (e.g. [25, 1, 31]). Here, we briefly introduce the problem formulation and some useful properties, and we refer the reader to [3] for a more complete description.

Definition 3.1**.**

The linear complementarity problem $\mathrm{LCP}(\vec{M},\vec{w})$ for the matrix $\vec{M}\in\mathbb{R}^{n\times n}$ and vector $\vec{w}\in\mathbb{R}^{n}$ describes the mathematical program

[TABLE]

for which the set of solutions is denoted $\mathrm{SOL}(\vec{M},\vec{w})$. Constraints (3)–(5), called complementarity constraints, are often abbreviated as $\vec{0}\leq\vec{z}\perp\vec{M}\vec{z}+\vec{w}\geq\vec{0}$.

Note that vector inequalities in the above definition, as well as elsewhere in this work, are taken element-wise. We will find that for LCPs related to frictional behavior, the matrix parameter $\vec{M}$ is often copositive (i.e. $\vec{x}^{T}\vec{M}\vec{x}\geq 0$ for all $\vec{x}\geq\vec{0}$). This property is often theoretically useful, as Corollary 4.4.12 of [3], reproduced below, gives a sufficient condition for copositive LCP feasibility.

Proposition 3.2**.**

Let $\vec{w}\in\mathbb{R}^{n}$, and let $\vec{M}\in\mathbb{R}^{n\times n}$ be copositive. Suppose that for every $\vec{z}\in\mathrm{SOL}(\vec{M},\vec{0})$, we have $\vec{z}^{T}\vec{w}\geq 0$. It follows that $\mathrm{SOL}(\vec{M},\vec{w})\neq\emptyset$, and an element of $\mathrm{SOL}(\vec{M},\vec{w})$ can be discovered by Lemke’s Algorithm in finite time.

3.2 Friction at Point Contacts Between Object and Manipulator

Common to many models of friction is the maximum dissipation principle, which states that if $\mathcal{F}$ is the set of all possible forces acting at a contact, then the force at any given moment is one such that the power dissipated at the contact is maximized. For contact at a single point between two rigid bodies with relative velocity $\vec{v}$, this condition is realized as

[TABLE]

For point contacts, $\mathcal{F}$ is often modeled as a cone by Coulomb friction, which enforces the following:

• •

For each contact, either the normal velocity is zero and the normal force is non-negative, or vice versa:

[TABLE]

• •

The magnitude of the frictional force at the $j$th contact is bounded above by $\vec{\mu}_{j,j}\vec{\lambda}_{N_{j}}$. For sliding contacts, the frictional force has magnitude $\vec{\mu}_{j,j}\vec{\lambda}_{N_{j}}$ and is antiparallel to the sliding velocity. This behavior can be captured as complementarity constraints with the addition of a slack variable $\vec{\gamma}$:

[TABLE]

3.3 Friction at Contact Between Object and Surface

Coulomb friction behavior cannot readily be applied to contacts where the normal force is not concentrated at a finite set of points. Zhou et al. [31] therefore devise and experimentally validate a model that directly approximates the limit surface assuming a constant pressure distribution. They parameterize this behavior with a symmetric, scale-invariant, and strictly convex function $H(\vec{F}):\mathbb{R}^{3}\longrightarrow[0,\infty)$, defined over the space of body-axis friction wrenches $\vec{F}=\tilde{\vec{R}}(q_{\theta})^{T}\vec{F}_{o}$. The physical meaning of this function is as follows:

• •

The set of possible static friction wrenches is $\mathcal{F}=\{H(\vec{F})\leq 1\}$.

• •

If the contact is sliding, then the maximum dissipation principle requires that $\vec{F}$ be on the boundary of $\mathcal{F}$, $\{H(\vec{F})=1\}$. Furthermore, $\vec{V}$ must lie in the normal cone of $\mathcal{F}$ at $\vec{F}$.

As $\mathcal{F}$ is strictly convex, the latter condition is exactly satisfied by

[TABLE]

We note that (10) will also hold in the case of static friction with $k=0$.

3.4 Friction Behavior as an LCP

We examine the quasi-static model of Zhou et al. [31], which composes both point and surface contacts. From (10), if $H(\vec{F})$ has the ellipsoid form $\vec{F}^{T}\tilde{\vec{A}}\vec{F},\tilde{\vec{A}}\succ\vec{0}$,

[TABLE]

[TABLE]

where $\vec{A}=\tilde{\vec{R}}(q_{\theta})\tilde{\vec{A}}\tilde{\vec{R}}(q_{\theta})^{T}\succ\vec{0}$. Assuming perfect velocity control (i.e. $\vec{\dot{q}}_{m}=\vec{u}$), (7)–(9) reduce to $\mathrm{LCP}(\vec{M},\vec{w}(\vec{u}))$, a generalization of (27) in [31], where

[TABLE]

Each $\vec{z}\in\mathrm{SOL}(\vec{M},\vec{w}(\vec{u}))$ is a choice for $[k\vec{\lambda};\vec{\gamma}]$ that complies with both the point and surface contact models. While solutions for $k$ and $\vec{\lambda}$ are not computed separately, their product is sufficient to calculate object velocity. For simplicity, we will denote $\vec{z}$ as $[\vec{\lambda};\vec{\gamma}]$ for the rest of this paper. We also define $\mathcal{F}_{\vec{u}}=\{\vec{\lambda}:\exists\vec{\gamma},[\vec{\lambda};\vec{\gamma}]\in\mathrm{SOL}(\vec{M},\vec{w}(\vec{u}))\}$, the set of feasible point contact forces for command velocity $\vec{u}$. For $\vec{u}=\vec{0}$, $\mathcal{F}_{\vec{u}}=\mathcal{F}_{\vec{0}}$ is the set of admissible internal forces [16], i.e. forces that map to zero net force and torque on the object ($\vec{F}_{o}=\vec{0}$).

For non-quadratic descriptions of $H(\vec{F})$, low accuracy solutions may be computed quickly by approximating $H$ as an ellipsoid. If higher accuracy solutions are required, one may solve a sequence of programs such that $\vec{A}$ in the $j$th program is equal to the Hessian of $H$ evaluated at a solution of $(j-1)$th program.

4 Finite Velocity Feedback Quasi-statics

While the above formulation has been successful at simulating pushes [31] and planning grasps under stochasticity [30], the range of applications of this method is significantly limited due to undefined and ambiguous behaviors as displayed in Figure 2. Grasping and jamming commands may result in manipulator velocities that cannot be realized without penetrating (see Lemma 1, [31]). Additionally, when the manipulator is commanded to graze the object (that is, the commanded velocities of all the contact points are parallel to the object boundary), there can be an infinite set of possible solutions which result in wildly different motions.

The source of this ambiguity is a critical assumption of the perfect velocity control model—that there exists a feedback controller internal to the manipulator that has high enough (essentially, infinite) gain to overcome any external disturbance. From this perspective, grasping and jamming are undefined as they prescribe two unstoppable objects to oppose each other (”$\infty-\infty$”-like behavior). Additionally, while exact execution of a grazing maneuver may produce zero external disturbance for the manipulator controller to balance, a small perturbation of the commanded velocities towards the inward normal of the object surface could transform the command to an infeasible grasp or jam.

In order to resolve these issues, we explicitly interpret the quasi-statics of perfect velocity control as the limiting case of the quasi-statics of finite, stable, linear feedback. That is, we assume some relative feedback gain matrix $\vec{B}\succ\vec{0}$ and scaling factor $c$ such that the generalized force due to contact exerted on the manipulator is balanced by a feedback torque

[TABLE]

where we note that the gain is inversely proportional to $c$ and the scaling of $B$. While (13) and (14) assumed that the manipulator velocity directly tracked the desired velocity, $\vec{\dot{q}}_{m}=\vec{u}$, we instead solve (15) for $\vec{\dot{q}}_{m}$ and construct a new program $\mathrm{LCP}(\vec{M}(c\vec{B}),\vec{w}(\vec{u}))$ that accounts for the modified velocity:

[TABLE]

While we have eliminated the perfect velocity control assumption, our formulation introduces no additional computational complexity, as our LCP is of equal dimension. We now prove that our model is well-behaved in the sense that solutions are guaranteed to exist (Theorem 4.1); the manipulator velocity remains bounded as $c\rightarrow 0$ (Theorem 4.3); and solutions typically converge the perfect velocity control case when it is feasible (Theorem 4.7).

Theorem 4.1**.**

For all $(c,\vec{B},\vec{u})$, if $c>0$ and $\vec{B}\succ 0$, $\mathrm{SOL}(\vec{M}(c\vec{B}),\vec{w}(\vec{u}))\neq\emptyset$.

Proof 4.2**.**

Let $\vec{z}\geq\vec{0}$. As all the indices of $\vec{\mu}$ are non-negative, $\vec{\gamma}^{T}\vec{\mu}\vec{\lambda}_{N}\geq 0$. We also have that $\vec{F}_{o}^{T}\vec{A}\vec{F}_{o}\geq 0$ and $\vec{F}_{m}^{T}c\vec{B}\vec{F}_{m}\geq 0$ as $\vec{A}\succ 0$ and $c\vec{B}\succ 0$. Therefore,

[TABLE]

and $\vec{M}(c\vec{B})$ is copositive. Now, suppose $\vec{z}\in\mathrm{SOL}(\vec{M}(c\vec{B}),\vec{0})$. We must have

[TABLE]

which implies $\vec{F}_{m}=\vec{0}$. Therefore, $\vec{z}^{T}\vec{w}(\vec{u})=\vec{F}_{m}^{T}\vec{u}=\vec{0}$. The result follows from Proposition 3.2. ∎

If the desired velocity $\vec{u}$ lies within a neighborhood of infeasible velocities (symptomatic of grasping or jamming behavior), then as we take $c\rightarrow 0$, the force balance in (15) implies that $\vec{F}_{m}$, and therefore some of the individual finger forces, will grow unboundedly. However, the net manipulator velocity error $\vec{\dot{q}}_{m}-\vec{u}=c\vec{B}\vec{F}_{m}=\vec{B}\vec{J}_{m}^{T}(c\vec{\lambda})$ remains bounded due to the boundedness of $c\vec{F}_{m}$:

Theorem 4.3**.**

Let $\vec{B}\succ\vec{0}$. There exists $r(\vec{B})\in\mathbb{R}$ such that for all $c>0$, for all $\vec{z}\in\mathrm{SOL}(\vec{M}(c\vec{B}),\vec{w}(\vec{u}))$, $c\left\lVert\vec{F}_{m}\right\rVert_{2}\leq r(\vec{B})\left\lVert\vec{u}\right\rVert_{2}$.

Proof 4.4**.**

Let $\vec{z}\in\mathrm{SOL}(\vec{M}(c\vec{B}),\vec{w}(\vec{u}))$. From Definition 3.1, we have that

[TABLE]

Let $\lambda_{min}>0$ be the minimum eigenvalue of $\vec{B}$. Noting $\vec{A}\succ\vec{0}$, we have that

[TABLE]

∎

Intuitively, if individual finger forces grow without bound, yet the net force remains bounded, then there must be a “canceling out” effect. More precisely, the portion of $\vec{\lambda}_{i}$ that experiences this growth must be an internal force:

Theorem 4.5**.**

For all sequences $\left({\vec{z}}_{i}\right)_{i\in\mathbb{N}}$ and $\left({c}_{i}\right)_{i\in\mathbb{N}}$ such that $c_{i}\rightarrow 0$ and $\vec{z}_{i}\in\mathrm{SOL}(\vec{M}(c_{i}\vec{B}),\vec{w}(\vec{u}))$, $c_{i}\vec{\lambda}_{i}\rightarrow{\mathcal{F}}_{\vec{0}}$

Proof 4.6**.**

Let $\tilde{\mathcal{F}}_{\vec{0}}=\{\vec{\lambda}\geq\vec{0}:\vec{J}_{o}^{T}\vec{\lambda}=\vec{0}\wedge\vec{\mu}\vec{\lambda}_{N}-\vec{E}^{T}\vec{\lambda}_{T}\geq\vec{0}\}$. $\tilde{\mathcal{F}}_{\vec{0}}\subseteq{\mathcal{F}}_{\vec{0}}$ by constructing $\tilde{\vec{z}}=[\tilde{\vec{\lambda}};\vec{0}]\in\mathrm{SOL}(\vec{M}(\vec{0}),\vec{0})$ from $\tilde{\vec{\lambda}}\in\tilde{\mathcal{F}}_{\vec{0}}$. It is therefore sufficient to show $c_{i}\vec{\lambda}_{i}\rightarrow\tilde{\mathcal{F}}_{\vec{0}}$.

We first note that $c_{i}{\vec{z}}_{i}\geq\vec{0}$, and $\vec{\mu}c_{i}\vec{\lambda}_{N_{i}}-\vec{E}^{T}c_{i}\vec{\lambda}_{T_{i}}\geq\vec{0}$ follow directly from $\vec{z}_{i}\in\mathrm{SOL}(\vec{M}(c_{i}\vec{B}),\vec{w}(\vec{u}))$. Multiplying (22) by $c_{i}^{2}$, we have

[TABLE]

In the limit, the second and fourth terms in this equation vanish due to the boundedness of $c_{i}\vec{F}_{m,i}$, rendering

[TABLE]

As $\vec{A}\succ 0$, $c_{i}\vec{F}_{o,i}=\vec{J}_{o}^{T}c_{i}\vec{\lambda}_{i}\rightarrow\vec{0}$, and therefore $c_{i}\vec{\lambda}_{i}\rightarrow\tilde{\mathcal{F}}_{\vec{0}}$. ∎

$\mathcal{F}_{\vec{0}}$ can therefore be considered a basis from which one can generate the errors in the manipulator velocity; for $c$ sufficiently close to [math], there exists a $\vec{\delta}\in\mathcal{F}_{\vec{0}}$, such that $\vec{\dot{q}}_{m}-\vec{u}\approx\vec{B}\vec{J}_{m}^{T}\vec{\delta}$. When there are no non-zero internal forces, the manipulator displacement approaches zero, and the solution of the finite linear feedback model approaches the behavior for perfect velocity control:

Theorem 4.7**.**

Suppose that $\vec{M}(\vec{0})$ is chosen such that $\mathcal{F}_{\vec{0}}=\{\vec{0}\}$. For all sequences $\left({\vec{z}}_{i}\right)_{i\in\mathbb{N}}$,$\left({c}_{i}\right)_{i\in\mathbb{N}}$ such that $c_{i}\rightarrow 0$ and $\vec{z}_{i}\in\mathrm{SOL}(\vec{M}(c_{i}\vec{B}),\vec{w}(\vec{u}))$, then $\vec{\lambda}_{i}\rightarrow\mathcal{F}_{\vec{u}}$.

Proof 4.8**.**

Assume the contrary, so that there exists $\left({c}_{i}\right)_{i\in\mathbb{N}}\rightarrow 0$ and $\left({\vec{z}}_{i}\right)_{i\in\mathbb{N}}$ bounded away from $\mathrm{SOL}(\vec{M}(\vec{0}),\vec{w}(\vec{u}))$ such that $\vec{z}_{i}\in\mathrm{SOL}(\vec{M}(c_{i}\vec{B}),\vec{w}(\vec{u}))$. Letting ${\vec{\dot{q}}}_{i}=\vec{u}+{c}_{i}\vec{B}\vec{F}_{m,i}$, we have that ${\vec{z}}_{i}\in\mathrm{SOL}(\vec{M}(\vec{0}),\vec{w}({\vec{\dot{q}}}_{i}))$. ${c}_{i}{\vec{\lambda}}_{i}\rightarrow\vec{0}$, given by Theorem 4.5, implies ${\vec{\dot{q}}}_{i}\rightarrow\vec{u}$. For all $i$, we observe the complementarity condition

[TABLE]

which implies that $\vec{\gamma}_{i}\vec{\mu}\vec{\lambda}_{N_{i}}$ is bounded for bounded $\left\lVert\vec{\lambda}_{i}\right\rVert_{2}$ . As $\mathcal{F}_{\vec{0}}=\{\vec{0}\}$, for all $\vec{z}\in\mathrm{SOL}(\vec{M}(\vec{0}),\vec{0})$, we have $\vec{z}^{T}\vec{w}(\vec{u})=0$. Therefore by Proposition 3.2, $\mathcal{F}_{\vec{u}}$ and $\mathrm{SOL}(\vec{M}(\vec{0}),\vec{w}(\vec{u}))$ are non-empty.

If there were a subsequence ${\vec{z}}_{i_{j}}$ such that ${\vec{\lambda}}_{i_{j}}$ were bounded, then from (29) and the non-emptiness of $\mathcal{F}_{\vec{u}}$, ${\vec{\lambda}}_{i_{j}}$ would have a limit point in $\mathcal{F}_{\vec{u}}$, violating our assumptions. Therefore, we must have $\left\lVert{\vec{\lambda}}_{i}\right\rVert_{2}\rightarrow\infty$. Similar to Theorem 4.5, by dividing (29) by $\left\lVert\vec{\lambda}_{i}\right\rVert_{2}^{2}$,

[TABLE]

and thus $\tilde{\vec{\lambda}}_{i}=\frac{{\vec{\lambda}}_{i}}{\left\lVert\vec{\lambda}_{i}\right\rVert_{2}}\rightarrow\mathcal{F}_{\vec{0}}=\{\vec{0}\}$. But $\left\lVert\tilde{\vec{\lambda}}_{i}\right\rVert_{2}=\frac{\left\lVert\vec{\lambda}_{i}\right\rVert_{2}}{\left\lVert\vec{\lambda}_{i}\right\rVert_{2}}=1$, a contradiction. ∎

We note that $\mathcal{F}_{\vec{0}}=\{\vec{0}\}$ implies the current configuration is not a force-closure [16]. In practice, we expect the set of non-force-closure configurations that have $\mathcal{F}_{\vec{0}}\neq\{\vec{0}\}$ to be small. We also expect perfect velocity control models to behave poorly during force-closure, as they exhibit grasping behavior. Therefore, this model will behave more realistically in a variety of scenarios, with minimal accuracy loss for prehensile commands that perfect velocity control models handle well already.

5 Time-Stepping Scheme

Despite the guarantee (Theorem 4.1) that (17) and (18) provide feasible instantaneous velocity solutions, embedding the LCP into common ODE schemes is an incomplete approach, as the resulting velocities will be discontinuous whenever contact is initiated between the manipulator and objects. Anitescu and Potra [1] resolved a similar issue in their formulation for 3D multibody simulation by root-finding the first sub-time-step impact. While a similar modification could be applied to our LCP, the ability to resolve sub-time-step impacts in a single LCP would be beneficial. To that end, we take inspiration from Stewart and Trinkle [25], and instead formulate an alternative LCP that explicitly models the positions of the manipulator and object at the end of the time-step.

To construct this new program, instead of solving for the force at a particular time $t$, we solve for the the net normal and tangential impulse between times $t$ and $t+h$ at each contact, $\vec{\Lambda}_{N}$ and $\vec{\Lambda}_{T}$. Using the superscripts $-$ and $+$ to denote values calculated at the beginning and end of this interval, we linearize $\vec{\phi}$ as

[TABLE]

and we make a first-order approximation of $\vec{\Delta}\vec{q}_{o}$ and $\vec{\Delta}\vec{q}_{m}$:

[TABLE]

$\vec{\Lambda}_{N}$ and $\vec{\Lambda}_{T}$ are subject to the complementarity constraints

[TABLE]

Arranging into the standard format, we arrive at $\mathrm{LCP}(\tilde{\vec{M}}(\vec{B}),\tilde{\vec{w}}(\vec{u}^{-}))$, where

[TABLE]

With the exception of the added $\vec{\phi}^{-}$ in $\tilde{\vec{w}}$, this program is identical to an instantiation of the velocity formulation defined in (17) and (18). Noting that $\vec{\phi}^{-}\geq\vec{0}$ for any feasible initial condition, a trivial extension of Theorem 4.1 guarantees existence of solutions for all feasible initial conditions. This is a significant improvement over existing time-stepping schemes for the full dynamic behavior, as it circumvents both the expensive root-finding subroutine of [1] and the non-existance issues in [25]. However, it is important to note that if $\vec{\phi}(\vec{q})$ is non-linear, the linearization in (31) does not guarantee that the true value of $\vec{\phi}^{+}$ will be feasible (positive), even if $\vec{\phi}^{-}$ is feasible. In these cases, similar to [25], one may rectify this issue by solving a sequence of problems in a fixed-point iteration scheme, linearizing the problem about the best current estimate for $(\vec{q}_{o}^{+},\vec{q}_{m}^{+})$. This iteration can be conducted at the same time as the iteration for a non-ellipsoidal $H$. We additionally note that setting $\vec{B}=\vec{0}$ gives a time-stepping reformulation of the program in [31].

6 Examples

We provide a few examples to illustrate the capabilities and accuracy of our approach. Three examples from our open source MATLAB library111\urlhttps://github.com/mshalm/quasistaticGrasping are provided in conjunction with a video depiction222\urlhttps://www.youtube.com/watch?v=1wAH5o3OLck. Additionally, we compare the output of our model to a fully dynamic, compliant simulation using Drake [26]. All LCPs associated with our model are solved with PATH [4].

6.1 Pushing with Two Fingers

We first consider a flat disk of radius $1\,\textrm{m}$ pushed by two fully-actuated point fingers. The coefficient of friction between the fingers and the disk is set at $1$, and the force-motion map is set as $H(\vec{F}_{o})=\vec{F}_{o}^{T}\vec{F}_{o}$ such that $\nabla^{2}H=\vec{A}=\vec{I}_{3}$. We consider the configuration $\vec{q}_{m}=\begin{bmatrix}q_{x,1}&q_{y,1}&q_{x,2}&q_{y,2}\end{bmatrix}^{T}$ which directly represents the $x-y$ coordinates of each finger in a fixed frame. We assume each coordinate is controlled independently with equal gain, such that $\vec{B}=\vec{I}_{4}\,\textrm{m s}^{-1}\textrm{ N}^{-1}$.

We now empirically evaluate the validity of Theorem 4.7. Analytically, we expect that as we take ${c}_{i}\rightarrow 0$, finite feedback simulation should converge to perfect velocity command tracking. We simulate three motions over $t\in(0,10)\,\textrm{s}$ at $40\,\textrm{Hz}$ for various feedback scaling terms ${c}_{i}$, and plot the corresponding object trajectories $\vec{q}_{o,i}(t)$ in Figure 3. We compare the final configuration of each finite feedback trajectory with $\vec{q}_{o,\infty}(t)$, the trajectory resultant from executing the same manipulator command with a perfect velocity control. For sufficiently high gains, we see a linear relationship between the log of the feedback scaling term and that of $e_{i}=\left\lVert\vec{q}_{o,i}(10)-\vec{q}_{o,\infty}(10)\right\rVert_{2}$, the error in the final pose.

However, the low-gain performance, particularly on the semicircular command, showcases an important nuance in the convergent behavior described in Theorem 4.7. While the method may converge over a single time-step, differences in individual time-steps may accumulate such that there is a change in the contact mode during the motion. In these low-gain cases, the fingers tend to slide off of the sides of the object, leaving the object behind its intended goal.

We now show our formulation’s ability to simulate through jamming motions that perfect velocity control models cannot capture. We attempt to push the object into and roll along a wall. We use identical simulation parameters, and plot the results in Figure 4. We can see that the perfect velocity control simulation terminates at the collision with the wall, while the finite gain formulation not only captures the collision, but also still exhibits convergent behavior thereafter.

6.2 Polygonal Peg-in-hole

We apply our method to a peg-in-hole problem, a more complex task requiring initiation and release of several contacts and gripping motions. We simulate a new system consisting of a thin rectangular peg, two triangular manipulator fingers, and a slot into which the peg is fit. The slot has a $1\%$ tolerance on the width of the peg, the relative feedback gains are set to $\vec{B}=\vec{I}_{6}$ (in $\textrm{m s}^{-1}\textrm{ N}^{-1}$ units for linear terms and $\textrm{m}^{-1}\textrm{ s}^{-1}\textrm{ N}^{-1}$ for rotational terms), and the feedback scaling is set to $c=0.01$. In a hand-designed trajectory, the peg catches the corner of the slot in the initial insertion attempt, after which the manipulator reorients and successfully inserts it. For each time step, only bodies close enough to make contact are considered, allowing the LCPs to be kept small. We set our time step to $50\,\text{ms}$, for which PATH is able to compute solutions in $1.51\,\text{ms}$ on average. The trajectory is displayed in Figure 4.

6.3 Comparison to Full Dynamics

We now evaluate the similarity of our model to Drake [26], a framework for simulation of rigid-body dynamics with contact. We simulate a square object of mass $.01\,\textrm{kg}$ and side length $0.4\,\textrm{m}$ in contact with four manipulators, each consisting of two $1\,\textrm{m}$ long links with a circular contact at the end of the second link. The manipulators pinch the object, which is particularly numerically challenging to simulate. While the velocity commands are symmetric, each manipulator is driven with different feedback gains, causing the object to move in the $+y_{W}$ direction and spin in response to the pinch. The dynamic and quasi-static simulations exhibit qualitatively similar behavior, shown in Figure 5.

7 Discussion and Future Work

We have presented a method for generating motion using a quasi-static model for planar manipulation. By explicitly modeling manipulator feedback behavior, our method is able to synthesize physically-grounded motion in the face of infeasible velocity commands. Despite this added capability, our method is as computationally efficient as previous methods. We have validated our model theoretically and empirically by proving convergence to an established model.

An interesting property of many quasi-static manipulation models is that they allow decoupling of manipulator dynamics from object motion. However, we assert that Theorem 4.5 implies that controller choice inherently effects grasping equilibrium and is therefore an unavoidable component of a physically-grounded grasping model. We also view the decoupling of the relative feedback gains from the overall feedback intensity as essential for the accuracy of our model. As the force scaling term $k$ of (10) is not explicitly determined, even given true manipulator gains, one cannot synthesize $\vec{M}(c\vec{B})$ with complete accuracy. However, in light of Theorems 4.5 and 4.7, if the controller has high enough gain, a small enough choice for $c$ will produce accurate results. In these cases, $c$ is a numerical term rather than something physically meaningful; one should choose $c$ as small as possible without degrading numerical precision in the construction of $\vec{M}(c\vec{B})$.

One might suspect that embedding a high-gain controller into the model induces stiff behaviors, as the corresponding full dynamics are stiff. However, our quasi-static assumptions happen to eliminate this behavior. As Theorem 4.7 proves convergence to the perfect velocity control model in most cases and Theorem 4.3 proves that velocities are bounded by $\vec{u}$ otherwise, our feedback terms does not add significant stiffness. Some numerical precision may however be lost for small $c$ due to round-off error or poor conditioning of the associated LCPs. In future work, we will conduct a quantitative analysis of this behavior.

We do not expect solutions to our LCPs to be unique, as non-uniqueness is pervasive in complementarity-based contact models [2, 24]. While our model does construct a unique mapping between $\vec{F}_{m}$ and $\vec{\dot{q}}_{m}$, there are unmet assertions required for the mapping between $\vec{\dot{q}}_{o}$ and $\vec{u}$ to be unique. However, it does disambiguate some grazing cases (such as in Figure 2).

Future extension of this result to 3D motion poses significant challenges. In the 2D case, the contact between the object and the surface below generates a unique map from contact forces to object motion. In 3D motion, the manipulator must instead counteract gravity. Furthermore, quasi-static modeling cannot realistically capture certain actions; for instance, dropping the object may either result in a lack of solution, or in a $\vec{\Delta}\vec{q}_{o}$ large enough to make linearization of $\vec{\phi}$ inaccurate. Possible applications of this model include controller synthesis through sums-of-squares based Lyapunov analysis and model predictive control, as well as planning via trajectory optimization methods.

8 Acknowledgements

This material is based upon work supported by the National Science Foundation under Grant No. CMMI-1830218. The authors thank Joury Abdeljaber for her work on numerical experiments.