Contact Constraints

When a robot interacts with its environment, it does so through contacts. These contacts can be unilateral contacts, or bilateral contacts. Simply put, unilateral contacts are those the robot can only push, e.g. foot contact with the floor, and bilateral contacts are those which allow the robot to push or pull, e.g. gripping the rung of a ladder.

Todo

add citations: Following the formulations in citep{Salini2011} and citep{Saab2013}

For unilateral contact constraints, a linearized approximation of the Coulomb friction cone is employed. A friction contact constraint in the controller must ensure that the linear velocity at the contact point is zero,

(1) $\Jf_{i}(\q)\jsrd + \Jfd_{i}(\q, \jsr)\jsr = \0 \tc$

and that the wrench remains within a linearized approximation of a friction cone,

(2) $\Cf_{i}\wf_{i} \leq \0 \tp$

In (1), $\Jf$ and $\Jfd$ contain the linear components of the $i^{\text{th}}$ contact Jacobian. In (2), $\Cf_{i}$ is a matrix which linearly approximates the second-order norm cone,

(3) $\left\| \wf_{i} - (\wf_{i} \cdot \bs{\hat{n}}_{i})\bs{\hat{n}}_{i} \right\|_{2} \leq \mu_{i}(\wf_{i} \cdot \bs{\hat{n}}_{i}) \tc$

where $\wf_{i}$ is are the force components of the $i^{\text{th}}$ contact wrench, $\bs{\hat{n}}_{i}$ is the normal vector of the contact, and $\mu_{i}$ is the friction coefficient. Finally, expressing these two constraints in terms of $\optvar$ , and defining $\wf_{i} = S^{F}_{i}\optvar$ , gives the following coupled equality and inequality constraints,

(4) $\underbrace { \bmat { \Jf_{i}(\q) & \0 } }_{A^{\w}} \optvar &= \underbrace { -\Jfd_{i}(\q, \jsr)\jsr }_{\bs{b}^{\w}}$

(5) $\underbrace { \bmat { \0 & \Cf_{i}S^{F}_{i} } }_{G^{\w}} \optvar &\leq \underbrace { \0 }_{\bs{h}^{\w}} \tc$

where $S^{F}_{i}$ selects the $i^{\text{th}}$ contact force vector. Equations (4) and (5) are valid for a single contact point. For surface contacts, e.g. a foot sole, multiple points on the surface can be used for friction contact constraints — usually the four corners of the foot. Equation (4) introduces 3 equality constraints for the linear velocity of the contact point. The number of inequality constraints introduced by (5) depends on the number of polygon edges used to approximate the friction cone. Here, 6 edges are used, and because of symmetry, this introduces 3 inequality constraints per contact to the controller.

Important

To put these constraints into ORCA standard form we have,

$\bs{b}^{\w} &\leq A^{\w} \leq \bs{b}^{\w} \\ -\inf &\leq G^{\w} \optvar \leq \bs{h}^{\w}$

For bilateral contacts, it is sufficient to ensure no relative motion between the two links, $i$ and $j$ in contact. It should be noted that here a link can be some part of the environment for which a kinematic model exists. To ensure no motion between the links, the following relationship must be true,

(6) $\left( J_{i}(\q) - J_{j}(\q) \right)\jsrd + \left( \dot{J}_{i}(\q,\jsr) - \dot{J}_{j}(\q,\jsr) \right)\jsr = \0 \tc$

where $J_{i}(\q)$ , $\dot{J}_{i}(\q,\jsr)$ , $J_{j}(\q)$ , and $\dot{J}_{j}(\q,\jsr)$ , are the Jacobians and their derivatives for the $i$ textsuperscript{th} and $j$ textsuperscript{th} links respectively. Putting (6) in terms of $\optvar$ produces,

(7) $\underbrace { \bmat { \left( J_{i}(\q) - J_{j}(\q) \right) & \0 } }_{A^{bc}} \optvar = \underbrace { -\left( \dot{J}_{i}(\q,\jsr) - \dot{J}_{j}(\q,\jsr) \right)\jsr }_{\bs{b}^{bc}} \tp$

Important

To put this constraint into ORCA standard form we have,

$\bs{b}^{bc} &\leq A^{bc} \leq \bs{b}^{bc}$