1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_multibody_visitior_joint_unary_visitor_hpp__ |
6 |
|
|
#define __pinocchio_multibody_visitior_joint_unary_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 Unary visitation of a JointModel. |
23 |
|
|
/// This structure provides runners to call the right visitor according to the number of arguments. |
24 |
|
|
/// This should be used when deriving new rigid body algorithms. |
25 |
|
|
/// |
26 |
|
|
template<typename JointVisitorDerived, typename ReturnType = void> |
27 |
|
|
struct JointUnaryVisitorBase |
28 |
|
|
{ |
29 |
|
|
|
30 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsTmp> |
31 |
|
959202 |
static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, |
32 |
|
|
JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata, |
33 |
|
|
ArgsTmp args) |
34 |
|
|
{ |
35 |
✓✗✓✗
|
959202 |
InternalVisitorModelAndData<JointModelTpl<Scalar,Options,JointCollectionTpl>,ArgsTmp> visitor(jdata,args); |
36 |
✓✗ |
1918404 |
return boost::apply_visitor(visitor,jmodel); |
37 |
|
|
} |
38 |
|
|
|
39 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
40 |
|
21 |
static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, |
41 |
|
|
JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
42 |
|
|
{ |
43 |
|
21 |
InternalVisitorModelAndData<JointModelTpl<Scalar,Options,JointCollectionTpl>,NoArg> visitor(jdata); |
44 |
✓✗ |
42 |
return boost::apply_visitor(visitor,jmodel); |
45 |
|
|
} |
46 |
|
|
|
47 |
|
|
template<typename JointModelDerived, typename ArgsTmp> |
48 |
|
|
static ReturnType run(const JointModelBase<JointModelDerived> & jmodel, |
49 |
|
|
typename JointModelBase<JointModelDerived>::JointDataDerived & jdata, |
50 |
|
|
ArgsTmp args) |
51 |
|
|
{ |
52 |
|
|
InternalVisitorModelAndData<JointModelDerived,ArgsTmp> visitor(jdata,args); |
53 |
|
|
return visitor(jmodel.derived()); |
54 |
|
|
} |
55 |
|
|
|
56 |
|
|
template<typename JointModelDerived> |
57 |
|
|
static ReturnType run(const JointModelBase<JointModelDerived> & jmodel, |
58 |
|
|
typename JointModelBase<JointModelDerived>::JointDataDerived & jdata) |
59 |
|
|
{ |
60 |
|
|
InternalVisitorModelAndData<JointModelDerived,NoArg> visitor(jdata); |
61 |
|
|
return visitor(jmodel.derived()); |
62 |
|
|
} |
63 |
|
|
|
64 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsTmp> |
65 |
|
120918 |
static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, |
66 |
|
|
ArgsTmp args) |
67 |
|
|
{ |
68 |
✓✗✓✗
|
120918 |
InternalVisitorModel<ArgsTmp> visitor(args); |
69 |
✓✗ |
241836 |
return boost::apply_visitor(visitor,jmodel); |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ArgsTmp> |
73 |
|
80 |
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata, |
74 |
|
|
ArgsTmp args) |
75 |
|
|
{ |
76 |
✓✗✓✗
|
80 |
InternalVisitorModel<ArgsTmp> visitor(args); |
77 |
✓✗ |
160 |
return boost::apply_visitor(visitor,jdata); |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
81 |
|
331 |
static ReturnType run(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel) |
82 |
|
|
{ |
83 |
|
331 |
InternalVisitorModel<NoArg> visitor; |
84 |
✓✗ |
662 |
return boost::apply_visitor(visitor,jmodel); |
85 |
|
|
} |
86 |
|
|
|
87 |
|
|
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
88 |
|
|
static ReturnType run(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata) |
89 |
|
|
{ |
90 |
|
|
InternalVisitorModel<NoArg> visitor; |
91 |
|
|
return boost::apply_visitor(visitor,jdata); |
92 |
|
|
} |
93 |
|
|
|
94 |
|
|
template<typename JointModelDerived, typename ArgsTmp> |
95 |
|
|
static ReturnType run(const JointModelBase<JointModelDerived> & jmodel, |
96 |
|
|
ArgsTmp args) |
97 |
|
|
{ |
98 |
|
|
InternalVisitorModel<ArgsTmp> visitor(args); |
99 |
|
|
return visitor(jmodel.derived()); |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
template<typename JointDataDerived, typename ArgsTmp> |
103 |
|
|
static ReturnType run(const JointDataBase<JointDataDerived> & jdata, |
104 |
|
|
ArgsTmp args) |
105 |
|
|
{ |
106 |
|
|
InternalVisitorModel<ArgsTmp> visitor(args); |
107 |
|
|
return visitor(jdata.derived()); |
108 |
|
|
} |
109 |
|
|
|
110 |
|
|
template<typename JointModelDerived> |
111 |
|
|
static ReturnType run(const JointModelBase<JointModelDerived> & jmodel) |
112 |
|
|
{ |
113 |
|
|
InternalVisitorModel<NoArg> visitor; |
114 |
|
|
return visitor(jmodel.derived()); |
115 |
|
|
} |
116 |
|
|
|
117 |
|
|
template<typename JointDataDerived> |
118 |
|
|
static ReturnType run(const JointDataBase<JointDataDerived> & jdata) |
119 |
|
|
{ |
120 |
|
|
InternalVisitorModel<NoArg> visitor; |
121 |
|
|
return visitor(jdata.derived()); |
122 |
|
|
} |
123 |
|
|
|
124 |
|
|
private: |
125 |
|
|
|
126 |
|
|
template<typename JointModel, typename ArgType> |
127 |
|
|
struct InternalVisitorModelAndData |
128 |
|
|
: public boost::static_visitor<ReturnType> |
129 |
|
|
{ |
130 |
|
|
typedef typename JointModel::JointDataDerived JointData; |
131 |
|
|
|
132 |
|
959202 |
InternalVisitorModelAndData(JointData & jdata, ArgType args) |
133 |
|
959202 |
: jdata(jdata), args(args) |
134 |
|
959202 |
{} |
135 |
|
|
|
136 |
|
|
template<typename JointModelDerived> |
137 |
|
955946 |
ReturnType operator()(const JointModelBase<JointModelDerived> & jmodel) const |
138 |
|
|
{ |
139 |
✓✗ |
955946 |
return bf::invoke(&JointVisitorDerived::template algo<JointModelDerived>, |
140 |
✓✗ |
955946 |
bf::append(boost::ref(jmodel.derived()), |
141 |
|
1911892 |
boost::ref(boost::get<typename JointModelBase<JointModelDerived>::JointDataDerived >(jdata)), |
142 |
|
1911892 |
args)); |
143 |
|
|
} |
144 |
|
|
|
145 |
|
|
ReturnType operator()(const JointModelVoid) {return;} |
146 |
|
|
|
147 |
|
|
JointData & jdata; |
148 |
|
|
ArgType args; |
149 |
|
|
}; |
150 |
|
|
|
151 |
|
|
template<typename JointModel> |
152 |
|
|
struct InternalVisitorModelAndData<JointModel,NoArg> |
153 |
|
|
: public boost::static_visitor<ReturnType> |
154 |
|
|
{ |
155 |
|
|
typedef typename JointModel::JointDataDerived JointData; |
156 |
|
|
|
157 |
|
21 |
InternalVisitorModelAndData(JointData & jdata) |
158 |
|
21 |
: jdata(jdata) |
159 |
|
21 |
{} |
160 |
|
|
|
161 |
|
|
template<typename JointModelDerived> |
162 |
|
42 |
ReturnType operator()(const JointModelBase<JointModelDerived> & jmodel) const |
163 |
|
|
{ |
164 |
✓✗ |
42 |
return bf::invoke(&JointVisitorDerived::template algo<JointModelDerived>, |
165 |
✓✗ |
42 |
bf::make_vector(boost::ref(jmodel.derived()), |
166 |
|
126 |
boost::ref(boost::get<typename JointModelBase<JointModelDerived>::JointDataDerived >(jdata)))); |
167 |
|
|
} |
168 |
|
|
|
169 |
|
|
JointData & jdata; |
170 |
|
|
}; |
171 |
|
|
|
172 |
|
|
template<typename ArgType, typename Dummy = void> |
173 |
|
|
struct InternalVisitorModel |
174 |
|
|
: public boost::static_visitor<ReturnType> |
175 |
|
|
{ |
176 |
|
120998 |
InternalVisitorModel(ArgType args) |
177 |
|
120998 |
: args(args) |
178 |
|
120998 |
{} |
179 |
|
|
|
180 |
|
|
template<typename JointModelDerived> |
181 |
|
63360 |
ReturnType operator()(const JointModelBase<JointModelDerived> & jmodel) const |
182 |
|
|
{ |
183 |
✓✗ |
63360 |
return bf::invoke(&JointVisitorDerived::template algo<JointModelDerived>, |
184 |
✓✗ |
65974 |
bf::append(boost::ref(jmodel.derived()), |
185 |
|
129334 |
args)); |
186 |
|
|
} |
187 |
|
|
|
188 |
|
|
template<typename JointDataDerived> |
189 |
|
80 |
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
190 |
|
|
{ |
191 |
✓✗ |
80 |
return bf::invoke(&JointVisitorDerived::template algo<JointDataDerived>, |
192 |
✓✗ |
160 |
bf::append(boost::ref(jdata.derived()), |
193 |
|
240 |
args)); |
194 |
|
|
} |
195 |
|
|
|
196 |
|
|
ReturnType operator()(const JointModelVoid) {return;} |
197 |
|
|
|
198 |
|
|
ArgType args; |
199 |
|
|
}; |
200 |
|
|
|
201 |
|
|
template<typename Dummy> |
202 |
|
|
struct InternalVisitorModel<NoArg,Dummy> |
203 |
|
|
: public boost::static_visitor<ReturnType> |
204 |
|
|
{ |
205 |
|
331 |
InternalVisitorModel() {} |
206 |
|
|
|
207 |
|
|
template<typename JointModelDerived> |
208 |
|
176 |
ReturnType operator()(const JointModelBase<JointModelDerived> & jmodel) const |
209 |
|
|
{ |
210 |
|
176 |
return JointVisitorDerived::template algo<JointModelDerived>(jmodel.derived()); |
211 |
|
|
} |
212 |
|
|
|
213 |
|
|
template<typename JointDataDerived> |
214 |
|
|
ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const |
215 |
|
|
{ |
216 |
|
|
return JointVisitorDerived::template algo<JointDataDerived>(jdata.derived()); |
217 |
|
|
} |
218 |
|
|
}; |
219 |
|
|
}; // struct JointUnaryVisitorBase |
220 |
|
|
|
221 |
|
|
/// |
222 |
|
|
/// \brief This helper structure is now deprecated and has been replaced by JointUnaryVisitorBase. |
223 |
|
|
/// |
224 |
|
|
template<typename JointVisitorDerived, typename ReturnType = void> |
225 |
|
|
struct PINOCCHIO_DEPRECATED JointVisitorBase |
226 |
|
|
: JointUnaryVisitorBase<JointVisitorDerived,ReturnType> |
227 |
|
|
{ |
228 |
|
|
typedef JointUnaryVisitorBase<JointVisitorDerived,ReturnType> Base; |
229 |
|
|
using Base::run; |
230 |
|
|
}; |
231 |
|
|
|
232 |
|
|
} // namespace fusion |
233 |
|
|
} // namespace pinocchio |
234 |
|
|
|
235 |
|
|
#endif // ifndef __pinocchio_multibody_visitior_joint_unary_visitor_hpp__ |