7#include <pinocchio/algorithm/proximal.hpp>
18template <
typename _Scalar>
26 using SE3 = pinocchio::SE3Tpl<Scalar>;
29 pinocchio::RigidConstraintModel);
31 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
42 const int ndx,
const PinModel &model,
const MatrixXs &actuation,
44 const ProxSettings &prox_settings,
const std::string &contact_name,
46 :
Base(ndx, (int)actuation.cols(), 2)
52 if (model.nv != actuation.rows()) {
53 ALIGATOR_DOMAIN_ERROR(
"Actuation matrix should have number of rows = "
54 "model.nv ({:d} and {:d}).",
55 actuation.rows(), model.nv);
58 for (std::size_t i = 0; i < constraint_models.size(); i++) {
59 if (constraint_models[i].name == contact_name) {
65 "Contact name is not included in constraint models");
69 void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
76 return std::make_shared<Data>(
this);
80template <
typename Scalar>
82 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
84 using PinData = pinocchio::DataTpl<Scalar>;
88 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
97 pinocchio::ProximalSettingsTpl<Scalar>
settings;
103#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#define ALIGATOR_RUNTIME_ERROR(...)
Base definitions for ternary functions.
MultibodyFrictionConeDataTpl(const MultibodyFrictionConeResidualTpl< Scalar > *model)
typename math_types< Scalar >::MatrixXs MatrixXs
Eigen::Matrix< Scalar, 1, 3 > dcone_df_
StageFunctionDataTpl< Scalar > Base
typename math_types< Scalar >::VectorXs VectorXs
pinocchio::ProximalSettingsTpl< Scalar > settings
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) RigidConstraintDataVector
PinData pin_data_
Pinocchio data object.
pinocchio::DataTpl< Scalar > PinData
RigidConstraintDataVector constraint_datas_
This residual returns the derivative of centroidal momentum for a kinodynamics model.
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) RigidConstraintDataVector
pinocchio::SE3Tpl< Scalar > SE3
MultibodyFrictionConeResidualTpl(const int ndx, const PinModel &model, const MatrixXs &actuation, const RigidConstraintModelVector &constraint_models, const ProxSettings &prox_settings, const std::string &contact_name, const double mu)
ProxSettings prox_settings_
StageFunctionTpl< Scalar > Base
MatrixXs actuation_matrix_
void evaluate(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const
shared_ptr< BaseData > createData() const
Instantiate a Data object.
RigidConstraintModelVector constraint_models_
pinocchio::ProximalSettingsTpl< Scalar > ProxSettings
void computeJacobians(const ConstVectorRef &, const ConstVectorRef &, BaseData &data) const
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR( pinocchio::RigidConstraintModel) RigidConstraintModelVector
MultibodyFrictionConeDataTpl< Scalar > Data
typename Base::Data BaseData
pinocchio::ModelTpl< Scalar > PinModel
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
StageFunctionDataTpl(const int ndx, const int nu, const int nr)
StageFunctionDataTpl< Scalar > Data
StageFunctionTpl(const int ndx, const int nu, const int nr)
Typedefs for math (Eigen vectors, matrices) depending on scalar type.