Template Class DifferentialActionModelFreeInvDynamicsTpl
Defined in File free-invdyn.hpp
Nested Relationships
Nested Types
Inheritance Relationships
Base Type
public crocoddyl::DifferentialActionModelAbstractTpl< _Scalar >(Template Class DifferentialActionModelAbstractTpl)
Class Documentation
-
template<typename _Scalar>
class DifferentialActionModelFreeInvDynamicsTpl : public crocoddyl::DifferentialActionModelAbstractTpl<_Scalar> Differential action model for free inverse dynamics in multibody systems.
This class implements forward kinematic with an inverse-dynamics computed using the Recursive Newton Euler Algorithm (RNEA). The stack of cost and constraint functions are implemented in
CostModelSumTplandConstraintModelManagerTpl, respectively. The accelerations are the decision variables defined as the control inputs, and the under-actuation constraint is under the nametau, thus the user is not allowed to use it.Public Types
-
typedef MathBaseTpl<Scalar> MathBase
-
typedef DifferentialActionModelAbstractTpl<Scalar> Base
-
typedef DifferentialActionDataFreeInvDynamicsTpl<Scalar> Data
-
typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract
-
typedef CostModelSumTpl<Scalar> CostModelSum
-
typedef ConstraintModelManagerTpl<Scalar> ConstraintModelManager
-
typedef StateMultibodyTpl<Scalar> StateMultibody
-
typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract
-
typedef ConstraintModelResidualTpl<Scalar> ConstraintModelResidual
Public Functions
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW CROCODDYL_DERIVED_CAST (DifferentialActionModelBase, DifferentialActionModelFreeInvDynamicsTpl) typedef _Scalar Scalar
Initialize the free inverse-dynamics action model.
It describes the kinematic evolution of the multibody system and computes the needed torques using inverse dynamics.
- Parameters:
state – [in] State of the multibody system
actuation – [in] Actuation model
costs – [in] Cost model
Initialize the free inverse-dynamics action model.
- Parameters:
state – [in] State of the multibody system
actuation – [in] Actuation model
costs – [in] Cost model
constraints – [in] Constraints model
-
virtual ~DifferentialActionModelFreeInvDynamicsTpl() = default
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:
data – [in] Free inverse-dynamics data
x – [in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
u – [in] Control input \(\mathbf{u}\in\mathbb{R}^{nu}\)
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:
data – [in] Free inverse-dynamics data
x – [in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
u – [in] Control input \(\mathbf{u}\in\mathbb{R}^{nu}\)
-
virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override
Create the free inverse-dynamics data.
- Returns:
free inverse-dynamics data
-
template<typename NewScalar>
DifferentialActionModelFreeInvDynamicsTpl<NewScalar> cast() const Cast the free-invdyn 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:
DifferentialActionModelFreeInvDynamicsTpl<NewScalar> A differential-action model with the new scalar type.
Checks that a specific data belongs to the free inverse-dynamics model.
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:
data – [in] Free inverse-dynamics data
u – [out] Quasic-static commands
x – [in] State point (velocity has to be zero)
maxiter – [in] Maximum allowed number of iterations (default 100)
tol – [in] Tolerance (default 1e-9)
-
virtual std::size_t get_ng() const override
Return the number of inequality constraints.
-
virtual std::size_t get_nh() const override
Return the number of equality constraints.
-
virtual std::size_t get_ng_T() const override
Return the number of equality terminal constraints.
-
virtual std::size_t get_nh_T() const override
Return the number of equality terminal constraints.
-
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.
-
const std::shared_ptr<ActuationModelAbstract> &get_actuation() const
Return the actuation model.
-
const std::shared_ptr<CostModelSum> &get_costs() const
Return the cost model.
-
const std::shared_ptr<ConstraintModelManager> &get_constraints() const
Return the constraint model manager.
-
pinocchio::ModelTpl<Scalar> &get_pinocchio() const
Return the Pinocchio model.
-
virtual void print(std::ostream &os) const override
Print relevant information of the free inverse-dynamics model.
- Parameters:
os – [out] Output stream object
Protected Attributes
-
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<StateAbstract> state_
< Control dimension
-
class ResidualModelActuation : public crocoddyl::ResidualModelAbstractTpl<_Scalar>
Actuation residual.
This residual function enforces the torques of under-actuated joints (e.g., floating-base joints) to be zero. We compute these torques and their derivatives using RNEA inside
DifferentialActionModelFreeInvDynamicsTpl.As described in
ResidualModelAbstractTpl, the residual value and its Jacobians are calculated bycalcandcalcDiff, respectively.See also
Public Types
-
typedef MathBaseTpl<Scalar> MathBase
-
typedef ResidualModelAbstractTpl<Scalar> Base
-
typedef StateMultibodyTpl<Scalar> StateMultibody
-
typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract
-
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract
-
typedef ActuationModelAbstractTpl<Scalar> ActuationModelAbstract
Public Functions
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW CROCODDYL_INNER_DERIVED_CAST (ResidualModelBase, DifferentialActionModelFreeInvDynamicsTpl, ResidualModelActuation) typedef _Scalar Scalar
Initialize the actuation residual model.
- Parameters:
state – [in] State of the multibody system
nu – [in] Dimension of the joint torques
-
virtual ~ResidualModelActuation() = default
Compute the actuation residual.
- Parameters:
data – [in] Actuation residual data
x – [in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
u – [in] Control input \(\mathbf{u}\in\mathbb{R}^{nv+nu}\)
Compute the derivatives of the actuation residual.
- Parameters:
data – [in] Actuation residual data
x – [in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
u – [in] Control input \(\mathbf{u}\in\mathbb{R}^{nu}\)
-
inline virtual std::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *const data) override
Create the actuation residual data.
- Returns:
Actuation residual data
-
template<typename NewScalar>
inline DifferentialActionModelFreeInvDynamicsTpl<NewScalar>::ResidualModelActuation cast() const Cast the actuation-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:
typename DifferentialActionModelFreeInvDynamicsTpl<NewScalar>::ResidualModelActuation A residual model with the new scalar type.
-
inline virtual void print(std::ostream &os) const override
Print relevant information of the actuation residual model.
- Parameters:
os – [out] Output stream object
Protected Attributes
-
std::size_t na_
Dimension of the joint torques.
-
std::size_t nu_
Control dimension.
-
std::shared_ptr<StateAbstract> state_
State description.
-
typedef MathBaseTpl<Scalar> MathBase
-
typedef MathBaseTpl<Scalar> MathBase