17 #ifndef _IGNITION_POSE_HH_
18 #define _IGNITION_POSE_HH_
36 public:
Pose3() : p(0, 0, 0), q(1, 0, 0, 0)
55 public:
Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
56 : p(_x, _y, _z), q(_roll, _pitch, _yaw)
68 public:
Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
69 : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
76 : p(_pose.p), q(_pose.q)
110 public:
void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
112 this->p.Set(_x, _y, _z);
119 return this->p.IsFinite() && this->q.IsFinite();
134 return Pose3<T>(inv * (this->p*-1), inv);
201 return this->p == _pose.p && this->q == _pose.q;
209 return this->p != _pose.p || this->q != _pose.q;
237 tmp = this->q * (tmp * this->q.Inverse());
240 this->p.Y() + tmp.
Y(),
241 this->p.Z() + tmp.
Z());
250 this->p.X(), this->p.Y(), this->p.Z());
253 tmp = _pose.q * (tmp * _pose.q.Inverse());
256 _pose.p.Y() + tmp.
Y(),
257 _pose.p.Z() + tmp.
Z());
266 this->p.X() - _pose.p.X(),
267 this->p.Y() - _pose.p.Y(),
268 this->p.Z() - _pose.p.Z());
270 tmp = _pose.q.Inverse() * (tmp * _pose.q);
301 a.q = this->q.Inverse() * _b.q;
302 qt = a.q *
Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
303 qt = qt * a.q.Inverse();
323 a.p.X((1.0 - 2.0*_q.
Y()*_q.
Y() - 2.0*_q.
Z()*_q.
Z()) * this->p.X()
324 +(2.0*(_q.
X()*_q.
Y()+_q.
W()*_q.
Z())) * this->p.Y()
325 +(2.0*(_q.
X()*_q.
Z()-_q.
W()*_q.
Y())) * this->p.Z());
326 a.p.Y((2.0*(_q.
X()*_q.
Y()-_q.
W()*_q.
Z())) * this->p.X()
327 +(1.0 - 2.0*_q.
X()*_q.
X() - 2.0*_q.
Z()*_q.
Z()) * this->p.Y()
328 +(2.0*(_q.
Y()*_q.
Z()+_q.
W()*_q.
X())) * this->p.Z());
329 a.p.Z((2.0*(_q.
X()*_q.
Z()+_q.
W()*_q.
Y())) * this->p.X()
330 +(2.0*(_q.
Y()*_q.
Z()-_q.
W()*_q.
X())) * this->p.Y()
331 +(1.0 - 2.0*_q.
X()*_q.
X() - 2.0*_q.
Y()*_q.
Y()) * this->p.Z());
339 this->q.Round(_precision);
340 this->p.Round(_precision);
378 _out << _pose.
Pos() <<
" " << _pose.
Rot();
390 _in.setf(std::ios_base::skipws);
404 template<
typename T>
const Pose3<T>
Pose3<T>::Zero(0, 0, 0, 0, 0, 0);
void Set(const Vector3< T > &_pos, const Vector3< T > &_rpy)
Set the pose from pos and rpy vectors.
Definition: Pose3.hh:97
Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
Constructor.
Definition: Pose3.hh:68
Pose3()
Default constructors.
Definition: Pose3.hh:36
bool operator==(const Pose3< T > &_pose) const
Equality operator.
Definition: Pose3.hh:199
bool IsFinite() const
See if a pose is finite (e.g., not nan)
Definition: Pose3.hh:117
Quaternion< T > CoordRotationSub(const Quaternion< T > &_rot) const
Subtract one rotation from another: result = this->q - rot.
Definition: Pose3.hh:285
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:114
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:30
const Pose3< T > & operator-=(const Pose3< T > &_pose)
Subtraction operator.
Definition: Pose3.hh:188
const Pose3< T > & operator+=(const Pose3< T > &_pose)
Add-Equals operator.
Definition: Pose3.hh:156
const T & Y() const
Get the y component.
Definition: Quaternion.hh:750
void Reset()
Reset the pose.
Definition: Pose3.hh:310
Pose3< T > operator-(const Pose3< T > &_pose) const
Subtraction operator A is the transform from O to P in frame O B is the transform from O to Q in fram...
Definition: Pose3.hh:179
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Pose3.hh:337
friend std::istream & operator>>(std::istream &_in, ignition::math::Pose3< T > &_pose)
Stream extraction operator.
Definition: Pose3.hh:386
T X() const
Get the x value.
Definition: Vector3.hh:545
const T & Z() const
Get the z component.
Definition: Quaternion.hh:757
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Pose3< T > &_pose)
Stream insertion operator.
Definition: Pose3.hh:375
Pose3(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Constructor.
Definition: Pose3.hh:43
Pose3< T > operator-() const
Negation operator A is the transform from O to P in frame O then -A is transform from P to O specifie...
Definition: Pose3.hh:168
void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Set the pose from a six tuple.
Definition: Pose3.hh:110
Pose3< T > operator+(const Pose3< T > &_pose) const
Addition operator A is the transform from O to P specified in frame O B is the transform from P to Q ...
Definition: Pose3.hh:143
Pose3< double > Pose3d
Definition: Pose3.hh:407
Vector3< T > CoordPositionSub(const Pose3< T > &_pose) const
Subtract one position from another: result = this - pose.
Definition: Pose3.hh:263
bool operator!=(const Pose3< T > &_pose) const
Inequality operator.
Definition: Pose3.hh:207
T Y() const
Get the y value.
Definition: Vector3.hh:552
virtual ~Pose3()
Destructor.
Definition: Pose3.hh:81
Pose3< int > Pose3i
Definition: Pose3.hh:406
static const Quaternion Identity
math::Quaternion(1, 0, 0, 1)
Definition: Quaternion.hh:34
Vector3< T > & Pos()
Get a mutable reference to the position.
Definition: Pose3.hh:352
Vector3< T > CoordPositionAdd(const Vector3< T > &_pos) const
Add one point to a vector: result = this + pos.
Definition: Pose3.hh:232
Pose3< T > operator*(const Pose3< T > &_pose)
Multiplication operator.
Definition: Pose3.hh:215
T Z() const
Get the z value.
Definition: Vector3.hh:559
Quaternion< T > & Rot()
Get a mutuable reference to the rotation.
Definition: Pose3.hh:366
const Quaternion< T > & Rot() const
Get the rotation.
Definition: Pose3.hh:359
Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Constructor.
Definition: Pose3.hh:55
void Set(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Set the pose from a Vector3 and a Quaternion<T>
Definition: Pose3.hh:88
Pose3(const Pose3< T > &_pose)
Copy constructor.
Definition: Pose3.hh:75
Pose3< T > Inverse() const
Get the inverse of this pose.
Definition: Pose3.hh:131
Pose3< T > CoordPoseSolve(const Pose3< T > &_b) const
Find the inverse of a pose; i.e., if b = this + a, given b and this, find a.
Definition: Pose3.hh:296
Pose3< T > RotatePositionAboutOrigin(const Quaternion< T > &_q) const
Rotate vector part of a pose about the origin.
Definition: Pose3.hh:320
void Correct()
Fix any nan values.
Definition: Pose3.hh:123
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
Quaternion< T > CoordRotationAdd(const Quaternion< T > &_rot) const
Add one rotation to another: result = this->q + rot.
Definition: Pose3.hh:277
Vector3< T > CoordPositionAdd(const Pose3< T > &_pose) const
Add one point to another: result = this + pose.
Definition: Pose3.hh:247
static const Pose3< T > Zero
math::Pose3<T>(0, 0, 0, 0, 0, 0)
Definition: Pose3.hh:33
const T & W() const
Get the w component.
Definition: Quaternion.hh:736
Pose3< float > Pose3f
Definition: Pose3.hh:408
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:206
A quaternion class.
Definition: Quaternion.hh:31
Pose3< T > & operator=(const Pose3< T > &_pose)
Equal operator.
Definition: Pose3.hh:222
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:345
const T & X() const
Get the x component.
Definition: Quaternion.hh:743