|
| DifferentialActionModelFreeFwdDynamicsTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActuationModelAbstract > actuation, boost::shared_ptr< CostModelSum > costs) |
|
virtual void | calc (const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
|
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) |
|
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 free forward-dynamics data.
|
|
virtual boost::shared_ptr< DifferentialActionDataAbstract > | createData () |
| Create the free 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< CostModelSum > & | get_costs () const |
| Return the cost model.
|
|
pinocchio::ModelTpl< Scalar > & | get_pinocchio () const |
| Return the Pinocchio model.
|
|
virtual void | print (std::ostream &os) const |
| Print relevant information of the free 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.
|
|
| 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.
|
|
template<typename _Scalar>
class crocoddyl::DifferentialActionModelFreeFwdDynamicsTpl< _Scalar >
Differential action model for free forward dynamics in multibody systems.
This class implements free forward dynamics, i.e.,
\[ \mathbf{M}\dot{\mathbf{v}} + \mathbf{h}(\mathbf{q},\mathbf{v}) = \boldsymbol{\tau}, \]
where \(\mathbf{q}\in Q\), \(\mathbf{v}\in\mathbb{R}^{nv}\) are the configuration point and generalized velocity (its tangent vector), respectively; \(\boldsymbol{\tau}\) is the torque inputs and \(\mathbf{h}(\mathbf{q},\mathbf{v})\) are the Coriolis effect and gravity field.
The derivatives of the system acceleration is computed efficiently based on the analytical derivatives of Articulate Body Algorithm (ABA) as described in [1].
The stack of cost functions is implemented in CostModelSumTpl
. The computation of the free forward 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 50 of file free-fwddyn.hpp.