Template Class ResidualModelContactForceTpl

Inheritance Relationships

Base Type

Class Documentation

template<typename _Scalar>
class ResidualModelContactForceTpl : public crocoddyl::ResidualModelAbstractTpl<_Scalar>

Define a contact force residual function.

This residual function is defined as \(\mathbf{r}=\boldsymbol{\lambda}-\boldsymbol{\lambda}^*\), where \(\boldsymbol{\lambda}, \boldsymbol{\lambda}^*\) are the current and reference spatial forces, respectively. The current spatial forces \(\boldsymbol{\lambda}\in\mathbb{R}^{nc}\) is computed by DifferentialActionModelContactFwdDynamicsTpl or ActionModelImpulseFwdDynamicTpl, with nc as the dimension of the contact.

Both residual and residual Jacobians are computed analytically, where the force vector \(\boldsymbol{\lambda}\) and its Jacobians \(\left(\frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{x}}, \frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{u}}\right)\) are computed by DifferentialActionModelContactFwdDynamicsTpl or ActionModelImpulseFwdDynamicTpl. These values are stored in a shared data (i.e., DataCollectorContactTpl or DataCollectorImpulseTpl). Note that this residual function cannot be used with other action models.

Public Types

typedef MathBaseTpl<Scalar> MathBase
typedef ResidualModelAbstractTpl<Scalar> Base
typedef ResidualDataContactForceTpl<Scalar> Data
typedef StateMultibodyTpl<Scalar> StateMultibody
typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract
typedef pinocchio::ForceTpl<Scalar> Force
typedef MathBase::VectorXs VectorXs
typedef MathBase::MatrixXs MatrixXs

Public Functions

ResidualModelContactForceTpl(std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Force &fref, const std::size_t nc, const std::size_t nu, const bool fwddyn = true)

Initialize the contact force residual model.

Note that for the inverse-dynamic cases, the control vector contains the generalized accelerations, torques, and all the contact forces.

Parameters:
  • state[in] Multibody state

  • id[in] Reference frame id

  • fref[in] Reference spatial contact force in the contact coordinates

  • nc[in] Dimension of the contact force (nc <= 6)

  • nu[in] Dimension of control vector

  • fwddyn[in] Indicates that we have a forward dynamics problem (true) or inverse dynamics (false)

ResidualModelContactForceTpl(std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, const Force &fref, const std::size_t nc)

Initialize the contact force residual model.

The default nu is obtained from StateAbstractTpl::get_nv(). Note that this constructor can be used for forward-dynamics cases only.

Parameters:
  • state[in] Multibody state

  • id[in] Reference frame id

  • fref[in] Reference spatial contact force in the contact coordinates

  • nc[in] Dimension of the contact force (nc <= 6)

virtual ~ResidualModelContactForceTpl() = default
virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &u) override

Compute the contact force residual.

The CoP residual is computed based on the \(\mathbf{A}\) matrix, the force vector is computed by DifferentialActionModelContactFwdDynamicsTpl or ActionModelImpulseFwdDynamicTpl which is stored in DataCollectorContactTpl or DataCollectorImpulseTpl.

Parameters:
  • data[in] Contact force data

  • x[in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)

  • u[in] Control input \(\mathbf{u}\in\mathbb{R}^{nu}\)

virtual void calc(const std::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x) override

Compute the residual vector for nodes that depends only on the state.

It updates the residual vector based on the state only (i.e., it ignores the contact forces). This function is used in the terminal nodes of an optimal control problem.

Parameters:
  • data[in] Residual data

  • x[in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)

virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &u) override

Compute the derivatives of the contact force residual.

The CoP residual is computed based on the \(\mathbf{A}\) matrix, the force vector is computed by DifferentialActionModelContactFwdDynamicsTpl or ActionModelImpulseFwdDynamicTpl which is stored in DataCollectorContactTpl or DataCollectorImpulseTpl.

Parameters:
  • data[in] Contact force data

  • x[in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)

  • u[in] Control input \(\mathbf{u}\in\mathbb{R}^{nu}\)

virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract> &data, const Eigen::Ref<const VectorXs> &x) override

Compute the Jacobian of the residual functions with respect to the state only.

It updates the Jacobian of the residual function based on the state only (i.e., it ignores the contact forces). This function is used in the terminal nodes of an optimal control problem.

Parameters:
  • data[in] Residual data

  • x[in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)

virtual std::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *const data) override

Create the contact force residual data.

Parameters:

data[in] shared data (it should be of type DataCollectorContactTpl)

Returns:

the residual data.

void updateJacobians(const std::shared_ptr<ResidualDataAbstract> &data)

Update the Jacobians of the contact friction cone residual.

Parameters:

data[in] Contact friction cone residual data

template<typename NewScalar>
ResidualModelContactForceTpl<NewScalar> cast() const

Cast the contact-force residual model to a different scalar type.

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

Template Parameters:

NewScalar – The new scalar type to cast to.

Returns:

ResidualModelContactForceTpl<NewScalar> A residual model with the new scalar type.

bool is_fwddyn() const

Indicates if we are using the forward-dynamics (true) or inverse-dynamics (false)

pinocchio::FrameIndex get_id() const

Return the reference frame id.

const Force &get_reference() const

Return the reference spatial contact force in the contact coordinates.

DEPRECATED ("Do not use set_id, instead create a new model", void set_id(const pinocchio::FrameIndex id);) void set_reference(const Force &reference)

Modify the reference frame id.

Modify the reference spatial contact force in the contact coordinates

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

Print relevant information of the contact-force residual.

Parameters:

os[out] Output stream object

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar

Protected Attributes

std::size_t nr_

Residual vector dimension.

std::size_t nu_

Control dimension.

std::shared_ptr<StateAbstract> state_

State description.