Template Class DifferentialActionModelContactFwdDynamicsTpl

Defined in File contact-fwddyn.hpp

Inheritance Relationships

Base Type

public crocoddyl::DifferentialActionModelAbstractTpl< _Scalar > (Template Class DifferentialActionModelAbstractTpl)

Class Documentation

template<typename _Scalar> class DifferentialActionModelContactFwdDynamicsTpl : public crocoddyl::DifferentialActionModelAbstractTpl<_Scalar>

Differential action model for contact forward dynamics in multibody systems.

This class implements contact forward dynamics given a stack of rigid-contacts described in ContactModelMultipleTpl, i.e.,

\[\begin{split} \left[\begin{matrix}\dot{\mathbf{v}} \\ -\boldsymbol{\lambda}\end{matrix}\right] = \left[\begin{matrix}\mathbf{M} & \mathbf{J}^{\top}_c \\ {\mathbf{J}_{c}} & \mathbf{0} \end{matrix}\right]^{-1} \left[\begin{matrix}\boldsymbol{\tau}_b \\ -\mathbf{a}_0 \\\end{matrix}\right], \end{split}\]

where \(\mathbf{q}\in Q\), \(\mathbf{v}\in\mathbb{R}^{nv}\) are the configuration point and generalized velocity (its tangent vector), respectively; \(\boldsymbol{\tau}_b=\boldsymbol{\tau} - \mathbf{h}(\mathbf{q},\mathbf{v})\) is the bias forces that depends on the torque inputs \(\boldsymbol{\tau}\) and the Coriolis effect and gravity field \(\mathbf{h}(\mathbf{q},\mathbf{v})\); \(\mathbf{J}_c\in\mathbb{R}^{nc\times nv}\) is the contact Jacobian expressed in the local frame; and \(\mathbf{a}_0\in\mathbb{R}^{nc}\) is the desired acceleration in the constraint space. To improve stability in the numerical integration, we define PD gains that are similar in spirit to Baumgarte stabilization:

\[ \mathbf{a}_0 = \mathbf{a}_{\lambda(c)} - \alpha \,^oM^{ref}_{\lambda(c)}\ominus\,^oM_{\lambda(c)} - \beta\mathbf{v}_{\lambda(c)}, \]

where \(\mathbf{v}_{\lambda(c)}\), \(\mathbf{a}_{\lambda(c)}\) are the spatial velocity and acceleration at the parent body of the contact \(\lambda(c)\), respectively; \(\alpha\) and \(\beta\) are the stabilization gains; \(\,^oM^{ref}_{\lambda(c)}\ominus\,^oM_{\lambda(c)}\) is the \(\mathbb{SE}(3)\) inverse composition between the reference contact placement and the current one.

The derivatives of the system acceleration and contact forces are computed efficiently based on the analytical derivatives of Recursive Newton Euler Algorithm (RNEA) as described in mastalli-icra20. Note that the algorithm for computing the RNEA derivatives is described in carpentier-rss18.

The stack of cost and constraint functions are implemented in CostModelSumTpl and ConstraintModelAbstractTpl, respectively. The computation of the contact dynamics and its derivatives are carrying out inside calc() and calcDiff() functions, respectively. It is also important to remark that calcDiff() computes the derivatives using the latest stored values by calc(). Thus, we need to run calc() first.