Crocoddyl
 
Loading...
Searching...
No Matches
DifferentialActionModelContactInvDynamicsTpl< _Scalar > Class Template Reference

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

#include <contact-invdyn.hpp>

Inheritance diagram for DifferentialActionModelContactInvDynamicsTpl< _Scalar >:
DifferentialActionModelAbstractTpl< _Scalar > DifferentialActionModelBase

Classes

class  ResidualModelActuation
 Actuation residual. More...
 
class  ResidualModelContact
 Contact-acceleration residual. More...
 

Public Types

typedef ActuationModelAbstractTpl< Scalar > ActuationModelAbstract
 
typedef DifferentialActionModelAbstractTpl< Scalar > Base
 
typedef ConstraintModelManagerTpl< Scalar > ConstraintModelManager
 
typedef ContactItemTpl< Scalar > ContactItem
 
typedef ContactModelMultipleTpl< Scalar > ContactModelMultiple
 
typedef CostModelSumTpl< Scalar > CostModelSum
 
typedef DifferentialActionDataContactInvDynamicsTpl< Scalar > Data
 
typedef DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
 
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 ScalarSelector< Scalar >::type ScalarType
 
typedef StateAbstractTpl< Scalar > StateAbstract
 
typedef MathBase::VectorXs VectorXs
 

Public Member Functions

 DifferentialActionModelContactInvDynamicsTpl (std::shared_ptr< StateMultibody > state, std::shared_ptr< ActuationModelAbstract > actuation, std::shared_ptr< ContactModelMultiple > contacts, std::shared_ptr< CostModelSum > costs)
 Initialize the contact inverse-dynamics action model.
 
 DifferentialActionModelContactInvDynamicsTpl (std::shared_ptr< StateMultibody > state, std::shared_ptr< ActuationModelAbstract > actuation, std::shared_ptr< ContactModelMultiple > contacts, std::shared_ptr< CostModelSum > costs, std::shared_ptr< ConstraintModelManager > constraints)
 Initialize the contact inverse-dynamics action model.
 
virtual void calc (const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
 
virtual void calc (const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
 Compute the system acceleration, cost value and constraint residuals.
 
virtual void calcDiff (const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
 
virtual void calcDiff (const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
 Compute the derivatives of the dynamics, cost and constraint functions.
 
template<typename NewScalar >
DifferentialActionModelContactInvDynamicsTpl< NewScalar > cast () const
 Cast the contact-invdyn model to a different scalar type.
 
virtual bool checkData (const std::shared_ptr< DifferentialActionDataAbstract > &data) override
 Checks that a specific data belongs to the contact inverse-dynamics model.
 
virtual std::shared_ptr< DifferentialActionDataAbstractcreateData () override
 Create the contact inverse-dynamics data.
 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CROCODDYL_DERIVED_CAST (DifferentialActionModelBase, DifferentialActionModelContactInvDynamicsTpl) typedef _Scalar Scalar
 
const std::shared_ptr< ActuationModelAbstract > & get_actuation () const
 Return the actuation model.
 
const std::shared_ptr< ConstraintModelManager > & get_constraints () const
 Return the constraint model manager.
 
const std::shared_ptr< ContactModelMultiple > & get_contacts () const
 Return the contact model.
 
const std::shared_ptr< CostModelSum > & get_costs () const
 Return the cost model.
 
virtual const VectorXs & get_g_lb () const override
 Return the lower bound of the inequality constraints.
 
virtual const VectorXs & get_g_ub () const override
 Return the upper bound of the inequality constraints.
 
virtual std::size_t get_ng () const override
 Return the number of inequality constraints.
 
virtual std::size_t get_ng_T () const override
 Return the number of equality terminal constraints.
 
virtual std::size_t get_nh () const override
 Return the number of equality constraints.
 
virtual std::size_t get_nh_T () const override
 Return the number of equality terminal constraints.
 
pinocchio::ModelTpl< Scalar > & get_pinocchio () const
 Return the Pinocchio model.
 
virtual void print (std::ostream &os) const override
 Print relevant information of the contact inverse-dynamics model.
 
virtual void quasiStatic (const std::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)) override
 Computes the quasic static commands.
 
- Public Member Functions inherited from DifferentialActionModelAbstractTpl< _Scalar >
 DifferentialActionModelAbstractTpl (std::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0, const std::size_t ng=0, const std::size_t nh=0, const std::size_t ng_T=0, const std::size_t nh_T=0)
 Initialize the differential action model.
 
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 std::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.
 
VectorXs quasiStatic_x (const std::shared_ptr< DifferentialActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9))
 
void set_g_lb (const VectorXs &g_lb)
 Modify the lower bound of the inequality constraints.
 
void set_g_ub (const VectorXs &g_ub)
 Modify the upper bound of the inequality constraints.
 
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.
 

Protected Attributes

VectorXs g_lb_
 Lower bound of the inequality constraints.
 
VectorXs g_ub_
 < Lower bound of the inequality constraints
 
std::size_t ng_
 < Upper bound of the inequality constraints
 
std::size_t nh_
 < Number of inequality constraints
 
std::size_t nu_
 < Number of equality constraints
 
std::shared_ptr< StateAbstractstate_
 < Control dimension
 
- Protected Attributes inherited from DifferentialActionModelAbstractTpl< _Scalar >
VectorXs g_lb_
 Lower bound of the inequality constraints.
 
VectorXs g_ub_
 Lower bound of the inequality constraints.
 
bool has_control_limits_
 
std::size_t ng_
 Number of inequality constraints.
 
std::size_t ng_T_
 Number of inequality terminal constraints.
 
std::size_t nh_
 Number of equality constraints.
 
std::size_t nh_T_
 Number of equality terminal constraints.
 
std::size_t nr_
 Dimension of the cost residual.
 
std::size_t nu_
 Control dimension.
 
std::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

- Public Attributes inherited from DifferentialActionModelAbstractTpl< _Scalar >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
 
- Protected Member Functions inherited from DifferentialActionModelAbstractTpl< _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::DifferentialActionModelContactInvDynamicsTpl< _Scalar >

Differential action model for contact inverse dynamics in multibody systems.

This class implements forward kinematic with holonomic contact constraints (defined at the acceleration level) and inverse-dynamics computation using the Recursive Newton Euler Algorithm (RNEA). The stack of cost and constraint functions are implemented in CostModelSumTpl and ConstraintModelManagerTpl, respectively. The acceleration and contact forces are decision variables defined as the control inputs, and the under-actuation and contact constraint are under the name tau and its frame name, thus the user is not allowed to use it.

Additionally, it is important to note that calcDiff() computes the derivatives using the latest stored values by calc(). Thus, we need to first run calc().

See also
DifferentialActionModelAbstractTpl, calc(), calcDiff(), createData()

Definition at line 44 of file contact-invdyn.hpp.

Member Typedef Documentation

◆ Base

template<typename _Scalar >
typedef DifferentialActionModelAbstractTpl<Scalar> Base

Definition at line 52 of file contact-invdyn.hpp.

◆ Data

template<typename _Scalar >
typedef DifferentialActionDataContactInvDynamicsTpl<Scalar> Data

Definition at line 53 of file contact-invdyn.hpp.

◆ StateMultibody

template<typename _Scalar >
typedef StateMultibodyTpl<Scalar> StateMultibody

Definition at line 54 of file contact-invdyn.hpp.

◆ ActuationModelAbstract

template<typename _Scalar >
typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract

Definition at line 55 of file contact-invdyn.hpp.

◆ CostModelSum

template<typename _Scalar >
typedef CostModelSumTpl<Scalar> CostModelSum

Definition at line 56 of file contact-invdyn.hpp.

◆ ConstraintModelManager

template<typename _Scalar >
typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager

Definition at line 57 of file contact-invdyn.hpp.

◆ ContactModelMultiple

template<typename _Scalar >
typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple

Definition at line 58 of file contact-invdyn.hpp.

◆ DataCollectorAbstract

template<typename _Scalar >
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract

Definition at line 59 of file contact-invdyn.hpp.

◆ DifferentialActionDataAbstract

template<typename _Scalar >
typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract

Definition at line 61 of file contact-invdyn.hpp.

◆ ContactItem

template<typename _Scalar >
typedef ContactItemTpl<Scalar> ContactItem

Definition at line 62 of file contact-invdyn.hpp.

◆ MathBase

template<typename _Scalar >
typedef MathBaseTpl<Scalar> MathBase

Definition at line 63 of file contact-invdyn.hpp.

◆ VectorXs

template<typename _Scalar >
typedef MathBase::VectorXs VectorXs

Definition at line 64 of file contact-invdyn.hpp.

◆ MatrixXs

template<typename _Scalar >
typedef MathBase::MatrixXs MatrixXs

Definition at line 65 of file contact-invdyn.hpp.

Constructor & Destructor Documentation

◆ DifferentialActionModelContactInvDynamicsTpl() [1/2]

template<typename _Scalar >
DifferentialActionModelContactInvDynamicsTpl ( std::shared_ptr< StateMultibody state,
std::shared_ptr< ActuationModelAbstract actuation,
std::shared_ptr< ContactModelMultiple contacts,
std::shared_ptr< CostModelSum costs 
)

Initialize the contact inverse-dynamics action model.

It describes the kinematic evolution of the multibody system with contacts, and computes the needed torques using inverse-dynamics.

Parameters
[in]stateState of the multibody system
[in]actuationActuation model
[in]contactsMultiple contacts
[in]costsCost model

◆ DifferentialActionModelContactInvDynamicsTpl() [2/2]

template<typename _Scalar >
DifferentialActionModelContactInvDynamicsTpl ( std::shared_ptr< StateMultibody state,
std::shared_ptr< ActuationModelAbstract actuation,
std::shared_ptr< ContactModelMultiple contacts,
std::shared_ptr< CostModelSum costs,
std::shared_ptr< ConstraintModelManager constraints 
)

Initialize the contact inverse-dynamics action model.

Parameters
[in]stateState of the multibody system
[in]actuationActuation model
[in]contactsMultiple contacts
[in]costsCost model
[in]constraintsConstraints model

Member Function Documentation

◆ calc() [1/2]

template<typename _Scalar >
virtual void calc ( const std::shared_ptr< DifferentialActionDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x,
const Eigen::Ref< const VectorXs > &  u 
)
overridevirtual

Compute the system acceleration, cost value and constraint residuals.

It extracts the acceleration value from control vector and also computes the cost and constraints.

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

Implements DifferentialActionModelAbstractTpl< _Scalar >.

◆ calc() [2/2]

template<typename _Scalar >
virtual void calc ( const std::shared_ptr< DifferentialActionDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x 
)
overridevirtual

◆ calcDiff() [1/2]

template<typename _Scalar >
virtual void calcDiff ( const std::shared_ptr< DifferentialActionDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x,
const Eigen::Ref< const VectorXs > &  u 
)
overridevirtual

Compute the derivatives of the dynamics, cost and constraint functions.

It computes the partial derivatives of the dynamical system and the cost and contraint functions. It assumes that calc() has been run first. This function builds a quadratic approximation of the time-continuous action model (i.e., dynamical system, cost and constraint functions).

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

Implements DifferentialActionModelAbstractTpl< _Scalar >.

◆ calcDiff() [2/2]

template<typename _Scalar >
virtual void calcDiff ( const std::shared_ptr< DifferentialActionDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x 
)
overridevirtual

◆ createData()

template<typename _Scalar >
virtual std::shared_ptr< DifferentialActionDataAbstract > createData ( )
overridevirtual

Create the contact inverse-dynamics data.

Returns
contact inverse-dynamics data

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ cast()

template<typename _Scalar >
template<typename NewScalar >
DifferentialActionModelContactInvDynamicsTpl< NewScalar > cast ( ) const

Cast the contact-invdyn model to a different scalar type.

It is useful for operations requiring different precision or scalar types.

Template Parameters
NewScalarThe new scalar type to cast to.
Returns
DifferentialActionModelContactInvDynamicsTpl<NewScalar> A differential-action model with the new scalar type.

◆ checkData()

template<typename _Scalar >
virtual bool checkData ( const std::shared_ptr< DifferentialActionDataAbstract > &  data)
overridevirtual

Checks that a specific data belongs to the contact inverse-dynamics model.

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ quasiStatic()

template<typename _Scalar >
virtual void quasiStatic ( const std::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) 
)
overridevirtual

Computes the quasic static commands.

The quasic static commands are the ones produced for a reference posture as an equilibrium point with zero acceleration, i.e., for \(\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\)

Parameters
[in]dataContact inverse-dynamics data
[out]uQuasic-static commands
[in]xState point (velocity has to be zero)
[in]maxiterMaximum allowed number of iterations (default 100)
[in]tolTolerance (default 1e-9)

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ get_ng()

template<typename _Scalar >
virtual std::size_t get_ng ( ) const
overridevirtual

Return the number of inequality constraints.

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ get_nh()

template<typename _Scalar >
virtual std::size_t get_nh ( ) const
overridevirtual

Return the number of equality constraints.

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ get_ng_T()

template<typename _Scalar >
virtual std::size_t get_ng_T ( ) const
overridevirtual

Return the number of equality terminal constraints.

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ get_nh_T()

template<typename _Scalar >
virtual std::size_t get_nh_T ( ) const
overridevirtual

Return the number of equality terminal constraints.

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ get_g_lb()

template<typename _Scalar >
virtual const VectorXs & get_g_lb ( ) const
overridevirtual

Return the lower bound of the inequality constraints.

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ get_g_ub()

template<typename _Scalar >
virtual const VectorXs & get_g_ub ( ) const
overridevirtual

Return the upper bound of the inequality constraints.

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

◆ print()

template<typename _Scalar >
virtual void print ( std::ostream &  os) const
overridevirtual

Print relevant information of the contact inverse-dynamics model.

Parameters
[out]osOutput stream object

Reimplemented from DifferentialActionModelAbstractTpl< _Scalar >.

Member Data Documentation

◆ g_lb_

template<typename _Scalar >
VectorXs g_lb_
protected

Lower bound of the inequality constraints.

Definition at line 369 of file diff-action-base.hpp.

◆ g_ub_

template<typename _Scalar >
VectorXs g_ub_
protected

< Lower bound of the inequality constraints

Definition at line 370 of file diff-action-base.hpp.

◆ ng_

template<typename _Scalar >
std::size_t ng_
protected

< Upper bound of the inequality constraints

Definition at line 363 of file diff-action-base.hpp.

◆ nh_

template<typename _Scalar >
std::size_t nh_
protected

< Number of inequality constraints

Definition at line 364 of file diff-action-base.hpp.

◆ nu_

template<typename _Scalar >
std::size_t nu_
protected

< Number of equality constraints

Definition at line 361 of file diff-action-base.hpp.

◆ state_

template<typename _Scalar >
std::shared_ptr<StateAbstract> state_
protected

< Control dimension

Definition at line 367 of file diff-action-base.hpp.


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