pinocchio  3.9.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
constraint-model-visitor.hpp
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
13namespace pinocchio
14{
15
16 namespace fusion
17 {
18
24 template<typename ConstraintModelVisitorDerived, typename ReturnType = void>
25 struct ConstraintUnaryVisitorBase
26 {
27
28 template<
29 typename Scalar,
30 int Options,
31 template<typename, int> class ConstraintCollectionTpl,
32 typename ArgsTmp>
33 static ReturnType run(
34 const ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> & cmodel,
35 ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata,
36 ArgsTmp args)
37 {
38 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
39 InternalVisitorModelAndData<Scalar, Options, ConstraintCollectionTpl, ArgsTmp> visitor(
40 cdata, args);
41 return boost::apply_visitor(visitor, cmodel);
42 }
43
44 template<
45 typename Scalar,
46 int Options,
47 template<typename, int> class ConstraintCollectionTpl,
48 typename ArgsTmp>
49 static ReturnType
50 run(const ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata, ArgsTmp args)
51 {
52 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
53 InternalVisitorModel<Scalar, Options, ConstraintCollectionTpl, ArgsTmp> visitor(args);
54 return boost::apply_visitor(visitor, cdata);
55 }
56
57 private:
58 template<
59 typename Scalar,
60 int Options,
61 template<typename, int> class ConstraintCollectionTpl,
62 typename ArgsTmp>
63 struct InternalVisitorModel : public boost::static_visitor<ReturnType>
64 {
65 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
66 typedef ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintData;
67
68 InternalVisitorModel(ArgsTmp args)
69 : args(args)
70 {
71 }
72
73 template<typename ConstraintModelDerived>
74 ReturnType operator()(const ConstraintModelBase<ConstraintModelDerived> & cmodel) const
75 {
76 return bf::invoke(
77 &ConstraintModelVisitorDerived::template algo<ConstraintModelDerived>,
78 bf::append(boost::ref(cmodel.derived()), args));
79 }
80
81 template<typename ConstraintDataDerived>
82 ReturnType operator()(const ConstraintDataBase<ConstraintDataDerived> & cdata) const
83 {
84 return bf::invoke(
85 &ConstraintModelVisitorDerived::template algo<ConstraintDataDerived>,
86 bf::append(boost::ref(cdata.derived()), args));
87 }
88
89 ArgsTmp args;
90 };
91
92 template<
93 typename Scalar,
94 int Options,
95 template<typename, int> class ConstraintCollectionTpl,
96 typename ArgsTmp>
97 struct InternalVisitorModelAndData : public boost::static_visitor<ReturnType>
98 {
99 typedef ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintModel;
100 typedef ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> ConstraintData;
101
102 InternalVisitorModelAndData(ConstraintData & cdata, ArgsTmp args)
103 : cdata(cdata)
104 , args(args)
105 {
106 }
107
108 template<typename ConstraintModelDerived>
109 ReturnType operator()(const ConstraintModelBase<ConstraintModelDerived> & cmodel) const
110 {
111 return bf::invoke(
112 &ConstraintModelVisitorDerived::template algo<ConstraintModelDerived>,
113 bf::append(
114 boost::ref(cmodel.derived()),
115 boost::ref(
116 boost::get<typename ConstraintModelBase<ConstraintModelDerived>::ConstraintData>(
117 cdata)),
118 args));
119 }
120
121 ConstraintData & cdata;
122 ArgsTmp args;
123 };
124 };
125 } // namespace fusion
126
130 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
131 struct ConstraintModelCalcVisitor
132 : fusion::ConstraintUnaryVisitorBase<
133 ConstraintModelCalcVisitor<Scalar, Options, JointCollectionTpl>>
134 {
135 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
136 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
137 typedef boost::fusion::vector<const Model &, const Data &> ArgsType;
138
139 template<typename ConstraintModel>
140 static void algo(
141 const pinocchio::ConstraintModelBase<ConstraintModel> & cmodel,
142 typename ConstraintModel::ConstraintData & cdata,
143 const Model & model,
144 const Data & data)
145 {
146 cmodel.calc(model, data, cdata.derived());
147 }
148 };
149
150 template<
151 typename Scalar,
152 int Options,
153 template<typename S, int O> class JointCollectionTpl,
154 template<typename S, int O> class ConstraintCollectionTpl>
155 void calc(
156 const ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> & cmodel,
157 ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata,
158 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
159 const DataTpl<Scalar, Options, JointCollectionTpl> & data)
160 {
161 typedef ConstraintModelCalcVisitor<Scalar, Options, JointCollectionTpl> Algo;
162 Algo::run(cmodel, cdata, typename Algo::ArgsType(model, data));
163 }
164
168 template<
169 typename Scalar,
170 int Options,
171 template<typename, int> class JointCollectionTpl,
172 typename JacobianMatrix>
173 struct ConstraintModelJacobianVisitor
174 : fusion::ConstraintUnaryVisitorBase<
175 ConstraintModelJacobianVisitor<Scalar, Options, JointCollectionTpl, JacobianMatrix>>
176 {
177 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
178 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
179 typedef boost::fusion::vector<const Model &, const Data &, JacobianMatrix &> ArgsType;
180
181 template<typename ConstraintModel>
182 static void algo(
183 const pinocchio::ConstraintModelBase<ConstraintModel> & cmodel,
184 typename ConstraintModel::ConstraintData & cdata,
185 const Model & model,
186 const Data & data,
187 const Eigen::MatrixBase<JacobianMatrix> & jacobian_matrix)
188 {
189 cmodel.jacobian(model, data, cdata.derived(), jacobian_matrix.const_cast_derived());
190 }
191 };
192
193 template<
194 typename Scalar,
195 int Options,
196 template<typename S, int O> class JointCollectionTpl,
197 template<typename S, int O> class ConstraintCollectionTpl,
198 typename JacobianMatrix>
199 void jacobian(
200 const ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> & cmodel,
201 ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata,
202 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
203 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
204 const Eigen::MatrixBase<JacobianMatrix> & jacobian_matrix)
205 {
206 typedef ConstraintModelJacobianVisitor<Scalar, Options, JointCollectionTpl, JacobianMatrix>
207 Algo;
208 Algo::run(
209 cmodel, cdata, typename Algo::ArgsType(model, data, jacobian_matrix.const_cast_derived()));
210 }
211
215 template<typename Scalar, int Options, template<typename S, int O> class ConstraintCollectionTpl>
216 struct ConstraintModelCreateDataVisitor
217 : boost::static_visitor<typename ConstraintCollectionTpl<Scalar, Options>::ConstraintDataVariant>
218 {
219 typedef fusion::NoArg ArgsType;
220 typedef ConstraintCollectionTpl<Scalar, Options> ConstraintCollection;
221 typedef typename ConstraintCollection::ConstraintModelVariant ConstraintModelVariant;
222 typedef typename ConstraintCollection::ConstraintDataVariant ConstraintDataVariant;
223
224 template<typename ConstraintModel>
225 ConstraintDataVariant
226 operator()(const pinocchio::ConstraintModelBase<ConstraintModel> & cmodel) const
227 {
228 return cmodel.createData();
229 }
230
231 static ConstraintDataVariant run(const ConstraintModelVariant & cmodel)
232 {
233 return boost::apply_visitor(ConstraintModelCreateDataVisitor(), cmodel);
234 }
235 };
236
237 template<typename Scalar, int Options, template<typename S, int O> class ConstraintCollectionTpl>
238 ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl>
239 createData(const ConstraintModelTpl<Scalar, Options, ConstraintCollectionTpl> & cmodel)
240 {
241 return ConstraintModelCreateDataVisitor<Scalar, Options, ConstraintCollectionTpl>::run(cmodel);
242 }
243
244 template<
245 typename Scalar,
246 int Options,
247 template<typename S, int O> class ConstraintCollectionTpl,
248 typename ConstraintDataDerived>
249 struct ConstraintDataComparisonOperatorVisitor
250 : fusion::ConstraintUnaryVisitorBase<
251 ConstraintDataComparisonOperatorVisitor<
252 Scalar,
253 Options,
254 ConstraintCollectionTpl,
255 ConstraintDataDerived>,
256 bool>
257 {
258 typedef boost::fusion::vector<const ConstraintDataDerived &> ArgsType;
259
260 template<typename ConstraintData>
261 static bool algo(
262 const ConstraintDataBase<ConstraintData> & cdata_lhs, const ConstraintDataDerived & cdata_rhs)
263 {
264 return cdata_lhs.derived() == cdata_rhs;
265 }
266 };
267
268 template<
269 typename Scalar,
270 int Options,
271 template<typename S, int O> class ConstraintCollectionTpl,
272 typename ConstraintDataDerived>
273 bool isEqual(
274 const ConstraintDataTpl<Scalar, Options, ConstraintCollectionTpl> & cdata_generic,
275 const ConstraintDataBase<ConstraintDataDerived> & cdata)
276 {
277 typedef ConstraintDataComparisonOperatorVisitor<
278 Scalar, Options, ConstraintCollectionTpl, ConstraintDataDerived>
279 Algo;
280 return Algo::run(cdata_generic, typename Algo::ArgsType(boost::ref(cdata.derived())));
281 }
282
283} // namespace pinocchio
284
285#endif // ifdef __pinocchio_algorithm_constraints_constraint_model_visitor_hpp__
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::ConstraintModelBase::calc
void calc(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, ConstraintData &cdata) const
Evaluate the constraint values at the current state given by data and store the results in cdata.
Definition constraint-model-base.hpp:46
pinocchio::fusion::ConstraintUnaryVisitorBase
Base structure for Unary visitation of a ConstraintModel. This structure provides runners to call the...
Definition constraint-model-visitor.hpp:26