GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/constraints/visitors/constraint-model-visitor.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 29 0.0%
Branches: 0 20 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2023 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_constraints_constraint_model_visitor_hpp__
6 #define __pinocchio_algorithm_constraints_constraint_model_visitor_hpp__
7
8 #include "pinocchio/algorithm/constraints/fwd.hpp"
9 // #include "pinocchio/algorithm/constraints/constraint-model-generic.hpp"
10 // #include "pinocchio/algorithm/constraints/constraint-data-generic.hpp"
11 #include "pinocchio/multibody/visitor/fusion.hpp"
12
13 namespace pinocchio
14 {
15
16 namespace fusion
17 {
18
19 ///
20 /// \brief Base structure for \b Unary visitation of a ConstraintModel.
21 /// This structure provides runners to call the right visitor according to the number of
22 /// arguments.
23 ///
24 template<typename ConstraintModelVisitorDerived, typename ReturnType = void>
25 struct ConstraintUnaryVisitorBase
26 {
27
28 template<
29 typename Scalar,
30 int Options,
31 template<typename, int>
32 class ConstraintCollectionTpl,
33 typename ArgsTmp>
34 ✗ static ReturnType run(
35 const ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> & cmodel,
36 ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata,
37 ArgsTmp args)
38 {
39 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
40 ✗ InternalVisitorModelAndData<Scalar, Options, ConstraintCollectionTpl, ArgsTmp> visitor(
41 cdata, args);
42 ✗ return boost::apply_visitor(visitor, cmodel);
43 }
44
45 template<
46 typename Scalar,
47 int Options,
48 template<typename, int>
49 class ConstraintCollectionTpl,
50 typename ArgsTmp>
51 static ReturnType
52 run(const ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata, ArgsTmp args)
53 {
54 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
55 InternalVisitorModel<Scalar, Options, ConstraintCollectionTpl, ArgsTmp> visitor(args);
56 return boost::apply_visitor(visitor, cdata);
57 }
58
59 private:
60 template<
61 typename Scalar,
62 int Options,
63 template<typename, int>
64 class ConstraintCollectionTpl,
65 typename ArgsTmp>
66 struct InternalVisitorModel : public boost::static_visitor<ReturnType>
67 {
68 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
69 typedef ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintData;
70
71 InternalVisitorModel(ArgsTmp args)
72 : args(args)
73 {
74 }
75
76 template<typename ConstraintModelDerived>
77 ReturnType operator()(const ConstraintModelBase<ConstraintModelDerived> & cmodel) const
78 {
79 return bf::invoke(
80 &ConstraintModelVisitorDerived::template algo<ConstraintModelDerived>,
81 bf::append(boost::ref(cmodel.derived()), args));
82 }
83
84 template<typename ConstraintDataDerived>
85 ReturnType operator()(const ConstraintDataBase<ConstraintDataDerived> & cdata) const
86 {
87 return bf::invoke(
88 &ConstraintModelVisitorDerived::template algo<ConstraintDataDerived>,
89 bf::append(boost::ref(cdata.derived()), args));
90 }
91
92 ArgsTmp args;
93 };
94
95 template<
96 typename Scalar,
97 int Options,
98 template<typename, int>
99 class ConstraintCollectionTpl,
100 typename ArgsTmp>
101 struct InternalVisitorModelAndData : public boost::static_visitor<ReturnType>
102 {
103 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
104 typedef ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintData;
105
106 ✗ InternalVisitorModelAndData(ConstraintData & cdata, ArgsTmp args)
107 ✗ : cdata(cdata)
108 ✗ , args(args)
109 {
110 }
111
112 template<typename ConstraintModelDerived>
113 ✗ ReturnType operator()(const ConstraintModelBase<ConstraintModelDerived> & cmodel) const
114 {
115 ✗ return bf::invoke(
116 &ConstraintModelVisitorDerived::template algo<ConstraintModelDerived>,
117 ✗ bf::append(
118 ✗ boost::ref(cmodel.derived()),
119 ✗ boost::ref(
120 ✗ boost::get<typename ConstraintModelBase<ConstraintModelDerived>::ConstraintData>(
121 ✗ cdata)),
122 ✗ args));
123 }
124
125 ConstraintData & cdata;
126 ArgsTmp args;
127 };
128 };
129 } // namespace fusion
130
131 /**
132 * @brief ConstraintModelCalcVisitor fusion visitor
133 */
134 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
135 struct ConstraintModelCalcVisitor
136 : fusion::ConstraintUnaryVisitorBase<
137 ConstraintModelCalcVisitor<Scalar, Options, JointCollectionTpl>>
138 {
139 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
140 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
141 typedef boost::fusion::vector<const Model &, const Data &> ArgsType;
142
143 template<typename ConstraintModel>
144 ✗ static void algo(
145 const pinocchio::ConstraintModelBase<ConstraintModel> & cmodel,
146 typename ConstraintModel::ConstraintData & cdata,
147 const Model & model,
148 const Data & data)
149 {
150 ✗ cmodel.calc(model, data, cdata.derived());
151 }
152 };
153
154 template<
155 typename Scalar,
156 int Options,
157 template<typename S, int O>
158 class JointCollectionTpl,
159 template<typename S, int O>
160 class ConstraintCollectionTpl>
161 ✗ void calc(
162 const ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> & cmodel,
163 ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata,
164 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
165 const DataTpl<Scalar, Options, JointCollectionTpl> & data)
166 {
167 typedef ConstraintModelCalcVisitor<Scalar, Options, JointCollectionTpl> Algo;
168 ✗ Algo::run(cmodel, cdata, typename Algo::ArgsType(model, data));
169 }
170
171 /**
172 * @brief ConstraintModelJacobianVisitor fusion visitor
173 */
174 template<
175 typename Scalar,
176 int Options,
177 template<typename, int>
178 class JointCollectionTpl,
179 typename JacobianMatrix>
180 struct ConstraintModelJacobianVisitor
181 : fusion::ConstraintUnaryVisitorBase<
182 ConstraintModelJacobianVisitor<Scalar, Options, JointCollectionTpl, JacobianMatrix>>
183 {
184 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
185 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
186 typedef boost::fusion::vector<const Model &, const Data &, JacobianMatrix &> ArgsType;
187
188 template<typename ConstraintModel>
189 ✗ static void algo(
190 const pinocchio::ConstraintModelBase<ConstraintModel> & cmodel,
191 typename ConstraintModel::ConstraintData & cdata,
192 const Model & model,
193 const Data & data,
194 const Eigen::MatrixBase<JacobianMatrix> & jacobian_matrix)
195 {
196 ✗ cmodel.jacobian(model, data, cdata.derived(), jacobian_matrix.const_cast_derived());
197 }
198 };
199
200 template<
201 typename Scalar,
202 int Options,
203 template<typename S, int O>
204 class JointCollectionTpl,
205 template<typename S, int O>
206 class ConstraintCollectionTpl,
207 typename JacobianMatrix>
208 ✗ void jacobian(
209 const ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> & cmodel,
210 ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata,
211 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
212 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
213 const Eigen::MatrixBase<JacobianMatrix> & jacobian_matrix)
214 {
215 typedef ConstraintModelJacobianVisitor<Scalar, Options, JointCollectionTpl, JacobianMatrix>
216 Algo;
217 ✗ Algo::run(
218 ✗ cmodel, cdata, typename Algo::ArgsType(model, data, jacobian_matrix.const_cast_derived()));
219 }
220
221 /**
222 * @brief ConstraintModelCreateDataVisitor fusion visitor
223 */
224 template<typename Scalar, int Options, template<typename S, int O> class ConstraintCollectionTpl>
225 struct ConstraintModelCreateDataVisitor
226 : boost::static_visitor<typename ConstraintCollectionTpl<Scalar, Options>::ConstraintDataVariant>
227 {
228 typedef fusion::NoArg ArgsType;
229 typedef ConstraintCollectionTpl<Scalar, Options> ConstraintCollection;
230 typedef typename ConstraintCollection::ConstraintModelVariant ConstraintModelVariant;
231 typedef typename ConstraintCollection::ConstraintDataVariant ConstraintDataVariant;
232
233 template<typename ConstraintModel>
234 ConstraintDataVariant
235 ✗ operator()(const pinocchio::ConstraintModelBase<ConstraintModel> & cmodel) const
236 {
237 ✗ return cmodel.createData();
238 }
239
240 ✗ static ConstraintDataVariant run(const ConstraintModelVariant & cmodel)
241 {
242 ✗ return boost::apply_visitor(ConstraintModelCreateDataVisitor(), cmodel);
243 }
244 };
245
246 template<typename Scalar, int Options, template<typename S, int O> class ConstraintCollectionTpl>
247 ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl>
248 ✗ createData(const ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> & cmodel)
249 {
250 ✗ return ConstraintModelCreateDataVisitor<Scalar, Options, ConstraintCollectionTpl>::run(cmodel);
251 }
252
253 template<
254 typename Scalar,
255 int Options,
256 template<typename S, int O>
257 class ConstraintCollectionTpl,
258 typename ConstraintDataDerived>
259 struct ConstraintDataComparisonOperatorVisitor
260 : fusion::ConstraintUnaryVisitorBase<
261 ConstraintDataComparisonOperatorVisitor<
262 Scalar,
263 Options,
264 ConstraintCollectionTpl,
265 ConstraintDataDerived>,
266 bool>
267 {
268 typedef boost::fusion::vector<const ConstraintDataDerived &> ArgsType;
269
270 template<typename ConstraintData>
271 static bool algo(
272 const ConstraintDataBase<ConstraintData> & cdata_lhs, const ConstraintDataDerived & cdata_rhs)
273 {
274 return cdata_lhs.derived() == cdata_rhs;
275 }
276 };
277
278 template<
279 typename Scalar,
280 int Options,
281 template<typename S, int O>
282 class ConstraintCollectionTpl,
283 typename ConstraintDataDerived>
284 bool isEqual(
285 const ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata_generic,
286 const ConstraintDataBase<ConstraintDataDerived> & cdata)
287 {
288 typedef ConstraintDataComparisonOperatorVisitor<
289 Scalar, Options, ConstraintCollectionTpl, ConstraintDataDerived>
290 Algo;
291 return Algo::run(cdata_generic, typename Algo::ArgsType(boost::ref(cdata.derived())));
292 }
293
294 } // namespace pinocchio
295
296 #endif // ifdef __pinocchio_algorithm_constraints_constraint_model_visitor_hpp__
297