8template <
typename Scalar>
struct AngularAccelerationDataTpl;
21template <
typename _Scalar>
27 using Base = StageFunctionTpl<Scalar>;
29 using Data = AngularAccelerationDataTpl<Scalar>;
33 const Vector3s &gravity,
39 if (contact_map.getSize() !=
nk_) {
41 fmt::format(
"Contact ids and nk should be the same: now "
43 contact_map.getSize(),
nk_));
47 void evaluate(
const ConstVectorRef &,
const ConstVectorRef &u,
48 const ConstVectorRef &,
BaseData &data)
const;
51 const ConstVectorRef &,
BaseData &data)
const;
54 return allocate_shared_eigen_aligned<Data>(
this);
66template <
typename Scalar>
68 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 using Base = StageFunctionDataTpl<Scalar>;
75 const AngularAccelerationResidualTpl<Scalar> *model);
80#include "aligator/modelling/centroidal/angular-acceleration.hxx"
82#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
83#include "./angular-acceleration.txx"
#define ALIGATOR_DOMAIN_ERROR(msg)
Base definitions for ternary functions.
AngularAccelerationDataTpl(const AngularAccelerationResidualTpl< Scalar > *model)
Eigen::Matrix< Scalar, 3, 3 > Matrix3s
This residual returns the angular acceleration of a centroidal model with state , computed from the e...
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
void computeJacobians(const ConstVectorRef &, const ConstVectorRef &, const ConstVectorRef &, BaseData &data) const
AngularAccelerationResidualTpl(const int ndx, const int nu, const double mass, const Vector3s &gravity, const ContactMap &contact_map, const int force_size)
shared_ptr< BaseData > createData() const
Instantiate a Data object.
typename Base::Data BaseData
void evaluate(const ConstVectorRef &, const ConstVectorRef &u, const ConstVectorRef &, BaseData &data) const
AngularAccelerationDataTpl< Scalar > Data
Base struct for function data.
Class representing ternary functions .
const int nu
Control dimension.
StageFunctionDataTpl< Scalar > Data