DART  6.7.3
Inertia.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, 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:
46 
48  enum Param {
49 
50  // Overall mass
51  MASS = 0,
52 
53  // Center of mass components
55 
56  // Moment of inertia components
59 
60  };
61 
62  Inertia(double _mass=1, const Eigen::Vector3d& _com = Eigen::Vector3d::Zero(),
63  const Eigen::Matrix3d& _momentOfInertia = Eigen::Matrix3d::Identity());
64 
65  Inertia(const Eigen::Matrix6d& _spatialInertiaTensor);
66 
67  Inertia(double _mass,
68  double _comX, double _comY, double _comZ,
69  double _Ixx, double _Iyy, double _Izz,
70  double _Ixy, double _Ixz, double _Iyz);
71 
73  void setParameter(Param _param, double _value);
74 
76  double getParameter(Param _param) const;
77 
79  void setMass(double _mass);
80 
82  double getMass() const;
83 
85  void setLocalCOM(const Eigen::Vector3d& _com);
86 
88  const Eigen::Vector3d& getLocalCOM() const;
89 
93  void setMoment(const Eigen::Matrix3d& _moment);
94 
96  void setMoment(double _Ixx, double _Iyy, double _Izz,
97  double _Ixy, double _Ixz, double _Iyz);
98 
100  Eigen::Matrix3d getMoment() const;
101 
103  void setSpatialTensor(const Eigen::Matrix6d& _spatial);
104 
106  const Eigen::Matrix6d& getSpatialTensor() const;
107 
109  static bool verifyMoment(const Eigen::Matrix3d& _moment,
110  bool _printWarnings = true,
111  double _tolerance = 1e-8);
112 
114  static bool verifySpatialTensor(const Eigen::Matrix6d& _spatial,
115  bool _printWarnings = true,
116  double _tolerance = 1e-8);
117 
119  bool verify(bool _printWarnings = true,
120  double _tolerance = 1e-8) const;
121 
123  bool operator==(const Inertia& other) const;
124 
125 protected:
126 
128  void computeSpatialTensor();
129 
131  void computeParameters();
132 
134  double mMass;
135 
137  Eigen::Vector3d mCenterOfMass;
138 
140  std::array<double,6> mMoment;
141 
144 
145 public:
146  // To get byte-aligned Eigen vectors
147  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
148 };
149 
150 } // namespace dynamics
151 } // namespace dart
152 
153 #endif // DART_DYNAMICS_INERTIA_HPP_
Definition: Inertia.hpp:44
Eigen::Matrix6d mSpatialTensor
Cache for generalized spatial inertia of the Body.
Definition: Inertia.hpp:143
const Eigen::Matrix6d & getSpatialTensor() const
Get the spatial inertia tensor.
Definition: Inertia.cpp:194
void setParameter(Param _param, double _value)
Set an inertial parameter.
Definition: Inertia.cpp:67
Param
Enumeration for minimal inertia parameters.
Definition: Inertia.hpp:48
@ I_XY
Definition: Inertia.hpp:58
@ MASS
Definition: Inertia.hpp:51
@ COM_Y
Definition: Inertia.hpp:54
@ I_ZZ
Definition: Inertia.hpp:57
@ COM_X
Definition: Inertia.hpp:54
@ COM_Z
Definition: Inertia.hpp:54
@ I_XZ
Definition: Inertia.hpp:58
@ I_YY
Definition: Inertia.hpp:57
@ I_YZ
Definition: Inertia.hpp:58
@ I_XX
Definition: Inertia.hpp:57
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:240
void setMoment(const Eigen::Matrix3d &_moment)
Set the moment of inertia (about the center of mass).
Definition: Inertia.cpp:136
void setSpatialTensor(const Eigen::Matrix6d &_spatial)
Set the spatial tensor.
Definition: Inertia.cpp:182
void setLocalCOM(const Eigen::Vector3d &_com)
Set the center of mass with respect to the Body-fixed frame.
Definition: Inertia.cpp:123
std::array< double, 6 > mMoment
The six parameters of the moment of inertia located at the center of mass.
Definition: Inertia.hpp:140
const Eigen::Vector3d & getLocalCOM() const
Get the center of mass with respect to the Body-fixed frame.
Definition: Inertia.cpp:130
void setMass(double _mass)
Set the mass.
Definition: Inertia.cpp:110
void computeParameters()
Compute the inertial parameters from the spatial tensor.
Definition: Inertia.cpp:412
Eigen::Matrix3d getMoment() const
Get the moment of inertia.
Definition: Inertia.cpp:168
bool operator==(const Inertia &other) const
Check for equality.
Definition: Inertia.cpp:387
void computeSpatialTensor()
Compute the spatial tensor based on the inertial parameters.
Definition: Inertia.cpp:394
double getParameter(Param _param) const
Get an inertial parameter.
Definition: Inertia.cpp:93
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:200
double getMass() const
Get the mass.
Definition: Inertia.cpp:117
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:134
Eigen::Vector3d mCenterOfMass
Center of mass in the Body frame.
Definition: Inertia.hpp:137
bool verify(bool _printWarnings=true, double _tolerance=1e-8) const
Returns true iff this Inertia object is physically valid.
Definition: Inertia.cpp:381
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
Definition: BulletCollisionDetector.cpp:63