/* * 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_POSE_HH_ #define _GAZEBO_POSE_HH_ #include #include #include "gazebo/math/Vector3.hh" #include "gazebo/math/Quaternion.hh" #include "gazebo/util/system.hh" namespace gazebo { namespace math { /// \addtogroup gazebo_math /// \{ /// \class Pose Pose.hh math/gzmath.hh /// \brief Encapsulates a position and rotation in three space class GZ_MATH_VISIBLE Pose { /// \brief math::Pose(0, 0, 0, 0, 0, 0) public: static const Pose Zero; /// \brief Default constructors public: Pose(); /// \brief Constructor /// \param[in] _pos A position /// \param[in] _rot A rotation public: Pose(const Vector3 &_pos, const Quaternion &_rot); /// \brief Constructor /// \param[in] _x x position in meters. /// \param[in] _y y position in meters. /// \param[in] _z z position in meters. /// \param[in] _roll Roll (rotation about X-axis) in radians. /// \param[in] _pitch Pitch (rotation about y-axis) in radians. /// \param[in] _yaw Yaw (rotation about z-axis) in radians. public: Pose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw); /// \brief Copy constructor /// \param[in] _pose Pose to copy public: Pose(const Pose &_pose); /// \brief Copy constructor for ignition math /// \param[in] _pose Pose to copy public: Pose(const ignition::math::Pose3d &_pose); /// \brief Destructor public: virtual ~Pose(); /// \brief Set the pose from a Vector3 and a Quaternion /// \param[in] _pos The position. /// \param[in] _rot The rotation. public: void Set(const Vector3 &_pos, const Quaternion &_rot); /// \brief Set the pose from pos and rpy vectors /// \param[in] _pos The position. /// \param[in] _rpy The rotation expressed as Euler angles. public: void Set(const Vector3 &_pos, const Vector3 &_rpy); /// \brief Set the pose from a six tuple. /// \param[in] _x x position in meters. /// \param[in] _y y position in meters. /// \param[in] _z z position in meters. /// \param[in] _roll Roll (rotation about X-axis) in radians. /// \param[in] _pitch Pitch (rotation about y-axis) in radians. /// \param[in] _yaw Pitch (rotation about z-axis) in radians. public: void Set(double _x, double _y, double _z, double _roll, double _pitch, double _yaw); /// \brief See if a pose is finite (e.g., not nan) public: bool IsFinite() const; /// \brief Fix any nan values public: inline void Correct() { this->pos.Correct(); this->rot.Correct(); } /// \brief Get the inverse of this pose /// \return the inverse pose public: Pose GetInverse() const; /// \brief Addition operator /// A is the transform from O to P specified in frame O /// B is the transform from P to Q specified in frame P /// then, B + A is the transform from O to Q specified in frame O /// \param[in] _pose Pose to add to this pose /// \return The resulting pose public: Pose operator+(const Pose &_pose) const; /// \brief Add-Equals operator /// \param[in] _pose Pose to add to this pose /// \return The resulting pose public: const Pose &operator+=(const Pose &_pose); /// \brief Negation operator /// A is the transform from O to P in frame O /// then -A is transform from P to O specified in frame P /// \return The resulting pose public: inline Pose operator-() const { return Pose() - *this; } /// \brief Subtraction operator /// A is the transform from O to P in frame O /// B is the transform from O to Q in frame O /// B - A is the transform from P to Q in frame P /// \param[in] _pose Pose to subtract from this one /// \return The resulting pose public: inline Pose operator-(const Pose &_pose) const { return Pose(this->CoordPositionSub(_pose), this->CoordRotationSub(_pose.rot)); } /// \brief Subtraction operator /// \param[in] _pose Pose to subtract from this one /// \return The resulting pose public: const Pose &operator-=(const Pose &_pose); /// \brief Equality operator /// \param[in] _pose Pose for comparison /// \return True if equal public: bool operator ==(const Pose &_pose) const; /// \brief Inequality operator /// \param[in] _pose Pose for comparison /// \return True if not equal public: bool operator!=(const Pose &_pose) const; /// \brief Multiplication operator /// \param[in] _pose the other pose /// \return itself public: Pose operator*(const Pose &_pose); /// \brief Equal operator /// \param[in] _pose Pose to copy public: Pose &operator=(const Pose &_pose); /// \brief Equal operator for ignition math /// \param[in] _pose Pose to copy public: Pose &operator=(const ignition::math::Pose3d &_pose); /// \brief Add one point to a vector: result = this + pos /// \param[in] _pos Position to add to this pose /// \return the resulting position public: Vector3 CoordPositionAdd(const Vector3 &_pos) const; /// \brief Add one point to another: result = this + pose /// \param[in] _pose The Pose to add /// \return The resulting position public: Vector3 CoordPositionAdd(const Pose &_pose) const; /// \brief Subtract one position from another: result = this - pose /// \param[in] _pose Pose to subtract /// \return The resulting position public: inline Vector3 CoordPositionSub(const Pose &_pose) const { Quaternion tmp(0.0, this->pos.x - _pose.pos.x, this->pos.y - _pose.pos.y, this->pos.z - _pose.pos.z); tmp = _pose.rot.GetInverse() * (tmp * _pose.rot); return Vector3(tmp.x, tmp.y, tmp.z); } /// \brief Add one rotation to another: result = this->rot + rot /// \param[in] _rot Rotation to add /// \return The resulting rotation public: Quaternion CoordRotationAdd(const Quaternion &_rot) const; /// \brief Subtract one rotation from another: result = this->rot - rot /// \param[in] _rot The rotation to subtract /// \return The resulting rotation public: inline Quaternion CoordRotationSub(const Quaternion &_rot) const { Quaternion result(_rot.GetInverse() * this->rot); result.Normalize(); return result; } /// \brief Find the inverse of a pose; i.e., if b = this + a, given b and /// this, find a /// \param[in] _b the other pose public: Pose CoordPoseSolve(const Pose &_b) const; /// \brief Reset the pose public: void Reset(); /// \brief Rotate vector part of a pose about the origin /// \param[in] _rot rotation /// \return the rotated pose public: Pose RotatePositionAboutOrigin(const Quaternion &_rot) const; /// \brief Round all values to _precision decimal places /// \param[in] _precision public: void Round(int _precision); /// \brief Convert this pose to an ignition::math::Pose3d object. /// \return This pose represented as an ignition::math::Pose3d. public: ignition::math::Pose3d Ign() const; /// \brief Stream insertion operator /// \param[in] _out output stream /// \param[in] _pose pose to output /// \return the stream public: friend std::ostream &operator<<(std::ostream &_out, const gazebo::math::Pose &_pose) { _out << _pose.pos << " " << _pose.rot; return _out; } /// \brief Stream extraction operator /// \param[in] _in the input stream /// \param[in] _pose the pose /// \return the stream public: friend std::istream &operator>>(std::istream &_in, gazebo::math::Pose &_pose) { // Skip white spaces _in.setf(std::ios_base::skipws); _in >> _pose.pos >> _pose.rot; return _in; } /// \brief The position public: Vector3 pos; /// \brief The rotation public: Quaternion rot; }; /// \} } } #endif