Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2019 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_multibody_visitor_joint_binary_visitor_hpp__ |
6 |
|
|
#define __pinocchio_multibody_visitor_joint_binary_visitor_hpp__ |
7 |
|
|
|
8 |
|
|
#include <boost/variant.hpp> |
9 |
|
|
|
10 |
|
|
#include "pinocchio/multibody/joint/joint-base.hpp" |
11 |
|
|
#include "pinocchio/multibody/visitor/fusion.hpp" |
12 |
|
|
|
13 |
|
|
namespace pinocchio |
14 |
|
|
{ |
15 |
|
|
namespace fusion |
16 |
|
|
{ |
17 |
|
|
namespace bf = boost::fusion; |
18 |
|
|
|
19 |
|
|
typedef boost::blank NoArg; |
20 |
|
|
|
21 |
|
|
/// |
22 |
|
|
/// \brief Base structure for \b Binary visitation of two JointModels. |
23 |
|
|
/// This structure provides runners to call the right visitor according to the number of |
24 |
|
|
/// arguments. This should be used when deriving new rigid body algorithms. |
25 |
|
|
/// |
26 |
|
|
template<typename JointVisitorDerived, typename ReturnType = void> |
27 |
|
|
struct JointBinaryVisitorBase |
28 |
|
|
{ |
29 |
|
|
|
30 |
|
|
template<typename JointModelDerived1, typename JointModelDerived2, typename ArgsTmp> |
31 |
|
|
static ReturnType run( |
32 |
|
|
const JointModelBase<JointModelDerived1> & jmodel1, |
33 |
|
|
const JointModelBase<JointModelDerived2> & jmodel2, |
34 |
|
|
typename JointModelBase<JointModelDerived1>::JointDataDerived & jdata1, |
35 |
|
|
typename JointModelBase<JointModelDerived2>::JointDataDerived & jdata2, |
36 |
|
|
ArgsTmp args) |
37 |
|
|
{ |
38 |
|
|
InternalVisitorModelAndData<JointModelDerived1, JointModelDerived2, ArgsTmp> visitor( |
39 |
|
|
jdata1, jdata2, args); |
40 |
|
|
return visitor(jmodel1.derived(), jmodel2.derived()); |
41 |
|
|
} |
42 |
|
|
|
43 |
|
|
template< |
44 |
|
|
typename Scalar, |
45 |
|
|
int Options, |
46 |
|
|
template<typename, int> |
47 |
|
|
class JointCollectionTpl, |
48 |
|
|
typename ArgsTmp> |
49 |
|
✗ |
static ReturnType run( |
50 |
|
|
const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel1, |
51 |
|
|
const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel2, |
52 |
|
|
JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata1, |
53 |
|
|
JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata2, |
54 |
|
|
ArgsTmp args) |
55 |
|
|
{ |
56 |
|
|
typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel; |
57 |
|
✗ |
InternalVisitorModelAndData<JointModel, JointModel, ArgsTmp> visitor(jdata1, jdata2, args); |
58 |
|
✗ |
return boost::apply_visitor(visitor, jmodel1, jmodel2); |
59 |
|
|
} |
60 |
|
|
|
61 |
|
|
template<typename JointModelDerived1, typename JointModelDerived2> |
62 |
|
|
static ReturnType run( |
63 |
|
|
const JointModelBase<JointModelDerived1> & jmodel1, |
64 |
|
|
const JointModelBase<JointModelDerived2> & jmodel2, |
65 |
|
|
typename JointModelBase<JointModelDerived1>::JointDataDerived & jdata1, |
66 |
|
|
typename JointModelBase<JointModelDerived2>::JointDataDerived & jdata2) |
67 |
|
|
{ |
68 |
|
|
InternalVisitorModelAndData<JointModelDerived1, JointModelDerived2, NoArg> visitor( |
69 |
|
|
jdata1, jdata2); |
70 |
|
|
return visitor(jmodel1.derived(), jmodel2.derived()); |
71 |
|
|
} |
72 |
|
|
|
73 |
|
|
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> |
74 |
|
✗ |
static ReturnType run( |
75 |
|
|
const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel1, |
76 |
|
|
const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel2, |
77 |
|
|
JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata1, |
78 |
|
|
JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata2) |
79 |
|
|
{ |
80 |
|
|
typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel; |
81 |
|
✗ |
InternalVisitorModelAndData<JointModel, JointModel, NoArg> visitor(jdata1, jdata2); |
82 |
|
✗ |
return boost::apply_visitor(visitor, jmodel1, jmodel2); |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
template<typename JointModelDerived1, typename JointModelDerived2, typename ArgsTmp> |
86 |
|
|
static ReturnType run( |
87 |
|
|
const JointModelBase<JointModelDerived1> & jmodel1, |
88 |
|
|
const JointModelBase<JointModelDerived2> & jmodel2, |
89 |
|
|
ArgsTmp args) |
90 |
|
|
{ |
91 |
|
|
InternalVisitorModel<ArgsTmp> visitor(args); |
92 |
|
|
return visitor(jmodel1.derived(), jmodel2.derived()); |
93 |
|
|
} |
94 |
|
|
|
95 |
|
|
template< |
96 |
|
|
typename Scalar, |
97 |
|
|
int Options, |
98 |
|
|
template<typename, int> |
99 |
|
|
class JointCollectionTpl, |
100 |
|
|
typename ArgsTmp> |
101 |
|
✗ |
static ReturnType run( |
102 |
|
|
const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel1, |
103 |
|
|
const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel2, |
104 |
|
|
ArgsTmp args) |
105 |
|
|
{ |
106 |
|
✗ |
InternalVisitorModel<ArgsTmp> visitor(args); |
107 |
|
✗ |
return boost::apply_visitor(visitor, jmodel1, jmodel2); |
108 |
|
|
} |
109 |
|
|
|
110 |
|
|
template<typename JointModelDerived1, typename JointModelDerived2> |
111 |
|
|
static ReturnType run( |
112 |
|
|
const JointModelBase<JointModelDerived1> & jmodel1, |
113 |
|
|
const JointModelBase<JointModelDerived2> & jmodel2) |
114 |
|
|
{ |
115 |
|
|
InternalVisitorModel<NoArg> visitor; |
116 |
|
|
return visitor(jmodel1.derived(), jmodel2.derived()); |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> |
120 |
|
✗ |
static ReturnType run( |
121 |
|
|
const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel1, |
122 |
|
|
const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel2) |
123 |
|
|
{ |
124 |
|
✗ |
InternalVisitorModel<NoArg> visitor; |
125 |
|
✗ |
return boost::apply_visitor(visitor, jmodel1, jmodel2); |
126 |
|
|
} |
127 |
|
|
|
128 |
|
|
private: |
129 |
|
|
template<typename JointModel1, typename JointModel2, typename ArgType> |
130 |
|
|
struct InternalVisitorModelAndData : public boost::static_visitor<ReturnType> |
131 |
|
|
{ |
132 |
|
|
typedef typename JointModel1::JointDataDerived JointData1; |
133 |
|
|
typedef typename JointModel2::JointDataDerived JointData2; |
134 |
|
|
|
135 |
|
✗ |
InternalVisitorModelAndData(JointData1 & jdata1, JointData2 & jdata2, ArgType args) |
136 |
|
✗ |
: jdata1(jdata1) |
137 |
|
✗ |
, jdata2(jdata2) |
138 |
|
✗ |
, args(args) |
139 |
|
|
{ |
140 |
|
|
} |
141 |
|
|
|
142 |
|
|
template<typename JointModelDerived1, typename JointModelDerived2> |
143 |
|
✗ |
ReturnType operator()( |
144 |
|
|
const JointModelBase<JointModelDerived1> & jmodel1, |
145 |
|
|
const JointModelBase<JointModelDerived2> & jmodel2) const |
146 |
|
|
{ |
147 |
|
✗ |
return bf::invoke( |
148 |
|
|
&JointVisitorDerived::template algo<JointModelDerived1, JointModelDerived2>, |
149 |
|
✗ |
bf::append( |
150 |
|
✗ |
boost::ref(jmodel1.derived()), boost::ref(jmodel2.derived()), |
151 |
|
✗ |
boost::ref( |
152 |
|
✗ |
boost::get<typename JointModelBase<JointModelDerived1>::JointDataDerived>(jdata1)), |
153 |
|
✗ |
boost::ref( |
154 |
|
✗ |
boost::get<typename JointModelBase<JointModelDerived2>::JointDataDerived>(jdata2)), |
155 |
|
✗ |
args)); |
156 |
|
|
} |
157 |
|
|
|
158 |
|
|
JointData1 & jdata1; |
159 |
|
|
JointData2 & jdata2; |
160 |
|
|
|
161 |
|
|
ArgType args; |
162 |
|
|
}; |
163 |
|
|
|
164 |
|
|
template<typename JointModel1, typename JointModel2> |
165 |
|
|
struct InternalVisitorModelAndData<JointModel1, JointModel2, NoArg> |
166 |
|
|
: public boost::static_visitor<ReturnType> |
167 |
|
|
{ |
168 |
|
|
typedef typename JointModel1::JointDataDerived JointData1; |
169 |
|
|
typedef typename JointModel2::JointDataDerived JointData2; |
170 |
|
|
|
171 |
|
✗ |
InternalVisitorModelAndData(JointData1 & jdata1, JointData2 & jdata2) |
172 |
|
✗ |
: jdata1(jdata1) |
173 |
|
✗ |
, jdata2(jdata2) |
174 |
|
|
{ |
175 |
|
|
} |
176 |
|
|
|
177 |
|
|
template<typename JointModelDerived1, typename JointModelDerived2> |
178 |
|
✗ |
ReturnType operator()( |
179 |
|
|
const JointModelBase<JointModelDerived1> & jmodel1, |
180 |
|
|
const JointModelBase<JointModelDerived2> & jmodel2) const |
181 |
|
|
{ |
182 |
|
✗ |
return bf::invoke( |
183 |
|
|
&JointVisitorDerived::template algo<JointModelDerived1, JointModelDerived2>, |
184 |
|
|
bf::make_vector( |
185 |
|
✗ |
boost::ref(jmodel1.derived()), boost::ref(jmodel2.derived()), |
186 |
|
✗ |
boost::ref( |
187 |
|
✗ |
boost::get<typename JointModelBase<JointModelDerived1>::JointDataDerived>(jdata1)), |
188 |
|
✗ |
boost::ref(boost::get<typename JointModelBase<JointModelDerived2>::JointDataDerived>( |
189 |
|
✗ |
jdata2)))); |
190 |
|
|
} |
191 |
|
|
|
192 |
|
|
JointData1 & jdata1; |
193 |
|
|
JointData2 & jdata2; |
194 |
|
|
}; |
195 |
|
|
|
196 |
|
|
template<typename ArgType, typename Dummy = void> |
197 |
|
|
struct InternalVisitorModel : public boost::static_visitor<ReturnType> |
198 |
|
|
{ |
199 |
|
✗ |
InternalVisitorModel(ArgType args) |
200 |
|
✗ |
: args(args) |
201 |
|
|
{ |
202 |
|
|
} |
203 |
|
|
|
204 |
|
|
template<typename JointModel1, typename JointModel2> |
205 |
|
✗ |
ReturnType operator()( |
206 |
|
|
const JointModelBase<JointModel1> & jmodel1, |
207 |
|
|
const JointModelBase<JointModel2> & jmodel2) const |
208 |
|
|
{ |
209 |
|
✗ |
return bf::invoke( |
210 |
|
|
&JointVisitorDerived::template algo<JointModel1, JointModel2>, |
211 |
|
✗ |
bf::append(boost::ref(jmodel1.derived()), boost::ref(jmodel2.derived()), args)); |
212 |
|
|
} |
213 |
|
|
|
214 |
|
|
ArgType args; |
215 |
|
|
}; |
216 |
|
|
|
217 |
|
|
template<typename Dummy> |
218 |
|
|
struct InternalVisitorModel<NoArg, Dummy> : public boost::static_visitor<ReturnType> |
219 |
|
|
{ |
220 |
|
✗ |
InternalVisitorModel() |
221 |
|
|
{ |
222 |
|
|
} |
223 |
|
|
|
224 |
|
|
template<typename JointModel1, typename JointModel2> |
225 |
|
✗ |
ReturnType operator()( |
226 |
|
|
const JointModelBase<JointModel1> & jmodel1, |
227 |
|
|
const JointModelBase<JointModel2> & jmodel2) const |
228 |
|
|
{ |
229 |
|
✗ |
return JointVisitorDerived::template algo<JointModel1, JointModel2>( |
230 |
|
✗ |
jmodel1.derived(), jmodel2.derived()); |
231 |
|
|
} |
232 |
|
|
}; |
233 |
|
|
|
234 |
|
|
}; // struct JointBinaryVisitorBase |
235 |
|
|
|
236 |
|
|
} // namespace fusion |
237 |
|
|
} // namespace pinocchio |
238 |
|
|
|
239 |
|
|
#endif // ifndef __pinocchio_multibody_visitor_joint_binary_visitor_hpp__ |
240 |
|
|
|