GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2021 CNRS INRIA |
||
3 |
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
||
4 |
// |
||
5 |
|||
6 |
#ifndef __pinocchio_inertia_hpp__ |
||
7 |
#define __pinocchio_inertia_hpp__ |
||
8 |
|||
9 |
#include <iostream> |
||
10 |
|||
11 |
#include "pinocchio/math/fwd.hpp" |
||
12 |
#include "pinocchio/spatial/symmetric3.hpp" |
||
13 |
#include "pinocchio/spatial/force.hpp" |
||
14 |
#include "pinocchio/spatial/motion.hpp" |
||
15 |
#include "pinocchio/spatial/skew.hpp" |
||
16 |
|||
17 |
namespace pinocchio |
||
18 |
{ |
||
19 |
|||
20 |
template< class Derived> |
||
21 |
class InertiaBase |
||
22 |
{ |
||
23 |
protected: |
||
24 |
|||
25 |
typedef Derived Derived_t; |
||
26 |
SPATIAL_TYPEDEF_TEMPLATE(Derived_t); |
||
27 |
|||
28 |
public: |
||
29 |
51104 |
Derived_t & derived() { return *static_cast<Derived_t*>(this); } |
|
30 |
108384 |
const Derived_t & derived() const { return *static_cast<const Derived_t*>(this); } |
|
31 |
|||
32 |
Scalar mass() const { return static_cast<const Derived_t*>(this)->mass(); } |
||
33 |
Scalar & mass() { return static_cast<const Derived_t*>(this)->mass(); } |
||
34 |
const Vector3 & lever() const { return static_cast<const Derived_t*>(this)->lever(); } |
||
35 |
Vector3 & lever() { return static_cast<const Derived_t*>(this)->lever(); } |
||
36 |
const Symmetric3 & inertia() const { return static_cast<const Derived_t*>(this)->inertia(); } |
||
37 |
Symmetric3 & inertia() { return static_cast<const Derived_t*>(this)->inertia(); } |
||
38 |
|||
39 |
7542 |
Matrix6 matrix() const { return derived().matrix_impl(); } |
|
40 |
390 |
operator Matrix6 () const { return matrix(); } |
|
41 |
|||
42 |
Derived_t& operator= (const Derived_t& clone){return derived().__equl__(clone);} |
||
43 |
7981 |
bool operator==(const Derived_t & other) const {return derived().isEqual(other);} |
|
44 |
bool operator!=(const Derived_t & other) const { return !(*this == other); } |
||
45 |
|||
46 |
51104 |
Derived_t& operator+= (const Derived_t & Yb) { return derived().__pequ__(Yb); } |
|
47 |
3 |
Derived_t operator+(const Derived_t & Yb) const { return derived().__plus__(Yb); } |
|
48 |
|||
49 |
template<typename MotionDerived> |
||
50 |
ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options> |
||
51 |
96645 |
operator*(const MotionDense<MotionDerived> & v) const |
|
52 |
96645 |
{ return derived().__mult__(v); } |
|
53 |
|||
54 |
5648 |
Scalar vtiv(const Motion & v) const { return derived().vtiv_impl(v); } |
|
55 |
Matrix6 variation(const Motion & v) const { return derived().variation_impl(v); } |
||
56 |
|||
57 |
/// \brief Time variation operator. |
||
58 |
/// It computes the time derivative of an inertia I corresponding to the formula \f$ \dot{I} = v \times^{*} I \f$. |
||
59 |
/// |
||
60 |
/// \param[in] v The spatial velocity of the frame supporting the inertia. |
||
61 |
/// \param[in] I The spatial inertia in motion. |
||
62 |
/// \param[out] Iout The time derivative of the inertia I. |
||
63 |
/// |
||
64 |
template<typename M6> |
||
65 |
131 |
static void vxi(const Motion & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout) |
|
66 |
{ |
||
67 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); |
||
68 |
131 |
Derived::vxi_impl(v,I,Iout); |
|
69 |
131 |
} |
|
70 |
|||
71 |
129 |
Matrix6 vxi(const Motion & v) const |
|
72 |
{ |
||
73 |
129 |
Matrix6 Iout; |
|
74 |
129 |
vxi(v,derived(),Iout); |
|
75 |
129 |
return Iout; |
|
76 |
} |
||
77 |
|||
78 |
/// \brief Time variation operator. |
||
79 |
/// It computes the time derivative of an inertia I corresponding to the formula \f$ \dot{I} = v \times^{*} I \f$. |
||
80 |
/// |
||
81 |
/// \param[in] v The spatial velocity of the frame supporting the inertia. |
||
82 |
/// \param[in] I The spatial inertia in motion. |
||
83 |
/// \param[out] Iout The time derivative of the inertia I. |
||
84 |
/// |
||
85 |
template<typename M6> |
||
86 |
3 |
static void ivx(const Motion & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout) |
|
87 |
{ |
||
88 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(M6, Matrix6); |
||
89 |
3 |
Derived::ivx_impl(v,I,Iout); |
|
90 |
3 |
} |
|
91 |
|||
92 |
1 |
Matrix6 ivx(const Motion & v) const |
|
93 |
{ |
||
94 |
1 |
Matrix6 Iout; |
|
95 |
1 |
ivx(v,derived(),Iout); |
|
96 |
1 |
return Iout; |
|
97 |
} |
||
98 |
|||
99 |
void setZero() { derived().setZero(); } |
||
100 |
void setIdentity() { derived().setIdentity(); } |
||
101 |
void setRandom() { derived().setRandom(); } |
||
102 |
|||
103 |
149 |
bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
|
104 |
149 |
{ return derived().isApprox_impl(other, prec); } |
|
105 |
|||
106 |
1078 |
bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
|
107 |
1078 |
{ return derived().isZero_impl(prec); } |
|
108 |
|||
109 |
/// aI = aXb.act(bI) |
||
110 |
51407 |
Derived_t se3Action(const SE3 & M) const { return derived().se3Action_impl(M); } |
|
111 |
|||
112 |
/// bI = aXb.actInv(aI) |
||
113 |
5 |
Derived_t se3ActionInverse(const SE3 & M) const { return derived().se3ActionInverse_impl(M); } |
|
114 |
|||
115 |
2 |
void disp(std::ostream & os) const { static_cast<const Derived_t*>(this)->disp_impl(os); } |
|
116 |
2 |
friend std::ostream & operator << (std::ostream & os,const InertiaBase<Derived_t> & X) |
|
117 |
{ |
||
118 |
2 |
X.disp(os); |
|
119 |
2 |
return os; |
|
120 |
} |
||
121 |
|||
122 |
}; // class InertiaBase |
||
123 |
|||
124 |
|||
125 |
template<typename T, int U> |
||
126 |
struct traits< InertiaTpl<T, U> > |
||
127 |
{ |
||
128 |
typedef T Scalar; |
||
129 |
typedef Eigen::Matrix<T,3,1,U> Vector3; |
||
130 |
typedef Eigen::Matrix<T,4,1,U> Vector4; |
||
131 |
typedef Eigen::Matrix<T,6,1,U> Vector6; |
||
132 |
typedef Eigen::Matrix<T,3,3,U> Matrix3; |
||
133 |
typedef Eigen::Matrix<T,4,4,U> Matrix4; |
||
134 |
typedef Eigen::Matrix<T,6,6,U> Matrix6; |
||
135 |
typedef Matrix6 ActionMatrix_t; |
||
136 |
typedef Vector3 Angular_t; |
||
137 |
typedef Vector3 Linear_t; |
||
138 |
typedef const Vector3 ConstAngular_t; |
||
139 |
typedef const Vector3 ConstLinear_t; |
||
140 |
typedef Eigen::Quaternion<T,U> Quaternion_t; |
||
141 |
typedef SE3Tpl<T,U> SE3; |
||
142 |
typedef ForceTpl<T,U> Force; |
||
143 |
typedef MotionTpl<T,U> Motion; |
||
144 |
typedef Symmetric3Tpl<T,U> Symmetric3; |
||
145 |
enum { |
||
146 |
LINEAR = 0, |
||
147 |
ANGULAR = 3 |
||
148 |
}; |
||
149 |
}; // traits InertiaTpl |
||
150 |
|||
151 |
template<typename _Scalar, int _Options> |
||
152 |
class InertiaTpl : public InertiaBase< InertiaTpl< _Scalar, _Options > > |
||
153 |
{ |
||
154 |
public: |
||
155 |
friend class InertiaBase< InertiaTpl< _Scalar, _Options > >; |
||
156 |
SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl); |
||
157 |
enum { Options = _Options }; |
||
158 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
159 |
|||
160 |
typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare; |
||
161 |
|||
162 |
typedef typename Eigen::Matrix<_Scalar, 10, 1, _Options> Vector10; |
||
163 |
|||
164 |
public: |
||
165 |
// Constructors |
||
166 |
1061 |
InertiaTpl() |
|
167 |
1061 |
{} |
|
168 |
|||
169 |
1585 |
InertiaTpl(const Scalar mass, const Vector3 & com, const Matrix3 & rotational_inertia) |
|
170 |
1585 |
: m_mass(mass), m_com(com), m_inertia(rotational_inertia) |
|
171 |
1585 |
{} |
|
172 |
|||
173 |
1 |
InertiaTpl(const Matrix6 & I6) |
|
174 |
1 |
{ |
|
175 |
✓✗✓✗ ✓✗✗✓ |
1 |
assert((I6 - I6.transpose()).isMuchSmallerThan(I6)); |
176 |
✓✗ | 1 |
mass() = I6(LINEAR, LINEAR); |
177 |
✓✗✓✗ |
1 |
const Matrix3 & mc_cross = I6.template block <3,3>(ANGULAR,LINEAR); |
178 |
✓✗ | 1 |
lever() = unSkew(mc_cross); |
179 |
✓✗ | 1 |
lever() /= mass(); |
180 |
|||
181 |
✓✗✓✗ |
1 |
Matrix3 I3 (mc_cross * mc_cross); |
182 |
✓✗ | 1 |
I3 /= mass(); |
183 |
✓✗✓✗ |
1 |
I3 += I6.template block<3,3>(ANGULAR,ANGULAR); |
184 |
✓✗ | 1 |
const Symmetric3 S3(I3); |
185 |
✓✗ | 1 |
inertia() = S3; |
186 |
1 |
} |
|
187 |
|||
188 |
104352 |
InertiaTpl(Scalar mass, const Vector3 & com, const Symmetric3 & rotational_inertia) |
|
189 |
104352 |
: m_mass(mass), m_com(com), m_inertia(rotational_inertia) |
|
190 |
104352 |
{} |
|
191 |
|||
192 |
208237 |
InertiaTpl(const InertiaTpl & clone) // Copy constructor |
|
193 |
208237 |
: m_mass(clone.mass()), m_com(clone.lever()), m_inertia(clone.inertia()) |
|
194 |
208237 |
{} |
|
195 |
|||
196 |
20540 |
InertiaTpl& operator=(const InertiaTpl & clone) // Copy assignment operator |
|
197 |
{ |
||
198 |
20540 |
m_mass = clone.mass(); |
|
199 |
20540 |
m_com = clone.lever(); |
|
200 |
20540 |
m_inertia = clone.inertia(); |
|
201 |
20540 |
return *this; |
|
202 |
} |
||
203 |
|||
204 |
template<int O2> |
||
205 |
InertiaTpl(const InertiaTpl<Scalar,O2> & clone) |
||
206 |
: m_mass(clone.mass()) |
||
207 |
, m_com(clone.lever()) |
||
208 |
, m_inertia(clone.inertia().matrix()) |
||
209 |
{} |
||
210 |
|||
211 |
// Initializers |
||
212 |
40020 |
static InertiaTpl Zero() |
|
213 |
{ |
||
214 |
return InertiaTpl(Scalar(0), |
||
215 |
Vector3::Zero(), |
||
216 |
✓✗✓✗ ✓✗ |
40020 |
Symmetric3::Zero()); |
217 |
} |
||
218 |
|||
219 |
281 |
void setZero() { mass() = Scalar(0); lever().setZero(); inertia().setZero(); } |
|
220 |
|||
221 |
2 |
static InertiaTpl Identity() |
|
222 |
{ |
||
223 |
return InertiaTpl(Scalar(1), |
||
224 |
Vector3::Zero(), |
||
225 |
✓✗✓✗ ✓✗ |
2 |
Symmetric3::Identity()); |
226 |
} |
||
227 |
|||
228 |
void setIdentity () |
||
229 |
{ |
||
230 |
mass() = Scalar(1); lever().setZero(); inertia().setIdentity(); |
||
231 |
} |
||
232 |
|||
233 |
5870 |
static InertiaTpl Random() |
|
234 |
{ |
||
235 |
// We have to shoot "I" definite positive and not only symmetric. |
||
236 |
✓✗ | 5870 |
return InertiaTpl(Eigen::internal::random<Scalar>()+1, |
237 |
Vector3::Random(), |
||
238 |
✓✗✓✗ ✓✗ |
11740 |
Symmetric3::RandomPositive()); |
239 |
} |
||
240 |
|||
241 |
/// |
||
242 |
/// \brief Computes the Inertia of a sphere defined by its mass and its radius. |
||
243 |
/// |
||
244 |
/// \param[in] mass of the sphere. |
||
245 |
/// \param[in] radius of the sphere. |
||
246 |
/// |
||
247 |
3 |
static InertiaTpl FromSphere(const Scalar mass, const Scalar radius) |
|
248 |
{ |
||
249 |
3 |
return FromEllipsoid(mass,radius,radius,radius); |
|
250 |
} |
||
251 |
|||
252 |
/// |
||
253 |
/// \brief Computes the Inertia of an ellipsoid defined by its mass and main semi-axis dimensions (x,y,z). |
||
254 |
/// |
||
255 |
/// \param[in] mass of the ellipsoid. |
||
256 |
/// \param[in] x semi-axis dimension along the local X axis. |
||
257 |
/// \param[in] y semi-axis dimension along the local Y axis. |
||
258 |
/// \param[in] z semi-axis dimension along the local Z axis. |
||
259 |
/// |
||
260 |
4 |
static InertiaTpl FromEllipsoid(const Scalar mass, |
|
261 |
const Scalar x, |
||
262 |
const Scalar y, |
||
263 |
const Scalar z) |
||
264 |
{ |
||
265 |
4 |
const Scalar a = mass * (y*y + z*z) / Scalar(5); |
|
266 |
4 |
const Scalar b = mass * (x*x + z*z) / Scalar(5); |
|
267 |
4 |
const Scalar c = mass * (y*y + x*x) / Scalar(5); |
|
268 |
return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, |
||
269 |
✓✗✓✗ ✓✗✓✗ |
4 |
Scalar(0), Scalar(0), c)); |
270 |
} |
||
271 |
|||
272 |
/// |
||
273 |
/// \brief Computes the Inertia of a cylinder defined by its mass, radius and length along the Z axis. |
||
274 |
/// |
||
275 |
/// \param[in] mass of the cylinder. |
||
276 |
/// \param[in] radius of the cylinder. |
||
277 |
/// \param[in] length of the cylinder. |
||
278 |
/// |
||
279 |
1 |
static InertiaTpl FromCylinder(const Scalar mass, |
|
280 |
const Scalar radius, |
||
281 |
const Scalar length) |
||
282 |
{ |
||
283 |
1 |
const Scalar radius_square = radius * radius; |
|
284 |
1 |
const Scalar a = mass * (radius_square / Scalar(4) + length*length / Scalar(12)); |
|
285 |
1 |
const Scalar c = mass * (radius_square / Scalar(2)); |
|
286 |
return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), a, |
||
287 |
✓✗✓✗ ✓✗✓✗ |
1 |
Scalar(0), Scalar(0), c)); |
288 |
} |
||
289 |
|||
290 |
/// |
||
291 |
/// \brief Computes the Inertia of a box defined by its mass and main dimensions (x,y,z). |
||
292 |
/// |
||
293 |
/// \param[in] mass of the box. |
||
294 |
/// \param[in] x dimension along the local X axis. |
||
295 |
/// \param[in] y dimension along the local Y axis. |
||
296 |
/// \param[in] z dimension along the local Z axis. |
||
297 |
/// |
||
298 |
1 |
static InertiaTpl FromBox(const Scalar mass, |
|
299 |
const Scalar x, |
||
300 |
const Scalar y, |
||
301 |
const Scalar z) |
||
302 |
{ |
||
303 |
1 |
const Scalar a = mass * (y*y + z*z) / Scalar(12); |
|
304 |
1 |
const Scalar b = mass * (x*x + z*z) / Scalar(12); |
|
305 |
1 |
const Scalar c = mass * (y*y + x*x) / Scalar(12); |
|
306 |
return InertiaTpl(mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, |
||
307 |
✓✗✓✗ ✓✗✓✗ |
1 |
Scalar(0), Scalar(0), c)); |
308 |
} |
||
309 |
|||
310 |
1 |
void setRandom() |
|
311 |
{ |
||
312 |
1 |
mass() = static_cast<Scalar>(std::rand())/static_cast<Scalar>(RAND_MAX); |
|
313 |
1 |
lever().setRandom(); inertia().setRandom(); |
|
314 |
1 |
} |
|
315 |
|||
316 |
7542 |
Matrix6 matrix_impl() const |
|
317 |
{ |
||
318 |
7542 |
Matrix6 M; |
|
319 |
|||
320 |
✓✗ | 7542 |
M.template block<3,3>(LINEAR, LINEAR ).setZero(); |
321 |
✓✗✓✗ |
7542 |
M.template block<3,3>(LINEAR, LINEAR ).diagonal().fill (mass()); |
322 |
✓✗✓✗ |
7542 |
M.template block<3,3>(ANGULAR,LINEAR ) = alphaSkew(mass(),lever()); |
323 |
✓✗✓✗ ✓✗ |
7542 |
M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3>(ANGULAR, LINEAR); |
324 |
✓✗✓✗ ✓✗✓✗ |
7542 |
M.template block<3,3>(ANGULAR,ANGULAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix(); |
325 |
|||
326 |
7542 |
return M; |
|
327 |
} |
||
328 |
|||
329 |
/** Returns the representation of the matrix as a vector of dynamic parameters. |
||
330 |
* The parameters are given as |
||
331 |
* \f$ v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T \f$ |
||
332 |
* where \f$ c \f$ is the center of mass, \f$ I = I_C + mS^T(c)S(c) \f$ and \f$ I_C \f$ has its origin at the barycenter |
||
333 |
* and \f$ S(c) \f$ is the the skew matrix representation of the cross product operator. |
||
334 |
*/ |
||
335 |
31 |
Vector10 toDynamicParameters() const |
|
336 |
{ |
||
337 |
31 |
Vector10 v; |
|
338 |
|||
339 |
31 |
v[0] = mass(); |
|
340 |
✓✗✓✗ ✓✗✓✗ |
31 |
v.template segment<3>(1).noalias() = mass() * lever(); |
341 |
✓✗✓✗ ✓✗ |
31 |
v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(),lever())).data(); |
342 |
|||
343 |
31 |
return v; |
|
344 |
} |
||
345 |
|||
346 |
/** Builds and inertia matrix from a vector of dynamic parameters. |
||
347 |
* |
||
348 |
* @param[in] params The dynamic parameters. |
||
349 |
* |
||
350 |
* The parameters are given as |
||
351 |
* \f$ v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T \f$ |
||
352 |
* where \f$ I = I_C + mS^T(c)S(c) \f$ and \f$ I_C \f$ has its origin at the barycenter. |
||
353 |
*/ |
||
354 |
template<typename Vector10Like> |
||
355 |
1 |
static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params) |
|
356 |
{ |
||
357 |
✓✗✓✗ ✓✗✓✗ |
1 |
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1); |
358 |
|||
359 |
✓✗ | 1 |
const Scalar & mass = params[0]; |
360 |
✓✗✓✗ |
1 |
Vector3 lever = params.template segment<3>(1); |
361 |
✓✗ | 1 |
lever /= mass; |
362 |
|||
363 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
return InertiaTpl(mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass,lever)); |
364 |
} |
||
365 |
|||
366 |
// Arithmetic operators |
||
367 |
InertiaTpl & __equl__(const InertiaTpl & clone) |
||
368 |
{ |
||
369 |
mass() = clone.mass(); lever() = clone.lever(); inertia() = clone.inertia(); |
||
370 |
return *this; |
||
371 |
} |
||
372 |
|||
373 |
// Required by std::vector boost::python bindings. |
||
374 |
7981 |
bool isEqual( const InertiaTpl& Y2 ) const |
|
375 |
{ |
||
376 |
✓✗✓✗ ✓✗ |
7981 |
return (mass()==Y2.mass()) && (lever()==Y2.lever()) && (inertia()==Y2.inertia()); |
377 |
} |
||
378 |
|||
379 |
149 |
bool isApprox_impl(const InertiaTpl & other, |
|
380 |
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
||
381 |
{ |
||
382 |
using math::fabs; |
||
383 |
149 |
return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec |
|
384 |
✓✗✓✗ |
149 |
&& lever().isApprox(other.lever(),prec) |
385 |
✓✗✓✗ ✓✗ |
298 |
&& inertia().isApprox(other.inertia(),prec); |
386 |
} |
||
387 |
|||
388 |
1078 |
bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
|
389 |
{ |
||
390 |
using math::fabs; |
||
391 |
1078 |
return fabs(mass()) <= prec |
|
392 |
✓✗✓✗ |
23 |
&& lever().isZero(prec) |
393 |
✓✓✓✗ ✓✗ |
1101 |
&& inertia().isZero(prec); |
394 |
} |
||
395 |
|||
396 |
3 |
InertiaTpl __plus__(const InertiaTpl & Yb) const |
|
397 |
{ |
||
398 |
/* Y_{a+b} = ( m_a+m_b, |
||
399 |
* (m_a*c_a + m_b*c_b ) / (m_a + m_b), |
||
400 |
* I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x ) |
||
401 |
*/ |
||
402 |
|||
403 |
3 |
const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); |
|
404 |
|||
405 |
3 |
const Scalar & mab = mass()+Yb.mass(); |
|
406 |
✓✗ | 3 |
const Scalar mab_inv = Scalar(1)/math::max((Scalar)(mass()+Yb.mass()),eps); |
407 |
✓✗✓✗ |
3 |
const Vector3 & AB = (lever()-Yb.lever()).eval(); |
408 |
return InertiaTpl(mab, |
||
409 |
6 |
(mass()*lever()+Yb.mass()*Yb.lever())*mab_inv, |
|
410 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
9 |
inertia()+Yb.inertia() - (mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB)); |
411 |
} |
||
412 |
|||
413 |
51104 |
InertiaTpl& __pequ__(const InertiaTpl & Yb) |
|
414 |
{ |
||
415 |
51104 |
const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon(); |
|
416 |
|||
417 |
51104 |
const InertiaTpl& Ya = *this; |
|
418 |
51104 |
const Scalar & mab = mass()+Yb.mass(); |
|
419 |
✓✗ | 51104 |
const Scalar mab_inv = Scalar(1)/math::max((Scalar)(mass()+Yb.mass()),eps); |
420 |
✓✗✓✗ |
51104 |
const Vector3 & AB = (Ya.lever()-Yb.lever()).eval(); |
421 |
✓✗✓✗ ✓✗ |
51104 |
lever() *= (mass()*mab_inv); lever() += (Yb.mass()*mab_inv)*Yb.lever(); //c *= mab_inv; |
422 |
✓✗✓✗ ✓✗ |
51104 |
inertia() += Yb.inertia(); inertia() -= (Ya.mass()*Yb.mass()*mab_inv)* typename Symmetric3::SkewSquare(AB); |
423 |
51104 |
mass() = mab; |
|
424 |
51104 |
return *this; |
|
425 |
} |
||
426 |
|||
427 |
template<typename MotionDerived> |
||
428 |
ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options> |
||
429 |
96645 |
__mult__(const MotionDense<MotionDerived> & v) const |
|
430 |
{ |
||
431 |
typedef ForceTpl<typename traits<MotionDerived>::Scalar,traits<MotionDerived>::Options> ReturnType; |
||
432 |
96645 |
ReturnType f; |
|
433 |
96645 |
__mult__(v,f); |
|
434 |
96645 |
return f; |
|
435 |
} |
||
436 |
|||
437 |
template<typename MotionDerived, typename ForceDerived> |
||
438 |
180448 |
void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const |
|
439 |
{ |
||
440 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
180448 |
f.linear().noalias() = mass()*(v.linear() - lever().cross(v.angular())); |
441 |
✓✗✓✗ |
180448 |
Symmetric3::rhsMult(inertia(),v.angular(),f.angular()); |
442 |
✓✗✓✗ ✓✗ |
180448 |
f.angular() += lever().cross(f.linear()); |
443 |
// f.angular().noalias() = c.cross(f.linear()) + I*v.angular(); |
||
444 |
180448 |
} |
|
445 |
|||
446 |
5648 |
Scalar vtiv_impl(const Motion & v) const |
|
447 |
{ |
||
448 |
✓✗✓✗ |
5648 |
const Vector3 cxw (lever().cross(v.angular())); |
449 |
✓✗✓✗ ✓✗✓✗ |
5648 |
Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2)*v.linear().dot(cxw)); |
450 |
✓✗✓✗ ✓✗ |
5648 |
const Vector3 mcxcxw (-mass()*lever().cross(cxw)); |
451 |
✓✗✓✗ |
5648 |
res += v.angular().dot(mcxcxw); |
452 |
✓✗✓✗ ✓✗ |
5648 |
res += inertia().vtiv(v.angular()); |
453 |
|||
454 |
5648 |
return res; |
|
455 |
} |
||
456 |
|||
457 |
11784 |
Matrix6 variation(const Motion & v) const |
|
458 |
{ |
||
459 |
✓✗ | 11784 |
Matrix6 res; |
460 |
✓✗ | 11784 |
const Motion mv(v*mass()); |
461 |
|||
462 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
11784 |
res.template block<3,3>(LINEAR,ANGULAR) = -skew(mv.linear()) - skewSquare(mv.angular(),lever()) + skewSquare(lever(),mv.angular()); |
463 |
✓✗✓✗ ✓✗✓✗ |
11784 |
res.template block<3,3>(ANGULAR,LINEAR) = res.template block<3,3>(LINEAR,ANGULAR).transpose(); |
464 |
|||
465 |
// res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as temporary variable |
||
466 |
// res.template block<3,3>(ANGULAR,ANGULAR) = res.template block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose(); |
||
467 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
11784 |
res.template block<3,3>(ANGULAR,ANGULAR) = -skewSquare(mv.linear(),lever()) - skewSquare(lever(),mv.linear()); |
468 |
|||
469 |
✓✗✓✗ ✓✗✓✗ |
11784 |
res.template block<3,3>(LINEAR,LINEAR) = (inertia() - AlphaSkewSquare(mass(),lever())).matrix(); |
470 |
|||
471 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
11784 |
res.template block<3,3>(ANGULAR,ANGULAR) -= res.template block<3,3>(LINEAR,LINEAR) * skew(v.angular()); |
472 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
11784 |
res.template block<3,3>(ANGULAR,ANGULAR) += cross(v.angular(),res.template block<3,3>(LINEAR,LINEAR)); |
473 |
|||
474 |
✓✗✓✗ |
11784 |
res.template block<3,3>(LINEAR,LINEAR).setZero(); |
475 |
23568 |
return res; |
|
476 |
} |
||
477 |
|||
478 |
template<typename M6> |
||
479 |
131 |
static void vxi_impl(const Motion & v, |
|
480 |
const InertiaTpl & I, |
||
481 |
const Eigen::MatrixBase<M6> & Iout) |
||
482 |
{ |
||
483 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6); |
||
484 |
131 |
M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout); |
|
485 |
|||
486 |
// Block 1,1 |
||
487 |
✓✗✓✗ ✓✗ |
131 |
alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR)); |
488 |
// Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular()); |
||
489 |
✓✗✓✗ |
131 |
const Vector3 mc(I.mass()*I.lever()); |
490 |
|||
491 |
// Block 1,2 |
||
492 |
✓✗✓✗ ✓✗✓✗ |
131 |
skewSquare(-v.angular(),mc,Iout_.template block<3,3>(LINEAR,ANGULAR)); |
493 |
|||
494 |
|||
495 |
//// Block 2,1 |
||
496 |
✓✗✓✗ ✓✗ |
131 |
alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(ANGULAR,LINEAR)); |
497 |
✓✗✓✗ ✓✗ |
131 |
Iout_.template block<3,3>(ANGULAR,LINEAR) -= Iout_.template block<3,3>(LINEAR,ANGULAR); |
498 |
|||
499 |
//// Block 2,2 |
||
500 |
✓✗✓✗ ✓✗✓✗ |
131 |
skewSquare(-v.linear(),mc,Iout_.template block<3,3>(ANGULAR,ANGULAR)); |
501 |
|||
502 |
// TODO: I do not why, but depending on the CPU, these three lines can give |
||
503 |
// wrong output. |
||
504 |
// typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever()); |
||
505 |
// const Symmetric3 I_mcxcx(I.inertia() - mcxcx); |
||
506 |
// Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular()); |
||
507 |
✓✗ | 131 |
Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(),I.lever())); |
508 |
✓✗✓✗ ✓✗✓✗ |
131 |
Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().vxs(v.angular()); |
509 |
✓✗✓✗ ✓✗✓✗ |
131 |
Iout_.template block<3,3>(ANGULAR,ANGULAR) -= mcxcx.vxs(v.angular()); |
510 |
|||
511 |
131 |
} |
|
512 |
|||
513 |
template<typename M6> |
||
514 |
3 |
static void ivx_impl(const Motion & v, |
|
515 |
const InertiaTpl & I, |
||
516 |
const Eigen::MatrixBase<M6> & Iout) |
||
517 |
{ |
||
518 |
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6,6,6); |
||
519 |
3 |
M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,Iout); |
|
520 |
|||
521 |
// Block 1,1 |
||
522 |
✓✗✓✗ ✓✗ |
3 |
alphaSkew(I.mass(),v.angular(),Iout_.template block<3,3>(LINEAR,LINEAR)); |
523 |
|||
524 |
// Block 2,1 |
||
525 |
✓✗✓✗ |
3 |
const Vector3 mc(I.mass()*I.lever()); |
526 |
✓✗✓✗ ✓✗ |
3 |
skewSquare(mc,v.angular(),Iout_.template block<3,3>(ANGULAR,LINEAR)); |
527 |
|||
528 |
// Block 1,2 |
||
529 |
✓✗✓✗ ✓✗ |
3 |
alphaSkew(I.mass(),v.linear(),Iout_.template block<3,3>(LINEAR,ANGULAR)); |
530 |
|||
531 |
// Block 2,2 |
||
532 |
✓✗✓✗ ✓✗✓✗ |
3 |
cross(-I.lever(),Iout_.template block<3,3>(ANGULAR,LINEAR),Iout_.template block<3,3>(ANGULAR,ANGULAR)); |
533 |
✓✗✓✗ ✓✗✓✗ |
3 |
Iout_.template block<3,3>(ANGULAR,ANGULAR) += I.inertia().svx(v.angular()); |
534 |
✓✓ | 12 |
for(int k = 0; k < 3; ++k) |
535 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
9 |
Iout_.template block<3,3>(ANGULAR,ANGULAR).col(k) += I.lever().cross(Iout_.template block<3,3>(LINEAR,ANGULAR).col(k)); |
536 |
|||
537 |
// Block 1,2 |
||
538 |
✓✗✓✗ ✓✗ |
3 |
Iout_.template block<3,3>(LINEAR,ANGULAR) -= Iout_.template block<3,3>(ANGULAR,LINEAR); |
539 |
|||
540 |
3 |
} |
|
541 |
|||
542 |
// Getters |
||
543 |
764442 |
Scalar mass() const { return m_mass; } |
|
544 |
650424 |
const Vector3 & lever() const { return m_com; } |
|
545 |
378652 |
const Symmetric3 & inertia() const { return m_inertia; } |
|
546 |
|||
547 |
212258 |
Scalar & mass() { return m_mass; } |
|
548 |
87055 |
Vector3 & lever() { return m_com; } |
|
549 |
81529 |
Symmetric3 & inertia() { return m_inertia; } |
|
550 |
|||
551 |
/// aI = aXb.act(bI) |
||
552 |
51407 |
InertiaTpl se3Action_impl(const SE3 & M) const |
|
553 |
{ |
||
554 |
/* The multiplication RIR' has a particular form that could be used, however it |
||
555 |
* does not seems to be more efficient, see http://stackoverflow.m_comom/questions/ |
||
556 |
* 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/ |
||
557 |
51407 |
return InertiaTpl(mass(), |
|
558 |
✓✗✓✗ ✓✗ |
51407 |
M.translation()+M.rotation()*lever(), |
559 |
✓✗✓✗ ✓✗ |
154221 |
inertia().rotate(M.rotation())); |
560 |
} |
||
561 |
|||
562 |
///bI = aXb.actInv(aI) |
||
563 |
5 |
InertiaTpl se3ActionInverse_impl(const SE3 & M) const |
|
564 |
{ |
||
565 |
10 |
return InertiaTpl(mass(), |
|
566 |
✓✗✓✗ ✓✗ |
10 |
M.rotation().transpose()*(lever()-M.translation()), |
567 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
15 |
inertia().rotate(M.rotation().transpose()) ); |
568 |
} |
||
569 |
|||
570 |
5876 |
Force vxiv( const Motion& v ) const |
|
571 |
{ |
||
572 |
✓✗✓✗ ✓✗✓✗ |
5876 |
const Vector3 & mcxw = mass()*lever().cross(v.angular()); |
573 |
✓✗✓✗ ✓✗✓✗ |
5876 |
const Vector3 & mv_mcxw = mass()*v.linear()-mcxw; |
574 |
return Force( v.angular().cross(mv_mcxw), |
||
575 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
5876 |
v.angular().cross(lever().cross(mv_mcxw)+inertia()*v.angular())-v.linear().cross(mcxw) ); |
576 |
} |
||
577 |
|||
578 |
2 |
void disp_impl(std::ostream & os) const |
|
579 |
{ |
||
580 |
os |
||
581 |
2 |
<< " m = " << mass() << "\n" |
|
582 |
✓✗ | 2 |
<< " c = " << lever().transpose() << "\n" |
583 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
<< " I = \n" << inertia().matrix() << ""; |
584 |
2 |
} |
|
585 |
|||
586 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
587 |
template<typename NewScalar> |
||
588 |
1182 |
InertiaTpl<NewScalar,Options> cast() const |
|
589 |
{ |
||
590 |
1182 |
return InertiaTpl<NewScalar,Options>(static_cast<NewScalar>(mass()), |
|
591 |
1182 |
lever().template cast<NewScalar>(), |
|
592 |
✓✗ | 2534 |
inertia().template cast<NewScalar>()); |
593 |
} |
||
594 |
|||
595 |
// TODO: adjust code |
||
596 |
// /// \brief Check whether *this is a valid inertia, resulting from a positive mass distribution |
||
597 |
// bool isValid() const |
||
598 |
// { |
||
599 |
// return |
||
600 |
// (m_mass > Scalar(0) && m_inertia.isValid()) |
||
601 |
// || (m_mass == Scalar(0) && (m_inertia.data().array() == Scalar(0)).all()); |
||
602 |
// } |
||
603 |
|||
604 |
protected: |
||
605 |
Scalar m_mass; |
||
606 |
Vector3 m_com; |
||
607 |
Symmetric3 m_inertia; |
||
608 |
|||
609 |
}; // class InertiaTpl |
||
610 |
|||
611 |
} // namespace pinocchio |
||
612 |
|||
613 |
#endif // ifndef __pinocchio_inertia_hpp__ |
Generated by: GCOVR (Version 4.2) |