Document the theory behind the contact handling

[Giulero]

I implemented a model that maximizes the dissipation of the kinetic energy during the contact.

The Lagrangian dynamics in presence of external forces are:

$$ M(q) \dot{\nu}+h(q, \nu) = S \tau +\sum_{k=1}^{m} J_{k}^{\top} F_{k}. $$

There are various approaches to compute the contact forces:

• Soft contact;
• Rigid contacts;
• Unilateral contact with maximum dissipation principle ⬅️ chosen approach.

Problem: Knowing $q$ and $\nu$ we want to compute the contact forces $\lambda$

Let's define the free acceleration:

$$ \dot{\nu} _f := M^{-1} (S \tau - h). $$

We want to minimize the distance with the free acceleration, subject to the contact constraint

$$ \min _{\dot{\nu}} \quad \frac{1}{2} || \dot{\nu} - \dot{\nu} _f || _{M(q)} \\\ J _{c} \dot{\nu}+\dot{J} _{c} \nu=0, $$

where $| _{M(q)}$ describes a metric induced by the kinetic energy

The solution of the problem can be retrieved solving writing the Lagrangian:

$$ L = \frac{1}{2} ||\dot{\nu} - \dot{\nu} _f || _{M(q)} - \lambda ^{\top}(J _c \dot{\nu} + \dot{J} _{c} \nu). $$

With some manipulations, we can see that the dual variable $\lambda^T$ represents the contact forces.

By solving the KKT we obtain the contact forces:

$$ \underbrace{J_c M^{-1} J_c^\top}_G \lambda + J_c \dot{\nu} _ f + \dot{J} _{c} \nu = 0 $$

$$ \lambda = - G^{-1} (J_c \dot{\nu} _ f + \dot{J} _{c} \nu). $$

This solution fulfills the rigid contact condition.

From an energetic point of view this solution minimizes :

$$ \min _{\lambda} \frac{1}{2} \lambda ^\top G \lambda + \lambda^\top (J_c \dot{\nu} _ f + \dot{J} _{c} \nu) $$

that means that the contact forces tend to maximize the dissipation of the kinetic energy.

This formulation allows us to relax the rigid contact assumption and unlocks the possibility to simulate contacts with slipping. Indeed we can complement the optimization problem with 3 additional constraints, namely:

• Coulomb friction: $\sqrt{\lambda _x ^2 + \lambda _y ^2} \le \mu \lambda _z ^2$ ➡️ linear approximation will be used
• complementarity condition: $\lambda _z \cdot c = 0$;
• $\lambda _z \ge 0$

where $c \in \text{{0,1}}$, 0 if the foot is in contact, 1 if it is not in contact.

In this way, the rigid contact constraint is relaxed (the acceleration of the foot can be nonzero) and the dissipation of the energy is still maximized.

In order to simulate a contact we must also model the impact with the ground. Let's assume that the contact is rigid and hence the acceleration of the foot is equal to zero: the velocity could be non zero and the foot penetrate the ground.

We introduce an impulse in the dynamic model and we integrate it over the duration of the impact

$$ M(\nu^{+} - \nu^{-}) = J _c^\top \mathbf{F}, $$

where $\mathbf{F}$ is the intensity if the impulsive contact. The equation express the conservation of the generalized momentum (see Hurmuzlu and Marghitu - 1994).

After the collision the foot is at rest: we impose:

$$ J _c \nu ^{+} = 0. $$

The resulting system is:

$$ \begin{bmatrix} M & - J_c^\top \\\ J _c & 0 \end{bmatrix} \begin{bmatrix} \nu^{+} \\ \mathbf{F} \end{bmatrix} = \begin{bmatrix} -M \nu ^- \\ 0 \end{bmatrix}, $$

and

$$ \nu ^+ = (I - M^{-1} J _c^\top (J _c M^{-1} J^\top)^{-1} J) \nu ^{-} $$

(only!) at the impact.

The previous formulation computes the contact wrenches when the sole frame comes in contact with the ground. The computed wrench will keep the pose of the foot constant after the collision (assuming that the contacts are rigid), even if the sole isn't parallel to the ground.

To solve this problem I extended the previous formulation to 4 vertices per foot, assuming that just a pure force will act on them.

Let's compute $J _i$ and $\dot{J} _i\nu _i$, where $i$ is one of the vertices on the foot.

The linear velocity of the vertex $i$ in the inertial frame $I$ is:

$$ {^I}\dot{\mathbf{x}} _i = {^I}\dot{\mathbf{x}} _{\text{foot}} + S({\omega} ){^I}R _{\text{foot}} {^{\text{foot}}}\mathbf{p} _i, $$

where ${^I}R _{\text{foot}}$ the rotation matrix between the foot frame and the inertial one and ${^{\text{foot}}}\mathbf{p} _i$ the coordinate of the vertex $i$ in the foot frame.

We can rewrite the equation:

$$ \begin{aligned} {^I}\dot{\mathbf{x}} _i &=J _L \nu + S{(J _A \nu)}{^I}R _{\text{foot}} {^{\text{foot}}}\mathbf{p} _i \\\ & = J _L \nu - S({^I}R _{\text{foot}} {^{\text{foot}}}\mathbf{p} _i) J _A \nu \\\ & = {(J _L - S({^I}R _{\text{foot}} {^{\text{foot}}}\mathbf{p} _i) J _A)} _{J _i} \nu \\\ & = \underbrace{\begin{bmatrix} I & -S({^I}R _{\text{foot}} {^{\text{foot}}}\mathbf{p} _i) \end{bmatrix} J _{\text{foot}}} _{J _i} \nu \end{aligned} $$

being $J _{\text{foot}} = \begin{bmatrix} J _L \\ J _A \end{bmatrix}$ the jacobian of the foot.

With the same reasoning:

$$ \dot{J} _i\nu _i = \begin{bmatrix} I & -S({^I}R _{\text{foot}} {^{\text{foot}}}\mathbf{p} _i) \end{bmatrix} \dot{J} _{\text{foot}} \nu $$

Stacking all the components:

$$ J _c = \begin{bmatrix} I & -S({^I}R _{\text{left foot}} {^{\text{left foot}}}\mathbf{p} _1) \\ \vdots & \vdots \\ I & -S({^I}R _{\text{right foot}} {^{\text{right foot}}}\mathbf{p} _n) \end{bmatrix} \begin{bmatrix} J _{\text{left foot}} \\ J _{\text{right foot}} \end{bmatrix} \qquad \dot{J} _c \nu = \begin{bmatrix} I & -S({^I}R _{\text{left foot}} {^{\text{left foot}}}\mathbf{p} _1) \\ \vdots & \vdots \\ I & -S({^I}R _{\text{right foot}} {^{\text{right foot}}}\mathbf{p} _n) \end{bmatrix} \begin{bmatrix} \dot{J} _{\text{left foot}} \nu \\ \dot{J} _{\text{right foot}} \nu \end{bmatrix} $$

with $n$ the number of vertices, $n/2$ for every foot.

With regard to the impact, instead, now we have to impose that the velocity of the vertex that is colliding with the ground must be zero. Also, the vertices that were in contact must be at rest. In this case, we select the Jacobians of the vertices that are colliding that were in contact and the condition

$$ J _c \nu ^{+} = 0. $$

will be imposed during the impact.

[Giulero]

@nunoguedelha this could be interesting!

[nunoguedelha]

CC @VenusPasandi

[VenusPasandi]

Thank you @Giulero for the nice explanation of the theory. Could you please give me some references for it?

[nunoguedelha]

Hi @Giulero , just for clarifying, the ones @VenusPasandi and me have so far are:

[1] Hurmuzlu, Yildirim, and Dan B. Marghitu. “Rigid Body Collisions of Planar Kinematic Chains With Multiple Contact Points.” The International Journal of Robotics Research 13, no. 1 (February 1994): 82–92. https://doi.org/10.1177/027836499401300106.

[2] E. Drumwright and D. A. Shell, “Modeling Contact Friction and Joint Friction in Dynamic Robotic Simulation Using the Principle of Maximum Dissipation,” in Algorithmic Foundations of Robotics IX: Selected Contributions of the Ninth International Workshop on the Algorithmic Foundations of Robotics, D. Hsu, V. Isler, J.-C. Latombe, and M. C. Lin, Eds. Berlin, Heidelberg: Springer Berlin Heidelberg, 2011, pp. 249–266.

[3] Y.-B. Jia, “Energy-Based Modeling of Tangential Compliance in 3-Dimensional Impact,” in Algorithmic Foundations of Robotics IX: Selected Contributions of the Ninth International Workshop on the Algorithmic Foundations of Robotics, D. Hsu, V. Isler, J.-C. Latombe, and M. C. Lin, Eds. Berlin, Heidelberg: Springer Berlin Heidelberg, 2011, pp. 267–284.

[4] D. E. Stewart, Rigid-Body Dynamics with Friction and Impact To cite this version : HAL Id : hal-01570533 Rigid-Body Dynamics with Friction and Impact, vol. 42, no. 1. 2000.

@VenusPasandi I just realized that you might not have the reference [4], which I had mentioned in the slides but not on the respective Gitlab issue.

[Giulero]

Hi! I would add also some practical references:

• Justin Carpentier's presentation on contacts models during the Memmo summer school;
• Michele Fiocchi's course (this one for the impact dynamics).

There is also chapter 11 of Featherstone's Rigid Body Dynamics Algorithms, but I just skimmed over it.

[VenusPasandi]

Hi guys, Maybe it is better to write the theory in the wiki page and close this issue. what do u think?

[nunoguedelha]

I think it's a good idea. It can always be gradually improved there, eventually via new issues if we consider them as an intermediate step for updating the wiki.