Differential action model for contact forward dynamics in multibody systems. More...
#include <sobec/crocomplements/contact/contact-fwddyn.hpp>
Public Types | |
| typedef crocoddyl::DifferentialActionModelContactFwdDynamicsTpl< Scalar > | Base |
| typedef crocoddyl::DifferentialActionDataContactFwdDynamicsTpl< Scalar > | Data |
| typedef crocoddyl::MathBaseTpl< Scalar > | MathBase |
| typedef crocoddyl::CostModelSumTpl< Scalar > | CostModelSum |
| typedef crocoddyl::StateMultibodyTpl< Scalar > | StateMultibody |
| typedef crocoddyl::ContactModelMultipleTpl< Scalar > | crocoContactModelMultiple |
| typedef crocoddyl::ActuationModelAbstractTpl< Scalar > | ActuationModelAbstract |
| typedef crocoddyl::DifferentialActionDataAbstractTpl< Scalar > | DifferentialActionDataAbstract |
| typedef MathBase::VectorXs | VectorXs |
| typedef MathBase::MatrixXs | MatrixXs |
Public Member Functions | |
| DifferentialActionModelContactFwdDynamicsTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActuationModelAbstract > actuation, boost::shared_ptr< crocoContactModelMultiple > contacts, boost::shared_ptr< CostModelSum > costs, const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false) | |
| Initialize the contact forward-dynamics action model. More... | |
| virtual | ~DifferentialActionModelContactFwdDynamicsTpl () |
| virtual void | calcDiff (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
| Compute the derivatives of the contact dynamics, and cost function. More... | |
Public Attributes | |
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Detailed Description
template<typename _Scalar>
class sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >
Differential action model for contact forward dynamics in multibody systems.
This class is derived from crocoddyl::DifferentialActionModelContactFwdDynamicsTpl with the additional feature that it allows to compute the contact forward dynamics under sobec contact models (1D & 3D), which can be expressed in the LOCAL or LOCAL_WORLD_ALIGNED / WORLD reference frame. It amounts to add in calcDiff some terms to the RNEA derivatives (and to the contact force derivatives if a contact force cost is defined). The maths of these modifications are detailed here : https://www.overleaf.com/read/tzvrrxxtntwk
This class implements contact forward dynamics given a stack of rigid-contacts described in ContactModelMultipleTpl, i.e.,
\[ \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], \]
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 functions is implemented in CostModelSumTpl. 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.
- See also
DifferentialActionModelAbstractTpl,calc(),calcDiff(),createData()
Member Typedef Documentation
◆ ActuationModelAbstract
| typedef crocoddyl::ActuationModelAbstractTpl<Scalar> sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::ActuationModelAbstract |
◆ Base
| typedef crocoddyl::DifferentialActionModelContactFwdDynamicsTpl<Scalar> sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::Base |
◆ CostModelSum
| typedef crocoddyl::CostModelSumTpl<Scalar> sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::CostModelSum |
◆ crocoContactModelMultiple
| typedef crocoddyl::ContactModelMultipleTpl<Scalar> sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::crocoContactModelMultiple |
◆ Data
| typedef crocoddyl::DifferentialActionDataContactFwdDynamicsTpl<Scalar> sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::Data |
◆ DifferentialActionDataAbstract
| typedef crocoddyl::DifferentialActionDataAbstractTpl<Scalar> sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::DifferentialActionDataAbstract |
◆ MathBase
| typedef crocoddyl::MathBaseTpl<Scalar> sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::MathBase |
◆ MatrixXs
| typedef MathBase::MatrixXs sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::MatrixXs |
◆ StateMultibody
| typedef crocoddyl::StateMultibodyTpl<Scalar> sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::StateMultibody |
◆ VectorXs
| typedef MathBase::VectorXs sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::VectorXs |
Constructor & Destructor Documentation
◆ DifferentialActionModelContactFwdDynamicsTpl()
| sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< Scalar >::DifferentialActionModelContactFwdDynamicsTpl | ( | boost::shared_ptr< StateMultibody > | state, |
| boost::shared_ptr< ActuationModelAbstract > | actuation, | ||
| boost::shared_ptr< crocoContactModelMultiple > | contacts, | ||
| boost::shared_ptr< CostModelSum > | costs, | ||
| const Scalar | JMinvJt_damping = Scalar(0.), |
||
| const bool | enable_force = false |
||
| ) |
Initialize the contact forward-dynamics action model.
It describes the dynamics evolution of a multibody system under rigid-contact constraints defined by ContactModelMultipleTpl. It computes the cost described in CostModelSumTpl.
- Parameters
-
[in] state State of the multibody system [in] actuation Actuation model [in] contacts Stack of rigid contact [in] costs Stack of cost functions [in] JMinvJt_damping Damping term used in operational space inertia matrix (default 0.) [in] enable_force Enable the computation of the contact force derivatives (default false)
◆ ~DifferentialActionModelContactFwdDynamicsTpl()
|
virtual |
Member Function Documentation
◆ calcDiff()
|
virtual |
Compute the derivatives of the contact dynamics, and cost function.
- Parameters
-
[in] data Contact forward-dynamics data [in] x State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) [in] u Control input \(\mathbf{u}\in\mathbb{R}^{nu}\)
Member Data Documentation
◆ Scalar
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl< _Scalar >::Scalar |
The documentation for this class was generated from the following files:
- include/sobec/crocomplements/contact/contact-fwddyn.hpp
- include/sobec/crocomplements/contact/contact-fwddyn.hxx
