3#ifdef ALIGATOR_PINOCCHIO_V3
7#include <proxsuite-nlp/modelling/spaces/multibody.hpp>
9#include <pinocchio/algorithm/proximal.hpp>
13template <
typename Scalar>
struct MultibodyFrictionConeDataTpl;
20template <
typename _Scalar>
21struct MultibodyFrictionConeResidualTpl : StageFunctionTpl<_Scalar> {
23 using Scalar = _Scalar;
25 using Base = StageFunctionTpl<Scalar>;
26 using BaseData =
typename Base::Data;
27 using Model = pinocchio::ModelTpl<Scalar>;
28 using SE3 = pinocchio::SE3Tpl<Scalar>;
29 using Data = MultibodyFrictionConeDataTpl<Scalar>;
30 using RigidConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(
31 pinocchio::RigidConstraintModel);
32 using RigidConstraintDataVector =
33 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
34 using ProxSettings = pinocchio::ProximalSettingsTpl<Scalar>;
37 MatrixXs actuation_matrix_;
38 RigidConstraintModelVector constraint_models_;
39 ProxSettings prox_settings_;
43 MultibodyFrictionConeResidualTpl(
44 const int ndx,
const Model &model,
const MatrixXs &actuation,
45 const RigidConstraintModelVector &constraint_models,
46 const ProxSettings &prox_settings,
const std::string &contact_name,
48 : Base(ndx, (int)actuation.cols(), 2), pin_model_(model),
49 actuation_matrix_(actuation), constraint_models_(constraint_models),
50 prox_settings_(prox_settings), mu_(mu) {
51 if (model.nv != actuation.rows()) {
53 fmt::format(
"actuation matrix should have number of rows = pinocchio "
54 "model nv ({} and {}).",
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) {
63 if (contact_id_ == -1) {
65 "Contact name is not included in constraint models");
69 void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
70 BaseData &data)
const;
72 void computeJacobians(
const ConstVectorRef &,
const ConstVectorRef &,
73 BaseData &data)
const;
75 shared_ptr<BaseData> createData()
const {
76 return std::make_shared<Data>(
this);
80template <
typename Scalar>
81struct MultibodyFrictionConeDataTpl : StageFunctionDataTpl<Scalar> {
82 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
83 using Base = StageFunctionDataTpl<Scalar>;
84 using PinData = pinocchio::DataTpl<Scalar>;
85 using VectorXs =
typename math_types<Scalar>::VectorXs;
86 using MatrixXs =
typename math_types<Scalar>::MatrixXs;
87 using RigidConstraintDataVector =
88 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
94 Eigen::Matrix<Scalar, 1, 3> dcone_df_;
96 RigidConstraintDataVector constraint_datas_;
97 pinocchio::ProximalSettingsTpl<Scalar> settings;
99 MultibodyFrictionConeDataTpl(
100 const MultibodyFrictionConeResidualTpl<Scalar> *model);
105#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
106#include "aligator/modelling/multibody/multibody-friction-cone.txx"
#define ALIGATOR_RUNTIME_ERROR(...)
#define ALIGATOR_DOMAIN_ERROR(msg)
Base definitions for ternary functions.
#define ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
pinocchio::DataTpl< Scalar, Options > PinData