6#include <pinocchio/multibody/model.hpp>
7#include <proxsuite-nlp/modelling/spaces/multibody.hpp>
8#include <pinocchio/multibody/data.hpp>
10#ifdef ALIGATOR_PINOCCHIO_V3
11#include <pinocchio/algorithm/proximal.hpp>
15template <
typename Scalar>
struct MultibodyWrenchConeDataTpl;
22template <
typename _Scalar>
23struct MultibodyWrenchConeResidualTpl : StageFunctionTpl<_Scalar>, frame_api {
25 using Scalar = _Scalar;
27 using Base = StageFunctionTpl<Scalar>;
28 using BaseData =
typename Base::Data;
29 using Model = pinocchio::ModelTpl<Scalar>;
30 using SE3 = pinocchio::SE3Tpl<Scalar>;
31 using Data = MultibodyWrenchConeDataTpl<Scalar>;
32 using RigidConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(
33 pinocchio::RigidConstraintModel);
34 using RigidConstraintDataVector =
35 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
36 using ProxSettings = pinocchio::ProximalSettingsTpl<Scalar>;
39 MatrixXs actuation_matrix_;
40 RigidConstraintModelVector constraint_models_;
41 ProxSettings prox_settings_;
46 Eigen::Matrix<Scalar, 17, 6> Acone_;
48 MultibodyWrenchConeResidualTpl(
49 const int ndx,
const Model &model,
const MatrixXs &actuation,
50 const RigidConstraintModelVector &constraint_models,
51 const ProxSettings &prox_settings,
const int contact_id,
const double mu,
52 const double half_length,
const double half_width)
53 : Base(ndx, (int)actuation.cols(), 17), pin_model_(model),
54 actuation_matrix_(actuation), constraint_models_(constraint_models),
55 prox_settings_(prox_settings), mu_(mu), hL_(half_length),
56 hW_(half_width), contact_id_(contact_id) {
57 if (model.nv != actuation.rows()) {
59 fmt::format(
"actuation matrix should have number of rows = pinocchio "
60 "model nv ({} and {}).",
61 actuation.rows(), model.nv));
63 Acone_ << 0, 0, -1, 0, 0, 0, -1, 0, -mu_, 0, 0, 0, 1, 0, -mu_, 0, 0, 0, 0,
64 -1, -mu_, 0, 0, 0, 0, 1, -mu_, 0, 0, 0, 0, 0, -hW_, -1, 0, 0, 0, 0,
65 -hW_, 1, 0, 0, 0, 0, -hL_, 0, -1, 0, 0, 0, -hL_, 0, 1, 0, -hW_, -hL_,
66 -(hL_ + hW_) * mu_, mu_, mu_, -1, -hW_, hL_, -(hL_ + hW_) * mu_, mu_,
67 -mu_, -1, hW_, -hL_, -(hL_ + hW_) * mu_, -mu_, mu_, -1, hW_, hL_,
68 -(hL_ + hW_) * mu_, -mu_, -mu_, -1, hW_, hL_, -(hL_ + hW_) * mu_, mu_,
69 mu_, 1, hW_, -hL_, -(hL_ + hW_) * mu_, mu_, -mu_, 1, -hW_, hL_,
70 -(hL_ + hW_) * mu_, -mu_, mu_, 1, -hW_, -hL_, -(hL_ + hW_) * mu_, -mu_,
74 void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
75 const ConstVectorRef &, BaseData &data)
const;
77 void computeJacobians(
const ConstVectorRef &,
const ConstVectorRef &,
78 const ConstVectorRef &, BaseData &data)
const;
80 shared_ptr<BaseData> createData()
const {
81 return allocate_shared_eigen_aligned<Data>(
this);
85template <
typename Scalar>
86struct MultibodyWrenchConeDataTpl : StageFunctionDataTpl<Scalar> {
87 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
88 using Base = StageFunctionDataTpl<Scalar>;
89 using PinData = pinocchio::DataTpl<Scalar>;
90 using VectorXs =
typename math_types<Scalar>::VectorXs;
91 using MatrixXs =
typename math_types<Scalar>::MatrixXs;
92 using Matrix6Xs =
typename math_types<Scalar>::Matrix6Xs;
93 using RigidConstraintDataVector =
94 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
101 RigidConstraintDataVector constraint_datas_;
102 pinocchio::ProximalSettingsTpl<Scalar> settings;
104 MultibodyWrenchConeDataTpl(
105 const MultibodyWrenchConeResidualTpl<Scalar> *model);
110#include "aligator/modelling/multibody/multibody-wrench-cone.hxx"
112#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
113#include "./multibody-wrench-cone.txx"
#define ALIGATOR_DOMAIN_ERROR(msg)
Base definitions for ternary functions.
#define ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
pinocchio::DataTpl< Scalar, Options > PinData