| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/constraints/constraint-model-base.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 22 | 29 | 75.9% |
| Branches: | 6 | 12 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2023 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_algorithm_constraint_model_base_hpp__ | ||
| 6 | #define __pinocchio_algorithm_constraint_model_base_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/model.hpp" | ||
| 9 | #include "pinocchio/algorithm/fwd.hpp" | ||
| 10 | |||
| 11 | namespace pinocchio | ||
| 12 | { | ||
| 13 | |||
| 14 | template<class Derived> | ||
| 15 | struct ConstraintModelBase : NumericalBase<Derived> | ||
| 16 | { | ||
| 17 | typedef typename traits<Derived>::Scalar Scalar; | ||
| 18 | enum | ||
| 19 | { | ||
| 20 | Options = traits<Derived>::Options | ||
| 21 | }; | ||
| 22 | typedef typename traits<Derived>::ConstraintData ConstraintData; | ||
| 23 | |||
| 24 | typedef Eigen::Matrix<bool, Eigen::Dynamic, 1, Options> BooleanVector; | ||
| 25 | // typedef Eigen::Matrix<Eigen::DenseIndex,Eigen::Dynamic,1,Options> IndexVector; | ||
| 26 | typedef std::vector<Eigen::DenseIndex> IndexVector; | ||
| 27 | |||
| 28 | Derived & derived() | ||
| 29 | { | ||
| 30 | return static_cast<Derived &>(*this); | ||
| 31 | } | ||
| 32 | 8 | const Derived & derived() const | |
| 33 | { | ||
| 34 | 8 | return static_cast<const Derived &>(*this); | |
| 35 | } | ||
| 36 | |||
| 37 | template<typename NewScalar> | ||
| 38 | typename CastType<NewScalar, Derived>::type cast() const | ||
| 39 | { | ||
| 40 | return derived().template cast<NewScalar>(); | ||
| 41 | } | ||
| 42 | |||
| 43 | /// \brief Evaluate the constraint values at the current state given by data and store the | ||
| 44 | /// results in cdata. | ||
| 45 | template<int Options, template<typename, int> class JointCollectionTpl> | ||
| 46 | 1 | void calc( | |
| 47 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 48 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 49 | ConstraintData & cdata) const | ||
| 50 | { | ||
| 51 | 1 | derived().calc(model, data, cdata); | |
| 52 | 1 | } | |
| 53 | |||
| 54 | template<typename JacobianMatrix, int Options, template<typename, int> class JointCollectionTpl> | ||
| 55 | 1 | void jacobian( | |
| 56 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 57 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 58 | ConstraintData & cdata, | ||
| 59 | const Eigen::MatrixBase<JacobianMatrix> & jacobian_matrix) const | ||
| 60 | { | ||
| 61 | 1 | derived().jacobian(model, data, cdata, jacobian_matrix.const_cast_derived()); | |
| 62 | 1 | } | |
| 63 | |||
| 64 | // Attributes common to all constraints | ||
| 65 | |||
| 66 | /// \brief Name of the constraint | ||
| 67 | std::string name; | ||
| 68 | |||
| 69 | /// \brief Sparsity pattern associated to the constraint; | ||
| 70 | BooleanVector colwise_sparsity; | ||
| 71 | |||
| 72 | /// \brief Indexes of the columns spanned by the constraints. | ||
| 73 | IndexVector colwise_span_indexes; | ||
| 74 | |||
| 75 | template<typename OtherDerived> | ||
| 76 | 1 | bool operator==(const ConstraintModelBase<OtherDerived> & other) const | |
| 77 | { | ||
| 78 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | return name == other.name && colwise_sparsity == other.colwise_sparsity |
| 79 |
2/4✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2 | && colwise_span_indexes == other.colwise_span_indexes; |
| 80 | } | ||
| 81 | |||
| 82 | template<typename OtherDerived> | ||
| 83 | ✗ | ConstraintModelBase & operator=(const ConstraintModelBase<OtherDerived> & other) | |
| 84 | { | ||
| 85 | ✗ | name = other.name; | |
| 86 | ✗ | colwise_sparsity = other.colwise_sparsity; | |
| 87 | ✗ | colwise_span_indexes = other.colwise_span_indexes; | |
| 88 | |||
| 89 | ✗ | return *this; | |
| 90 | } | ||
| 91 | |||
| 92 | 1 | ConstraintData createData() const | |
| 93 | { | ||
| 94 | 1 | return derived().createData(); | |
| 95 | } | ||
| 96 | |||
| 97 | protected: | ||
| 98 | template<int Options, template<typename, int> class JointCollectionTpl> | ||
| 99 | 116 | ConstraintModelBase(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) | |
| 100 |
1/2✓ Branch 2 taken 116 times.
✗ Branch 3 not taken.
|
116 | : colwise_sparsity(model.nv) |
| 101 | { | ||
| 102 | static const bool default_sparsity_value = false; | ||
| 103 |
1/2✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
|
116 | colwise_sparsity.fill(default_sparsity_value); |
| 104 | 116 | } | |
| 105 | |||
| 106 | /// \brief Default constructor | ||
| 107 | 3 | ConstraintModelBase() | |
| 108 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | { |
| 109 | 3 | } | |
| 110 | |||
| 111 | ✗ | ConstraintModelBase & base() | |
| 112 | { | ||
| 113 | ✗ | return *this; | |
| 114 | } | ||
| 115 | 2 | const ConstraintModelBase & base() const | |
| 116 | { | ||
| 117 | 2 | return *this; | |
| 118 | } | ||
| 119 | }; | ||
| 120 | |||
| 121 | } // namespace pinocchio | ||
| 122 | |||
| 123 | #endif // ifndef __pinocchio_algorithm_constraint_model_base_hpp__ | ||
| 124 |