17#ifndef GAZEBO_PHYSICS_INERTIAL_HH_
18#define GAZEBO_PHYSICS_INERTIAL_HH_
23#include <ignition/math/Inertial.hh>
25#include <ignition/math/Vector3.hh>
26#include <ignition/math/Quaternion.hh>
27#include <ignition/math/Matrix3.hh>
38 class InertialPrivate;
57 public:
Inertial(
const ignition::math::Inertiald &_inertial);
68 public:
void Load(sdf::ElementPtr _sdf);
78 public: ignition::math::Inertiald
Ign()
const;
86 public:
double Mass()
const;
96 const double _ixx,
const double _iyy,
const double _izz,
97 const double _ixy,
const double _ixz,
const double iyz);
103 public:
void SetCoG(
const double _cx,
const double _cy,
const double _cz);
107 public:
void SetCoG(
const ignition::math::Vector3d &_center);
117 public:
void SetCoG(
const double _cx,
const double _cy,
const double _cz,
118 const double _rx,
const double _ry,
const double _rz);
122 public:
void SetCoG(
const ignition::math::Pose3d &_c);
126 public:
const ignition::math::Vector3d &
CoG()
const;
131 public: ignition::math::Pose3d
Pose()
const;
143 public:
double IXX()
const;
147 public:
double IYY()
const;
151 public:
double IZZ()
const;
155 public:
double IXY()
const;
159 public:
double IXZ()
const;
163 public:
double IYZ()
const;
191 public:
void Rotate(
const ignition::math::Quaterniond &_rot);
229 public: ignition::math::Matrix3d
MOI(
230 const ignition::math::Pose3d &_pose)
const;
238 const ignition::math::Pose3d &_frameOffset)
const;
246 const ignition::math::Vector3d &_frameOffset)
const;
254 _out <<
"Mass[" << _inertial.
Mass() <<
"] CoG["
255 << _inertial.
CoG() <<
"]\n";
267 public: ignition::math::Matrix3d
MOI()
const;
271 public:
void SetMOI(
const ignition::math::Matrix3d &_moi);
275 private: std::unique_ptr<InertialPrivate> dataPtr;
A class for inertial information about a link.
Definition Inertial.hh:46
Inertial(const Inertial &_inertial)
Copy constructor.
ignition::math::Matrix3d MOI() const
returns Moments of Inertia as a Matrix3
Inertial & operator=(const ignition::math::Inertiald &_inertial)
Equal operator.
Inertial(const double _mass)
Constructor.
void SetIZZ(const double _v)
Set IZZ.
double IXX() const
Get IXX.
void SetIXZ(const double _v)
Set IXZ.
Inertial & operator=(const Inertial &_inertial)
Equal operator.
const Inertial & operator+=(const Inertial &_inertial)
Addition equal operator.
void ProcessMsg(const msgs::Inertial &_msg)
Update parameters from a message.
void Reset()
Reset all the mass properties.
double IXY() const
Get IXY.
void SetInertiaMatrix(const double _ixx, const double _iyy, const double _izz, const double _ixy, const double _ixz, const double iyz)
Set the mass matrix.
Inertial operator+(const Inertial &_inertial) const
Addition operator.
void SetCoG(const double _cx, const double _cy, const double _cz)
Set the center of gravity.
const ignition::math::Vector3d & CoG() const
Get the center of gravity.
Inertial()
Default Constructor.
void Rotate(const ignition::math::Quaterniond &_rot)
Rotate this mass.
Inertial operator()(const ignition::math::Vector3d &_frameOffset) const
Get equivalent Inertia values with the Link frame offset, while holding the Pose of CoG constant in t...
Inertial(const ignition::math::Inertiald &_inertial)
Constructor from ignition::math::Inertial.
const ignition::math::Vector3d & PrincipalMoments() const
Get the principal moments of inertia (Ixx, Iyy, Izz).
void UpdateParameters(sdf::ElementPtr _sdf)
update the parameters using new sdf values.
const ignition::math::Vector3d & ProductsOfInertia() const
Get the products of inertia (Ixy, Ixz, Iyz).
void SetCoG(const ignition::math::Pose3d &_c)
Set the center of gravity.
void SetIYZ(const double _v)
Set IYZ.
ignition::math::Matrix3d MOI(const ignition::math::Pose3d &_pose) const
Get the equivalent inertia from a point in local Link frame If you specify MOI(this->GetPose()),...
double IXZ() const
Get IXZ.
Inertial operator()(const ignition::math::Pose3d &_frameOffset) const
Get equivalent Inertia values with the Link frame offset, while holding the Pose of CoG constant in t...
void SetCoG(const double _cx, const double _cy, const double _cz, const double _rx, const double _ry, const double _rz)
Set the center of gravity and rotation offset of inertial coordinate frame relative to Link frame.
double IZZ() const
Get IZZ.
void SetMOI(const ignition::math::Matrix3d &_moi)
Sets Moments of Inertia (MOI) from a Matrix3.
void SetIXX(const double _v)
Set IXX.
void SetCoG(const ignition::math::Vector3d &_center)
Set the center of gravity.
ignition::math::Pose3d Pose() const
Get the pose about which the mass and inertia matrix is specified in the Link frame.
virtual ~Inertial()
Destructor.
void SetIXY(const double _v)
Set IXY.
double IYZ() const
Get IYZ.
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::Inertial &_inertial)
Output operator.
Definition Inertial.hh:251
void Load(sdf::ElementPtr _sdf)
Load from SDF values.
double Mass() const
Get the mass.
void SetIYY(const double _v)
Set IYY.
double IYY() const
Get IYY.
ignition::math::Inertiald Ign() const
Return copy of Inertial in ignition format.
void SetMass(const double _m)
Set the mass.
Forward declarations for the common classes.
Definition Animation.hh:27