GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 CNRS INRIA |
||
3 |
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
||
4 |
// |
||
5 |
|||
6 |
#ifndef __pinocchio_joint_planar_hpp__ |
||
7 |
#define __pinocchio_joint_planar_hpp__ |
||
8 |
|||
9 |
#include "pinocchio/macros.hpp" |
||
10 |
#include "pinocchio/multibody/joint/joint-base.hpp" |
||
11 |
#include "pinocchio/spatial/cartesian-axis.hpp" |
||
12 |
#include "pinocchio/multibody/constraint.hpp" |
||
13 |
#include "pinocchio/spatial/motion.hpp" |
||
14 |
#include "pinocchio/spatial/inertia.hpp" |
||
15 |
|||
16 |
namespace pinocchio |
||
17 |
{ |
||
18 |
|||
19 |
template<typename Scalar, int Options = 0> struct MotionPlanarTpl; |
||
20 |
typedef MotionPlanarTpl<double> MotionPlanar; |
||
21 |
|||
22 |
template<typename Scalar, int Options> |
||
23 |
struct SE3GroupAction< MotionPlanarTpl<Scalar,Options> > |
||
24 |
{ |
||
25 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
26 |
}; |
||
27 |
|||
28 |
template<typename Scalar, int Options, typename MotionDerived> |
||
29 |
struct MotionAlgebraAction< MotionPlanarTpl<Scalar,Options>, MotionDerived> |
||
30 |
{ |
||
31 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
32 |
}; |
||
33 |
|||
34 |
template<typename _Scalar, int _Options> |
||
35 |
struct traits< MotionPlanarTpl<_Scalar,_Options> > |
||
36 |
{ |
||
37 |
typedef _Scalar Scalar; |
||
38 |
enum { Options = _Options }; |
||
39 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
40 |
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6; |
||
41 |
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4; |
||
42 |
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6; |
||
43 |
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; |
||
44 |
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; |
||
45 |
typedef Vector3 AngularType; |
||
46 |
typedef Vector3 LinearType; |
||
47 |
typedef const Vector3 ConstAngularType; |
||
48 |
typedef const Vector3 ConstLinearType; |
||
49 |
typedef Matrix6 ActionMatrixType; |
||
50 |
typedef Matrix4 HomogeneousMatrixType; |
||
51 |
typedef MotionTpl<Scalar,Options> MotionPlain; |
||
52 |
typedef MotionPlain PlainReturnType; |
||
53 |
enum { |
||
54 |
LINEAR = 0, |
||
55 |
ANGULAR = 3 |
||
56 |
}; |
||
57 |
}; // traits MotionPlanarTpl |
||
58 |
|||
59 |
template<typename _Scalar, int _Options> |
||
60 |
struct MotionPlanarTpl |
||
61 |
: MotionBase< MotionPlanarTpl<_Scalar,_Options> > |
||
62 |
{ |
||
63 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
64 |
MOTION_TYPEDEF_TPL(MotionPlanarTpl); |
||
65 |
|||
66 |
typedef CartesianAxis<2> AxisZ; |
||
67 |
|||
68 |
7 |
MotionPlanarTpl() {} |
|
69 |
|||
70 |
1 |
MotionPlanarTpl(const Scalar & x_dot, const Scalar & y_dot, |
|
71 |
const Scalar & theta_dot) |
||
72 |
1 |
{ |
|
73 |
✓✗✓✗ |
1 |
m_data << x_dot, y_dot, theta_dot; |
74 |
1 |
} |
|
75 |
|||
76 |
template<typename Vector3Like> |
||
77 |
3135 |
MotionPlanarTpl(const Eigen::MatrixBase<Vector3Like> & vj) |
|
78 |
3135 |
: m_data(vj) |
|
79 |
{ |
||
80 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); |
||
81 |
3135 |
} |
|
82 |
|||
83 |
1 |
inline PlainReturnType plain() const |
|
84 |
{ |
||
85 |
✓✗ | 1 |
return PlainReturnType(typename PlainReturnType::Vector3(vx(),vy(),Scalar(0)), |
86 |
✓✗✓✗ ✓✗✓✗ |
2 |
typename PlainReturnType::Vector3(Scalar(0),Scalar(0),wz())); |
87 |
} |
||
88 |
|||
89 |
template<typename Derived> |
||
90 |
2 |
void addTo(MotionDense<Derived> & other) const |
|
91 |
{ |
||
92 |
✓✗ | 2 |
other.linear()[0] += vx(); |
93 |
✓✗ | 2 |
other.linear()[1] += vy(); |
94 |
✓✗ | 2 |
other.angular()[2] += wz(); |
95 |
2 |
} |
|
96 |
|||
97 |
template<typename MotionDerived> |
||
98 |
1032 |
void setTo(MotionDense<MotionDerived> & other) const |
|
99 |
{ |
||
100 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1032 |
other.linear() << vx(), vy(), (Scalar)0; |
101 |
✓✗✓✗ ✓✗✓✗ |
1032 |
other.angular() << (Scalar)0, (Scalar)0, wz(); |
102 |
1032 |
} |
|
103 |
|||
104 |
template<typename S2, int O2, typename D2> |
||
105 |
1 |
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
106 |
{ |
||
107 |
✓✗✓✗ ✓✗✓✗ |
1 |
v.angular().noalias() = m.rotation().col(2) * wz(); |
108 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
v.linear().noalias() = m.rotation().template leftCols<2>() * m_data.template head<2>(); |
109 |
✓✗✓✗ ✓✗ |
1 |
v.linear() += m.translation().cross(v.angular()); |
110 |
1 |
} |
|
111 |
|||
112 |
template<typename S2, int O2> |
||
113 |
1 |
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const |
|
114 |
{ |
||
115 |
1 |
MotionPlain res; |
|
116 |
1 |
se3Action_impl(m,res); |
|
117 |
1 |
return res; |
|
118 |
} |
||
119 |
|||
120 |
template<typename S2, int O2, typename D2> |
||
121 |
1 |
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
122 |
{ |
||
123 |
// Linear |
||
124 |
// TODO: use v.angular() as temporary variable |
||
125 |
✓✗ | 1 |
Vector3 v3_tmp; |
126 |
✓✗✓✗ ✓✗ |
1 |
AxisZ::alphaCross(wz(),m.translation(),v3_tmp); |
127 |
✓✗✓✗ ✓✗✓✗ |
1 |
v3_tmp[0] += vx(); v3_tmp[1] += vy(); |
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
v.linear().noalias() = m.rotation().transpose() * v3_tmp; |
129 |
|||
130 |
// Angular |
||
131 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
v.angular().noalias() = m.rotation().transpose().col(2) * wz(); |
132 |
1 |
} |
|
133 |
|||
134 |
template<typename S2, int O2> |
||
135 |
1 |
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const |
|
136 |
{ |
||
137 |
1 |
MotionPlain res; |
|
138 |
1 |
se3ActionInverse_impl(m,res); |
|
139 |
1 |
return res; |
|
140 |
} |
||
141 |
|||
142 |
template<typename M1, typename M2> |
||
143 |
4 |
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const |
|
144 |
{ |
||
145 |
// Linear |
||
146 |
✓✗✓✗ ✓✗✓✗ |
4 |
AxisZ::alphaCross(-wz(),v.linear(),mout.linear()); |
147 |
|||
148 |
✓✗ | 4 |
typename M1::ConstAngularType w_in = v.angular(); |
149 |
✓✗ | 4 |
typename M2::LinearType v_out = mout.linear(); |
150 |
|||
151 |
✓✗✓✗ ✓✗ |
4 |
v_out[0] -= w_in[2] * vy(); |
152 |
✓✗✓✗ ✓✗ |
4 |
v_out[1] += w_in[2] * vx(); |
153 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
v_out[2] += -w_in[1] * vx() + w_in[0] * vy() ; |
154 |
|||
155 |
// Angular |
||
156 |
✓✗✓✗ ✓✗✓✗ |
4 |
AxisZ::alphaCross(-wz(),v.angular(),mout.angular()); |
157 |
4 |
} |
|
158 |
|||
159 |
template<typename M1> |
||
160 |
4 |
MotionPlain motionAction(const MotionDense<M1> & v) const |
|
161 |
{ |
||
162 |
4 |
MotionPlain res; |
|
163 |
4 |
motionAction(v,res); |
|
164 |
4 |
return res; |
|
165 |
} |
||
166 |
|||
167 |
1044 |
const Scalar & vx() const { return m_data[0]; } |
|
168 |
1033 |
Scalar & vx() { return m_data[0]; } |
|
169 |
|||
170 |
1044 |
const Scalar & vy() const { return m_data[1]; } |
|
171 |
1033 |
Scalar & vy() { return m_data[1]; } |
|
172 |
|||
173 |
1046 |
const Scalar & wz() const { return m_data[2]; } |
|
174 |
1033 |
Scalar & wz() { return m_data[2]; } |
|
175 |
|||
176 |
const Vector3 & data() const { return m_data; } |
||
177 |
28 |
Vector3 & data() { return m_data; } |
|
178 |
|||
179 |
17 |
bool isEqual_impl(const MotionPlanarTpl & other) const |
|
180 |
{ |
||
181 |
17 |
return m_data == other.m_data; |
|
182 |
} |
||
183 |
|||
184 |
protected: |
||
185 |
|||
186 |
Vector3 m_data; |
||
187 |
|||
188 |
}; // struct MotionPlanarTpl |
||
189 |
|||
190 |
template<typename Scalar, int Options, typename MotionDerived> |
||
191 |
inline typename MotionDerived::MotionPlain |
||
192 |
operator+(const MotionPlanarTpl<Scalar,Options> & m1, const MotionDense<MotionDerived> & m2) |
||
193 |
{ |
||
194 |
typename MotionDerived::MotionPlain result(m2); |
||
195 |
result.linear()[0] += m1.vx(); |
||
196 |
result.linear()[1] += m1.vy(); |
||
197 |
|||
198 |
result.angular()[2] += m1.wz(); |
||
199 |
|||
200 |
return result; |
||
201 |
} |
||
202 |
|||
203 |
template<typename Scalar, int Options> struct ConstraintPlanarTpl; |
||
204 |
|||
205 |
template<typename _Scalar, int _Options> |
||
206 |
struct traits< ConstraintPlanarTpl<_Scalar,_Options> > |
||
207 |
{ |
||
208 |
typedef _Scalar Scalar; |
||
209 |
enum { Options = _Options }; |
||
210 |
enum { |
||
211 |
LINEAR = 0, |
||
212 |
ANGULAR = 3 |
||
213 |
}; |
||
214 |
typedef MotionPlanarTpl<Scalar,Options> JointMotion; |
||
215 |
typedef Eigen::Matrix<Scalar,3,1,Options> JointForce; |
||
216 |
typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase; |
||
217 |
typedef DenseBase MatrixReturnType; |
||
218 |
typedef const DenseBase ConstMatrixReturnType; |
||
219 |
}; // struct traits ConstraintPlanarTpl |
||
220 |
|||
221 |
template<typename _Scalar, int _Options> |
||
222 |
struct ConstraintPlanarTpl |
||
223 |
: ConstraintBase< ConstraintPlanarTpl<_Scalar,_Options> > |
||
224 |
{ |
||
225 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
226 |
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPlanarTpl) |
||
227 |
|||
228 |
enum { NV = 3 }; |
||
229 |
|||
230 |
3130 |
ConstraintPlanarTpl() {}; |
|
231 |
|||
232 |
template<typename Vector3Like> |
||
233 |
5 |
JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & vj) const |
|
234 |
{ |
||
235 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); |
||
236 |
5 |
return JointMotion(vj); |
|
237 |
} |
||
238 |
|||
239 |
22 |
int nv_impl() const { return NV; } |
|
240 |
|||
241 |
struct ConstraintTranspose |
||
242 |
{ |
||
243 |
const ConstraintPlanarTpl & ref; |
||
244 |
6 |
ConstraintTranspose(const ConstraintPlanarTpl & ref) : ref(ref) {} |
|
245 |
|||
246 |
template<typename Derived> |
||
247 |
4 |
typename ForceDense<Derived>::Vector3 operator* (const ForceDense<Derived> & phi) |
|
248 |
{ |
||
249 |
typedef typename ForceDense<Derived>::Vector3 Vector3; |
||
250 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
8 |
return Vector3(phi.linear()[0], phi.linear()[1], phi.angular()[2]); |
251 |
} |
||
252 |
|||
253 |
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ |
||
254 |
template<typename Derived> |
||
255 |
friend typename Eigen::Matrix <typename Eigen::MatrixBase<Derived>::Scalar, 3, Derived::ColsAtCompileTime> |
||
256 |
2 |
operator*(const ConstraintTranspose &, const Eigen::MatrixBase<Derived> & F) |
|
257 |
{ |
||
258 |
typedef typename Eigen::MatrixBase<Derived>::Scalar Scalar; |
||
259 |
typedef Eigen::Matrix<Scalar, 3, Derived::ColsAtCompileTime> MatrixReturnType; |
||
260 |
✗✓ | 2 |
assert(F.rows()==6); |
261 |
|||
262 |
✓✗ | 2 |
MatrixReturnType result(3, F.cols ()); |
263 |
✓✗✗✓ ✗✗✓✗ |
2 |
result.template topRows<2>() = F.template topRows<2>(); |
264 |
✓✗✗✓ ✗✗✓✗ |
2 |
result.template bottomRows<1>() = F.template bottomRows<1>(); |
265 |
2 |
return result; |
|
266 |
} |
||
267 |
|||
268 |
}; // struct ConstraintTranspose |
||
269 |
|||
270 |
6 |
ConstraintTranspose transpose () const { return ConstraintTranspose(*this); } |
|
271 |
|||
272 |
21 |
DenseBase matrix_impl() const |
|
273 |
{ |
||
274 |
21 |
DenseBase S; |
|
275 |
✓✗ | 21 |
S.template block<3,3>(Inertia::LINEAR, 0).setZero (); |
276 |
✓✗ | 21 |
S.template block<3,3>(Inertia::ANGULAR, 0).setZero (); |
277 |
21 |
S(Inertia::LINEAR + 0,0) = Scalar(1); |
|
278 |
21 |
S(Inertia::LINEAR + 1,1) = Scalar(1); |
|
279 |
21 |
S(Inertia::ANGULAR + 2,2) = Scalar(1); |
|
280 |
21 |
return S; |
|
281 |
} |
||
282 |
|||
283 |
template<typename S1, int O1> |
||
284 |
5 |
DenseBase se3Action(const SE3Tpl<S1,O1> & m) const |
|
285 |
{ |
||
286 |
5 |
DenseBase X_subspace; |
|
287 |
|||
288 |
// LINEAR |
||
289 |
✓✗✓✗ |
5 |
X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation ().template leftCols <2> (); |
290 |
X_subspace.template block <3,1>(Motion::LINEAR, 2).noalias() |
||
291 |
✓✗✓✗ ✓✗✓✗ |
5 |
= m.translation().cross(m.rotation ().template rightCols<1>()); |
292 |
|||
293 |
// ANGULAR |
||
294 |
✓✗ | 5 |
X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero (); |
295 |
✓✗✓✗ |
5 |
X_subspace.template block <3,1>(Motion::ANGULAR, 2) = m.rotation ().template rightCols<1>(); |
296 |
|||
297 |
5 |
return X_subspace; |
|
298 |
} |
||
299 |
|||
300 |
template<typename S1, int O1> |
||
301 |
3 |
DenseBase se3ActionInverse(const SE3Tpl<S1,O1> & m) const |
|
302 |
{ |
||
303 |
3 |
DenseBase X_subspace; |
|
304 |
|||
305 |
// LINEAR |
||
306 |
✓✗✓✗ ✓✗ |
3 |
X_subspace.template block <3,2>(Motion::LINEAR, 0) = m.rotation().transpose().template leftCols <2>(); |
307 |
✓✗✓✗ ✓✗✓✗ |
3 |
X_subspace.template block <3,1>(Motion::ANGULAR,2).noalias() = m.rotation().transpose() * m.translation(); // tmp variable |
308 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3 |
X_subspace.template block <3,1>(Motion::LINEAR, 2).noalias() = -X_subspace.template block <3,1>(Motion::ANGULAR,2).cross(m.rotation().transpose().template rightCols<1>()); |
309 |
|||
310 |
// ANGULAR |
||
311 |
✓✗ | 3 |
X_subspace.template block <3,2>(Motion::ANGULAR, 0).setZero(); |
312 |
✓✗✓✗ ✓✗ |
3 |
X_subspace.template block <3,1>(Motion::ANGULAR, 2) = m.rotation().transpose().template rightCols<1>(); |
313 |
|||
314 |
3 |
return X_subspace; |
|
315 |
} |
||
316 |
|||
317 |
template<typename MotionDerived> |
||
318 |
2 |
DenseBase motionAction(const MotionDense<MotionDerived> & m) const |
|
319 |
{ |
||
320 |
✓✗ | 2 |
const typename MotionDerived::ConstLinearType v = m.linear(); |
321 |
✓✗ | 2 |
const typename MotionDerived::ConstAngularType w = m.angular(); |
322 |
✓✗✓✗ |
2 |
DenseBase res(DenseBase::Zero()); |
323 |
|||
324 |
✓✗✓✗ ✓✗✓✗ |
2 |
res(0,1) = -w[2]; res(0,2) = v[1]; |
325 |
✓✗✓✗ ✓✗✓✗ |
2 |
res(1,0) = w[2]; res(1,2) = -v[0]; |
326 |
✓✗✓✗ ✓✗✓✗ |
2 |
res(2,0) = -w[1]; res(2,1) = w[0]; |
327 |
✓✗✓✗ |
2 |
res(3,2) = w[1]; |
328 |
✓✗✓✗ |
2 |
res(4,2) = -w[0]; |
329 |
|||
330 |
4 |
return res; |
|
331 |
} |
||
332 |
|||
333 |
17 |
bool isEqual(const ConstraintPlanarTpl &) const { return true; } |
|
334 |
|||
335 |
}; // struct ConstraintPlanarTpl |
||
336 |
|||
337 |
template<typename MotionDerived, typename S2, int O2> |
||
338 |
inline typename MotionDerived::MotionPlain |
||
339 |
3 |
operator^(const MotionDense<MotionDerived> & m1, const MotionPlanarTpl<S2,O2> & m2) |
|
340 |
{ |
||
341 |
3 |
return m2.motionAction(m1); |
|
342 |
} |
||
343 |
|||
344 |
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
||
345 |
template<typename S1, int O1, typename S2, int O2> |
||
346 |
inline typename Eigen::Matrix<S1,6,3,O1> |
||
347 |
3 |
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPlanarTpl<S2,O2> &) |
|
348 |
{ |
||
349 |
typedef InertiaTpl<S1,O1> Inertia; |
||
350 |
typedef typename Inertia::Scalar Scalar; |
||
351 |
typedef Eigen::Matrix<S1,6,3,O1> ReturnType; |
||
352 |
|||
353 |
✓✗ | 3 |
ReturnType M; |
354 |
3 |
const Scalar mass = Y.mass(); |
|
355 |
3 |
const typename Inertia::Vector3 & com = Y.lever(); |
|
356 |
3 |
const typename Inertia::Symmetric3 & inertia = Y.inertia(); |
|
357 |
|||
358 |
✓✗✓✗ |
3 |
M.template topLeftCorner<3,3>().setZero(); |
359 |
✓✗✓✗ ✓✗ |
3 |
M.template topLeftCorner<2,2>().diagonal().fill(mass); |
360 |
|||
361 |
✓✗✓✗ |
3 |
const typename Inertia::Vector3 mc(mass * com); |
362 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3 |
M.template rightCols<1>().template head<2>() << -mc(1), mc(0); |
363 |
|||
364 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
M.template bottomLeftCorner<3,2>() << (Scalar)0, -mc(2), mc(2), (Scalar)0, -mc(1), mc(0); |
365 |
✓✗✓✗ ✓✗✓✗ |
3 |
M.template rightCols<1>().template tail<3>() = inertia.data().template tail<3>(); |
366 |
✓✗✓✗ ✓✗✓✗ |
3 |
M.template rightCols<1>()[3] -= mc(0)*com(2); |
367 |
✓✗✓✗ ✓✗✓✗ |
3 |
M.template rightCols<1>()[4] -= mc(1)*com(2); |
368 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3 |
M.template rightCols<1>()[5] += mass*(com(0)*com(0) + com(1)*com(1)); |
369 |
|||
370 |
6 |
return M; |
|
371 |
} |
||
372 |
|||
373 |
/* [ABA] Y*S operator (Inertia Y,Constraint S) */ |
||
374 |
// inline Eigen::Matrix<double,6,1> |
||
375 |
|||
376 |
template<typename M6Like, typename S2, int O2> |
||
377 |
inline Eigen::Matrix<S2,6,3,O2> |
||
378 |
2 |
operator*(const Eigen::MatrixBase<M6Like> & Y, |
|
379 |
const ConstraintPlanarTpl<S2,O2> &) |
||
380 |
{ |
||
381 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6); |
||
382 |
typedef Eigen::Matrix<S2,6,3,O2> Matrix63; |
||
383 |
|||
384 |
2 |
Matrix63 IS; |
|
385 |
✓✗✓✗ |
2 |
IS.template leftCols<2>() = Y.template leftCols<2>(); |
386 |
✓✗✓✗ |
2 |
IS.template rightCols<1>() = Y.template rightCols<1>(); |
387 |
|||
388 |
2 |
return IS; |
|
389 |
} |
||
390 |
|||
391 |
template<typename S1, int O1> |
||
392 |
struct SE3GroupAction< ConstraintPlanarTpl<S1,O1> > |
||
393 |
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; }; |
||
394 |
|||
395 |
template<typename S1, int O1, typename MotionDerived> |
||
396 |
struct MotionAlgebraAction< ConstraintPlanarTpl<S1,O1>,MotionDerived > |
||
397 |
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; }; |
||
398 |
|||
399 |
template<typename Scalar, int Options> struct JointPlanarTpl; |
||
400 |
|||
401 |
template<typename _Scalar, int _Options> |
||
402 |
struct traits< JointPlanarTpl<_Scalar,_Options> > |
||
403 |
{ |
||
404 |
enum { |
||
405 |
NQ = 4, |
||
406 |
NV = 3 |
||
407 |
}; |
||
408 |
enum { Options = _Options }; |
||
409 |
typedef _Scalar Scalar; |
||
410 |
typedef JointDataPlanarTpl<Scalar,Options> JointDataDerived; |
||
411 |
typedef JointModelPlanarTpl<Scalar,Options> JointModelDerived; |
||
412 |
typedef ConstraintPlanarTpl<Scalar,Options> Constraint_t; |
||
413 |
typedef SE3Tpl<Scalar,Options> Transformation_t; |
||
414 |
typedef MotionPlanarTpl<Scalar,Options> Motion_t; |
||
415 |
typedef MotionZeroTpl<Scalar,Options> Bias_t; |
||
416 |
|||
417 |
// [ABA] |
||
418 |
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
||
419 |
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
||
420 |
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
||
421 |
|||
422 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
423 |
|||
424 |
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
||
425 |
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
||
426 |
}; |
||
427 |
|||
428 |
template<typename Scalar, int Options> |
||
429 |
struct traits< JointDataPlanarTpl<Scalar,Options> > { typedef JointPlanarTpl<Scalar,Options> JointDerived; }; |
||
430 |
template<typename Scalar, int Options> |
||
431 |
struct traits< JointModelPlanarTpl<Scalar,Options> > { typedef JointPlanarTpl<Scalar,Options> JointDerived; }; |
||
432 |
|||
433 |
template<typename _Scalar, int _Options> |
||
434 |
struct JointDataPlanarTpl : public JointDataBase< JointDataPlanarTpl<_Scalar,_Options> > |
||
435 |
{ |
||
436 |
48 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
437 |
typedef JointPlanarTpl<_Scalar,_Options> JointDerived; |
||
438 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
439 |
686 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
|
440 |
|||
441 |
Constraint_t S; |
||
442 |
Transformation_t M; |
||
443 |
Motion_t v; |
||
444 |
Bias_t c; |
||
445 |
|||
446 |
// [ABA] specific data |
||
447 |
U_t U; |
||
448 |
D_t Dinv; |
||
449 |
UD_t UDinv; |
||
450 |
|||
451 |
D_t StU; |
||
452 |
|||
453 |
3123 |
JointDataPlanarTpl () |
|
454 |
: M(Transformation_t::Identity()) |
||
455 |
, v(Motion_t::Vector3::Zero()) |
||
456 |
, U(U_t::Zero()) |
||
457 |
, Dinv(D_t::Zero()) |
||
458 |
✓✗✓✗ ✓✗✓✗ |
3123 |
, UDinv(UD_t::Zero()) |
459 |
3123 |
{} |
|
460 |
|||
461 |
✓✗ | 44 |
static std::string classname() { return std::string("JointDataPlanar"); } |
462 |
3 |
std::string shortname() const { return classname(); } |
|
463 |
|||
464 |
}; // struct JointDataPlanarTpl |
||
465 |
|||
466 |
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPlanarTpl); |
||
467 |
template<typename _Scalar, int _Options> |
||
468 |
struct JointModelPlanarTpl |
||
469 |
: public JointModelBase< JointModelPlanarTpl<_Scalar,_Options> > |
||
470 |
{ |
||
471 |
192 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
472 |
typedef JointPlanarTpl<_Scalar,_Options> JointDerived; |
||
473 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
474 |
|||
475 |
typedef JointModelBase<JointModelPlanarTpl> Base; |
||
476 |
using Base::id; |
||
477 |
using Base::idx_q; |
||
478 |
using Base::idx_v; |
||
479 |
using Base::setIndexes; |
||
480 |
|||
481 |
3097 |
JointDataDerived createData() const { return JointDataDerived(); } |
|
482 |
|||
483 |
1 |
const std::vector<bool> hasConfigurationLimit() const |
|
484 |
{ |
||
485 |
✓✗ | 1 |
return {true, true, false, false}; |
486 |
} |
||
487 |
|||
488 |
1 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
|
489 |
{ |
||
490 |
✓✗ | 1 |
return {true, true, false}; |
491 |
} |
||
492 |
|||
493 |
template<typename ConfigVector> |
||
494 |
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVector> & q_joint) const |
||
495 |
{ |
||
496 |
const Scalar |
||
497 |
& c_theta = q_joint(2), |
||
498 |
& s_theta = q_joint(3); |
||
499 |
|||
500 |
M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta; |
||
501 |
M.translation().template head<2>() = q_joint.template head<2>(); |
||
502 |
} |
||
503 |
|||
504 |
template<typename ConfigVector> |
||
505 |
6164 |
void calc(JointDataDerived & data, |
|
506 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
507 |
{ |
||
508 |
typedef typename ConfigVector::Scalar Scalar; |
||
509 |
✓✗✓✗ |
6164 |
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q()); |
510 |
|||
511 |
const Scalar |
||
512 |
✓✗ | 6164 |
&c_theta = q(2), |
513 |
✓✗ | 6164 |
&s_theta = q(3); |
514 |
|||
515 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
6164 |
data.M.rotation().template topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta; |
516 |
✓✗✓✗ ✓✗✓✗ |
6164 |
data.M.translation().template head<2>() = q.template head<2>(); |
517 |
|||
518 |
6164 |
} |
|
519 |
|||
520 |
template<typename ConfigVector, typename TangentVector> |
||
521 |
1037 |
void calc(JointDataDerived & data, |
|
522 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
523 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
524 |
{ |
||
525 |
✓✗ | 1037 |
calc(data,qs.derived()); |
526 |
|||
527 |
✓✗✓✗ |
1037 |
typename TangentVector::template ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.template segment<NV>(idx_v ()); |
528 |
|||
529 |
✓✗✓✗ |
1037 |
data.v.vx() = q_dot(0); |
530 |
✓✗✓✗ |
1037 |
data.v.vy() = q_dot(1); |
531 |
✓✗✓✗ |
1037 |
data.v.wz() = q_dot(2); |
532 |
1037 |
} |
|
533 |
|||
534 |
template<typename Matrix6Like> |
||
535 |
8 |
void calc_aba(JointDataDerived & data, |
|
536 |
const Eigen::MatrixBase<Matrix6Like> & I, |
||
537 |
const bool update_I) const |
||
538 |
{ |
||
539 |
✓✗✓✗ |
8 |
data.U.template leftCols<2>() = I.template leftCols<2>(); |
540 |
✓✗✓✗ |
8 |
data.U.template rightCols<1>() = I.template rightCols<1>(); |
541 |
|||
542 |
✓✗✓✗ ✓✗ |
8 |
data.StU.template leftCols<2>() = data.U.template topRows<2>().transpose(); |
543 |
✓✗✓✗ |
8 |
data.StU.template rightCols<1>() = data.U.template bottomRows<1>(); |
544 |
|||
545 |
// compute inverse |
||
546 |
// data.Dinv.setIdentity(); |
||
547 |
// data.StU.llt().solveInPlace(data.Dinv); |
||
548 |
8 |
internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv); |
|
549 |
|||
550 |
✓✗✓✗ |
8 |
data.UDinv.noalias() = data.U * data.Dinv; |
551 |
|||
552 |
✓✓ | 8 |
if(update_I) |
553 |
✓✗✓✗ ✓✗ |
2 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose(); |
554 |
8 |
} |
|
555 |
|||
556 |
✓✗ | 18428 |
static std::string classname() { return std::string("JointModelPlanar");} |
557 |
18386 |
std::string shortname() const { return classname(); } |
|
558 |
|||
559 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
560 |
template<typename NewScalar> |
||
561 |
4 |
JointModelPlanarTpl<NewScalar,Options> cast() const |
|
562 |
{ |
||
563 |
typedef JointModelPlanarTpl<NewScalar,Options> ReturnType; |
||
564 |
4 |
ReturnType res; |
|
565 |
4 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
566 |
4 |
return res; |
|
567 |
} |
||
568 |
|||
569 |
}; // struct JointModelPlanarTpl |
||
570 |
|||
571 |
} // namespace pinocchio |
||
572 |
|||
573 |
#include <boost/type_traits.hpp> |
||
574 |
|||
575 |
namespace boost |
||
576 |
{ |
||
577 |
template<typename Scalar, int Options> |
||
578 |
struct has_nothrow_constructor< ::pinocchio::JointModelPlanarTpl<Scalar,Options> > |
||
579 |
: public integral_constant<bool,true> {}; |
||
580 |
|||
581 |
template<typename Scalar, int Options> |
||
582 |
struct has_nothrow_copy< ::pinocchio::JointModelPlanarTpl<Scalar,Options> > |
||
583 |
: public integral_constant<bool,true> {}; |
||
584 |
|||
585 |
template<typename Scalar, int Options> |
||
586 |
struct has_nothrow_constructor< ::pinocchio::JointDataPlanarTpl<Scalar,Options> > |
||
587 |
: public integral_constant<bool,true> {}; |
||
588 |
|||
589 |
template<typename Scalar, int Options> |
||
590 |
struct has_nothrow_copy< ::pinocchio::JointDataPlanarTpl<Scalar,Options> > |
||
591 |
: public integral_constant<bool,true> {}; |
||
592 |
} |
||
593 |
|||
594 |
#endif // ifndef __pinocchio_joint_planar_hpp__ |
Generated by: GCOVR (Version 4.2) |