GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 CNRS INRIA |
||
3 |
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
||
4 |
// |
||
5 |
|||
6 |
#ifndef __pinocchio_multibody_joint_model_base_hpp__ |
||
7 |
#define __pinocchio_multibody_joint_model_base_hpp__ |
||
8 |
|||
9 |
#include "pinocchio/multibody/joint/joint-base.hpp" |
||
10 |
#include "pinocchio/multibody/joint/joint-common-operations.hpp" |
||
11 |
|||
12 |
#include "pinocchio/math/matrix-block.hpp" |
||
13 |
|||
14 |
#include <limits> |
||
15 |
|||
16 |
#define PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,TYPENAME) \ |
||
17 |
typedef Eigen::DenseIndex Index; \ |
||
18 |
typedef TYPENAME traits<Joint>::Scalar Scalar; \ |
||
19 |
typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived; \ |
||
20 |
typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived; \ |
||
21 |
typedef TYPENAME traits<Joint>::Constraint_t Constraint_t; \ |
||
22 |
typedef TYPENAME traits<Joint>::Transformation_t Transformation_t; \ |
||
23 |
typedef TYPENAME traits<Joint>::Motion_t Motion_t; \ |
||
24 |
typedef TYPENAME traits<Joint>::Bias_t Bias_t; \ |
||
25 |
typedef TYPENAME traits<Joint>::U_t U_t; \ |
||
26 |
typedef TYPENAME traits<Joint>::D_t D_t; \ |
||
27 |
typedef TYPENAME traits<Joint>::UD_t UD_t; \ |
||
28 |
enum { \ |
||
29 |
Options = traits<Joint>::Options, \ |
||
30 |
NQ = traits<Joint>::NQ, \ |
||
31 |
NV = traits<Joint>::NV \ |
||
32 |
}; \ |
||
33 |
typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \ |
||
34 |
typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t |
||
35 |
|||
36 |
#ifdef __clang__ |
||
37 |
|||
38 |
#define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG) |
||
39 |
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename) |
||
40 |
|||
41 |
#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2) |
||
42 |
|||
43 |
#define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG) |
||
44 |
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename) |
||
45 |
|||
46 |
#else |
||
47 |
|||
48 |
#define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename) |
||
49 |
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename) |
||
50 |
|||
51 |
#endif |
||
52 |
|||
53 |
#define PINOCCHIO_JOINT_USE_INDEXES(Joint) \ |
||
54 |
typedef JointModelBase<Joint> Base; \ |
||
55 |
using Base::idx_q; \ |
||
56 |
using Base::idx_v |
||
57 |
|||
58 |
#define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \ |
||
59 |
template<typename Scalar, int Options, typename NewScalar> \ |
||
60 |
struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \ |
||
61 |
{ typedef JointModelTpl<NewScalar,Options> type; } |
||
62 |
|||
63 |
namespace pinocchio |
||
64 |
{ |
||
65 |
|||
66 |
template<typename Derived> |
||
67 |
struct JointModelBase |
||
68 |
{ |
||
69 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
70 |
|||
71 |
typedef typename traits<Derived>::JointDerived JointDerived; |
||
72 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
73 |
|||
74 |
20070 |
JointModelDerived & derived() { return *static_cast<Derived*>(this); } |
|
75 |
26255171 |
const JointModelDerived & derived() const { return *static_cast<const Derived*>(this); } |
|
76 |
|||
77 |
19990 |
JointDataDerived createData() const { return derived().createData(); } |
|
78 |
|||
79 |
6 |
const std::vector<bool> hasConfigurationLimit() const |
|
80 |
{ |
||
81 |
6 |
return derived().hasConfigurationLimit(); |
|
82 |
} |
||
83 |
|||
84 |
6 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
|
85 |
{ |
||
86 |
6 |
return derived().hasConfigurationLimitInTangent(); |
|
87 |
} |
||
88 |
|||
89 |
template<typename ConfigVectorType> |
||
90 |
800856 |
void calc(JointDataDerived & data, |
|
91 |
const Eigen::MatrixBase<ConfigVectorType> & qs) const |
||
92 |
{ |
||
93 |
800856 |
derived().calc(data,qs.derived()); |
|
94 |
800856 |
} |
|
95 |
|||
96 |
template<typename ConfigVectorType, typename TangentVectorType> |
||
97 |
89670 |
void calc(JointDataDerived & data, |
|
98 |
const Eigen::MatrixBase<ConfigVectorType> & qs, |
||
99 |
const Eigen::MatrixBase<TangentVectorType> & vs) const |
||
100 |
{ |
||
101 |
89670 |
derived().calc(data,qs.derived(),vs.derived()); |
|
102 |
89670 |
} |
|
103 |
|||
104 |
template<typename Matrix6Type> |
||
105 |
13944 |
void calc_aba(JointDataDerived & data, |
|
106 |
const Eigen::MatrixBase<Matrix6Type> & I, |
||
107 |
const bool update_I = false) const |
||
108 |
{ |
||
109 |
13944 |
derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I); |
|
110 |
13944 |
} |
|
111 |
|||
112 |
27156068 |
int nv() const { return derived().nv_impl(); } |
|
113 |
2129815 |
int nq() const { return derived().nq_impl(); } |
|
114 |
|||
115 |
// Default _impl methods are reimplemented by dynamic-size joints. |
||
116 |
14284034 |
int nv_impl() const { return NV; } |
|
117 |
1176993 |
int nq_impl() const { return NQ; } |
|
118 |
|||
119 |
4972578 |
int idx_q() const { return derived().idx_q_impl(); } |
|
120 |
6001332 |
int idx_v() const { return derived().idx_v_impl(); } |
|
121 |
506143 |
JointIndex id() const { return derived().id_impl(); } |
|
122 |
|||
123 |
3131538 |
int idx_q_impl() const { return i_q; } |
|
124 |
3293697 |
int idx_v_impl() const { return i_v; } |
|
125 |
504431 |
JointIndex id_impl() const { return i_id; } |
|
126 |
|||
127 |
20068 |
void setIndexes(JointIndex id, int q, int v) |
|
128 |
20068 |
{ derived().setIndexes_impl(id, q, v); } |
|
129 |
|||
130 |
19932 |
void setIndexes_impl(JointIndex id,int q,int v) |
|
131 |
19932 |
{ i_id = id, i_q = q; i_v = v; } |
|
132 |
|||
133 |
72 |
void disp(std::ostream & os) const |
|
134 |
{ |
||
135 |
using namespace std; |
||
136 |
os |
||
137 |
✓✗✓✗ |
144 |
<< shortname() << endl |
138 |
✓✗✓✗ ✓✗✓✗ |
72 |
<< " index: " << id() << endl |
139 |
✓✗✓✗ ✓✗✓✗ |
72 |
<< " index q: " << idx_q() << endl |
140 |
✓✗✓✗ ✓✗✓✗ |
72 |
<< " index v: " << idx_v() << endl |
141 |
✓✗✓✗ ✓✗✓✗ |
72 |
<< " nq: " << nq() << endl |
142 |
✓✗✓✗ ✓✗✓✗ |
72 |
<< " nv: " << nv() << endl |
143 |
; |
||
144 |
72 |
} |
|
145 |
|||
146 |
72 |
friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint) |
|
147 |
{ |
||
148 |
72 |
joint.disp(os); |
|
149 |
72 |
return os; |
|
150 |
} |
||
151 |
|||
152 |
1134 |
std::string shortname() const { return derived().shortname(); } |
|
153 |
22 |
static std::string classname() { return Derived::classname(); } |
|
154 |
|||
155 |
template<typename NewScalar> |
||
156 |
442 |
typename CastType<NewScalar,Derived>::type cast() const |
|
157 |
442 |
{ return derived().template cast<NewScalar>(); } |
|
158 |
|||
159 |
template <class OtherDerived> |
||
160 |
3432 |
bool operator==(const JointModelBase<OtherDerived> & other) const |
|
161 |
3432 |
{ return derived().isEqual(other.derived()); } |
|
162 |
|||
163 |
template <class OtherDerived> |
||
164 |
47 |
bool operator!=(const JointModelBase<OtherDerived> & other) const |
|
165 |
47 |
{ return !(derived() == other.derived()); } |
|
166 |
|||
167 |
template <class OtherDerived> |
||
168 |
4 |
bool isEqual(const JointModelBase<OtherDerived> &) const |
|
169 |
4 |
{ return false; } |
|
170 |
|||
171 |
6138 |
bool isEqual(const JointModelBase<Derived> & other) const |
|
172 |
{ |
||
173 |
6138 |
return derived().hasSameIndexes(other.derived()); |
|
174 |
} |
||
175 |
|||
176 |
template <class OtherDerived> |
||
177 |
6178 |
bool hasSameIndexes(const JointModelBase<OtherDerived> & other) const |
|
178 |
{ |
||
179 |
6178 |
return other.id() == id() |
|
180 |
✓✗ | 6086 |
&& other.idx_q() == idx_q() |
181 |
✓✓✓✗ |
12264 |
&& other.idx_v() == idx_v(); |
182 |
} |
||
183 |
|||
184 |
/* Acces to dedicated segment in robot config space. */ |
||
185 |
// Const access |
||
186 |
template<typename D> |
||
187 |
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType |
||
188 |
121206 |
jointConfigSelector(const Eigen::MatrixBase<D> & a) const |
|
189 |
121206 |
{ return derived().jointConfigSelector_impl(a); } |
|
190 |
|||
191 |
template<typename D> |
||
192 |
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType |
||
193 |
121194 |
jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const |
|
194 |
121194 |
{ return SizeDepType<NQ>::segment(a.derived(),idx_q(),nq()); } |
|
195 |
|||
196 |
// Non-const access |
||
197 |
template<typename D> |
||
198 |
typename SizeDepType<NQ>::template SegmentReturn<D>::Type |
||
199 |
136234 |
jointConfigSelector(Eigen::MatrixBase<D> & a) const |
|
200 |
136234 |
{ return derived().jointConfigSelector_impl(a); } |
|
201 |
|||
202 |
template<typename D> |
||
203 |
typename SizeDepType<NQ>::template SegmentReturn<D>::Type |
||
204 |
136228 |
jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const |
|
205 |
136228 |
{ return SizeDepType<NQ>::segment(a,idx_q(),nq()); } |
|
206 |
|||
207 |
/* Acces to dedicated segment in robot config velocity space. */ |
||
208 |
// Const access |
||
209 |
template<typename D> |
||
210 |
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType |
||
211 |
159554 |
jointVelocitySelector(const Eigen::MatrixBase<D> & a) const |
|
212 |
159554 |
{ return derived().jointVelocitySelector_impl(a.derived()); } |
|
213 |
|||
214 |
template<typename D> |
||
215 |
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType |
||
216 |
159552 |
jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const |
|
217 |
159552 |
{ return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); } |
|
218 |
|||
219 |
// Non-const access |
||
220 |
template<typename D> |
||
221 |
typename SizeDepType<NV>::template SegmentReturn<D>::Type |
||
222 |
172796 |
jointVelocitySelector(Eigen::MatrixBase<D> & a) const |
|
223 |
172796 |
{ return derived().jointVelocitySelector_impl(a.derived()); } |
|
224 |
|||
225 |
template<typename D> |
||
226 |
typename SizeDepType<NV>::template SegmentReturn<D>::Type |
||
227 |
172796 |
jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const |
|
228 |
172796 |
{ return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); } |
|
229 |
|||
230 |
/* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ |
||
231 |
// Const access |
||
232 |
template<typename D> |
||
233 |
typename SizeDepType<NV>::template ColsReturn<D>::ConstType |
||
234 |
770 |
jointCols(const Eigen::MatrixBase<D>& A) const |
|
235 |
770 |
{ return derived().jointCols_impl(A.derived()); } |
|
236 |
|||
237 |
template<typename D> |
||
238 |
typename SizeDepType<NV>::template ColsReturn<D>::ConstType |
||
239 |
770 |
jointCols_impl(const Eigen::MatrixBase<D>& A) const |
|
240 |
770 |
{ return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); } |
|
241 |
|||
242 |
// Non-const access |
||
243 |
template<typename D> |
||
244 |
typename SizeDepType<NV>::template ColsReturn<D>::Type |
||
245 |
202960 |
jointCols(Eigen::MatrixBase<D>& A) const |
|
246 |
202960 |
{ return derived().jointCols_impl(A.derived()); } |
|
247 |
|||
248 |
template<typename D> |
||
249 |
typename SizeDepType<NV>::template ColsReturn<D>::Type |
||
250 |
202960 |
jointCols_impl(Eigen::MatrixBase<D> & A) const |
|
251 |
202960 |
{ return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); } |
|
252 |
|||
253 |
/* Acces to dedicated rows in a matrix.*/ |
||
254 |
// Const access |
||
255 |
template<typename D> |
||
256 |
typename SizeDepType<NV>::template RowsReturn<D>::ConstType |
||
257 |
36 |
jointRows(const Eigen::MatrixBase<D> & A) const |
|
258 |
36 |
{ return derived().jointRows_impl(A.derived()); } |
|
259 |
|||
260 |
template<typename D> |
||
261 |
typename SizeDepType<NV>::template RowsReturn<D>::ConstType |
||
262 |
36 |
jointRows_impl(const Eigen::MatrixBase<D> & A) const |
|
263 |
36 |
{ return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); } |
|
264 |
|||
265 |
// Non-const access |
||
266 |
template<typename D> |
||
267 |
typename SizeDepType<NV>::template RowsReturn<D>::Type |
||
268 |
72 |
jointRows(Eigen::MatrixBase<D> & A) const |
|
269 |
72 |
{ return derived().jointRows_impl(A.derived()); } |
|
270 |
|||
271 |
template<typename D> |
||
272 |
typename SizeDepType<NV>::template RowsReturn<D>::Type |
||
273 |
72 |
jointRows_impl(Eigen::MatrixBase<D> & A) const |
|
274 |
72 |
{ return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); } |
|
275 |
|||
276 |
/// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat |
||
277 |
// Const access |
||
278 |
template<typename D> |
||
279 |
typename SizeDepType<NV>::template BlockReturn<D>::ConstType |
||
280 |
jointBlock(const Eigen::MatrixBase<D> & Mat) const |
||
281 |
{ return derived().jointBlock_impl(Mat.derived()); } |
||
282 |
|||
283 |
template<typename D> |
||
284 |
typename SizeDepType<NV>::template BlockReturn<D>::ConstType |
||
285 |
jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const |
||
286 |
{ return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); } |
||
287 |
|||
288 |
// Non-const access |
||
289 |
template<typename D> |
||
290 |
typename SizeDepType<NV>::template BlockReturn<D>::Type |
||
291 |
504 |
jointBlock(Eigen::MatrixBase<D> & Mat) const |
|
292 |
504 |
{ return derived().jointBlock_impl(Mat.derived()); } |
|
293 |
|||
294 |
template<typename D> |
||
295 |
typename SizeDepType<NV>::template BlockReturn<D>::Type |
||
296 |
504 |
jointBlock_impl(Eigen::MatrixBase<D> & Mat) const |
|
297 |
504 |
{ return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); } |
|
298 |
|||
299 |
protected: |
||
300 |
|||
301 |
/// Default constructor: protected. |
||
302 |
/// |
||
303 |
/// Prevent the construction of stand-alone JointModelBase. |
||
304 |
40456 |
inline JointModelBase() |
|
305 |
40456 |
: i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {} |
|
306 |
|||
307 |
/// Copy constructor: protected. |
||
308 |
/// |
||
309 |
/// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting |
||
310 |
/// objects. Copy is done by calling copy operator. |
||
311 |
154926 |
inline JointModelBase(const JointModelBase & clone) |
|
312 |
154926 |
{ *this = clone; } |
|
313 |
|||
314 |
/// Copy operator: protected. |
||
315 |
/// |
||
316 |
/// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting |
||
317 |
/// objects. |
||
318 |
77760 |
inline JointModelBase & operator=(const JointModelBase & clone) |
|
319 |
{ |
||
320 |
77760 |
i_id = clone.i_id; |
|
321 |
77760 |
i_q = clone.i_q; |
|
322 |
77760 |
i_v = clone.i_v; |
|
323 |
77760 |
return *this; |
|
324 |
} |
||
325 |
|||
326 |
// data |
||
327 |
JointIndex i_id; // ID of the joint in the multibody list. |
||
328 |
int i_q; // Index of the joint configuration in the joint configuration vector. |
||
329 |
int i_v; // Index of the joint velocity in the joint velocity vector. |
||
330 |
|||
331 |
}; // struct JointModelBase |
||
332 |
|||
333 |
} // namespace pinocchio |
||
334 |
|||
335 |
#endif // ifndef __pinocchio_multibody_joint_model_base_hpp__ |
||
336 |
Generated by: GCOVR (Version 4.2) |