aligator  0.10.0
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
Loading...
Searching...
No Matches
multibody-friction-cone.hpp
Go to the documentation of this file.
1#pragma once
2
3#ifdef ALIGATOR_PINOCCHIO_V3
4#include "./fwd.hpp"
6
7#include <proxsuite-nlp/modelling/spaces/multibody.hpp>
8
9#include <pinocchio/algorithm/proximal.hpp>
10
11namespace aligator {
12
13template <typename Scalar> struct MultibodyFrictionConeDataTpl;
14
20template <typename _Scalar>
21struct MultibodyFrictionConeResidualTpl : StageFunctionTpl<_Scalar> {
22public:
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>;
35
36 Model pin_model_;
37 MatrixXs actuation_matrix_;
38 RigidConstraintModelVector constraint_models_;
39 ProxSettings prox_settings_;
40 double mu_;
41 int contact_id_;
42
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,
47 const double mu)
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));
56 }
57 contact_id_ = -1;
58 for (std::size_t i = 0; i < constraint_models.size(); i++) {
59 if (constraint_models[i].name == contact_name) {
60 contact_id_ = (int)i;
61 }
62 }
63 if (contact_id_ == -1) {
65 "Contact name is not included in constraint models");
66 }
67 }
68
69 void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
70 BaseData &data) const;
71
72 void computeJacobians(const ConstVectorRef &, const ConstVectorRef &,
73 BaseData &data) const;
74
75 shared_ptr<BaseData> createData() const {
76 return std::make_shared<Data>(this);
77 }
78};
79
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);
89
91 PinData pin_data_;
92 VectorXs tau_;
93 MatrixXs temp_;
94 Eigen::Matrix<Scalar, 1, 3> dcone_df_;
95
96 RigidConstraintDataVector constraint_datas_;
97 pinocchio::ProximalSettingsTpl<Scalar> settings;
98
99 MultibodyFrictionConeDataTpl(
100 const MultibodyFrictionConeResidualTpl<Scalar> *model);
101};
102
103} // namespace aligator
104
105#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
106#include "aligator/modelling/multibody/multibody-friction-cone.txx"
107#endif
108
109#endif // ALIGATOR_PINOCCHIO_V3
#define ALIGATOR_RUNTIME_ERROR(...)
Definition exceptions.hpp:7
#define ALIGATOR_DOMAIN_ERROR(msg)
Base definitions for ternary functions.
#define ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
Definition math.hpp:8
pinocchio::DataTpl< Scalar, Options > PinData
Definition context.hpp:10
Main package namespace.