|
| DifferentialActionModelContactFwdDynamicsTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActuationModelAbstract > actuation, boost::shared_ptr< ContactModelMultiple > 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 void | calc (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
| Compute the system acceleration, and cost value. More...
|
|
virtual void | calc (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
| Compute the total cost value for nodes that depends only on the state. More...
|
|
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...
|
|
virtual void | calcDiff (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
| Compute the derivatives of the cost functions with respect to the state only. More...
|
|
virtual bool | checkData (const boost::shared_ptr< DifferentialActionDataAbstract > &data) |
| Check that the given data belongs to the contact forward-dynamics data.
|
|
virtual boost::shared_ptr< DifferentialActionDataAbstract > | createData () |
| Create the contact forward-dynamics data. More...
|
|
const boost::shared_ptr< ActuationModelAbstract > & | get_actuation () const |
| Return the actuation model.
|
|
const VectorXs & | get_armature () const |
| Return the armature vector.
|
|
const boost::shared_ptr< ContactModelMultiple > & | get_contacts () const |
| Return the contact model.
|
|
const boost::shared_ptr< CostModelSum > & | get_costs () const |
| Return the cost model.
|
|
const Scalar | get_damping_factor () const |
| Return the damping factor used in operational space inertia matrix.
|
|
pinocchio::ModelTpl< Scalar > & | get_pinocchio () const |
| Return the Pinocchio model.
|
|
virtual void | print (std::ostream &os) const |
| Print relevant information of the contact forward-dynamics model. More...
|
|
virtual void | quasiStatic (const boost::shared_ptr< DifferentialActionDataAbstract > &data, Eigen::Ref< VectorXs > u, const Eigen::Ref< const VectorXs > &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) |
| Computes the quasic static commands. More...
|
|
void | set_armature (const VectorXs &armature) |
| Modify the armature vector.
|
|
void | set_damping_factor (const Scalar damping) |
| Modify the damping factor used in operational space inertia matrix.
|
|
| DifferentialActionModelAbstractTpl (boost::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0) |
| Initialize the differential action model. More...
|
|
virtual void | calc (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
| Compute the system acceleration and cost value. More...
|
|
virtual void | calc (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
| Compute the total cost value for nodes that depends only on the state. More...
|
|
virtual void | calcDiff (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
| Compute the derivatives of the dynamics and cost functions. More...
|
|
virtual void | calcDiff (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
| Compute the derivatives of the cost functions with respect to the state only. More...
|
|
bool | get_has_control_limits () const |
| Indicates if there are defined control limits.
|
|
std::size_t | get_nr () const |
| Return the dimension of the cost-residual vector.
|
|
std::size_t | get_nu () const |
| Return the dimension of the control input.
|
|
const boost::shared_ptr< StateAbstract > & | get_state () const |
| Return the state.
|
|
const VectorXs & | get_u_lb () const |
| Return the control lower bound.
|
|
const VectorXs & | get_u_ub () const |
| Return the control upper bound.
|
|
virtual void | quasiStatic (const boost::shared_ptr< DifferentialActionDataAbstract > &data, Eigen::Ref< VectorXs > u, const Eigen::Ref< const VectorXs > &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) |
| Computes the quasic static commands. More...
|
|
VectorXs | quasiStatic_x (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9)) |
|
void | set_u_lb (const VectorXs &u_lb) |
| Modify the control lower bounds.
|
|
void | set_u_ub (const VectorXs &u_ub) |
| Modify the control upper bounds.
|
|
template<typename _Scalar>
class crocoddyl::DifferentialActionModelContactFwdDynamicsTpl< _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.,
\[ \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 [2]. Note that the algorithm for computing the RNEA derivatives is described in [1].
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()
Definition at line 59 of file contact-fwddyn.hpp.