p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/math/Pose.hh

261 lines
9.1 KiB
C++
Raw Blame History

/*
* 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 <iostream>
#include <ignition/math/Pose3.hh>
#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
Reference in New Issue View Git Blame Copy Permalink