295 lines
10 KiB
C++
295 lines
10 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
||
|
|
*
|
||
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||
|
|
* you may not use this file except in compliance with the License.
|
||
|
|
* You may obtain a copy of the License at
|
||
|
|
*
|
||
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||
|
|
*
|
||
|
|
* Unless required by applicable law or agreed to in writing, software
|
||
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
|
|
* See the License for the specific language governing permissions and
|
||
|
|
* limitations under the License.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
#ifndef GAZEBO_PHYSICS_INERTIAL_HH_
|
||
|
|
#define GAZEBO_PHYSICS_INERTIAL_HH_
|
||
|
|
|
||
|
|
#ifdef _WIN32
|
||
|
|
// Ensure that Winsock2.h is included before Windows.h, which can get
|
||
|
|
// pulled in by anybody (e.g., Boost).
|
||
|
|
#include <Winsock2.h>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include <string>
|
||
|
|
|
||
|
|
#include <ignition/math/Inertial.hh>
|
||
|
|
#include <sdf/sdf.hh>
|
||
|
|
|
||
|
|
#include "gazebo/math/Pose.hh"
|
||
|
|
#include "gazebo/msgs/msgs.hh"
|
||
|
|
#include "gazebo/math/Quaternion.hh"
|
||
|
|
#include "gazebo/math/Vector3.hh"
|
||
|
|
#include "gazebo/math/Matrix3.hh"
|
||
|
|
#include "gazebo/util/system.hh"
|
||
|
|
|
||
|
|
namespace gazebo
|
||
|
|
{
|
||
|
|
namespace physics
|
||
|
|
{
|
||
|
|
/// \addtogroup gazebo_physics
|
||
|
|
/// \{
|
||
|
|
|
||
|
|
/// \class Inertial Inertial.hh physics/physics.hh
|
||
|
|
/// \brief A class for inertial information about a link
|
||
|
|
class GZ_PHYSICS_VISIBLE Inertial
|
||
|
|
{
|
||
|
|
/// \brief Default Constructor
|
||
|
|
public: Inertial();
|
||
|
|
|
||
|
|
/// \brief Constructor.
|
||
|
|
/// \param[in] _mass Mass value in kg if using metric.
|
||
|
|
public: explicit Inertial(double _mass);
|
||
|
|
|
||
|
|
/// \brief Constructor from ignition::math::Inertial.
|
||
|
|
/// \param[in] _inertial Ignition inertial object to copy.
|
||
|
|
public: Inertial(const ignition::math::Inertiald &_inertial);
|
||
|
|
|
||
|
|
/// \brief Copy constructor.
|
||
|
|
/// \param[in] _inertial Inertial element to copy
|
||
|
|
public: Inertial(const Inertial &_inertial);
|
||
|
|
|
||
|
|
/// \brief Destructor.
|
||
|
|
public: virtual ~Inertial();
|
||
|
|
|
||
|
|
/// \brief Load from SDF values.
|
||
|
|
/// \param[in] _sdf SDF value to load from.
|
||
|
|
public: void Load(sdf::ElementPtr _sdf);
|
||
|
|
|
||
|
|
/// \brief update the parameters using new sdf values.
|
||
|
|
/// \param[in] _sdf Update values from.
|
||
|
|
public: void UpdateParameters(sdf::ElementPtr _sdf);
|
||
|
|
|
||
|
|
/// \brief Reset all the mass properties.
|
||
|
|
public: void Reset();
|
||
|
|
|
||
|
|
/// \brief Return copy of Inertial in ignition format.
|
||
|
|
public: ignition::math::Inertiald Ign() const;
|
||
|
|
|
||
|
|
/// \brief Set the mass.
|
||
|
|
public: void SetMass(double m);
|
||
|
|
|
||
|
|
/// \brief Get the mass
|
||
|
|
public: double GetMass() const;
|
||
|
|
|
||
|
|
/// \brief Set the mass matrix.
|
||
|
|
/// \param[in] _ixx X second moment of inertia (MOI) about x axis.
|
||
|
|
/// \param[in] _iyy Y second moment of inertia about y axis.
|
||
|
|
/// \param[in] _izz Z second moment of inertia about z axis.
|
||
|
|
/// \param[in] _ixy XY inertia.
|
||
|
|
/// \param[in] _ixz XZ inertia.
|
||
|
|
/// \param[in] _iyz YZ inertia.
|
||
|
|
public: void SetInertiaMatrix(double _ixx, double _iyy, double _izz,
|
||
|
|
double _ixy, double _ixz, double iyz);
|
||
|
|
|
||
|
|
/// \brief Set the center of gravity.
|
||
|
|
/// \param[in] _cx X position.
|
||
|
|
/// \param[in] _cy Y position.
|
||
|
|
/// \param[in] _cz Z position.
|
||
|
|
public: void SetCoG(double _cx, double _cy, double _cz);
|
||
|
|
|
||
|
|
/// \brief Set the center of gravity.
|
||
|
|
/// \param[in] _center Center of the gravity.
|
||
|
|
public: void SetCoG(const math::Vector3 &_center);
|
||
|
|
|
||
|
|
/// \brief Set the center of gravity and rotation offset of inertial
|
||
|
|
/// coordinate frame relative to Link frame.
|
||
|
|
/// \param[in] _cx Center offset in x-direction in Link frame
|
||
|
|
/// \param[in] _cy Center offset in y-direction in Link frame
|
||
|
|
/// \param[in] _cz Center offset in z-direction in Link frame
|
||
|
|
/// \param[in] _rx Roll angle offset of inertial coordinate frame.
|
||
|
|
/// \param[in] _ry Pitch angle offset of inertial coordinate frame.
|
||
|
|
/// \param[in] _rz Yaw angle offset of inertial coordinate frame.
|
||
|
|
public: void SetCoG(double _cx, double _cy, double _cz,
|
||
|
|
double _rx, double _ry, double _rz);
|
||
|
|
|
||
|
|
/// \brief Set the center of gravity.
|
||
|
|
/// \param[in] _c Transform to center of gravity.
|
||
|
|
public: void SetCoG(const math::Pose &_c);
|
||
|
|
|
||
|
|
/// \brief Get the center of gravity.
|
||
|
|
/// \return The center of gravity.
|
||
|
|
public: inline const math::Vector3 &GetCoG() const
|
||
|
|
{
|
||
|
|
return this->cog.pos;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief Get the pose about which the mass and inertia matrix is
|
||
|
|
/// specified in the Link frame.
|
||
|
|
/// \return The inertial pose.
|
||
|
|
public: inline const math::Pose GetPose() const
|
||
|
|
{
|
||
|
|
return math::Pose(this->cog);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief Get the principal moments of inertia (Ixx, Iyy, Izz).
|
||
|
|
/// \return The principal moments.
|
||
|
|
public: math::Vector3 GetPrincipalMoments() const;
|
||
|
|
|
||
|
|
/// \brief Get the products of inertia (Ixy, Ixz, Iyz).
|
||
|
|
/// \return The products of inertia.
|
||
|
|
public: math::Vector3 GetProductsofInertia() const;
|
||
|
|
|
||
|
|
/// \brief Get IXX
|
||
|
|
/// \return IXX value
|
||
|
|
public: double GetIXX() const;
|
||
|
|
|
||
|
|
/// \brief Get IYY
|
||
|
|
/// \return IYY value
|
||
|
|
public: double GetIYY() const;
|
||
|
|
|
||
|
|
/// \brief Get IZZ
|
||
|
|
/// \return IZZ value
|
||
|
|
public: double GetIZZ() const;
|
||
|
|
|
||
|
|
/// \brief Get IXY
|
||
|
|
/// \return IXY value
|
||
|
|
public: double GetIXY() const;
|
||
|
|
|
||
|
|
/// \brief Get IXZ
|
||
|
|
/// \return IXZ value
|
||
|
|
public: double GetIXZ() const;
|
||
|
|
|
||
|
|
/// \brief Get IXZ
|
||
|
|
/// \return IYZ value
|
||
|
|
public: double GetIYZ() const;
|
||
|
|
|
||
|
|
/// \brief Set IXX
|
||
|
|
/// \param[in] _v IXX value
|
||
|
|
public: void SetIXX(double _v);
|
||
|
|
|
||
|
|
/// \brief Set IYY
|
||
|
|
/// \param[in] _v IYY value
|
||
|
|
public: void SetIYY(double _v);
|
||
|
|
|
||
|
|
/// \brief Set IZZ
|
||
|
|
/// \param[in] _v IZZ value
|
||
|
|
public: void SetIZZ(double _v);
|
||
|
|
|
||
|
|
/// \brief Set IXY
|
||
|
|
/// \param[in] _v IXY value
|
||
|
|
public: void SetIXY(double _v);
|
||
|
|
|
||
|
|
/// \brief Set IXZ
|
||
|
|
/// \param[in] _v IXZ value
|
||
|
|
public: void SetIXZ(double _v);
|
||
|
|
|
||
|
|
/// \brief Set IYZ
|
||
|
|
/// \param[in] _v IXX value
|
||
|
|
public: void SetIYZ(double _v);
|
||
|
|
|
||
|
|
/// \brief Rotate this mass.
|
||
|
|
/// \param[in] _rot Rotation amount.
|
||
|
|
public: void Rotate(const math::Quaternion &_rot);
|
||
|
|
|
||
|
|
/// \brief Equal operator.
|
||
|
|
/// \param[in] _inertial Inertial to copy.
|
||
|
|
/// \return Reference to this object.
|
||
|
|
public: Inertial &operator=(const Inertial &_inertial);
|
||
|
|
|
||
|
|
/// \brief Equal operator.
|
||
|
|
/// \param[in] _inertial Ignition inertial to copy.
|
||
|
|
/// \return Reference to this object.
|
||
|
|
public: Inertial &operator=(const ignition::math::Inertiald &_inertial);
|
||
|
|
|
||
|
|
/// \brief Addition operator.
|
||
|
|
/// Assuming both CG and Moment of Inertia (MOI) are defined
|
||
|
|
/// in the same reference Link frame.
|
||
|
|
/// New CG is computed from masses and perspective offsets,
|
||
|
|
/// and both MOI contributions relocated to the new cog.
|
||
|
|
/// \param[in] _inertial Inertial to add.
|
||
|
|
/// \return The result of the addition.
|
||
|
|
public: Inertial operator+(const Inertial &_inertial) const;
|
||
|
|
|
||
|
|
/// \brief Addition equal operator.
|
||
|
|
/// \param[in] _inertial Inertial to add.
|
||
|
|
/// \return Reference to this object.
|
||
|
|
public: const Inertial &operator+=(const Inertial &_inertial);
|
||
|
|
|
||
|
|
/// \brief Update parameters from a message
|
||
|
|
/// \param[in] _msg Message to read
|
||
|
|
public: void ProcessMsg(const msgs::Inertial &_msg);
|
||
|
|
|
||
|
|
/// \brief Get the equivalent inertia from a point in local Link frame
|
||
|
|
/// If you specify GetMOI(this->GetPose()), you should get
|
||
|
|
/// back the Moment of Inertia (MOI) exactly as specified in the SDF.
|
||
|
|
/// If _pose is different from pose of the Inertial block, then
|
||
|
|
/// the MOI is rotated accordingly, and contributions from changes
|
||
|
|
/// in MOI location location due to point mass is added to the final MOI.
|
||
|
|
/// \param[in] _pose location in Link local frame
|
||
|
|
/// \return equivalent inertia at _pose
|
||
|
|
public: math::Matrix3 GetMOI(const math::Pose &_pose)
|
||
|
|
const;
|
||
|
|
|
||
|
|
/// \brief Get equivalent Inertia values with the Link frame offset,
|
||
|
|
/// while holding the Pose of CoG constant in the world frame.
|
||
|
|
/// \param[in] _frameOffset amount to offset the Link frame by, this
|
||
|
|
/// is a transform defined in the Link frame.
|
||
|
|
/// \return Inertial parameters with the shifted frame.
|
||
|
|
public: Inertial GetInertial(const math::Pose &_frameOffset) const;
|
||
|
|
|
||
|
|
/// \brief Output operator.
|
||
|
|
/// \param[in] _out Output stream.
|
||
|
|
/// \param[in] _inertial Inertial object to output.
|
||
|
|
public: friend std::ostream &operator<<(std::ostream &_out,
|
||
|
|
const gazebo::physics::Inertial &_inertial)
|
||
|
|
{
|
||
|
|
_out << "Mass[" << _inertial.mass << "] CoG["
|
||
|
|
<< _inertial.cog << "]\n";
|
||
|
|
_out << "IXX[" << _inertial.principals.x << "] "
|
||
|
|
<< "IYY[" << _inertial.principals.y << "] "
|
||
|
|
<< "IZZ[" << _inertial.principals.z << "]\n";
|
||
|
|
_out << "IXY[" << _inertial.products.x << "] "
|
||
|
|
<< "IXZ[" << _inertial.products.y << "] "
|
||
|
|
<< "IYZ[" << _inertial.products.z << "]\n";
|
||
|
|
return _out;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// \brief returns Moments of Inertia as a Matrix3
|
||
|
|
/// \return Moments of Inertia as a Matrix3
|
||
|
|
public: math::Matrix3 GetMOI() const;
|
||
|
|
|
||
|
|
/// \brief Sets Moments of Inertia (MOI) from a Matrix3
|
||
|
|
/// \param[in] Moments of Inertia as a Matrix3
|
||
|
|
public: void SetMOI(const math::Matrix3 &_moi);
|
||
|
|
|
||
|
|
/// \brief Mass the object. Default is 1.0.
|
||
|
|
private: double mass;
|
||
|
|
|
||
|
|
/// \brief Center of gravity in the Link frame.
|
||
|
|
/// Default is (0.0 0.0 0.0 0.0 0.0 0.0)
|
||
|
|
private: math::Pose cog;
|
||
|
|
|
||
|
|
/// \brief Principal moments of inertia. Default is (1.0 1.0 1.0)
|
||
|
|
/// These Moments of Inertia are specified in the local Inertial frame.
|
||
|
|
private: math::Vector3 principals;
|
||
|
|
|
||
|
|
/// \brief Product moments of inertia. Default is (0.0 0.0 0.0)
|
||
|
|
/// These MOI off-diagonals are specified in the local Inertial frame.
|
||
|
|
/// Where products.x is Ixy, products.y is Ixz and products.z is Iyz.
|
||
|
|
private: math::Vector3 products;
|
||
|
|
|
||
|
|
/// \brief Our SDF values.
|
||
|
|
private: sdf::ElementPtr sdf;
|
||
|
|
|
||
|
|
/// \brief An SDF pointer that allows us to only read the inertial.sdf
|
||
|
|
/// file once, which in turns limits disk reads.
|
||
|
|
private: static sdf::ElementPtr sdfInertial;
|
||
|
|
};
|
||
|
|
/// \}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif
|