GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2019-2021 INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_multibody_joint_mimic_hpp__ |
||
6 |
#define __pinocchio_multibody_joint_mimic_hpp__ |
||
7 |
|||
8 |
#include "pinocchio/macros.hpp" |
||
9 |
#include "pinocchio/multibody/joint/joint-base.hpp" |
||
10 |
|||
11 |
namespace pinocchio |
||
12 |
{ |
||
13 |
|||
14 |
template<class Constraint> struct ScaledConstraint; |
||
15 |
|||
16 |
template<class Constraint> |
||
17 |
struct traits< ScaledConstraint<Constraint> > |
||
18 |
{ |
||
19 |
typedef typename traits<Constraint>::Scalar Scalar; |
||
20 |
enum { Options = traits<Constraint>::Options }; |
||
21 |
enum { |
||
22 |
LINEAR = traits<Constraint>::LINEAR, |
||
23 |
ANGULAR = traits<Constraint>::ANGULAR |
||
24 |
}; |
||
25 |
typedef typename traits<Constraint>::JointMotion JointMotion; |
||
26 |
typedef typename traits<Constraint>::JointForce JointForce; |
||
27 |
typedef typename traits<Constraint>::DenseBase DenseBase; |
||
28 |
typedef typename traits<Constraint>::MatrixReturnType MatrixReturnType; |
||
29 |
typedef typename traits<Constraint>::ConstMatrixReturnType ConstMatrixReturnType; |
||
30 |
}; // traits ScaledConstraint |
||
31 |
|||
32 |
template<class Constraint> |
||
33 |
struct SE3GroupAction< ScaledConstraint<Constraint> > |
||
34 |
{ typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; }; |
||
35 |
|||
36 |
template<class Constraint, typename MotionDerived> |
||
37 |
struct MotionAlgebraAction< ScaledConstraint<Constraint>, MotionDerived > |
||
38 |
{ typedef typename MotionAlgebraAction<Constraint,MotionDerived>::ReturnType ReturnType; }; |
||
39 |
|||
40 |
template<class Constraint, typename ForceDerived> |
||
41 |
struct ConstraintForceOp< ScaledConstraint<Constraint>, ForceDerived> |
||
42 |
{ |
||
43 |
typedef typename Constraint::Scalar Scalar; |
||
44 |
typedef typename ConstraintForceOp<Constraint,ForceDerived>::ReturnType OriginalReturnType; |
||
45 |
|||
46 |
typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType; |
||
47 |
typedef Eigen::Matrix<Scalar,IdealReturnType::RowsAtCompileTime,IdealReturnType::ColsAtCompileTime,Constraint::Options> ReturnType; |
||
48 |
}; |
||
49 |
|||
50 |
template<class Constraint, typename ForceSet> |
||
51 |
struct ConstraintForceSetOp< ScaledConstraint<Constraint>, ForceSet> |
||
52 |
{ |
||
53 |
typedef typename Constraint::Scalar Scalar; |
||
54 |
typedef typename ConstraintForceSetOp<Constraint,ForceSet>::ReturnType OriginalReturnType; |
||
55 |
typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType; |
||
56 |
typedef Eigen::Matrix<Scalar,Constraint::NV,ForceSet::ColsAtCompileTime,Constraint::Options | Eigen::RowMajor> ReturnType; |
||
57 |
}; |
||
58 |
|||
59 |
template<class Constraint> |
||
60 |
struct ScaledConstraint |
||
61 |
: ConstraintBase< ScaledConstraint<Constraint> > |
||
62 |
{ |
||
63 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
64 |
|||
65 |
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledConstraint) |
||
66 |
enum { NV = Constraint::NV }; |
||
67 |
typedef ConstraintBase<ScaledConstraint> Base; |
||
68 |
using Base::nv; |
||
69 |
|||
70 |
typedef typename SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType; |
||
71 |
|||
72 |
228 |
ScaledConstraint() {} |
|
73 |
|||
74 |
156 |
explicit ScaledConstraint(const Scalar & scaling_factor) |
|
75 |
156 |
: m_scaling_factor(scaling_factor) |
|
76 |
156 |
{} |
|
77 |
|||
78 |
284 |
ScaledConstraint(const Constraint & constraint, |
|
79 |
const Scalar & scaling_factor) |
||
80 |
: m_constraint(constraint) |
||
81 |
284 |
, m_scaling_factor(scaling_factor) |
|
82 |
284 |
{} |
|
83 |
|||
84 |
6 |
ScaledConstraint(const ScaledConstraint & other) |
|
85 |
: m_constraint(other.m_constraint) |
||
86 |
6 |
, m_scaling_factor(other.m_scaling_factor) |
|
87 |
6 |
{} |
|
88 |
|||
89 |
186 |
ScaledConstraint & operator=(const ScaledConstraint & other) |
|
90 |
{ |
||
91 |
m_constraint = other.m_constraint; |
||
92 |
186 |
m_scaling_factor = other.m_scaling_factor; |
|
93 |
186 |
return *this; |
|
94 |
} |
||
95 |
|||
96 |
template<typename VectorLike> |
||
97 |
28 |
JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const |
|
98 |
{ |
||
99 |
✓✗✓✗ ✗✓ |
28 |
assert(v.size() == nv()); |
100 |
✓✗ | 28 |
JointMotion jm = m_constraint * v; |
101 |
✓✗ | 56 |
return jm * m_scaling_factor; |
102 |
} |
||
103 |
|||
104 |
template<typename S1, int O1> |
||
105 |
SE3ActionReturnType |
||
106 |
40 |
se3Action(const SE3Tpl<S1,O1> & m) const |
|
107 |
{ |
||
108 |
✓✗ | 40 |
SE3ActionReturnType res = m_constraint.se3Action(m); |
109 |
✓✗✓✗ |
40 |
return m_scaling_factor * res; |
110 |
} |
||
111 |
|||
112 |
template<typename S1, int O1> |
||
113 |
SE3ActionReturnType |
||
114 |
18 |
se3ActionInverse(const SE3Tpl<S1,O1> & m) const |
|
115 |
{ |
||
116 |
✓✗ | 18 |
SE3ActionReturnType res = m_constraint.se3ActionInverse(m); |
117 |
✓✗✓✗ |
18 |
return m_scaling_factor * res; |
118 |
} |
||
119 |
|||
120 |
146 |
int nv_impl() const { return m_constraint.nv(); } |
|
121 |
|||
122 |
struct TransposeConst |
||
123 |
{ |
||
124 |
const ScaledConstraint & ref; |
||
125 |
56 |
TransposeConst(const ScaledConstraint & ref) : ref(ref) {} |
|
126 |
|||
127 |
template<typename Derived> |
||
128 |
typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType |
||
129 |
28 |
operator*(const ForceDense<Derived> & f) const |
|
130 |
{ |
||
131 |
// TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level; |
||
132 |
typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType; |
||
133 |
✓✗✓✗ ✓✗ |
28 |
return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); |
134 |
} |
||
135 |
|||
136 |
/// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) |
||
137 |
template<typename Derived> |
||
138 |
typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType |
||
139 |
28 |
operator*(const Eigen::MatrixBase<Derived> & F) const |
|
140 |
{ |
||
141 |
typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType; |
||
142 |
✓✗✓✗ ✓✗ |
28 |
return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F)); |
143 |
} |
||
144 |
|||
145 |
}; // struct TransposeConst |
||
146 |
|||
147 |
56 |
TransposeConst transpose() const { return TransposeConst(*this); } |
|
148 |
|||
149 |
144 |
DenseBase matrix_impl() const |
|
150 |
{ |
||
151 |
✓✗✓✗ |
144 |
DenseBase S = m_scaling_factor * m_constraint.matrix(); |
152 |
144 |
return S; |
|
153 |
} |
||
154 |
|||
155 |
template<typename MotionDerived> |
||
156 |
typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType |
||
157 |
34 |
motionAction(const MotionDense<MotionDerived> & m) const |
|
158 |
{ |
||
159 |
typedef typename MotionAlgebraAction<ScaledConstraint,MotionDerived>::ReturnType ReturnType; |
||
160 |
✓✗✓✗ |
34 |
ReturnType res = m_scaling_factor * m_constraint.motionAction(m); |
161 |
34 |
return res; |
|
162 |
} |
||
163 |
|||
164 |
46 |
inline const Scalar & scaling() const { return m_scaling_factor; } |
|
165 |
168 |
inline Scalar & scaling() { return m_scaling_factor; } |
|
166 |
|||
167 |
46 |
inline const Constraint & constraint() const { return m_constraint; } |
|
168 |
168 |
inline Constraint & constraint() { return m_constraint; } |
|
169 |
|||
170 |
96 |
bool isEqual(const ScaledConstraint & other) const |
|
171 |
{ |
||
172 |
96 |
return m_constraint == other.m_constraint |
|
173 |
✓✗✓✗ |
96 |
&& m_scaling_factor == other.m_scaling_factor; |
174 |
} |
||
175 |
|||
176 |
protected: |
||
177 |
|||
178 |
Constraint m_constraint; |
||
179 |
Scalar m_scaling_factor; |
||
180 |
}; // struct ScaledConstraint |
||
181 |
|||
182 |
template<typename S1, int O1, typename _Constraint> |
||
183 |
struct MultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> > |
||
184 |
{ |
||
185 |
typedef InertiaTpl<S1,O1> Inertia; |
||
186 |
typedef ScaledConstraint<_Constraint> Constraint; |
||
187 |
typedef typename Constraint::Scalar Scalar; |
||
188 |
|||
189 |
typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType; |
||
190 |
// typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type ReturnType; |
||
191 |
typedef OriginalReturnType ReturnType; |
||
192 |
}; |
||
193 |
|||
194 |
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
||
195 |
namespace impl |
||
196 |
{ |
||
197 |
template<typename S1, int O1, typename _Constraint> |
||
198 |
struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ScaledConstraint<_Constraint> > |
||
199 |
{ |
||
200 |
typedef InertiaTpl<S1,O1> Inertia; |
||
201 |
typedef ScaledConstraint<_Constraint> Constraint; |
||
202 |
typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType; |
||
203 |
|||
204 |
34 |
static inline ReturnType run(const Inertia & Y, |
|
205 |
const Constraint & scaled_constraint) |
||
206 |
{ |
||
207 |
✓✗✓✗ |
34 |
return scaled_constraint.scaling() * (Y * scaled_constraint.constraint()); |
208 |
} |
||
209 |
}; |
||
210 |
} // namespace impl |
||
211 |
|||
212 |
template<typename M6Like, typename _Constraint> |
||
213 |
struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> > |
||
214 |
{ |
||
215 |
typedef typename MultiplicationOp<Inertia,_Constraint>::ReturnType OriginalReturnType; |
||
216 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType; |
||
217 |
}; |
||
218 |
|||
219 |
/* [ABA] operator* (Inertia Y,Constraint S) */ |
||
220 |
namespace impl |
||
221 |
{ |
||
222 |
template<typename M6Like, typename _Constraint> |
||
223 |
struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledConstraint<_Constraint> > |
||
224 |
{ |
||
225 |
typedef ScaledConstraint<_Constraint> Constraint; |
||
226 |
typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType; |
||
227 |
|||
228 |
12 |
static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, |
|
229 |
const Constraint & scaled_constraint) |
||
230 |
{ |
||
231 |
✓✗✓✗ |
12 |
return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint()); |
232 |
} |
||
233 |
}; |
||
234 |
} // namespace impl |
||
235 |
|||
236 |
template<class Joint> struct JointMimic; |
||
237 |
template<class JointModel> struct JointModelMimic; |
||
238 |
template<class JointData> struct JointDataMimic; |
||
239 |
|||
240 |
template<class Joint> |
||
241 |
struct traits< JointMimic<Joint> > |
||
242 |
{ |
||
243 |
enum |
||
244 |
{ |
||
245 |
NQ = traits<Joint>::NV, |
||
246 |
NV = traits<Joint>::NQ |
||
247 |
}; |
||
248 |
typedef typename traits<Joint>::Scalar Scalar; |
||
249 |
enum { Options = traits<Joint>::Options }; |
||
250 |
|||
251 |
typedef typename traits<Joint>::JointDataDerived JointDataBase; |
||
252 |
typedef typename traits<Joint>::JointModelDerived JointModelBase; |
||
253 |
|||
254 |
typedef JointDataMimic<JointDataBase> JointDataDerived; |
||
255 |
typedef JointModelMimic<JointModelBase> JointModelDerived; |
||
256 |
|||
257 |
typedef ScaledConstraint<typename traits<Joint>::Constraint_t> Constraint_t; |
||
258 |
typedef typename traits<Joint>::Transformation_t Transformation_t; |
||
259 |
typedef typename traits<Joint>::Motion_t Motion_t; |
||
260 |
typedef typename traits<Joint>::Bias_t Bias_t; |
||
261 |
|||
262 |
// [ABA] |
||
263 |
typedef typename traits<Joint>::U_t U_t; |
||
264 |
typedef typename traits<Joint>::D_t D_t; |
||
265 |
typedef typename traits<Joint>::UD_t UD_t; |
||
266 |
|||
267 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
268 |
|||
269 |
typedef typename traits<Joint>::ConfigVector_t ConfigVector_t; |
||
270 |
typedef typename traits<Joint>::TangentVector_t TangentVector_t; |
||
271 |
}; |
||
272 |
|||
273 |
template<class Joint> |
||
274 |
struct traits< JointDataMimic<Joint> > |
||
275 |
{ typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; }; |
||
276 |
|||
277 |
template<class Joint> |
||
278 |
struct traits< JointModelMimic<Joint> > |
||
279 |
{ typedef JointMimic<typename traits<Joint>::JointDerived> JointDerived; }; |
||
280 |
|||
281 |
template<class JointData> |
||
282 |
struct JointDataMimic |
||
283 |
: public JointDataBase< JointDataMimic<JointData> > |
||
284 |
{ |
||
285 |
72 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
286 |
|||
287 |
typedef typename traits<JointDataMimic>::JointDerived JointDerived; |
||
288 |
typedef JointDataBase< JointDataMimic<JointData> > Base; |
||
289 |
|||
290 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
291 |
|||
292 |
156 |
JointDataMimic() |
|
293 |
: m_scaling((Scalar)0) |
||
294 |
, m_q_transform(ConfigVector_t::Zero()) |
||
295 |
, m_v_transform(TangentVector_t::Zero()) |
||
296 |
✓✗✓✗ |
156 |
, S((Scalar)0) |
297 |
156 |
{} |
|
298 |
|||
299 |
186 |
JointDataMimic(const JointDataMimic & other) |
|
300 |
186 |
{ *this = other; } |
|
301 |
|||
302 |
76 |
JointDataMimic(const JointDataBase<JointData> & jdata, |
|
303 |
const Scalar & scaling) |
||
304 |
: m_jdata_ref(jdata.derived()) |
||
305 |
, m_scaling(scaling) |
||
306 |
76 |
, S(m_jdata_ref.S,scaling) |
|
307 |
76 |
{} |
|
308 |
|||
309 |
186 |
JointDataMimic & operator=(const JointDataMimic & other) |
|
310 |
{ |
||
311 |
186 |
m_jdata_ref = other.m_jdata_ref; |
|
312 |
186 |
m_scaling = other.m_scaling; |
|
313 |
186 |
m_q_transform = other.m_q_transform; |
|
314 |
186 |
m_v_transform = other.m_v_transform; |
|
315 |
186 |
S = Constraint_t(m_jdata_ref.S,other.m_scaling); |
|
316 |
186 |
return *this; |
|
317 |
} |
||
318 |
|||
319 |
using Base::isEqual; |
||
320 |
54 |
bool isEqual(const JointDataMimic & other) const |
|
321 |
{ |
||
322 |
54 |
return Base::isEqual(other) |
|
323 |
✓✗ | 54 |
&& m_jdata_ref == other.m_jdata_ref |
324 |
✓✗ | 54 |
&& m_scaling == other.m_scaling |
325 |
✓✗ | 54 |
&& m_q_transform == other.m_q_transform |
326 |
✓✗✓✗ |
108 |
&& m_v_transform == other.m_v_transform |
327 |
; |
||
328 |
} |
||
329 |
|||
330 |
246 |
static std::string classname() |
|
331 |
{ |
||
332 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
246 |
return std::string("JointDataMimic<") + JointData::classname() + std::string(">"); |
333 |
} |
||
334 |
|||
335 |
12 |
std::string shortname() const |
|
336 |
{ |
||
337 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">"); |
338 |
} |
||
339 |
|||
340 |
// Accessors |
||
341 |
162 |
ConstraintTypeConstRef S_accessor() const { return S; } |
|
342 |
78 |
ConstraintTypeRef S_accessor() { return S; } |
|
343 |
|||
344 |
162 |
TansformTypeConstRef M_accessor() const { return m_jdata_ref.M; } |
|
345 |
104 |
TansformTypeRef M_accessor() { return m_jdata_ref.M; } |
|
346 |
|||
347 |
114 |
MotionTypeConstRef v_accessor() const { return m_jdata_ref.v; } |
|
348 |
70 |
MotionTypeRef v_accessor() { return m_jdata_ref.v; } |
|
349 |
|||
350 |
114 |
BiasTypeConstRef c_accessor() const { return m_jdata_ref.c; } |
|
351 |
48 |
BiasTypeRef c_accessor() { return m_jdata_ref.c; } |
|
352 |
|||
353 |
114 |
UTypeConstRef U_accessor() const { return m_jdata_ref.U; } |
|
354 |
45 |
UTypeRef U_accessor() { return m_jdata_ref.U; } |
|
355 |
|||
356 |
114 |
DTypeConstRef Dinv_accessor() const { return m_jdata_ref.Dinv; } |
|
357 |
45 |
DTypeRef Dinv_accessor() { return m_jdata_ref.Dinv; } |
|
358 |
|||
359 |
114 |
UDTypeConstRef UDinv_accessor() const { return m_jdata_ref.UDinv; } |
|
360 |
45 |
UDTypeRef UDinv_accessor() { return m_jdata_ref.UDinv; } |
|
361 |
|||
362 |
template<class JointModel> |
||
363 |
friend struct JointModelMimic; |
||
364 |
|||
365 |
const JointData & jdata() const { return m_jdata_ref; } |
||
366 |
84 |
JointData & jdata() { return m_jdata_ref; } |
|
367 |
|||
368 |
const Scalar & scaling() const { return m_scaling; } |
||
369 |
84 |
Scalar & scaling() { return m_scaling; } |
|
370 |
|||
371 |
84 |
ConfigVector_t & jointConfiguration() { return m_q_transform; } |
|
372 |
const ConfigVector_t & jointConfiguration() const { return m_q_transform; } |
||
373 |
|||
374 |
84 |
TangentVector_t & jointVelocity() { return m_v_transform; } |
|
375 |
const TangentVector_t & jointVelocity() const { return m_v_transform; } |
||
376 |
|||
377 |
protected: |
||
378 |
|||
379 |
JointData m_jdata_ref; |
||
380 |
Scalar m_scaling; |
||
381 |
|||
382 |
/// \brief Transform configuration vector |
||
383 |
ConfigVector_t m_q_transform; |
||
384 |
/// \brief Transform velocity vector. |
||
385 |
TangentVector_t m_v_transform; |
||
386 |
|||
387 |
public: |
||
388 |
|||
389 |
// data |
||
390 |
Constraint_t S; |
||
391 |
|||
392 |
}; // struct JointDataMimic |
||
393 |
|||
394 |
template<typename NewScalar, typename JointModel> |
||
395 |
struct CastType< NewScalar, JointModelMimic<JointModel> > |
||
396 |
{ |
||
397 |
typedef typename CastType<NewScalar,JointModel>::type JointModelNewType; |
||
398 |
typedef JointModelMimic<JointModelNewType> type; |
||
399 |
}; |
||
400 |
|||
401 |
template<class JointModel> |
||
402 |
struct JointModelMimic |
||
403 |
: public JointModelBase< JointModelMimic<JointModel> > |
||
404 |
{ |
||
405 |
86 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
406 |
|||
407 |
typedef typename traits<JointModelMimic>::JointDerived JointDerived; |
||
408 |
|||
409 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
410 |
|||
411 |
typedef JointModelBase<JointModelMimic> Base; |
||
412 |
using Base::id; |
||
413 |
using Base::idx_q; |
||
414 |
using Base::idx_v; |
||
415 |
using Base::nq; |
||
416 |
using Base::nv; |
||
417 |
using Base::setIndexes; |
||
418 |
|||
419 |
240 |
JointModelMimic() |
|
420 |
240 |
{} |
|
421 |
|||
422 |
120 |
JointModelMimic(const JointModelBase<JointModel> & jmodel, |
|
423 |
const Scalar & scaling, |
||
424 |
const Scalar & offset) |
||
425 |
: m_jmodel_ref(jmodel.derived()) |
||
426 |
, m_scaling(scaling) |
||
427 |
120 |
, m_offset(offset) |
|
428 |
120 |
{} |
|
429 |
|||
430 |
Base & base() { return *static_cast<Base*>(this); } |
||
431 |
const Base & base() const { return *static_cast<const Base*>(this); } |
||
432 |
|||
433 |
166 |
inline int nq_impl() const { return 0; } |
|
434 |
137 |
inline int nv_impl() const { return 0; } |
|
435 |
|||
436 |
328 |
inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); } |
|
437 |
188 |
inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); } |
|
438 |
|||
439 |
136 |
void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/) |
|
440 |
{ |
||
441 |
136 |
Base::i_id = id; // Only the id of the joint in the model is different. |
|
442 |
136 |
Base::i_q = m_jmodel_ref.idx_q(); |
|
443 |
136 |
Base::i_v = m_jmodel_ref.idx_v(); |
|
444 |
136 |
} |
|
445 |
|||
446 |
76 |
JointDataDerived createData() const |
|
447 |
{ |
||
448 |
✓✗ | 76 |
return JointDataDerived(m_jmodel_ref.createData(),scaling()); |
449 |
} |
||
450 |
|||
451 |
const std::vector<bool> hasConfigurationLimit() const |
||
452 |
{ |
||
453 |
return m_jmodel_ref.hasConfigurationLimit(); |
||
454 |
} |
||
455 |
|||
456 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
||
457 |
{ |
||
458 |
return m_jmodel_ref.hasConfigurationLimitInTangent(); |
||
459 |
} |
||
460 |
|||
461 |
template<typename ConfigVector> |
||
462 |
EIGEN_DONT_INLINE |
||
463 |
46 |
void calc(JointDataDerived & jdata, |
|
464 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
465 |
{ |
||
466 |
typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform; |
||
467 |
|||
468 |
✓✗ | 46 |
AffineTransform::run(qs.head(m_jmodel_ref.nq()), |
469 |
46 |
m_scaling,m_offset,jdata.m_q_transform); |
|
470 |
46 |
m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.m_q_transform); |
|
471 |
46 |
} |
|
472 |
|||
473 |
template<typename ConfigVector, typename TangentVector> |
||
474 |
EIGEN_DONT_INLINE |
||
475 |
52 |
void calc(JointDataDerived & jdata, |
|
476 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
477 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
478 |
{ |
||
479 |
typedef typename ConfigVectorAffineTransform<JointDerived>::Type AffineTransform; |
||
480 |
|||
481 |
✓✗ | 52 |
AffineTransform::run(qs.head(m_jmodel_ref.nq()), |
482 |
52 |
m_scaling,m_offset,jdata.m_q_transform); |
|
483 |
✓✗✓✗ |
52 |
jdata.m_v_transform = m_scaling * vs.head(m_jmodel_ref.nv()); |
484 |
52 |
m_jmodel_ref.calc(jdata.m_jdata_ref, |
|
485 |
52 |
jdata.m_q_transform, |
|
486 |
52 |
jdata.m_v_transform); |
|
487 |
52 |
} |
|
488 |
|||
489 |
template<typename Matrix6Like> |
||
490 |
24 |
void calc_aba(JointDataDerived & data, |
|
491 |
const Eigen::MatrixBase<Matrix6Like> & I, |
||
492 |
const bool update_I) const |
||
493 |
{ |
||
494 |
// TODO: fixme |
||
495 |
24 |
m_jmodel_ref.calc_aba(data.m_jdata_ref, |
|
496 |
24 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I), |
|
497 |
update_I); |
||
498 |
24 |
} |
|
499 |
|||
500 |
252 |
static std::string classname() |
|
501 |
{ |
||
502 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
252 |
return std::string("JointModelMimic<") + JointModel::classname() + std::string(">");; |
503 |
} |
||
504 |
|||
505 |
132 |
std::string shortname() const |
|
506 |
{ |
||
507 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
132 |
return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">"); |
508 |
} |
||
509 |
|||
510 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
511 |
template<typename NewScalar> |
||
512 |
24 |
typename CastType<NewScalar,JointModelMimic>::type cast() const |
|
513 |
{ |
||
514 |
typedef typename CastType<NewScalar,JointModelMimic>::type ReturnType; |
||
515 |
|||
516 |
✓✗ | 24 |
ReturnType res(m_jmodel_ref.template cast<NewScalar>(), |
517 |
(NewScalar)m_scaling, |
||
518 |
✓✗ | 24 |
(NewScalar)m_offset); |
519 |
24 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
520 |
24 |
return res; |
|
521 |
} |
||
522 |
|||
523 |
18 |
const JointModel & jmodel() const { return m_jmodel_ref; } |
|
524 |
84 |
JointModel & jmodel() { return m_jmodel_ref; } |
|
525 |
|||
526 |
76 |
const Scalar & scaling() const { return m_scaling; } |
|
527 |
42 |
Scalar & scaling() { return m_scaling; } |
|
528 |
|||
529 |
const Scalar & offset() const { return m_offset; } |
||
530 |
84 |
Scalar & offset() { return m_offset; } |
|
531 |
|||
532 |
protected: |
||
533 |
|||
534 |
// data |
||
535 |
JointModel m_jmodel_ref; |
||
536 |
Scalar m_scaling, m_offset; |
||
537 |
|||
538 |
public: |
||
539 |
|||
540 |
/* Acces to dedicated segment in robot config space. */ |
||
541 |
// Const access |
||
542 |
template<typename D> |
||
543 |
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType |
||
544 |
12 |
jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const |
|
545 |
{ |
||
546 |
12 |
return SizeDepType<NQ>::segment(a.derived(), |
|
547 |
m_jmodel_ref.idx_q(), |
||
548 |
12 |
m_jmodel_ref.nq()); |
|
549 |
} |
||
550 |
|||
551 |
// Non-const access |
||
552 |
template<typename D> |
||
553 |
typename SizeDepType<NQ>::template SegmentReturn<D>::Type |
||
554 |
3 |
jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const |
|
555 |
{ |
||
556 |
3 |
return SizeDepType<NQ>::segment(a.derived(), |
|
557 |
m_jmodel_ref.idx_q(), |
||
558 |
3 |
m_jmodel_ref.nq()); |
|
559 |
} |
||
560 |
|||
561 |
/* Acces to dedicated segment in robot config velocity space. */ |
||
562 |
// Const access |
||
563 |
template<typename D> |
||
564 |
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType |
||
565 |
jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const |
||
566 |
{ |
||
567 |
return SizeDepType<NV>::segment(a.derived(), |
||
568 |
m_jmodel_ref.idx_v(), |
||
569 |
m_jmodel_ref.nv()); |
||
570 |
} |
||
571 |
|||
572 |
// Non-const access |
||
573 |
template<typename D> |
||
574 |
typename SizeDepType<NV>::template SegmentReturn<D>::Type |
||
575 |
jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const |
||
576 |
{ |
||
577 |
return SizeDepType<NV>::segment(a.derived(), |
||
578 |
m_jmodel_ref.idx_v(), |
||
579 |
m_jmodel_ref.nv()); |
||
580 |
} |
||
581 |
|||
582 |
/* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ |
||
583 |
// Const access |
||
584 |
template<typename D> |
||
585 |
typename SizeDepType<NV>::template ColsReturn<D>::ConstType |
||
586 |
jointCols_impl(const Eigen::MatrixBase<D> & A) const |
||
587 |
{ |
||
588 |
return SizeDepType<NV>::middleCols(A.derived(), |
||
589 |
m_jmodel_ref.idx_v(), |
||
590 |
m_jmodel_ref.nv()); |
||
591 |
} |
||
592 |
|||
593 |
// Non-const access |
||
594 |
template<typename D> |
||
595 |
typename SizeDepType<NV>::template ColsReturn<D>::Type |
||
596 |
jointCols_impl(Eigen::MatrixBase<D> & A) const |
||
597 |
{ |
||
598 |
return SizeDepType<NV>::middleCols(A.derived(), |
||
599 |
m_jmodel_ref.idx_v(), |
||
600 |
m_jmodel_ref.nv()); |
||
601 |
} |
||
602 |
|||
603 |
/* Acces to dedicated rows in a matrix.*/ |
||
604 |
// Const access |
||
605 |
template<typename D> |
||
606 |
typename SizeDepType<NV>::template RowsReturn<D>::ConstType |
||
607 |
jointRows_impl(const Eigen::MatrixBase<D> & A) const |
||
608 |
{ |
||
609 |
return SizeDepType<NV>::middleRows(A.derived(), |
||
610 |
m_jmodel_ref.idx_v(), |
||
611 |
m_jmodel_ref.nv()); |
||
612 |
} |
||
613 |
|||
614 |
// Non-const access |
||
615 |
template<typename D> |
||
616 |
typename SizeDepType<NV>::template RowsReturn<D>::Type |
||
617 |
jointRows_impl(Eigen::MatrixBase<D> & A) const |
||
618 |
{ |
||
619 |
return SizeDepType<NV>::middleRows(A.derived(), |
||
620 |
m_jmodel_ref.idx_v(), |
||
621 |
m_jmodel_ref.nv()); |
||
622 |
} |
||
623 |
|||
624 |
/// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat |
||
625 |
// Const access |
||
626 |
template<typename D> |
||
627 |
typename SizeDepType<NV>::template BlockReturn<D>::ConstType |
||
628 |
jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const |
||
629 |
{ |
||
630 |
return SizeDepType<NV>::block(Mat.derived(), |
||
631 |
m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(), |
||
632 |
m_jmodel_ref.nv(),m_jmodel_ref.nv()); |
||
633 |
} |
||
634 |
|||
635 |
// Non-const access |
||
636 |
template<typename D> |
||
637 |
typename SizeDepType<NV>::template BlockReturn<D>::Type |
||
638 |
jointBlock_impl(Eigen::MatrixBase<D> & Mat) const |
||
639 |
{ |
||
640 |
return SizeDepType<NV>::block(Mat.derived(), |
||
641 |
m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(), |
||
642 |
m_jmodel_ref.nv(),m_jmodel_ref.nv()); |
||
643 |
} |
||
644 |
|||
645 |
}; // struct JointModelMimic |
||
646 |
|||
647 |
} // namespace pinocchio |
||
648 |
|||
649 |
#endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ |
Generated by: GCOVR (Version 4.2) |