DART  6.10.1
Inertia.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_INERTIA_HPP_
34 #define DART_DYNAMICS_INERTIA_HPP_
35 
36 #include <array>
37 
38 #include "dart/math/MathTypes.hpp"
39 
40 namespace dart {
41 namespace dynamics {
42 
43 class Inertia
44 {
45 public:
47  enum Param
48  {
49 
50  // Overall mass
51  MASS = 0,
52 
53  // Center of mass components
57 
58  // Moment of inertia components
64  I_YZ
65 
66  };
67 
68  Inertia(
69  double _mass = 1,
70  const Eigen::Vector3d& _com = Eigen::Vector3d::Zero(),
71  const Eigen::Matrix3d& _momentOfInertia = Eigen::Matrix3d::Identity());
72 
73  Inertia(const Eigen::Matrix6d& _spatialInertiaTensor);
74 
75  Inertia(
76  double _mass,
77  double _comX,
78  double _comY,
79  double _comZ,
80  double _Ixx,
81  double _Iyy,
82  double _Izz,
83  double _Ixy,
84  double _Ixz,
85  double _Iyz);
86 
88  void setParameter(Param _param, double _value);
89 
91  double getParameter(Param _param) const;
92 
94  void setMass(double _mass);
95 
97  double getMass() const;
98 
100  void setLocalCOM(const Eigen::Vector3d& _com);
101 
103  const Eigen::Vector3d& getLocalCOM() const;
104 
108  void setMoment(const Eigen::Matrix3d& _moment);
109 
111  void setMoment(
112  double _Ixx,
113  double _Iyy,
114  double _Izz,
115  double _Ixy,
116  double _Ixz,
117  double _Iyz);
118 
120  Eigen::Matrix3d getMoment() const;
121 
123  void setSpatialTensor(const Eigen::Matrix6d& _spatial);
124 
126  const Eigen::Matrix6d& getSpatialTensor() const;
127 
129  static bool verifyMoment(
130  const Eigen::Matrix3d& _moment,
131  bool _printWarnings = true,
132  double _tolerance = 1e-8);
133 
135  static bool verifySpatialTensor(
136  const Eigen::Matrix6d& _spatial,
137  bool _printWarnings = true,
138  double _tolerance = 1e-8);
139 
141  bool verify(bool _printWarnings = true, double _tolerance = 1e-8) const;
142 
144  bool operator==(const Inertia& other) const;
145 
146 protected:
148  void computeSpatialTensor();
149 
151  void computeParameters();
152 
154  double mMass;
155 
157  Eigen::Vector3d mCenterOfMass;
158 
160  std::array<double, 6> mMoment;
161 
164 
165 public:
166  // To get byte-aligned Eigen vectors
167  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
168 };
169 
170 } // namespace dynamics
171 } // namespace dart
172 
173 #endif // DART_DYNAMICS_INERTIA_HPP_
Definition: Inertia.hpp:44
Eigen::Matrix6d mSpatialTensor
Cache for generalized spatial inertia of the Body.
Definition: Inertia.hpp:163
const Eigen::Matrix6d & getSpatialTensor() const
Get the spatial inertia tensor.
Definition: Inertia.cpp:208
void setParameter(Param _param, double _value)
Set an inertial parameter.
Definition: Inertia.cpp:76
Param
Enumeration for minimal inertia parameters.
Definition: Inertia.hpp:48
@ I_XY
Definition: Inertia.hpp:62
@ MASS
Definition: Inertia.hpp:51
@ COM_Y
Definition: Inertia.hpp:55
@ I_ZZ
Definition: Inertia.hpp:61
@ COM_X
Definition: Inertia.hpp:54
@ COM_Z
Definition: Inertia.hpp:56
@ I_XZ
Definition: Inertia.hpp:63
@ I_YY
Definition: Inertia.hpp:60
@ I_YZ
Definition: Inertia.hpp:64
@ I_XX
Definition: Inertia.hpp:59
static bool verifySpatialTensor(const Eigen::Matrix6d &_spatial, bool _printWarnings=true, double _tolerance=1e-8)
Returns true iff _spatial is a physically valid spatial inertia tensor.
Definition: Inertia.cpp:254
void setMoment(const Eigen::Matrix3d &_moment)
Set the moment of inertia (about the center of mass).
Definition: Inertia.cpp:145
void setSpatialTensor(const Eigen::Matrix6d &_spatial)
Set the spatial tensor.
Definition: Inertia.cpp:196
void setLocalCOM(const Eigen::Vector3d &_com)
Set the center of mass with respect to the Body-fixed frame.
Definition: Inertia.cpp:132
const Eigen::Vector3d & getLocalCOM() const
Get the center of mass with respect to the Body-fixed frame.
Definition: Inertia.cpp:139
void setMass(double _mass)
Set the mass.
Definition: Inertia.cpp:119
void computeParameters()
Compute the inertial parameters from the spatial tensor.
Definition: Inertia.cpp:428
Eigen::Matrix3d getMoment() const
Get the moment of inertia.
Definition: Inertia.cpp:182
bool operator==(const Inertia &other) const
Check for equality.
Definition: Inertia.cpp:403
void computeSpatialTensor()
Compute the spatial tensor based on the inertial parameters.
Definition: Inertia.cpp:410
std::array< double, 6 > mMoment
The six parameters of the moment of inertia located at the center of mass.
Definition: Inertia.hpp:160
double getParameter(Param _param) const
Get an inertial parameter.
Definition: Inertia.cpp:102
static bool verifyMoment(const Eigen::Matrix3d &_moment, bool _printWarnings=true, double _tolerance=1e-8)
Returns true iff _moment is a physically valid moment of inertia.
Definition: Inertia.cpp:214
double getMass() const
Get the mass.
Definition: Inertia.cpp:126
Inertia(double _mass=1, const Eigen::Vector3d &_com=Eigen::Vector3d::Zero(), const Eigen::Matrix3d &_momentOfInertia=Eigen::Matrix3d::Identity())
Definition: Inertia.cpp:41
double mMass
Overall mass.
Definition: Inertia.hpp:154
Eigen::Vector3d mCenterOfMass
Center of mass in the Body frame.
Definition: Inertia.hpp:157
bool verify(bool _printWarnings=true, double _tolerance=1e-8) const
Returns true iff this Inertia object is physically valid.
Definition: Inertia.cpp:397
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
Definition: BulletCollisionDetector.cpp:65