Inertial.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GAZEBO_PHYSICS_INERTIAL_HH_
18 #define GAZEBO_PHYSICS_INERTIAL_HH_
19 
20 #include <string>
21 #include <memory>
22 
23 #include <ignition/math/Inertial.hh>
24 #include <sdf/sdf.hh>
25 #include <ignition/math/Vector3.hh>
26 #include <ignition/math/Quaternion.hh>
27 #include <ignition/math/Matrix3.hh>
28 
29 #include "gazebo/msgs/msgs.hh"
30 
31 #include "gazebo/util/system.hh"
32 
33 namespace gazebo
34 {
35  namespace physics
36  {
37  // Forward declare private data
38  class InertialPrivate;
39 
42 
45  class GZ_PHYSICS_VISIBLE Inertial
46  {
48  public: Inertial();
49 
52  public: explicit Inertial(const double _mass);
53 
56  // cppcheck-suppress noExplicitConstructor
57  public: Inertial(const ignition::math::Inertiald &_inertial);
58 
61  public: Inertial(const Inertial &_inertial);
62 
64  public: virtual ~Inertial();
65 
68  public: void Load(sdf::ElementPtr _sdf);
69 
72  public: void UpdateParameters(sdf::ElementPtr _sdf);
73 
75  public: void Reset();
76 
78  public: ignition::math::Inertiald Ign() const;
79 
82  public: void SetMass(const double _m);
83 
86  public: double Mass() const;
87 
95  public: void SetInertiaMatrix(
96  const double _ixx, const double _iyy, const double _izz,
97  const double _ixy, const double _ixz, const double iyz);
98 
103  public: void SetCoG(const double _cx, const double _cy, const double _cz);
104 
107  public: void SetCoG(const ignition::math::Vector3d &_center);
108 
117  public: void SetCoG(const double _cx, const double _cy, const double _cz,
118  const double _rx, const double _ry, const double _rz);
119 
122  public: void SetCoG(const ignition::math::Pose3d &_c);
123 
126  public: const ignition::math::Vector3d &CoG() const;
127 
131  public: ignition::math::Pose3d Pose() const;
132 
135  public: const ignition::math::Vector3d &PrincipalMoments() const;
136 
139  public: const ignition::math::Vector3d &ProductsOfInertia() const;
140 
143  public: double IXX() const;
144 
147  public: double IYY() const;
148 
151  public: double IZZ() const;
152 
155  public: double IXY() const;
156 
159  public: double IXZ() const;
160 
163  public: double IYZ() const;
164 
167  public: void SetIXX(const double _v);
168 
171  public: void SetIYY(const double _v);
172 
175  public: void SetIZZ(const double _v);
176 
179  public: void SetIXY(const double _v);
180 
183  public: void SetIXZ(const double _v);
184 
187  public: void SetIYZ(const double _v);
188 
191  public: void Rotate(const ignition::math::Quaterniond &_rot);
192 
196  public: Inertial &operator=(const Inertial &_inertial);
197 
201  public: Inertial &operator=(const ignition::math::Inertiald &_inertial);
202 
210  public: Inertial operator+(const Inertial &_inertial) const;
211 
215  public: const Inertial &operator+=(const Inertial &_inertial);
216 
219  public: void ProcessMsg(const msgs::Inertial &_msg);
220 
229  public: ignition::math::Matrix3d MOI(
230  const ignition::math::Pose3d &_pose) const;
231 
237  public: Inertial operator()(
238  const ignition::math::Pose3d &_frameOffset) const;
239 
245  public: Inertial operator()(
246  const ignition::math::Vector3d &_frameOffset) const;
247 
251  public: friend std::ostream &operator<<(std::ostream &_out,
252  const gazebo::physics::Inertial &_inertial)
253  {
254  _out << "Mass[" << _inertial.Mass() << "] CoG["
255  << _inertial.CoG() << "]\n";
256  _out << "IXX[" << _inertial.PrincipalMoments().X() << "] "
257  << "IYY[" << _inertial.PrincipalMoments().Y() << "] "
258  << "IZZ[" << _inertial.PrincipalMoments().Z() << "]\n";
259  _out << "IXY[" << _inertial.ProductsOfInertia().X() << "] "
260  << "IXZ[" << _inertial.ProductsOfInertia().Y() << "] "
261  << "IYZ[" << _inertial.ProductsOfInertia().Z() << "]\n";
262  return _out;
263  }
264 
267  public: ignition::math::Matrix3d MOI() const;
268 
271  public: void SetMOI(const ignition::math::Matrix3d &_moi);
272 
275  private: std::unique_ptr<InertialPrivate> dataPtr;
276  };
278  }
279 }
280 #endif
const ignition::math::Vector3d & PrincipalMoments() const
Get the principal moments of inertia (Ixx, Iyy, Izz).
Forward declarations for the common classes.
Definition: Animation.hh:26
const ignition::math::Vector3d & ProductsOfInertia() const
Get the products of inertia (Ixy, Ixz, Iyz).
const ignition::math::Vector3d & CoG() const
Get the center of gravity.
double Mass() const
Get the mass.
A class for inertial information about a link.
Definition: Inertial.hh:45
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::Inertial &_inertial)
Output operator.
Definition: Inertial.hh:251