1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2019 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_joint_basic_visitors_hxx__ |
6 |
|
|
#define __pinocchio_joint_basic_visitors_hxx__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp" |
9 |
|
|
#include "pinocchio/multibody/visitor.hpp" |
10 |
|
|
|
11 |
|
|
namespace pinocchio |
12 |
|
|
{ |
13 |
|
|
/// @cond DEV |
14 |
|
|
|
15 |
|
|
/** |
16 |
|
|
* @brief CreateJointData visitor |
17 |
|
|
*/ |
18 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
19 |
|
|
struct CreateJointData |
20 |
|
|
: boost::static_visitor< JointDataTpl<Scalar,Options,JointCollectionTpl> > |
21 |
|
|
{ |
22 |
|
|
typedef JointCollectionTpl<Scalar,Options> JointCollection; |
23 |
|
|
typedef typename JointCollection::JointModelVariant JointModelVariant; |
24 |
|
|
typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataVariant; |
25 |
|
|
|
26 |
|
|
template<typename JointModelDerived> |
27 |
|
19964 |
JointDataVariant operator()(const JointModelBase<JointModelDerived> & jmodel) const |
28 |
✓✗ |
19964 |
{ return JointDataVariant(jmodel.createData()); } |
29 |
|
|
|
30 |
|
9982 |
static JointDataVariant run(const JointModelVariant & jmodel) |
31 |
✓✗ |
9982 |
{ return boost::apply_visitor(CreateJointData(), jmodel); } |
32 |
|
|
}; |
33 |
|
|
|
34 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
35 |
|
|
inline JointDataTpl<Scalar,Options,JointCollectionTpl> |
36 |
|
103 |
createData(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
37 |
|
|
{ |
38 |
|
103 |
return CreateJointData<Scalar,Options,JointCollectionTpl>::run(jmodel); |
39 |
|
|
} |
40 |
|
|
|
41 |
|
|
/** |
42 |
|
|
* @brief JointCalcZeroOrderVisitor fusion visitor |
43 |
|
|
*/ |
44 |
|
|
template<typename ConfigVectorType> |
45 |
|
|
struct JointCalcZeroOrderVisitor |
46 |
|
|
: fusion::JointUnaryVisitorBase< JointCalcZeroOrderVisitor<ConfigVectorType> > |
47 |
|
|
{ |
48 |
|
|
typedef boost::fusion::vector<const ConfigVectorType &> ArgsType; |
49 |
|
|
|
50 |
|
|
template<typename JointModel> |
51 |
|
54 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
52 |
|
|
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
53 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q) |
54 |
|
|
{ |
55 |
|
54 |
jmodel.calc(jdata.derived(),q.derived()); |
56 |
|
|
} |
57 |
|
|
|
58 |
|
|
}; |
59 |
|
|
|
60 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType> |
61 |
|
27 |
inline void calc_zero_order(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, |
62 |
|
|
JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata, |
63 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q) |
64 |
|
|
{ |
65 |
|
|
typedef JointCalcZeroOrderVisitor<ConfigVectorType> Algo; |
66 |
|
|
|
67 |
✓✗ |
27 |
Algo::run(jmodel, jdata, |
68 |
|
|
typename Algo::ArgsType(q.derived())); |
69 |
|
27 |
} |
70 |
|
|
|
71 |
|
|
/** |
72 |
|
|
* @brief JointCalcFirstOrderVisitor fusion visitor |
73 |
|
|
*/ |
74 |
|
|
template<typename ConfigVectorType, typename TangentVectorType> |
75 |
|
|
struct JointCalcFirstOrderVisitor |
76 |
|
|
: fusion::JointUnaryVisitorBase< JointCalcFirstOrderVisitor<ConfigVectorType,TangentVectorType> > |
77 |
|
|
{ |
78 |
|
|
typedef boost::fusion::vector<const ConfigVectorType &, |
79 |
|
|
const TangentVectorType &> ArgsType; |
80 |
|
|
|
81 |
|
|
template<typename JointModel> |
82 |
|
80 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
83 |
|
|
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
84 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
85 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v |
86 |
|
|
) |
87 |
|
|
{ |
88 |
|
80 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
}; |
92 |
|
|
|
93 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl,typename ConfigVectorType, typename TangentVectorType> |
94 |
|
40 |
inline void calc_first_order(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, |
95 |
|
|
JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata, |
96 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
97 |
|
|
const Eigen::MatrixBase<TangentVectorType> & v) |
98 |
|
|
{ |
99 |
|
|
typedef JointCalcFirstOrderVisitor<ConfigVectorType,TangentVectorType> Algo; |
100 |
|
|
|
101 |
✓✗ |
40 |
Algo::run(jmodel, jdata, typename Algo::ArgsType(q.derived(),v.derived())); |
102 |
|
40 |
} |
103 |
|
|
|
104 |
|
|
|
105 |
|
|
/** |
106 |
|
|
* @brief JointCalcAbaVisitor fusion visitor |
107 |
|
|
*/ |
108 |
|
|
|
109 |
|
|
template<typename Matrix6Type> |
110 |
|
|
struct JointCalcAbaVisitor |
111 |
|
|
: fusion::JointUnaryVisitorBase< JointCalcAbaVisitor<Matrix6Type> > |
112 |
|
|
{ |
113 |
|
|
|
114 |
|
|
typedef boost::fusion::vector<Matrix6Type &, |
115 |
|
|
const bool &> ArgsType; |
116 |
|
|
|
117 |
|
|
template<typename JointModel> |
118 |
|
80 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
119 |
|
|
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
120 |
|
|
const Eigen::MatrixBase<Matrix6Type> & I, |
121 |
|
|
const bool & update_I |
122 |
|
|
) |
123 |
|
|
{ |
124 |
|
80 |
Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I); |
125 |
|
80 |
jmodel.calc_aba(jdata.derived(),I_,update_I); |
126 |
|
|
} |
127 |
|
|
|
128 |
|
|
}; |
129 |
|
|
|
130 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename Matrix6Type> |
131 |
|
40 |
inline void calc_aba(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, |
132 |
|
|
JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata, |
133 |
|
|
const Eigen::MatrixBase<Matrix6Type> & I, |
134 |
|
|
const bool update_I) |
135 |
|
|
{ |
136 |
|
|
typedef JointCalcAbaVisitor<Matrix6Type> Algo; |
137 |
|
|
|
138 |
|
40 |
Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I); |
139 |
✓✗ |
40 |
Algo::run(jmodel, jdata, typename Algo::ArgsType(I_, update_I) ); |
140 |
|
40 |
} |
141 |
|
|
|
142 |
|
|
/** |
143 |
|
|
* @brief JointNvVisitor visitor |
144 |
|
|
*/ |
145 |
|
|
struct JointNvVisitor |
146 |
|
|
: boost::static_visitor<int> |
147 |
|
|
{ |
148 |
|
|
template<typename JointModelDerived> |
149 |
|
12951648 |
int operator()(const JointModelBase<JointModelDerived> & jmodel) const |
150 |
|
12951648 |
{ return jmodel.nv(); } |
151 |
|
|
|
152 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
153 |
|
6541968 |
static int run( const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
154 |
✓✗ |
6541968 |
{ return boost::apply_visitor(JointNvVisitor(),jmodel); } |
155 |
|
|
}; |
156 |
|
|
|
157 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
158 |
|
6541968 |
inline int nv(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
159 |
|
6541968 |
{ return JointNvVisitor::run(jmodel); } |
160 |
|
|
|
161 |
|
|
|
162 |
|
|
/** |
163 |
|
|
* @brief JointNqVisitor visitor |
164 |
|
|
*/ |
165 |
|
|
struct JointNqVisitor |
166 |
|
|
: boost::static_visitor<int> |
167 |
|
|
{ |
168 |
|
|
template<typename JointModelDerived> |
169 |
|
1906162 |
int operator()(const JointModelBase<JointModelDerived> & jmodel) const |
170 |
|
1906162 |
{ return jmodel.nq(); } |
171 |
|
|
|
172 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
173 |
|
982876 |
static int run( const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
174 |
✓✗ |
982876 |
{ return boost::apply_visitor(JointNqVisitor(),jmodel); } |
175 |
|
|
}; |
176 |
|
|
|
177 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
178 |
|
982876 |
inline int nq(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
179 |
|
982876 |
{ return JointNqVisitor::run(jmodel); } |
180 |
|
|
|
181 |
|
|
/** |
182 |
|
|
* @brief JointConfigurationLimitVisitor visitor |
183 |
|
|
*/ |
184 |
|
|
struct JointConfigurationLimitVisitor |
185 |
|
|
: boost::static_visitor<std::vector<bool>> |
186 |
|
|
{ |
187 |
|
|
template<typename JointModelDerived> |
188 |
|
6 |
const std::vector<bool> operator()(const JointModelBase<JointModelDerived> & jmodel) const |
189 |
|
6 |
{ return jmodel.hasConfigurationLimit(); } |
190 |
|
|
|
191 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
192 |
|
3 |
static const std::vector<bool> run( const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
193 |
✓✗ |
3 |
{ return boost::apply_visitor(JointConfigurationLimitVisitor(),jmodel); } |
194 |
|
|
}; |
195 |
|
|
|
196 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
197 |
|
3 |
inline const std::vector<bool> hasConfigurationLimit(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
198 |
|
3 |
{ return JointConfigurationLimitVisitor::run(jmodel); } |
199 |
|
|
|
200 |
|
|
/** |
201 |
|
|
* @brief JointConfigurationLimitInTangentVisitor visitor |
202 |
|
|
*/ |
203 |
|
|
struct JointConfigurationLimitInTangentVisitor |
204 |
|
|
: boost::static_visitor<std::vector<bool>> |
205 |
|
|
{ |
206 |
|
|
template<typename JointModelDerived> |
207 |
|
6 |
const std::vector<bool> operator()(const JointModelBase<JointModelDerived> & jmodel) const |
208 |
|
6 |
{ return jmodel.hasConfigurationLimitInTangent(); } |
209 |
|
|
|
210 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
211 |
|
3 |
static const std::vector<bool> run( const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
212 |
✓✗ |
3 |
{ return boost::apply_visitor(JointConfigurationLimitInTangentVisitor(),jmodel); } |
213 |
|
|
}; |
214 |
|
|
|
215 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
216 |
|
3 |
inline const std::vector<bool> hasConfigurationLimitInTangent(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
217 |
|
3 |
{ return JointConfigurationLimitInTangentVisitor::run(jmodel); } |
218 |
|
|
|
219 |
|
|
/** |
220 |
|
|
* @brief JointIdxQVisitor visitor |
221 |
|
|
*/ |
222 |
|
|
struct JointIdxQVisitor |
223 |
|
|
: boost::static_visitor<int> |
224 |
|
|
{ |
225 |
|
|
template<typename JointModelDerived> |
226 |
|
1841268 |
int operator()(const JointModelBase<JointModelDerived> & jmodel) const |
227 |
|
1841268 |
{ return jmodel.idx_q(); } |
228 |
|
|
|
229 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
230 |
|
935159 |
static int run( const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
231 |
✓✗ |
935159 |
{ return boost::apply_visitor(JointIdxQVisitor(),jmodel); } |
232 |
|
|
}; |
233 |
|
|
|
234 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
235 |
|
935159 |
inline int idx_q(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
236 |
|
935159 |
{ return JointIdxQVisitor::run(jmodel); } |
237 |
|
|
|
238 |
|
|
/** |
239 |
|
|
* @brief JointIdxVVisitor visitor |
240 |
|
|
*/ |
241 |
|
|
struct JointIdxVVisitor |
242 |
|
|
: boost::static_visitor<int> |
243 |
|
|
{ |
244 |
|
|
template<typename JointModelDerived> |
245 |
|
5512848 |
int operator()(const JointModelBase<JointModelDerived> & jmodel) const |
246 |
|
5512848 |
{ return jmodel.idx_v(); } |
247 |
|
|
|
248 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
249 |
|
2790266 |
static int run( const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
250 |
✓✗ |
2790266 |
{ return boost::apply_visitor(JointIdxVVisitor(),jmodel); } |
251 |
|
|
}; |
252 |
|
|
|
253 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
254 |
|
2790266 |
inline int idx_v(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) { return JointIdxVVisitor::run(jmodel); } |
255 |
|
|
|
256 |
|
|
/** |
257 |
|
|
* @brief JointIdVisitor visitor |
258 |
|
|
*/ |
259 |
|
|
struct JointIdVisitor |
260 |
|
|
: boost::static_visitor<JointIndex> |
261 |
|
|
{ |
262 |
|
|
template<typename JointModelDerived> |
263 |
|
3424 |
JointIndex operator()(const JointModelBase<JointModelDerived> & jmodel) const |
264 |
|
3424 |
{ return jmodel.id(); } |
265 |
|
|
|
266 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
267 |
|
2820 |
static JointIndex run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
268 |
✓✗ |
2820 |
{ return boost::apply_visitor(JointIdVisitor(),jmodel); } |
269 |
|
|
}; |
270 |
|
|
|
271 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
272 |
|
2820 |
inline JointIndex id(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) { return JointIdVisitor::run(jmodel); } |
273 |
|
|
|
274 |
|
|
/** |
275 |
|
|
* @brief JointSetIndexesVisitor visitor |
276 |
|
|
*/ |
277 |
|
|
struct JointSetIndexesVisitor |
278 |
|
|
: boost::static_visitor<> |
279 |
|
|
{ |
280 |
|
|
JointIndex id; |
281 |
|
|
int q; |
282 |
|
|
int v; |
283 |
|
|
|
284 |
|
8533 |
JointSetIndexesVisitor(JointIndex id,int q,int v): id(id),q(q),v(v) {} |
285 |
|
|
|
286 |
|
|
template<typename JointModelDerived> |
287 |
|
17066 |
void operator()(JointModelBase<JointModelDerived> & jmodel) const |
288 |
|
17066 |
{ jmodel.setIndexes(id, q, v); } |
289 |
|
|
|
290 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
291 |
|
12605 |
static void run(JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, JointIndex id, int q, int v) |
292 |
✓✗ |
12605 |
{ return boost::apply_visitor(JointSetIndexesVisitor(id, q, v),jmodel); } |
293 |
|
|
}; |
294 |
|
|
|
295 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
296 |
|
12605 |
inline void setIndexes(JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, |
297 |
|
|
JointIndex id, int q,int v) |
298 |
|
12605 |
{ return JointSetIndexesVisitor::run(jmodel, id, q, v); } |
299 |
|
|
|
300 |
|
|
|
301 |
|
|
/** |
302 |
|
|
* @brief JointModelShortnameVisitor visitor |
303 |
|
|
*/ |
304 |
|
|
struct JointModelShortnameVisitor |
305 |
|
|
: boost::static_visitor<std::string> |
306 |
|
|
{ |
307 |
|
|
template<typename JointModelDerived> |
308 |
|
118 |
std::string operator()(const JointModelBase<JointModelDerived> & jmodel) const |
309 |
|
118 |
{ return jmodel.shortname(); } |
310 |
|
|
|
311 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
312 |
|
59 |
static std::string run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
313 |
✓✗ |
59 |
{ return boost::apply_visitor(JointModelShortnameVisitor(),jmodel); } |
314 |
|
|
}; |
315 |
|
|
|
316 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
317 |
|
59 |
inline std::string shortname(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
318 |
|
59 |
{ return JointModelShortnameVisitor::run(jmodel);} |
319 |
|
|
|
320 |
|
|
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
321 |
|
|
struct JointCastVisitor |
322 |
|
|
: fusion::JointUnaryVisitorBase< JointCastVisitor<NewScalar,Scalar,Options,JointCollectionTpl>, typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type > |
323 |
|
|
{ |
324 |
|
|
typedef fusion::NoArg ArgsType; |
325 |
|
|
|
326 |
|
|
typedef typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type ReturnType; |
327 |
|
|
|
328 |
|
|
template<typename JointModelDerived> |
329 |
|
310 |
static ReturnType algo(const JointModelBase<JointModelDerived> & jmodel) |
330 |
✓✗ |
310 |
{ return ReturnType(jmodel.template cast<NewScalar>()); } |
331 |
|
|
|
332 |
|
|
}; |
333 |
|
|
|
334 |
|
|
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
335 |
|
|
typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type |
336 |
|
310 |
cast_joint(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
337 |
|
|
{ |
338 |
|
|
typedef JointCastVisitor<NewScalar,Scalar,Options,JointCollectionTpl> Algo; |
339 |
|
310 |
return Algo::run(jmodel); |
340 |
|
|
} |
341 |
|
|
|
342 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived> |
343 |
|
|
struct JointModelComparisonOperatorVisitor |
344 |
|
|
: fusion::JointUnaryVisitorBase< JointModelComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived>,bool> |
345 |
|
|
{ |
346 |
|
|
typedef boost::fusion::vector<const JointModelDerived &> ArgsType; |
347 |
|
|
|
348 |
|
|
template<typename JointModel> |
349 |
|
88 |
static bool algo(const JointModelBase<JointModel> & jmodel_lhs, |
350 |
|
|
const JointModelDerived & jmodel_rhs) |
351 |
|
|
{ |
352 |
|
88 |
return jmodel_lhs.derived() == jmodel_rhs; |
353 |
|
|
} |
354 |
|
|
|
355 |
|
|
}; |
356 |
|
|
|
357 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived> |
358 |
|
88 |
bool isEqual(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic, |
359 |
|
|
const JointModelBase<JointModelDerived> & jmodel) |
360 |
|
|
{ |
361 |
|
|
typedef JointModelComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived> Algo; |
362 |
✓✗✓✗
|
176 |
return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived()))); |
363 |
|
|
} |
364 |
|
|
|
365 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived> |
366 |
|
|
struct JointModelHasSameIndexesVisitor |
367 |
|
|
: fusion::JointUnaryVisitorBase< JointModelHasSameIndexesVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived>,bool> |
368 |
|
|
{ |
369 |
|
|
typedef boost::fusion::vector<const JointModelDerived &> ArgsType; |
370 |
|
|
|
371 |
|
|
template<typename JointModel> |
372 |
|
2758 |
static bool algo(const JointModelBase<JointModel> & jmodel_lhs, |
373 |
|
|
const JointModelDerived & jmodel_rhs) |
374 |
|
|
{ |
375 |
|
2758 |
return jmodel_lhs.derived().hasSameIndexes(jmodel_rhs); |
376 |
|
|
} |
377 |
|
|
|
378 |
|
|
}; |
379 |
|
|
|
380 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived> |
381 |
|
2512 |
bool hasSameIndexes(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel_generic, |
382 |
|
|
const JointModelBase<JointModelDerived> & jmodel) |
383 |
|
|
{ |
384 |
|
|
typedef JointModelHasSameIndexesVisitor<Scalar,Options,JointCollectionTpl,JointModelDerived> Algo; |
385 |
✓✗✓✗
|
5024 |
return Algo::run(jmodel_generic,typename Algo::ArgsType(boost::ref(jmodel.derived()))); |
386 |
|
|
} |
387 |
|
|
|
388 |
|
|
// |
389 |
|
|
// Visitors on JointDatas |
390 |
|
|
// |
391 |
|
|
|
392 |
|
|
/** |
393 |
|
|
* @brief JointConstraintVisitor visitor |
394 |
|
|
*/ |
395 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
396 |
|
|
struct JointConstraintVisitor |
397 |
|
|
: boost::static_visitor< ConstraintTpl<Eigen::Dynamic,Scalar,Options> > |
398 |
|
|
{ |
399 |
|
|
typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> ReturnType; |
400 |
|
|
|
401 |
|
|
template<typename JointDataDerived> |
402 |
|
1552 |
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
403 |
|
|
{ |
404 |
✓✗ |
1552 |
return ReturnType(jdata.S().matrix()); |
405 |
|
|
} |
406 |
|
|
|
407 |
|
776 |
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
408 |
|
|
{ |
409 |
✓✗ |
776 |
return boost::apply_visitor(JointConstraintVisitor(), jdata); |
410 |
|
|
} |
411 |
|
|
}; |
412 |
|
|
|
413 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
414 |
|
|
inline ConstraintTpl<Eigen::Dynamic,Scalar,Options> |
415 |
|
776 |
constraint_xd(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
416 |
|
|
{ |
417 |
|
776 |
return JointConstraintVisitor<Scalar,Options,JointCollectionTpl>::run(jdata); |
418 |
|
|
} |
419 |
|
|
|
420 |
|
|
/** |
421 |
|
|
* @brief JointTransformVisitor visitor |
422 |
|
|
*/ |
423 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
424 |
|
|
struct JointTransformVisitor |
425 |
|
|
: boost::static_visitor< SE3Tpl<Scalar,Options> > |
426 |
|
|
{ |
427 |
|
|
typedef SE3Tpl<Scalar,Options> ReturnType; |
428 |
|
|
|
429 |
|
|
template<typename JointDataDerived> |
430 |
|
1590 |
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
431 |
|
|
{ |
432 |
|
1590 |
return ReturnType(jdata.M()); |
433 |
|
|
} |
434 |
|
|
|
435 |
|
795 |
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
436 |
|
|
{ |
437 |
✓✗ |
795 |
return boost::apply_visitor(JointTransformVisitor (), jdata); |
438 |
|
|
} |
439 |
|
|
}; |
440 |
|
|
|
441 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
442 |
|
|
inline SE3Tpl<Scalar,Options> |
443 |
|
795 |
joint_transform(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
444 |
|
|
{ |
445 |
|
795 |
return JointTransformVisitor<Scalar,Options,JointCollectionTpl>::run(jdata); |
446 |
|
|
} |
447 |
|
|
|
448 |
|
|
/** |
449 |
|
|
* @brief JointMotionVisitor visitor |
450 |
|
|
*/ |
451 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
452 |
|
|
struct JointMotionVisitor |
453 |
|
|
: boost::static_visitor< MotionTpl<Scalar,Options> > |
454 |
|
|
{ |
455 |
|
|
typedef MotionTpl<Scalar,Options> ReturnType; |
456 |
|
|
|
457 |
|
|
template<typename JointDataDerived> |
458 |
|
1232 |
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
459 |
|
|
{ |
460 |
|
1232 |
return ReturnType(jdata.v()); |
461 |
|
|
} |
462 |
|
|
|
463 |
|
616 |
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
464 |
|
|
{ |
465 |
✓✗ |
616 |
return boost::apply_visitor(JointMotionVisitor(),jdata); |
466 |
|
|
} |
467 |
|
|
}; |
468 |
|
|
|
469 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
470 |
|
|
inline MotionTpl<Scalar,Options> |
471 |
|
616 |
motion(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
472 |
|
|
{ |
473 |
|
616 |
return JointMotionVisitor<Scalar,Options,JointCollectionTpl>::run(jdata); |
474 |
|
|
} |
475 |
|
|
|
476 |
|
|
/** |
477 |
|
|
* @brief JointBiasVisitor visitor |
478 |
|
|
*/ |
479 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
480 |
|
|
struct JointBiasVisitor |
481 |
|
|
: boost::static_visitor< MotionTpl<Scalar,Options> > |
482 |
|
|
{ |
483 |
|
|
typedef MotionTpl<Scalar,Options> ReturnType; |
484 |
|
|
|
485 |
|
|
template<typename JointDataDerived> |
486 |
|
1232 |
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
487 |
|
|
{ |
488 |
|
1232 |
return ReturnType(jdata.c()); |
489 |
|
|
} |
490 |
|
|
|
491 |
|
616 |
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
492 |
|
|
{ |
493 |
✓✗ |
616 |
return boost::apply_visitor(JointBiasVisitor(), jdata); |
494 |
|
|
} |
495 |
|
|
}; |
496 |
|
|
|
497 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
498 |
|
|
inline MotionTpl<Scalar,Options> |
499 |
|
616 |
bias(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
500 |
|
|
{ |
501 |
|
616 |
return JointBiasVisitor<Scalar,Options,JointCollectionTpl>::run(jdata); |
502 |
|
|
} |
503 |
|
|
|
504 |
|
|
/** |
505 |
|
|
* @brief JointUInertiaVisitor visitor |
506 |
|
|
*/ |
507 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
508 |
|
|
struct JointUInertiaVisitor |
509 |
|
|
: boost::static_visitor< Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> > |
510 |
|
|
{ |
511 |
|
|
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> ReturnType; |
512 |
|
|
|
513 |
|
|
template<typename JointDataDerived> |
514 |
|
1232 |
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
515 |
|
|
{ |
516 |
|
1232 |
return ReturnType(jdata.U()); |
517 |
|
|
} |
518 |
|
|
|
519 |
|
616 |
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
520 |
|
|
{ |
521 |
✓✗ |
616 |
return boost::apply_visitor(JointUInertiaVisitor(), jdata); |
522 |
|
|
} |
523 |
|
|
}; |
524 |
|
|
|
525 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
526 |
|
|
inline Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> |
527 |
|
616 |
u_inertia(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
528 |
|
|
{ |
529 |
|
616 |
return JointUInertiaVisitor<Scalar,Options,JointCollectionTpl>::run(jdata); |
530 |
|
|
} |
531 |
|
|
|
532 |
|
|
/** |
533 |
|
|
* @brief JointDInvInertiaVisitor visitor |
534 |
|
|
*/ |
535 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
536 |
|
|
struct JointDInvInertiaVisitor |
537 |
|
|
: boost::static_visitor< Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> > |
538 |
|
|
{ |
539 |
|
|
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> ReturnType; |
540 |
|
|
|
541 |
|
|
template<typename JointDataDerived> |
542 |
|
1232 |
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
543 |
|
|
{ |
544 |
|
1232 |
return ReturnType(jdata.Dinv()); |
545 |
|
|
} |
546 |
|
|
|
547 |
|
616 |
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
548 |
|
|
{ |
549 |
✓✗ |
616 |
return boost::apply_visitor(JointDInvInertiaVisitor(), jdata); |
550 |
|
|
} |
551 |
|
|
}; |
552 |
|
|
|
553 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
554 |
|
|
inline Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> |
555 |
|
616 |
dinv_inertia(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
556 |
|
|
{ |
557 |
|
616 |
return JointDInvInertiaVisitor<Scalar,Options,JointCollectionTpl>::run(jdata); |
558 |
|
|
} |
559 |
|
|
|
560 |
|
|
/** |
561 |
|
|
* @brief JointUDInvInertiaVisitor visitor |
562 |
|
|
*/ |
563 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
564 |
|
|
struct JointUDInvInertiaVisitor |
565 |
|
|
: boost::static_visitor< Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> > |
566 |
|
|
{ |
567 |
|
|
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> ReturnType; |
568 |
|
|
|
569 |
|
|
template<typename JointDataDerived> |
570 |
|
1232 |
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
571 |
|
|
{ |
572 |
|
1232 |
return ReturnType(jdata.UDinv()); |
573 |
|
|
} |
574 |
|
|
|
575 |
|
616 |
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
576 |
|
|
{ |
577 |
✓✗ |
616 |
return boost::apply_visitor(JointUDInvInertiaVisitor(),jdata); |
578 |
|
|
|
579 |
|
|
} |
580 |
|
|
}; |
581 |
|
|
|
582 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
583 |
|
|
inline Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> |
584 |
|
616 |
udinv_inertia(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
585 |
|
|
{ |
586 |
|
616 |
return JointUDInvInertiaVisitor<Scalar,Options,JointCollectionTpl>::run(jdata); |
587 |
|
|
} |
588 |
|
|
|
589 |
|
|
/** |
590 |
|
|
* @brief JointDataShortnameVisitor visitor |
591 |
|
|
*/ |
592 |
|
|
struct JointDataShortnameVisitor |
593 |
|
|
: boost::static_visitor<std::string> |
594 |
|
|
{ |
595 |
|
|
template<typename JointDataDerived> |
596 |
|
44 |
std::string operator()(const JointDataBase<JointDataDerived> & jdata) const |
597 |
|
44 |
{ return jdata.shortname(); } |
598 |
|
|
|
599 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
600 |
|
22 |
static std::string run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
601 |
✓✗ |
22 |
{ return boost::apply_visitor(JointDataShortnameVisitor(),jdata); } |
602 |
|
|
}; |
603 |
|
|
|
604 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
605 |
|
22 |
inline std::string shortname(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
606 |
|
22 |
{ return JointDataShortnameVisitor::run(jdata);} |
607 |
|
|
|
608 |
|
|
|
609 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived> |
610 |
|
|
struct JointDataComparisonOperatorVisitor |
611 |
|
|
: fusion::JointUnaryVisitorBase< JointDataComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointDataDerived>,bool> |
612 |
|
|
{ |
613 |
|
|
typedef boost::fusion::vector<const JointDataDerived &> ArgsType; |
614 |
|
|
|
615 |
|
|
template<typename JointData> |
616 |
|
80 |
static bool algo(const JointDataBase<JointData> & jdata_lhs, |
617 |
|
|
const JointDataDerived & jdata_rhs) |
618 |
|
|
{ |
619 |
|
80 |
return jdata_lhs.derived() == jdata_rhs; |
620 |
|
|
} |
621 |
|
|
|
622 |
|
|
}; |
623 |
|
|
|
624 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived> |
625 |
|
40 |
bool isEqual(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata_generic, |
626 |
|
|
const JointDataBase<JointDataDerived> & jdata) |
627 |
|
|
{ |
628 |
|
|
typedef JointDataComparisonOperatorVisitor<Scalar,Options,JointCollectionTpl,JointDataDerived> Algo; |
629 |
|
80 |
return Algo::run(jdata_generic,typename Algo::ArgsType(boost::ref(jdata.derived()))); |
630 |
|
|
} |
631 |
|
|
|
632 |
|
|
|
633 |
|
|
/// @endcond |
634 |
|
|
|
635 |
|
|
} // namespace pinocchio |
636 |
|
|
|
637 |
|
|
#endif // ifndef __pinocchio_joint_basic_visitors_hxx__ |