- Aldebaran documentation |
- C++ Libraries |
- index

libalmath 2.8.7.4

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
bodymass.h
Go to the documentation of this file.
1 /*
2 * Copyright 2015 Aldebaran. All rights reserved.
3 *
4 */
5 #ifndef LIB_ALMATH_SCENEGRAPH_BODYMASS_H
6 #define LIB_ALMATH_SCENEGRAPH_BODYMASS_H
7
8 #include <Eigen/Dense>
9 #include <boost/math/special_functions/pow.hpp>
10
11 namespace AL {
12 namespace Math {
13
14
15 // add the rotational inertial of a point mass located at position, to output.
16 template <typename Scalar, typename Derived0, typename Derived1>
17 void addPointMassInertia(Scalar mass,
18 const Eigen::MatrixBase<Derived0> &position,
19 Eigen::MatrixBase<Derived1> &output) {
20 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived0, 3, 1)
21 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived1, 3, 3)
22 EIGEN_STATIC_ASSERT(
23 (Eigen::internal::is_same<Scalar, typename Derived0::Scalar>::value),
24 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
25 EIGEN_STATIC_ASSERT(
26 (Eigen::internal::is_same<Scalar, typename Derived1::Scalar>::value),
27 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
28 typedef Eigen::Array<Scalar, 3, 1> Array3;
29 Array3 position2 = position.array().square();
30 output(0, 0) += mass * (position2[1] + position2[2]);
31 output(1, 1) += mass * (position2[0] + position2[2]);
32 output(2, 2) += mass * (position2[0] + position2[1]);
33
34 output(0, 1) += -mass * position[0] * position[1];
35 output(1, 0) += -mass * position[0] * position[1];
36
37 output(0, 2) += -mass * position[0] * position[2];
38 output(2, 0) += -mass * position[0] * position[2];
39
40 output(1, 2) += -mass * position[1] * position[2];
41 output(2, 1) += -mass * position[1] * position[2];
42 }
43
44 // Mass of a rigid body, described in an implicitly-defined "body frame".
45 template <typename T>
46 struct BodyMass {
47 typedef T Scalar;
48 typedef Eigen::Matrix<Scalar, 3, 1, Eigen::DontAlign> Vector3;
49 typedef Eigen::Matrix<Scalar, 3, 3, Eigen::DontAlign> Matrix3;
50
51 Scalar mass;
52 // center of mass, expressed in the body frame.
53 Vector3 center_of_mass;
54 // rotational inertia, expressed at the body center of mass,
55 // in the body frame basis.
56 Matrix3 rotational_inertia;
57
58 static BodyMass Zero() {
59 return BodyMass{0, Vector3::Zero(), Matrix3::Zero()};
60 }
61
62 template <typename S>
63 BodyMass<S> cast() const {
64 return BodyMass<S>{static_cast<S>(mass), center_of_mass.template cast<S>(),
65 rotational_inertia.template cast<S>()};
66 }
67
68 // return the rotational inertial expressed at the given point,
69 // in the body frame basis.
70 // The point is given in the body frame.
71 Matrix3 get_rotational_inertia_at(const Vector3 &point) const {
72 Matrix3 ret = rotational_inertia;
73 addPointMassInertia(mass, point - center_of_mass, ret);
74 return ret;
75 }
76 };
77
78 template <typename T>
79 bool operator==(const BodyMass<T> &lhs, const BodyMass<T> &rhs) {
80 return lhs.mass == rhs.mass &&
81 lhs.center_of_mass == rhs.center_of_mass &&
82 lhs.rotational_inertia == rhs.rotational_inertia;
83 }
84
85 template <typename T, typename BodyMassRange>
86 BodyMass<T> squashBodyMasses(const BodyMassRange &bodymasses) {
87 using Scalar = typename BodyMass<T>::Scalar;
88 using Vector3 = typename BodyMass<T>::Vector3;
89 using Matrix3 = typename BodyMass<T>::Matrix3;
90 Scalar mass = 0;
91 Vector3 center_of_mass = Vector3::Zero();
92 for (const BodyMass<T> &bodymass: bodymasses) {
93 mass += bodymass.mass;
94 center_of_mass += bodymass.mass * bodymass.center_of_mass;
95 }
96 if (mass != static_cast<Scalar>(0))
97 center_of_mass /= mass;
98
99 Matrix3 rotational_inertia = Matrix3::Zero();
100 for (const BodyMass<T> &bodymass: bodymasses) {
101 rotational_inertia += bodymass.rotational_inertia;
102 addPointMassInertia(bodymass.mass,
103 center_of_mass - bodymass.center_of_mass,
104 rotational_inertia);
105 }
106 return BodyMass<T>{mass, center_of_mass, rotational_inertia};
107 }
108
109 // squash Body masses
110 template <typename T>
111 BodyMass<T> operator+(const BodyMass<T> &lhs, const BodyMass<T> &rhs) {
112 using Scalar = typename BodyMass<T>::Scalar;
113 using Vector3 = typename BodyMass<T>::Vector3;
114 using Matrix3 = typename BodyMass<T>::Matrix3;
115 const Scalar mass = lhs.mass + rhs.mass;
116 Vector3 center_of_mass = lhs.mass * lhs.center_of_mass +
117 rhs.mass * rhs.center_of_mass;
118 if (mass != static_cast<Scalar>(0))
119 center_of_mass /= mass;
120
121 Matrix3 rotational_inertia = lhs.rotational_inertia +
122 rhs.rotational_inertia;
123 for (const BodyMass<T> &bodymass: {lhs, rhs}) {
124 addPointMassInertia(bodymass.mass,
125 center_of_mass - bodymass.center_of_mass,
126 rotational_inertia);
127 }
128 return BodyMass<T>{mass, center_of_mass, rotational_inertia};
129 }
130
131 }
132 }
133
134 #endif
AL::Math::BodyMass::center_of_mass
Vector3 center_of_mass
Definition: bodymass.h:53
AL::Math::addPointMassInertia
void addPointMassInertia(Scalar mass, const Eigen::MatrixBase< Derived0 > &position, Eigen::MatrixBase< Derived1 > &output)
Definition: bodymass.h:17
AL::Math::BodyMass::Zero
static BodyMass Zero()
Definition: bodymass.h:58
AL::Math::BodyMass::get_rotational_inertia_at
Matrix3 get_rotational_inertia_at(const Vector3 &point) const
Definition: bodymass.h:71
AL::Math::operator+
BodyMass< T > operator+(const BodyMass< T > &lhs, const BodyMass< T > &rhs)
Definition: bodymass.h:111
AL::Math::BodyMass::mass
Scalar mass
Definition: bodymass.h:51
AL::Math::BodyMass
Definition: bodymass.h:46
AL::Math::BodyMass::Matrix3
Eigen::Matrix< Scalar, 3, 3, Eigen::DontAlign > Matrix3
Definition: bodymass.h:49
AL::Math::BodyMass::Vector3
Eigen::Matrix< Scalar, 3, 1, Eigen::DontAlign > Vector3
Definition: bodymass.h:48
AL::Math::BodyMass::cast
BodyMass< S > cast() const
Definition: bodymass.h:63
AL::Math::squashBodyMasses
BodyMass< T > squashBodyMasses(const BodyMassRange &bodymasses)
Definition: bodymass.h:86
AL::Math::operator==
bool operator==(const BodyMass< T > &lhs, const BodyMass< T > &rhs)
Definition: bodymass.h:79
AL::Math::BodyMass::Scalar
T Scalar
Definition: bodymass.h:47
AL::Math::BodyMass::rotational_inertia
Matrix3 rotational_inertia
Definition: bodymass.h:56