1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_joint_generic_hpp__ |
6 |
|
|
#define __pinocchio_joint_generic_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/multibody/joint/joint-collection.hpp" |
9 |
|
|
#include "pinocchio/multibody/joint/joint-composite.hpp" |
10 |
|
|
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx" |
11 |
|
|
#include "pinocchio/container/aligned-vector.hpp" |
12 |
|
|
|
13 |
|
|
#include <boost/mpl/contains.hpp> |
14 |
|
|
|
15 |
|
|
namespace pinocchio |
16 |
|
|
{ |
17 |
|
|
|
18 |
|
|
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl> |
19 |
|
|
struct JointTpl; |
20 |
|
|
typedef JointTpl<double> Joint; |
21 |
|
|
|
22 |
|
|
template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> |
23 |
|
|
struct traits< JointTpl<_Scalar,_Options,JointCollectionTpl> > |
24 |
|
|
{ |
25 |
|
|
enum { |
26 |
|
|
Options = _Options, |
27 |
|
|
NQ = Eigen::Dynamic, // Dynamic because unknown at compile time |
28 |
|
|
NV = Eigen::Dynamic |
29 |
|
|
}; |
30 |
|
|
|
31 |
|
|
typedef _Scalar Scalar; |
32 |
|
|
typedef JointCollectionTpl<Scalar,Options> JointCollection; |
33 |
|
|
typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataDerived; |
34 |
|
|
typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelDerived; |
35 |
|
|
|
36 |
|
|
typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t; |
37 |
|
|
typedef SE3Tpl<Scalar,Options> Transformation_t; |
38 |
|
|
typedef MotionTpl<Scalar,Options> Motion_t; |
39 |
|
|
typedef MotionTpl<Scalar,Options> Bias_t; |
40 |
|
|
|
41 |
|
|
// [ABA] |
42 |
|
|
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t; |
43 |
|
|
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t; |
44 |
|
|
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t; |
45 |
|
|
|
46 |
|
|
typedef Constraint_t ConstraintTypeConstRef; |
47 |
|
|
typedef Constraint_t ConstraintTypeRef; |
48 |
|
|
typedef Transformation_t TansformTypeConstRef; |
49 |
|
|
typedef Transformation_t TansformTypeRef; |
50 |
|
|
typedef Motion_t MotionTypeConstRef; |
51 |
|
|
typedef Motion_t MotionTypeRef; |
52 |
|
|
typedef Bias_t BiasTypeConstRef; |
53 |
|
|
typedef Bias_t BiasTypeRef; |
54 |
|
|
typedef U_t UTypeConstRef; |
55 |
|
|
typedef U_t UTypeRef; |
56 |
|
|
typedef D_t DTypeConstRef; |
57 |
|
|
typedef D_t DTypeRef; |
58 |
|
|
typedef UD_t UDTypeConstRef; |
59 |
|
|
typedef UD_t UDTypeRef; |
60 |
|
|
|
61 |
|
|
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t; |
62 |
|
|
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t; |
63 |
|
|
}; |
64 |
|
|
|
65 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
66 |
|
|
struct traits< JointDataTpl<Scalar,Options,JointCollectionTpl> > |
67 |
|
|
{ typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; }; |
68 |
|
|
|
69 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
70 |
|
|
struct traits< JointModelTpl<Scalar,Options,JointCollectionTpl> > |
71 |
|
|
{ typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; }; |
72 |
|
|
|
73 |
|
|
template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> |
74 |
|
|
struct JointDataTpl |
75 |
|
|
: public JointDataBase< JointDataTpl<_Scalar,_Options,JointCollectionTpl> > |
76 |
|
|
, JointCollectionTpl<_Scalar,_Options>::JointDataVariant |
77 |
|
|
{ |
78 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
79 |
|
|
|
80 |
|
|
typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; |
81 |
|
|
typedef JointDataBase<JointDataTpl> Base; |
82 |
|
|
|
83 |
|
|
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
84 |
|
|
|
85 |
|
|
typedef JointCollectionTpl<_Scalar,_Options> JointCollection; |
86 |
|
|
typedef typename JointCollection::JointDataVariant JointDataVariant; |
87 |
|
|
|
88 |
|
|
using Base::operator==; |
89 |
|
|
using Base::operator!=; |
90 |
|
|
|
91 |
|
|
JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); } |
92 |
|
596 |
const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); } |
93 |
|
|
|
94 |
|
776 |
Constraint_t S() const { return constraint_xd(*this); } |
95 |
|
795 |
Transformation_t M() const { return joint_transform(*this); } |
96 |
|
616 |
Motion_t v() const { return motion(*this); } |
97 |
|
616 |
Bias_t c() const { return bias(*this); } |
98 |
|
|
|
99 |
|
|
// [ABA CCRBA] |
100 |
|
616 |
U_t U() const { return u_inertia(*this); } |
101 |
|
616 |
D_t Dinv() const { return dinv_inertia(*this); } |
102 |
|
616 |
UD_t UDinv() const { return udinv_inertia(*this); } |
103 |
|
|
|
104 |
|
644 |
JointDataTpl() |
105 |
|
644 |
: JointDataVariant() |
106 |
|
644 |
{} |
107 |
|
|
|
108 |
|
|
JointDataTpl(const JointDataVariant & jdata_variant) |
109 |
|
|
: JointDataVariant(jdata_variant) |
110 |
|
|
{} |
111 |
|
|
|
112 |
|
|
template<typename JointDataDerived> |
113 |
|
19988 |
JointDataTpl(const JointDataBase<JointDataDerived> & jdata) |
114 |
|
19988 |
: JointCollection::JointDataVariant((JointDataVariant)jdata.derived()) |
115 |
|
|
{ |
116 |
|
|
BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types,JointDataDerived>)); |
117 |
|
19988 |
} |
118 |
|
|
|
119 |
|
|
// Define all the standard accessors |
120 |
|
756 |
Constraint_t S_accessor() const { return S(); } |
121 |
|
748 |
Transformation_t M_accessor() const { return M(); } |
122 |
|
596 |
Motion_t v_accessor() const { return v(); } |
123 |
|
596 |
Bias_t c_accessor() const { return c(); } |
124 |
|
596 |
U_t U_accessor() const { return U(); } |
125 |
|
596 |
D_t Dinv_accessor() const { return Dinv(); } |
126 |
|
596 |
UD_t UDinv_accessor() const { return UDinv(); } |
127 |
|
|
|
128 |
✓✗ |
1 |
static std::string classname() { return "JointData"; } |
129 |
|
20 |
std::string shortname() const { return ::pinocchio::shortname(*this); } |
130 |
|
|
|
131 |
|
|
template<typename JointDataDerived> |
132 |
|
80 |
bool isEqual(const JointDataBase<JointDataDerived> & other) const |
133 |
|
|
{ |
134 |
|
80 |
return ::pinocchio::isEqual(*this,other.derived()); |
135 |
|
|
} |
136 |
|
|
|
137 |
|
378 |
bool isEqual(const JointDataTpl & other) const |
138 |
|
|
{ |
139 |
✓✓✓✗
|
378 |
return Base::isEqual(other) && toVariant() == other.toVariant(); |
140 |
|
|
} |
141 |
|
|
|
142 |
|
378 |
bool operator==(const JointDataTpl & other) const |
143 |
|
|
{ |
144 |
|
378 |
return isEqual(other); |
145 |
|
|
} |
146 |
|
|
|
147 |
|
80 |
bool operator!=(const JointDataTpl & other) const |
148 |
|
|
{ |
149 |
|
80 |
return !(*this == other); |
150 |
|
|
} |
151 |
|
|
|
152 |
|
|
}; |
153 |
|
|
|
154 |
|
|
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
155 |
|
|
struct CastType< NewScalar, JointModelTpl<Scalar,Options,JointCollectionTpl> > |
156 |
|
|
{ |
157 |
|
|
typedef JointModelTpl<NewScalar,Options,JointCollectionTpl> type; |
158 |
|
|
}; |
159 |
|
|
|
160 |
|
|
template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> |
161 |
|
|
struct JointModelTpl |
162 |
|
|
: JointModelBase< JointModelTpl<_Scalar,_Options,JointCollectionTpl> > |
163 |
|
|
, JointCollectionTpl<_Scalar,_Options>::JointModelVariant |
164 |
|
|
{ |
165 |
|
16 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
166 |
|
|
|
167 |
|
|
typedef JointTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; |
168 |
|
|
|
169 |
|
|
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
170 |
|
|
PINOCCHIO_JOINT_USE_INDEXES(JointModelTpl); |
171 |
|
|
|
172 |
|
|
typedef JointCollectionTpl<Scalar,Options> JointCollection; |
173 |
|
|
typedef typename JointCollection::JointDataVariant JointDataVariant; |
174 |
|
|
typedef typename JointCollection::JointModelVariant JointModelVariant; |
175 |
|
|
|
176 |
|
|
using Base::id; |
177 |
|
|
using Base::setIndexes; |
178 |
|
|
using Base::operator==; |
179 |
|
|
using Base::operator!=; |
180 |
|
|
|
181 |
|
1123 |
JointModelTpl() : JointModelVariant() {} |
182 |
|
|
|
183 |
|
|
JointModelTpl(const JointModelVariant & jmodel_variant) |
184 |
|
|
: JointCollection::JointModelVariant(jmodel_variant) |
185 |
|
|
{ |
186 |
|
|
} |
187 |
|
|
|
188 |
|
3 |
const std::vector<bool> hasConfigurationLimit() const |
189 |
|
|
{ |
190 |
|
3 |
return ::pinocchio::hasConfigurationLimit(*this); |
191 |
|
|
} |
192 |
|
|
|
193 |
|
3 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
194 |
|
|
{ |
195 |
|
3 |
return ::pinocchio::hasConfigurationLimitInTangent(*this); |
196 |
|
|
} |
197 |
|
|
|
198 |
|
|
template<typename JointModelDerived> |
199 |
|
16597 |
JointModelTpl(const JointModelBase<JointModelDerived> & jmodel) |
200 |
|
16597 |
: JointModelVariant((JointModelVariant)jmodel.derived()) |
201 |
|
|
{ |
202 |
|
|
BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>)); |
203 |
|
16597 |
} |
204 |
|
|
|
205 |
|
|
JointModelVariant & toVariant() |
206 |
|
|
{ return *static_cast<JointModelVariant*>(this); } |
207 |
|
|
|
208 |
|
4928 |
const JointModelVariant & toVariant() const |
209 |
|
4928 |
{ return *static_cast<const JointModelVariant*>(this); } |
210 |
|
|
|
211 |
|
28 |
JointDataDerived createData() const |
212 |
|
28 |
{ return ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(*this); } |
213 |
|
|
|
214 |
|
|
template<typename JointModelDerived> |
215 |
|
88 |
bool isEqual(const JointModelBase<JointModelDerived> & other) const |
216 |
|
|
{ |
217 |
|
88 |
return ::pinocchio::isEqual(*this,other.derived());; |
218 |
|
|
} |
219 |
|
|
|
220 |
|
|
template<typename JointModelDerived> |
221 |
|
2512 |
bool hasSameIndexes(const JointModelBase<JointModelDerived> & other) const |
222 |
|
|
{ |
223 |
|
2512 |
return ::pinocchio::hasSameIndexes(*this,other.derived()); |
224 |
|
|
} |
225 |
|
|
|
226 |
|
2467 |
bool isEqual(const JointModelTpl & other) const |
227 |
|
|
{ |
228 |
✓✓✓✗
|
2467 |
return Base::isEqual(other) && toVariant() == other.toVariant(); |
229 |
|
|
} |
230 |
|
|
|
231 |
|
2463 |
bool operator==(const JointModelTpl & other) const |
232 |
|
|
{ |
233 |
|
2463 |
return isEqual(other); |
234 |
|
|
} |
235 |
|
|
|
236 |
|
2 |
bool operator!=(const JointModelTpl & other) const |
237 |
|
|
{ |
238 |
|
2 |
return !(*this == other); |
239 |
|
|
} |
240 |
|
|
|
241 |
|
|
template<typename ConfigVector> |
242 |
|
27 |
void calc(JointDataDerived & data, |
243 |
|
|
const Eigen::MatrixBase<ConfigVector> & q) const |
244 |
|
27 |
{ calc_zero_order(*this,data,q); } |
245 |
|
|
|
246 |
|
|
template<typename ConfigVector, typename TangentVector> |
247 |
|
40 |
void calc(JointDataDerived & data, |
248 |
|
|
const Eigen::MatrixBase<ConfigVector> & q, |
249 |
|
|
const Eigen::MatrixBase<TangentVector> & v) const |
250 |
|
40 |
{ calc_first_order(*this,data,q,v); } |
251 |
|
|
|
252 |
|
|
template<typename Matrix6Like> |
253 |
|
40 |
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const |
254 |
|
40 |
{ ::pinocchio::calc_aba(*this,data,PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),update_I); } |
255 |
|
|
|
256 |
|
37 |
std::string shortname() const { return ::pinocchio::shortname(*this); } |
257 |
✓✗ |
1 |
static std::string classname() { return "JointModel"; } |
258 |
|
|
|
259 |
|
952507 |
int nq_impl() const { return ::pinocchio::nq(*this); } |
260 |
|
6435496 |
int nv_impl() const { return ::pinocchio::nv(*this); } |
261 |
|
|
|
262 |
|
934881 |
int idx_q_impl() const { return ::pinocchio::idx_q(*this); } |
263 |
|
2707447 |
int idx_v_impl() const { return ::pinocchio::idx_v(*this); } |
264 |
|
|
|
265 |
|
2820 |
JointIndex id_impl() const { return ::pinocchio::id(*this); } |
266 |
|
|
|
267 |
|
8323 |
void setIndexes(JointIndex id, int nq, int nv) |
268 |
|
|
{ |
269 |
|
8323 |
::pinocchio::setIndexes(*this, id, nq, nv); |
270 |
|
8323 |
} |
271 |
|
|
|
272 |
|
|
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
273 |
|
|
template<typename NewScalar> |
274 |
|
170 |
JointModelTpl<NewScalar,Options,JointCollectionTpl> cast() const |
275 |
|
|
{ |
276 |
|
170 |
return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*this); |
277 |
|
|
} |
278 |
|
|
}; |
279 |
|
|
|
280 |
|
|
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector; |
281 |
|
|
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector; |
282 |
|
|
|
283 |
|
|
|
284 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived> |
285 |
|
40 |
bool operator==(const JointDataBase<JointDataDerived> & joint_data, |
286 |
|
|
const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic) |
287 |
|
|
{ |
288 |
|
40 |
return joint_data_generic == joint_data.derived(); |
289 |
|
|
} |
290 |
|
|
|
291 |
|
|
|
292 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointDataDerived> |
293 |
|
40 |
bool operator!=(const JointDataBase<JointDataDerived> & joint_data, |
294 |
|
|
const JointDataTpl<Scalar,Options,JointCollectionTpl> & joint_data_generic) |
295 |
|
|
{ |
296 |
✓✗ |
40 |
return joint_data_generic != joint_data.derived(); |
297 |
|
|
} |
298 |
|
|
|
299 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived> |
300 |
|
22 |
bool operator==(const JointModelBase<JointModelDerived> & joint_model, |
301 |
|
|
const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic) |
302 |
|
|
{ |
303 |
|
22 |
return joint_model_generic == joint_model.derived(); |
304 |
|
|
} |
305 |
|
|
|
306 |
|
|
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename JointModelDerived> |
307 |
|
|
bool operator!=(const JointModelBase<JointModelDerived> & joint_model, |
308 |
|
|
const JointModelTpl<Scalar,Options,JointCollectionTpl> & joint_model_generic) |
309 |
|
|
{ |
310 |
|
|
return joint_model_generic != joint_model.derived(); |
311 |
|
|
} |
312 |
|
|
|
313 |
|
|
} // namespace pinocchio |
314 |
|
|
|
315 |
|
|
#endif // ifndef __pinocchio_joint_generic_hpp__ |