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

Action model for impulse forward dynamics in multibody systems. More...

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

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

Public Types

typedef ActionDataAbstractTpl< Scalar > ActionDataAbstract
 
typedef ActionModelAbstractTpl< Scalar > Base
 
typedef CostModelSumTpl< Scalar > CostModelSum
 
typedef ActionDataImpulseFwdDynamicsTpl< Scalar > Data
 
typedef ImpulseModelMultipleTpl< Scalar > ImpulseModelMultiple
 
typedef MathBaseTpl< Scalar > MathBase
 
typedef MathBase::MatrixXs MatrixXs
 
typedef StateMultibodyTpl< Scalar > StateMultibody
 
typedef MathBase::VectorXs VectorXs
 
- Public Types inherited from ActionModelAbstractTpl< _Scalar >
typedef ActionDataAbstractTpl< Scalar > ActionDataAbstract
 
typedef MathBaseTpl< Scalar > MathBase
 
typedef StateAbstractTpl< Scalar > StateAbstract
 
typedef MathBase::VectorXs VectorXs
 

Public Member Functions

 ActionModelImpulseFwdDynamicsTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ImpulseModelMultiple > impulses, boost::shared_ptr< CostModelSum > costs, const Scalar r_coeff=Scalar(0.), const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
 Initialize the impulse forward-dynamics action model. More...
 
virtual void calc (const boost::shared_ptr< ActionDataAbstract > &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< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
 Compute the derivatives of the impulse dynamics, and cost function. More...
 
virtual bool checkData (const boost::shared_ptr< ActionDataAbstract > &data)
 Check that the given data belongs to the impulse forward-dynamics data.
 
virtual boost::shared_ptr< ActionDataAbstractcreateData ()
 Create the impulse forward-dynamics data. More...
 
const VectorXs & get_armature () const
 Return the armature vector.
 
const boost::shared_ptr< CostModelSum > & get_costs () const
 Return the cost model.
 
const Scalar get_damping_factor () const
 Return the damping factor used in the operational space inertia matrix.
 
const boost::shared_ptr< ImpulseModelMultiple > & get_impulses () const
 Return the impulse model.
 
pinocchio::ModelTpl< Scalar > & get_pinocchio () const
 Return the Pinocchio model.
 
const Scalar get_restitution_coefficient () const
 Return the restituion coefficient.
 
virtual void print (std::ostream &os) const
 Print relevant information of the impulse forward-dynamics model. More...
 
void set_armature (const VectorXs &armature)
 Modify the armature vector.
 
void set_damping_factor (const Scalar damping)
 Modify the damping factor used in the operational space inertia matrix.
 
void set_restitution_coefficient (const Scalar r_coeff)
 Modify the restituion coefficient.
 
- Public Member Functions inherited from ActionModelAbstractTpl< _Scalar >
 ActionModelAbstractTpl (boost::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0)
 Initialize the action model. More...
 
virtual void calc (const boost::shared_ptr< ActionDataAbstract > &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< ActionDataAbstract > &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< ActionDataAbstract > &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< ActionDataAbstract > &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 ActionModelAbstractTpl< _Scalar >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
 

Protected Attributes

boost::shared_ptr< StateAbstractstate_
 Model of the state.
 
- Protected Attributes inherited from ActionModelAbstractTpl< _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 ActionModelAbstractTpl< _Scalar >
void update_has_control_limits ()
 Update the status of the control limits (i.e. if there are defined limits)
 

Detailed Description

template<typename _Scalar>
class crocoddyl::ActionModelImpulseFwdDynamicsTpl< _Scalar >

Action model for impulse forward dynamics in multibody systems.

This class implements impulse forward dynamics given a stack of rigid-impulses described in ImpulseModelMultipleTpl, i.e.,

\[ \left[\begin{matrix}\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}\mathbf{M}\mathbf{v}^- \\ -e\mathbf{J}_c\mathbf{v}^- \\\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; \(\mathbf{v}^+\), \(\mathbf{v}^-\) are the discontinuous changes in the generalized velocity (i.e., velocity before and after impact, respectively); \(\mathbf{J}_c\in\mathbb{R}^{nc\times nv}\) is the contact Jacobian expressed in the local frame; and \(\boldsymbol{\Lambda}\in\mathbb{R}^{nc}\) is the impulse vector.

The derivatives of the next state and contact impulses 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 impulse 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
ActionModelAbstractTpl, calc(), calcDiff(), createData()

Definition at line 62 of file impulse-fwddyn.hpp.

Constructor & Destructor Documentation

◆ ActionModelImpulseFwdDynamicsTpl()

ActionModelImpulseFwdDynamicsTpl ( boost::shared_ptr< StateMultibody state,
boost::shared_ptr< ImpulseModelMultiple impulses,
boost::shared_ptr< CostModelSum costs,
const Scalar  r_coeff = Scalar(0.),
const Scalar  JMinvJt_damping = Scalar(0.),
const bool  enable_force = false 
)

Initialize the impulse forward-dynamics action model.

It describes the impulse dynamics of a multibody system under rigid-contact constraints defined by ImpulseModelMultipleTpl. It computes the cost described in CostModelSumTpl.

Parameters
[in]stateState of the multibody system
[in]actuationActuation model
[in]impulsesStack of rigid impulses
[in]costsStack of cost functions
[in]r_coeffRestitution coefficient (default 0.)
[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()

virtual void calc ( const boost::shared_ptr< ActionDataAbstract > &  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 impulse dynamics.

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

Implements ActionModelAbstractTpl< _Scalar >.

◆ calcDiff()

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

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

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

Implements ActionModelAbstractTpl< _Scalar >.

◆ createData()

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

Create the impulse forward-dynamics data.

Returns
impulse forward-dynamics data

Reimplemented from ActionModelAbstractTpl< _Scalar >.

◆ print()

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

Print relevant information of the impulse forward-dynamics model.

Parameters
[out]osOutput stream object

Reimplemented from ActionModelAbstractTpl< _Scalar >.


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