crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
DifferentialActionModelContactFwdDynamicsTpl< _Scalar > Class Template Reference

Differential action model for contact forward dynamics in multibody systems. More...

#include <crocoddyl/multibody/actions/contact-fwddyn.hpp>

Inheritance diagram for DifferentialActionModelContactFwdDynamicsTpl< _Scalar >:
Collaboration diagram for DifferentialActionModelContactFwdDynamicsTpl< _Scalar >:

Public Types

typedef ActuationModelAbstractTpl< Scalar > ActuationModelAbstract
 
typedef DifferentialActionModelAbstractTpl< Scalar > Base
 
typedef ContactModelMultipleTpl< Scalar > ContactModelMultiple
 
typedef CostModelSumTpl< Scalar > CostModelSum
 
typedef DifferentialActionDataContactFwdDynamicsTpl< Scalar > Data
 
typedef DifferentialActionDataAbstractTpl< Scalar > DifferentialActionDataAbstract
 
typedef MathBaseTpl< Scalar > MathBase
 
typedef MathBase::MatrixXs MatrixXs
 
typedef StateMultibodyTpl< Scalar > StateMultibody
 
typedef MathBase::VectorXs VectorXs
 
- Public Types inherited from DifferentialActionModelAbstractTpl< _Scalar >
typedef DifferentialActionDataAbstractTpl< Scalar > DifferentialActionDataAbstract
 
typedef MathBaseTpl< Scalar > MathBase
 
typedef MathBase::MatrixXs MatrixXs
 
typedef StateAbstractTpl< Scalar > StateAbstract
 
typedef MathBase::VectorXs VectorXs
 

Public Member Functions

 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)
 Compute the total cost value for nodes that depends only on the state. 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 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 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 bool checkData (const boost::shared_ptr< DifferentialActionDataAbstract > &data)
 Check that the given data belongs to the contact forward-dynamics data.
 
virtual boost::shared_ptr< DifferentialActionDataAbstractcreateData ()
 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.
 
- Public Member Functions inherited from DifferentialActionModelAbstractTpl< _Scalar >
 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)
 Compute the total cost value for nodes that depends only on the state. 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 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 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...
 
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.
 

Public Attributes

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
 
- Public Attributes inherited from DifferentialActionModelAbstractTpl< _Scalar >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
 

Protected Attributes

std::size_t nu_
 Control dimension.
 
boost::shared_ptr< StateAbstractstate_
 < Control dimension
 
- Protected Attributes inherited from DifferentialActionModelAbstractTpl< _Scalar >
bool has_control_limits_
 Indicates whether any of the control limits is finite.
 
std::size_t nr_
 Dimension of the cost residual.
 
std::size_t nu_
 Control dimension.
 
boost::shared_ptr< StateAbstractstate_
 Model of the state.
 
VectorXs u_lb_
 Lower control limits.
 
VectorXs u_ub_
 Upper control limits.
 
VectorXs unone_
 Neutral state.
 

Additional Inherited Members

- Protected Member Functions inherited from DifferentialActionModelAbstractTpl< _Scalar >
void update_has_control_limits ()
 

Detailed Description

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.

Constructor & Destructor Documentation

◆ DifferentialActionModelContactFwdDynamicsTpl()

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.

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]stateState of the multibody system
[in]actuationActuation model
[in]contactsStack of rigid contact
[in]costsStack of cost functions
[in]JMinvJt_dampingDamping term used in operational space inertia matrix (default 0.)
[in]enable_forceEnable the computation of the contact force derivatives (default false)

Member Function Documentation

◆ calc() [1/2]

virtual void calc ( const boost::shared_ptr< DifferentialActionDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x,
const Eigen::Ref< const VectorXs > &  u 
)
virtual

Compute the system acceleration, and cost value.

It computes the system acceleration using the contact dynamics.

Parameters
[in]dataContact forward-dynamics data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
[in]uControl input \(\mathbf{u}\in\mathbb{R}^{nu}\)

◆ calc() [2/2]

virtual void calc ( const boost::shared_ptr< DifferentialActionDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x 
)
virtual

Compute the total cost value for nodes that depends only on the state.

It updates the total cost and the system acceleration is not updated as it is expected to be zero. Additionally, it does not update the contact forces. This function is used in the terminal nodes of an optimal control problem.

Parameters
[in]dataContact forward-dynamics data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)

◆ calcDiff() [1/2]

virtual void calcDiff ( const boost::shared_ptr< DifferentialActionDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x,
const Eigen::Ref< const VectorXs > &  u 
)
virtual

Compute the derivatives of the contact dynamics, and cost function.

Parameters
[in]dataContact forward-dynamics data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
[in]uControl input \(\mathbf{u}\in\mathbb{R}^{nu}\)

◆ calcDiff() [2/2]

virtual void calcDiff ( const boost::shared_ptr< DifferentialActionDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x 
)
virtual

Compute the derivatives of the cost functions with respect to the state only.

It updates the derivatives of the cost function with respect to the state only. Additionally, it does not update the contact forces derivatives. This function is used in the terminal nodes of an optimal control problem.

Parameters
[in]dataAction data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)

◆ createData()

virtual boost::shared_ptr<DifferentialActionDataAbstract> createData ( )
virtual

Create the contact forward-dynamics data.

Returns
contact forward-dynamics data

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ quasiStatic()

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) 
)
virtual

Computes the quasic static commands.

The quasic static commands are the ones produced for a the reference posture as an equilibrium point, i.e. for \(\mathbf{f}(\mathbf{q},\mathbf{v}=\mathbf{0},\mathbf{u})=\mathbf{0}\)

Parameters
[in]dataDifferential action data
[out]uQuasic static commands
[in]xState point (velocity has to be zero)
[in]maxiterMaximum allowed number of iterations
[in]tolTolerance

◆ print()

virtual void print ( std::ostream &  os) const
virtual

Print relevant information of the contact forward-dynamics model.

Parameters
[out]osOutput stream object

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.


The documentation for this class was generated from the following file: