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 #ifdef _WIN32
21  // Ensure that Winsock2.h is included before Windows.h, which can get
22  // pulled in by anybody (e.g., Boost).
23  #include <Winsock2.h>
24 #endif
25 
26 #include <string>
27 #include <memory>
28 
29 #include <ignition/math/Inertial.hh>
30 #include <sdf/sdf.hh>
31 #include <ignition/math/Vector3.hh>
32 #include <ignition/math/Quaternion.hh>
33 #include <ignition/math/Matrix3.hh>
34 
35 #include "gazebo/msgs/msgs.hh"
36 
37 #include "gazebo/util/system.hh"
38 
39 namespace gazebo
40 {
41  namespace physics
42  {
43  // Forward declare private data
44  class InertialPrivate;
45 
48 
51  class GZ_PHYSICS_VISIBLE Inertial
52  {
54  public: Inertial();
55 
58  public: explicit Inertial(const double _mass);
59 
62  // cppcheck-suppress noExplicitConstructor
63  public: Inertial(const ignition::math::Inertiald &_inertial);
64 
67  public: Inertial(const Inertial &_inertial);
68 
70  public: virtual ~Inertial();
71 
74  public: void Load(sdf::ElementPtr _sdf);
75 
78  public: void UpdateParameters(sdf::ElementPtr _sdf);
79 
81  public: void Reset();
82 
84  public: ignition::math::Inertiald Ign() const;
85 
88  public: void SetMass(const double _m);
89 
92  public: double Mass() const;
93 
101  public: void SetInertiaMatrix(
102  const double _ixx, const double _iyy, const double _izz,
103  const double _ixy, const double _ixz, const double iyz);
104 
109  public: void SetCoG(const double _cx, const double _cy, const double _cz);
110 
113  public: void SetCoG(const ignition::math::Vector3d &_center);
114 
123  public: void SetCoG(const double _cx, const double _cy, const double _cz,
124  const double _rx, const double _ry, const double _rz);
125 
128  public: void SetCoG(const ignition::math::Pose3d &_c);
129 
132  public: const ignition::math::Vector3d &CoG() const;
133 
137  public: ignition::math::Pose3d Pose() const;
138 
141  public: const ignition::math::Vector3d &PrincipalMoments() const;
142 
145  public: const ignition::math::Vector3d &ProductsOfInertia() const;
146 
149  public: double IXX() const;
150 
153  public: double IYY() const;
154 
157  public: double IZZ() const;
158 
161  public: double IXY() const;
162 
165  public: double IXZ() const;
166 
169  public: double IYZ() const;
170 
173  public: void SetIXX(const double _v);
174 
177  public: void SetIYY(const double _v);
178 
181  public: void SetIZZ(const double _v);
182 
185  public: void SetIXY(const double _v);
186 
189  public: void SetIXZ(const double _v);
190 
193  public: void SetIYZ(const double _v);
194 
197  public: void Rotate(const ignition::math::Quaterniond &_rot);
198 
202  public: Inertial &operator=(const Inertial &_inertial);
203 
207  public: Inertial &operator=(const ignition::math::Inertiald &_inertial);
208 
216  public: Inertial operator+(const Inertial &_inertial) const;
217 
221  public: const Inertial &operator+=(const Inertial &_inertial);
222 
225  public: void ProcessMsg(const msgs::Inertial &_msg);
226 
235  public: ignition::math::Matrix3d MOI(
236  const ignition::math::Pose3d &_pose) const;
237 
243  public: Inertial operator()(
244  const ignition::math::Pose3d &_frameOffset) const;
245 
251  public: Inertial operator()(
252  const ignition::math::Vector3d &_frameOffset) const;
253 
257  public: friend std::ostream &operator<<(std::ostream &_out,
258  const gazebo::physics::Inertial &_inertial)
259  {
260  _out << "Mass[" << _inertial.Mass() << "] CoG["
261  << _inertial.CoG() << "]\n";
262  _out << "IXX[" << _inertial.PrincipalMoments().X() << "] "
263  << "IYY[" << _inertial.PrincipalMoments().Y() << "] "
264  << "IZZ[" << _inertial.PrincipalMoments().Z() << "]\n";
265  _out << "IXY[" << _inertial.ProductsOfInertia().X() << "] "
266  << "IXZ[" << _inertial.ProductsOfInertia().Y() << "] "
267  << "IYZ[" << _inertial.ProductsOfInertia().Z() << "]\n";
268  return _out;
269  }
270 
273  public: ignition::math::Matrix3d MOI() const;
274 
277  public: void SetMOI(const ignition::math::Matrix3d &_moi);
278 
281  private: std::unique_ptr<InertialPrivate> dataPtr;
282  };
284  }
285 }
286 #endif
const ignition::math::Vector3d & CoG() const
Get the center of gravity.
const ignition::math::Vector3d & PrincipalMoments() const
Get the principal moments of inertia (Ixx, Iyy, Izz).
double Mass() const
Get the mass.
A class for inertial information about a link.
Definition: Inertial.hh:51
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::Inertial &_inertial)
Output operator.
Definition: Inertial.hh:257
const ignition::math::Vector3d & ProductsOfInertia() const
Get the products of inertia (Ixy, Ixz, Iyz).