Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/constraints/constraint-model-base.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 4 | 20 | 20.0% |
Branches: | 2 | 12 | 16.7% |
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 | ✗ | const Derived & derived() const | |
33 | { | ||
34 | ✗ | 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 | ✗ | void calc( | |
47 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
48 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
49 | ConstraintData & cdata) const | ||
50 | { | ||
51 | ✗ | derived().calc(model, data, cdata); | |
52 | } | ||
53 | |||
54 | template<typename JacobianMatrix, int Options, template<typename, int> class JointCollectionTpl> | ||
55 | ✗ | 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 | ✗ | derived().jacobian(model, data, cdata, jacobian_matrix.const_cast_derived()); | |
62 | } | ||
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 | ✗ | bool operator==(const ConstraintModelBase<OtherDerived> & other) const | |
77 | { | ||
78 | ✗ | return name == other.name && colwise_sparsity == other.colwise_sparsity | |
79 | ✗ | && 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 | ✗ | ConstraintData createData() const | |
93 | { | ||
94 | ✗ | return derived().createData(); | |
95 | } | ||
96 | |||
97 | protected: | ||
98 | template<int Options, template<typename, int> class JointCollectionTpl> | ||
99 | 69 | ConstraintModelBase(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) | |
100 |
1/2✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
|
69 | : colwise_sparsity(model.nv) |
101 | { | ||
102 | static const bool default_sparsity_value = false; | ||
103 |
1/2✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
|
69 | colwise_sparsity.fill(default_sparsity_value); |
104 | 69 | } | |
105 | |||
106 | /// \brief Default constructor | ||
107 | ✗ | ConstraintModelBase() | |
108 | { | ||
109 | } | ||
110 | |||
111 | ✗ | ConstraintModelBase & base() | |
112 | { | ||
113 | ✗ | return *this; | |
114 | } | ||
115 | ✗ | const ConstraintModelBase & base() const | |
116 | { | ||
117 | ✗ | return *this; | |
118 | } | ||
119 | }; | ||
120 | |||
121 | } // namespace pinocchio | ||
122 | |||
123 | #endif // ifndef __pinocchio_algorithm_constraint_model_base_hpp__ | ||
124 |