22template <
typename _Scalar>
33 const double mu,
const double half_length,
34 const double half_width)
37 void evaluate(
const ConstVectorRef &,
const ConstVectorRef &u,
44 return std::make_shared<Data>(
this);
54template <
typename Scalar>
56 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
68#include "./centroidal-wrench-cone.txx"
Base definitions for ternary functions.
This residual implements the wrench cone for a centroidal model with control with 6D spatial force.
Eigen::Matrix< Scalar, 17, 6 > Jtemp_
CentroidalWrenchConeDataTpl(const CentroidalWrenchConeResidualTpl< Scalar > *model)
StageFunctionDataTpl< Scalar > Base
CentroidalWrenchConeResidualTpl(const int ndx, const int nu, const int k, const double mu, const double half_length, const double half_width)
typename Base::Data BaseData
void computeJacobians(const ConstVectorRef &, const ConstVectorRef &, BaseData &data) const
CentroidalWrenchConeDataTpl< Scalar > Data
shared_ptr< BaseData > createData() const
Instantiate a Data object.
StageFunctionTpl< Scalar > Base
void evaluate(const ConstVectorRef &, const ConstVectorRef &u, BaseData &data) const
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
StageFunctionDataTpl(const int ndx, const int nu, const int nr)
const int nu
Control dimension.
StageFunctionDataTpl< Scalar > Data
StageFunctionTpl(const int ndx, const int nu, const int nr)