Add new `ViscoElasticContacts`

[diegoferigo]

WIP

---

📚 Documentation preview 📚: https://jaxsim--248.org.readthedocs.build//248/

[xela-95]

Hi @diegoferigo, another early comment since I'm testing this contact model: in jaxsim.api.ode.system_dynamics @ https://github.com/ami-iit/jaxsim/blob/986854e16ba0f37c21a673aaabe15d1211e71c49/src/jaxsim/api/ode.py#L378-L387 we should add a case for this class of contact model.

From collidable_point_dynamics I can see this model outputs some auxiliary data, that should be extracted: https://github.com/ami-iit/jaxsim/blob/986854e16ba0f37c21a673aaabe15d1211e71c49/src/jaxsim/api/contact.py#L235-L236

[diegoferigo]

[...] we should add a case for this class of contact model.

From collidable_point_dynamics I can see this model outputs some auxiliary data, that should be extracted: [...]

Thanks @xela-95 for the feedback. The dictionary returned by the visco-elastic contact model is somewhat different then the one returned by the soft contact model. The latter returns $\dot{\mathbf{m}}$ (the derivative of the material deformation) that needs to be integrated together with the generalized state $(\mathbf{q}, \, \boldsymbol{\nu})$. The visco-elastic contacts, instead, directly compute $\mathbf{m}(t+\Delta t)$, therefore we need to override $\mathbf{m}$ of the extended state similarly than the velocity reset done in by the rigid contacts.

Regardless, I'm still unsure how to integrate the new visco-elastic contacts with the default jaxsim.model.step function. What I just wrote is only necessary when the default integrators are used. The best way to exploit this new contact model, however, is to use its own integrator jaxsim.rbda.contacts.visco_elastic.step, that integrates both the generalized position and velocity using the contact forces computed explicitly with the continuous exponential integrator.

[traversaro]

Regardless, I'm still unsure how to integrate the new visco-elastic contacts with the default jaxsim.model.step function. What I just wrote is only necessary when the default integrators are used. The best way to exploit this new contact model, however, is to use its own integrator jaxsim.rbda.contacts.visco_elastic.step, that integrates both the generalized position and velocity using the contact forces computed explicitly with the continuous exponential integrator.

Perhaps we can simply print a nice error if jaxsim.model.step is called with visco-elastic contacts ?

[diegoferigo]

Regardless, I'm still unsure how to integrate the new visco-elastic contacts with the default jaxsim.model.step function. What I just wrote is only necessary when the default integrators are used. The best way to exploit this new contact model, however, is to use its own integrator jaxsim.rbda.contacts.visco_elastic.step, that integrates both the generalized position and velocity using the contact forces computed explicitly with the continuous exponential integrator.

Perhaps we can simply print a nice error if jaxsim.model.step is called with visco-elastic contacts ?

After my comment, I already implemented a jit-compatible exception in this PR that is raised when the new contact model is used with the regular integrators.

https://github.com/ami-iit/jaxsim/blob/2bd727b78d20ac234ee6bcaf6f9460cc9c3d6727/src/jaxsim/api/contact.py#L212-L220

[diegoferigo]

@andreadelprete you might be interested in having a look at this PR. After we discussed about this methodology f2f, I was interested in drafting an implementation. In open-loop, it seems working pretty well. We are investigating if we can run our closed-loop control stack that currently works using both RigidContacts and RelaxedRigidContacts (as shown in https://github.com/ami-iit/jaxsim/pull/251).

Note that, as I wrote in the PR description, I updated the formulation to properly account for static friction and non-linear contact models. Performance are not yet the best due to the usage of the plain jax.scipy.linalg.expm, but it's a good starting point for playing with this integration method. Note also that we use our own estimation of the spring and damper parameters, that seemed to be quite effective also in this case.

Edit: last comment, the video reported above was recorded by running a simulation on GPU with 32-bits floats :)

[andreadelprete]

Hi @diegoferigo , thanks for pointing this out to me. Actually @DanielePucci has been bragging about this work with me for the last two weeks, so I was curious to see it! :)

3. The new formulation does not integrate the contact dynamics in terms of displacement w.r.t. the anchor point $\mathbf{x} = [\Delta \mathbf{p}; \, \Delta \dot{\mathbf{p}}],$ but it integrates the contact dynamics ignoring the anchor point entirely, therefore $\mathbf{x} = [\mathbf{p}; \, \dot{\mathbf{p}}].$ This enables to do not keep track of the location where the contact was made.

This is interesting, but I really don't get how you could achieve this. I guess I could understand by looking at the code, but that would take me a long time. Maybe you can explain me here instead?

4. The tangential displacement to obtain the $\Delta$ variables are computed by considering an additional state $\mathbf{m}$ of the terrain deformation (that, in a sense, replaces the need of keeping track of the contact creation). This workaround was inspired by the soft-contact model already implemented in the soft contacts of JaxSim. For this reason, the state of the contact dynamics becomes $\mathbf{x} = [\mathbf{p}; \, \dot{\mathbf{p}}; \, \mathbf{m}].$

Nice that you changed the tangential force computation. I was not completely satisfied by our results when slippage occured, so I expect you probably improved this.

I adapted the sticking/slipping logic used by the default soft contact model of JaxSim to this new contact model, and I obtained sound theoretical results (that also works well in practice, paying the price of a larger $A$ matrix used in $\exp^{At}$).

How much larger?

If one day you write the theory down, I'd love to have a look.

Andrea

[diegoferigo]

3. The new formulation does not integrate the contact dynamics in terms of displacement w.r.t. the anchor point $\mathbf{x} = [\Delta \mathbf{p}; , \Delta \dot{\mathbf{p}}],$ but it integrates the contact dynamics ignoring the anchor point entirely, therefore $\mathbf{x} = [\mathbf{p}; , \dot{\mathbf{p}}].$ This enables to do not keep track of the location where the contact was made.

This is interesting, but I really don't get how you could achieve this. I guess I could understand by looking at the code, but that would take me a long time. Maybe you can explain me here instead?

This is a workaround that we were already using in our simple SoftContacts model[^1]. It is also the methodology that enables the new formulation of sticking/slipping transitions.

The main intuition is that, instead of detecting/saving/updating $\mathbf{p}_0$, we introduce a new variable $\mathbf{m}$ that models the material deformation. Contrarily to $\mathbf{p}_0$, that is static and is updated only when a new contact is made, we consider $\mathbf{m}$ as a variable with its own dynamics $\dot{\mathbf{m}}$. Our non-linear contact model takes care to select the right dynamics of the material deformation depending on the contact status (off/sticking/slipping). You can grasp the intuition by considering that you can kind-of compute the displacement between the current position of the contact point w.r.t. $\mathbf{p}_0$ by introducing an extra variable in your state that integrates step-after-step the contact velocity. This is usually an approximation with discrete integrators, however with the exponential integration it becomes much more precise.

The original idea was taken from this paper[^2], that I extended to non-flat surfaces in my PhD thesis[^3]. The resulting system that allows for sticking/slipping can be seen as a spring/damper/clutch (Fig. 12 of the Azad paper):

How much larger?

We include this extra $\mathbf{m}$ variable in the system that is integrated with the matrix exponential. Therefore, the matrix $A$ of Eq. 11 of your paper is 50% larger, since the state vector in our case becomes $\mathbf{x} = [\mathbf{p}, \, \dot{\mathbf{p}}, \, \mathbf{m}]$.

[^1]: This contact model operates independently on all contact points. Is does not consider them attached to a multibody system. [^2]: Azad and Featherstone, Modeling the contact between a rolling sphere and a compliant ground plane, 2010, url. [^3]: Ferigo, Chapter 7, Simulation Architectures for Reinforcement Learning applied to Robotics, 2022, url.

[andreadelprete]

You can grasp the intuition by considering that you can kind-of compute the displacement between the current position of the contact point w.r.t. p0\mathbf{p}_0 by introducing an extra variable in your state that integrates step-after-step the contact velocity.

Ok, got it. So you get rid of p0 but you introduce m, so there is no gain in terms of quantity of information that you have to remember. The gain is regarding the sticking-slipping dynamics.

We include this extra m\mathbf{m} variable in the system that is integrated with the matrix exponential. Therefore, the matrix AA of Eq. 11 of your paper is 50% larger, since the state vector in our case becomes x=[p,p˙,m]\mathbf{x} = [\mathbf{p}, , \dot{\mathbf{p}}, , \mathbf{m}].

Ok. Unfortunately 50% larger is a significant increase, but if that's the price to pay to get a nice stick-slip behavior it's definitely worth it!

Thanks for the quick and clear reply!

Andrea

[diegoferigo]

Ok, got it. So you get rid of p0 but you introduce m, so there is no gain in terms of quantity of information that you have to remember. The gain is regarding the sticking-slipping dynamics.

Ok. Unfortunately 50% larger is a significant increase, but if that's the price to pay to get a nice stick-slip behavior it's definitely worth it!

Exactly, there is no real workaround to obtain sticking/slipping without somehow tracking the point displacement. You can find different ways to accumulate/compute this displacement, but if this information (corresponding to the tangential terrain direction) is used together with a second Hunt/Crossley-like model to compute the friction force, there's no really any solution I can think of that allows us to remove its need.

I agree that 50% more rows and columns is a significant increase, and this is the real trade-off of my proposed changes. In a sense, the formulation of this PR provides a self-contained solution without tricks that happen outside the contact model itself to obtain a behavior that intrinsically supports stick/slip transitions operating with a real friction cone (no pyramid or other approximations).

In a JAX-based context, this approach seems to be beneficial under many terms. I see this methodology as something that is more complex than our simple SoftContacts model (plain Hunt/Crossley model) since these visco-elastic contacts account for the actual robot dynamics projected to the contact space. There's more computation involved (Delassus matrix and exponential integration), but that's ok. The main benefit I see is that this exponential integration represents, similarly to SoftContacts an alternative contact model that does not rely on an iterative solver. This is really relevant for our context, as we are noticing that other contact models using either QP solvers (RigidContacts) or L-BFGS (RelaxedRigidContacts) currently run pretty slow and do not scale well with neither the number of contact points nor the number of DoFs.

Anyway, thanks for pointing me out to this methodology. I had fun studying the theory and implementing it, linearization and exponential integration brought me back to my time at university :)