aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
multibody-friction-cone.hpp
Go to the documentation of this file.
1#pragma once
2
3#include "fwd.hpp"
6
7#include <pinocchio/algorithm/proximal.hpp>
8
9namespace aligator {
10
11template <typename Scalar> struct MultibodyFrictionConeDataTpl;
12
17
18template <typename _Scalar>
20public:
21 using Scalar = _Scalar;
24 using BaseData = typename Base::Data;
25 using PinModel = pinocchio::ModelTpl<Scalar>;
26 using SE3 = pinocchio::SE3Tpl<Scalar>;
28 using RigidConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(
29 pinocchio::RigidConstraintModel);
31 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
32 using ProxSettings = pinocchio::ProximalSettingsTpl<Scalar>;
33
38 double mu_;
40
42 const int ndx, const PinModel &model, const MatrixXs &actuation,
43 const RigidConstraintModelVector &constraint_models,
44 const ProxSettings &prox_settings, const std::string &contact_name,
45 const double mu)
46 : Base(ndx, (int)actuation.cols(), 2)
47 , pin_model_(model)
48 , actuation_matrix_(actuation)
49 , constraint_models_(constraint_models)
50 , prox_settings_(prox_settings)
51 , mu_(mu) {
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);
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>
82 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
84 using PinData = pinocchio::DataTpl<Scalar>;
88 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
89
94 Eigen::Matrix<Scalar, 1, 3> dcone_df_;
95
97 pinocchio::ProximalSettingsTpl<Scalar> settings;
98
101};
102
103#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
106#endif
107} // namespace aligator
#define ALIGATOR_RUNTIME_ERROR(...)
Definition exceptions.hpp:7
Base definitions for ternary functions.
Main package namespace.
MultibodyFrictionConeDataTpl(const MultibodyFrictionConeResidualTpl< Scalar > *model)
typename math_types< Scalar >::MatrixXs MatrixXs
typename math_types< Scalar >::VectorXs VectorXs
pinocchio::ProximalSettingsTpl< Scalar > settings
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) RigidConstraintDataVector
This residual returns the derivative of centroidal momentum for a kinodynamics model.
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) RigidConstraintDataVector
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)
void evaluate(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const
shared_ptr< BaseData > createData() const
Instantiate a Data object.
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
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.
Definition math.hpp:122