17 #ifndef GAZEBO_PHYSICS_INERTIAL_HH_
18 #define GAZEBO_PHYSICS_INERTIAL_HH_
29 #include <ignition/math/Inertial.hh>
31 #include <ignition/math/Vector3.hh>
32 #include <ignition/math/Quaternion.hh>
33 #include <ignition/math/Matrix3.hh>
44 class InertialPrivate;
63 public:
Inertial(
const ignition::math::Inertiald &_inertial);
74 public:
void Load(sdf::ElementPtr _sdf);
84 public: ignition::math::Inertiald
Ign()
const;
92 public:
double Mass()
const;
102 const double _ixx,
const double _iyy,
const double _izz,
103 const double _ixy,
const double _ixz,
const double iyz);
109 public:
void SetCoG(
const double _cx,
const double _cy,
const double _cz);
113 public:
void SetCoG(
const ignition::math::Vector3d &_center);
123 public:
void SetCoG(
const double _cx,
const double _cy,
const double _cz,
124 const double _rx,
const double _ry,
const double _rz);
128 public:
void SetCoG(
const ignition::math::Pose3d &_c);
132 public:
const ignition::math::Vector3d &
CoG()
const;
137 public: ignition::math::Pose3d
Pose()
const;
149 public:
double IXX()
const;
153 public:
double IYY()
const;
157 public:
double IZZ()
const;
161 public:
double IXY()
const;
165 public:
double IXZ()
const;
169 public:
double IYZ()
const;
197 public:
void Rotate(
const ignition::math::Quaterniond &_rot);
235 public: ignition::math::Matrix3d
MOI(
236 const ignition::math::Pose3d &_pose)
const;
244 const ignition::math::Pose3d &_frameOffset)
const;
252 const ignition::math::Vector3d &_frameOffset)
const;
260 _out <<
"Mass[" << _inertial.
Mass() <<
"] CoG["
261 << _inertial.
CoG() <<
"]\n";
273 public: ignition::math::Matrix3d
MOI()
const;
277 public:
void SetMOI(
const ignition::math::Matrix3d &_moi);
281 private: std::unique_ptr<InertialPrivate> dataPtr;
A class for inertial information about a link.
Definition: Inertial.hh:52
Inertial(const Inertial &_inertial)
Copy constructor.
ignition::math::Matrix3d MOI() const
returns Moments of Inertia as a Matrix3
Inertial(const double _mass)
Constructor.
Inertial & operator=(const Inertial &_inertial)
Equal operator.
void SetIZZ(const double _v)
Set IZZ.
double IXX() const
Get IXX.
void SetIXZ(const double _v)
Set IXZ.
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.
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.
void UpdateParameters(sdf::ElementPtr _sdf)
update the parameters using new sdf values.
void SetCoG(const ignition::math::Pose3d &_c)
Set the center of gravity.
void SetIYZ(const double _v)
Set IYZ.
const ignition::math::Vector3d & CoG() const
Get the center of gravity.
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()),...
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::Inertial &_inertial)
Output operator.
Definition: Inertial.hh:257
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.
const ignition::math::Vector3d & PrincipalMoments() const
Get the principal moments of inertia (Ixx, Iyy, Izz).
void SetIXY(const double _v)
Set IXY.
double IYZ() const
Get IYZ.
void Load(sdf::ElementPtr _sdf)
Load from SDF values.
double Mass() const
Get the mass.
Inertial & operator=(const ignition::math::Inertiald &_inertial)
Equal operator.
void SetIYY(const double _v)
Set IYY.
double IYY() const
Get IYY.
const Inertial & operator+=(const Inertial &_inertial)
Addition equal operator.
ignition::math::Inertiald Ign() const
Return copy of Inertial in ignition format.
const ignition::math::Vector3d & ProductsOfInertia() const
Get the products of inertia (Ixy, Ixz, Iyz).
void SetMass(const double _m)
Set the mass.
Forward declarations for the common classes.
Definition: Animation.hh:27