The behavior of a simulation with contact will depend on three factors:
The three factors are interdependent; specific choices for one factor may require supporting changes in the other factors.
Contact modeling in Drake is controlled by a small set of parameters:
MultibodyPlant offers two different modalities to model mechanical systems in time. These are:
In this section we'll only provide a very limited distinction between a continuous and a discrete system. A complete discussion of the subject, including the modeling of hybrid systems is beyond the scope of this section and thus the interested is referred to the documentation for Simulator.
Currently, this is the preferred modality given its speed and robustness. In this modality, the system is updated through periodic updates of length time_step
. This can essentially be seen as a time-stepping strategy with a fixed time_step
. The value of time_step
is provided at construction of the MultibodyPlant. In Drake we use a custom semi-implicit Euler scheme, TAMSI, for multibody systems with regularized friction. Details for this solver are provided in the documentation for TamsiSolver and in [Castro et al., 2019].
[Castro et al., 2019] Castro, A.M, Qu, A., Kuppuswamy, N., Alspach, A., Sherman, M.A., 2019. A Transition-Aware Method for the Simulation of Compliant Contact with Regularized Friction. arXiv:1909.05700 [cs.RO].
If the time_step
defined above is specified to be exactly zero at MultibodyPlant construction, the system is modeled as continuous. What that means is that the system is modeled to follow continuous dynamics of the form ẋ = f(t, x, u)
, where x
is the state of the plant, t is time, and u is the set of externally applied inputs (either actuation or external body forces). In this mode, any of Drake's integrators can be used to advance the system forward in time. The following text outlines the implications of using particular integrators.
Integrators can be broadly categorized as one of:
While fixed time step integrators often provide faster simulations, they can miss slip/stick transitions when the stiction tolerance vₛ
of the Stribeck approximation is small (say, smaller than 1e-3 m/s). In addition, they might suffer from stability problems when the penalty forces are made stiff (see set_penetration_allowance()).
Error controlled integrators such as RungeKutta3Integrator offer a stable integration scheme by adapting the time step to satisfy a pre-specified accuracy tolerance. However, this stability comes with the price of slower simulations given that often these integrators need to take very small time steps in order to resolve stiff contact dynamics. In addition, these integrators usually perform additional computations to estimate error bounds used to determine step size.
Implicit integrators have the potential to integrate stiff continuous systems forward in time using larger time steps and therefore reduce computational cost. Thus far, with our ImplicitEulerIntegrator we have not observed this advantage for multibody systems using the Stribeck approximation.
The compliant point contact model only reports a single contact between bodies. More particularly, the contact is characterized by a single point. The point is associated with a characteristic area in the model (see above). This has two implications:
Surface-on-surface contacts
If the contact between bodies is better characterized as a surface instead of a point (such as one box lying on another), the contact point will
Both of these issues can be addressed by changing the geometry that represent the body's contact surface. For some shapes (e.g., boxes), we can introduce two sets of collision elements: discrete "points" at the corners, and a box capturing the volume (see block_for_pick_and_place.urdf
as an example). With this strategy, the contact "points" are actually small-radius spheres. The volume-capturing box should actually be inset from those spheres such that when the box is lying on a plane (such that the logical contact manifold would be a face), only the contact points make contact, providing reliable points of contact. However, for arbitrary configurations contact with the box will provide more general contact.
Next topic: Continuous Approximation of Coulomb Friction