Gazebo Math

API Reference

6.15.1
gz/math/Pose3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GZ_MATH_POSE_HH_
18 #define GZ_MATH_POSE_HH_
19 
20 #include <gz/math/Quaternion.hh>
21 #include <gz/math/Vector3.hh>
22 #include <gz/math/config.hh>
23 
24 namespace ignition
25 {
26  namespace math
27  {
28  // Inline bracket to help doxygen filtering.
29  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
30  //
33  template<typename T>
34  class Pose3
35  {
37  public: static const Pose3<T> Zero;
38 
40  public: Pose3() : p(0, 0, 0), q(1, 0, 0, 0)
41  {
42  }
43 
47  public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
48  : p(_pos), q(_rot)
49  {
50  }
51 
59  public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
60  : p(_x, _y, _z), q(_roll, _pitch, _yaw)
61  {
62  }
63 
72  public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
73  : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
74  {
75  }
76 
79  public: Pose3(const Pose3<T> &_pose)
80  : p(_pose.p), q(_pose.q)
81  {
82  }
83 
85  public: virtual ~Pose3()
86  {
87  }
88 
92  public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
93  {
94  this->p = _pos;
95  this->q = _rot;
96  }
97 
101  public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
102  {
103  this->p = _pos;
104  this->q.Euler(_rpy);
105  }
106 
114  public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
115  {
116  this->p.Set(_x, _y, _z);
117  this->q.Euler(math::Vector3<T>(_roll, _pitch, _yaw));
118  }
119 
121  public: bool IsFinite() const
122  {
123  return this->p.IsFinite() && this->q.IsFinite();
124  }
125 
127  public: inline void Correct()
128  {
129  this->p.Correct();
130  this->q.Correct();
131  }
132 
135  public: Pose3<T> Inverse() const
136  {
137  Quaternion<T> inv = this->q.Inverse();
138  return Pose3<T>(inv * (this->p*-1), inv);
139  }
140 
147  public: Pose3<T> operator+(const Pose3<T> &_pose) const
148  {
149  Pose3<T> result;
150 
151  result.p = this->CoordPositionAdd(_pose);
152  result.q = this->CoordRotationAdd(_pose.q);
153 
154  return result;
155  }
156 
160  public: const Pose3<T> &operator+=(const Pose3<T> &_pose)
161  {
162  this->p = this->CoordPositionAdd(_pose);
163  this->q = this->CoordRotationAdd(_pose.q);
164 
165  return *this;
166  }
167 
172  public: inline Pose3<T> operator-() const
173  {
174  return Pose3<T>() - *this;
175  }
176 
183  public: inline Pose3<T> operator-(const Pose3<T> &_pose) const
184  {
185  return Pose3<T>(this->CoordPositionSub(_pose),
186  this->CoordRotationSub(_pose.q));
187  }
188 
192  public: const Pose3<T> &operator-=(const Pose3<T> &_pose)
193  {
194  this->p = this->CoordPositionSub(_pose);
195  this->q = this->CoordRotationSub(_pose.q);
196 
197  return *this;
198  }
199 
203  public: bool operator==(const Pose3<T> &_pose) const
204  {
205  return this->p == _pose.p && this->q == _pose.q;
206  }
207 
211  public: bool operator!=(const Pose3<T> &_pose) const
212  {
213  return this->p != _pose.p || this->q != _pose.q;
214  }
215 
221  public: Pose3<T> operator*(const Pose3<T> &_pose) const
222  {
223  return Pose3<T>(_pose.CoordPositionAdd(*this), this->q * _pose.q);
224  }
225 
230  public: const Pose3<T> &operator*=(const Pose3<T> &_pose)
231  {
232  *this = *this * _pose;
233  return *this;
234  }
235 
238  public: Pose3<T> &operator=(const Pose3<T> &_pose)
239  {
240  this->p = _pose.p;
241  this->q = _pose.q;
242  return *this;
243  }
244 
248  public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
249  {
250  Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
251 
252  // result = pose.q + pose.q * this->p * pose.q!
253  tmp = this->q * (tmp * this->q.Inverse());
254 
255  return Vector3<T>(this->p.X() + tmp.X(),
256  this->p.Y() + tmp.Y(),
257  this->p.Z() + tmp.Z());
258  }
259 
263  public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
264  {
265  Quaternion<T> tmp(static_cast<T>(0),
266  this->p.X(), this->p.Y(), this->p.Z());
267 
268  // result = _pose.q + _pose.q * this->p * _pose.q!
269  tmp = _pose.q * (tmp * _pose.q.Inverse());
270 
271  return Vector3<T>(_pose.p.X() + tmp.X(),
272  _pose.p.Y() + tmp.Y(),
273  _pose.p.Z() + tmp.Z());
274  }
275 
279  public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
280  {
281  Quaternion<T> tmp(0,
282  this->p.X() - _pose.p.X(),
283  this->p.Y() - _pose.p.Y(),
284  this->p.Z() - _pose.p.Z());
285 
286  tmp = _pose.q.Inverse() * (tmp * _pose.q);
287  return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
288  }
289 
293  public: Quaternion<T> CoordRotationAdd(const Quaternion<T> &_rot) const
294  {
295  return Quaternion<T>(_rot * this->q);
296  }
297 
302  const Quaternion<T> &_rot) const
303  {
304  Quaternion<T> result(_rot.Inverse() * this->q);
305  result.Normalize();
306  return result;
307  }
308 
312  public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
313  {
314  Quaternion<T> qt;
315  Pose3<T> a;
316 
317  a.q = this->q.Inverse() * _b.q;
318  qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
319  qt = qt * a.q.Inverse();
320  a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
321 
322  return a;
323  }
324 
326  public: void Reset()
327  {
328  // set the position to zero
329  this->p.Set();
330  this->q = Quaternion<T>::Identity;
331  }
332 
337  {
338  Pose3<T> a = *this;
339  a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
340  +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
341  +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
342  a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
343  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
344  +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
345  a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
346  +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
347  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
348  return a;
349  }
350 
353  public: void Round(int _precision)
354  {
355  this->q.Round(_precision);
356  this->p.Round(_precision);
357  }
358 
361  public: inline const Vector3<T> &Pos() const
362  {
363  return this->p;
364  }
365 
368  public: inline Vector3<T> &Pos()
369  {
370  return this->p;
371  }
372 
377  public: inline const T X() const
378  {
379  return this->p.X();
380  }
381 
383  public: inline void SetX(T x)
384  {
385  this->p.X() = x;
386  }
387 
392  public: inline const T Y() const
393  {
394  return this->p.Y();
395  }
396 
398  public: inline void SetY(T y)
399  {
400  this->p.Y() = y;
401  }
402 
407  public: inline const T Z() const
408  {
409  return this->p.Z();
410  }
411 
413  public: inline void SetZ(T z)
414  {
415  this->p.Z() = z;
416  }
417 
420  public: inline const Quaternion<T> &Rot() const
421  {
422  return this->q;
423  }
424 
427  public: inline Quaternion<T> &Rot()
428  {
429  return this->q;
430  }
431 
436  public: inline const T Roll() const
437  {
438  return this->q.Roll();
439  }
440 
445  public: inline const T Pitch() const
446  {
447  return this->q.Pitch();
448  }
449 
454  public: inline const T Yaw() const
455  {
456  return this->q.Yaw();
457  }
458 
463  public: friend std::ostream &operator<<(
464  std::ostream &_out, const gz::math::Pose3<T> &_pose)
465  {
466  _out << _pose.Pos() << " " << _pose.Rot();
467  return _out;
468  }
469 
474  public: friend std::istream &operator>>(
475  std::istream &_in, gz::math::Pose3<T> &_pose)
476  {
477  // Skip white spaces
478  _in.setf(std::ios_base::skipws);
479  Vector3<T> pos;
480  Quaternion<T> rot;
481  _in >> pos >> rot;
482  _pose.Set(pos, rot);
483  return _in;
484  }
485 
487  private: Vector3<T> p;
488 
490  private: Quaternion<T> q;
491  };
492  template<typename T> const Pose3<T> Pose3<T>::Zero(0, 0, 0, 0, 0, 0);
493 
497  }
498  }
499 }
500 #endif
void SetZ(T z)
Set the Z value of the position.
Definition: gz/math/Pose3.hh:413
Quaternion< T > CoordRotationAdd(const Quaternion< T > &_rot) const
Add one rotation to another: result = this->q + rot.
Definition: gz/math/Pose3.hh:293
Vector3< T > CoordPositionSub(const Pose3< T > &_pose) const
Subtract one position from another: result = this - pose.
Definition: gz/math/Pose3.hh:279
Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
Constructor.
Definition: gz/math/Pose3.hh:72
Encapsulates a position and rotation in three space.
Definition: gz/math/Pose3.hh:34
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
void Correct()
Fix any nan values.
Definition: gz/math/Pose3.hh:127
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: gz/math/Pose3.hh:312
const T Yaw() const
Get the Yaw value of the rotation.
Definition: gz/math/Pose3.hh:454
T Roll() const
Get the Euler roll angle in radians.
Definition: gz/math/Quaternion.hh:432
Pose3< T > operator*(const Pose3< T > &_pose) const
Multiplication operator. Given X_OP (frame P relative to O) and X_PQ (frame Q relative to P) then X_O...
Definition: gz/math/Pose3.hh:221
Pose3< T > Inverse() const
Get the inverse of this pose.
Definition: gz/math/Pose3.hh:135
void Set(T _w, T _x, T _y, T _z)
Set this quaternion from 4 floating numbers.
Definition: gz/math/Quaternion.hh:300
bool operator==(const Pose3< T > &_pose) const
Equality operator.
Definition: gz/math/Pose3.hh:203
friend std::ostream & operator<<(std::ostream &_out, const Pose3< T > &_pose)
Stream insertion operator.
Definition: gz/math/Pose3.hh:463
Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Constructor.
Definition: gz/math/Pose3.hh:59
Vector3< T > CoordPositionAdd(const Vector3< T > &_pos) const
Add one point to a vector: result = this + pos.
Definition: gz/math/Pose3.hh:248
Vector3< T > CoordPositionAdd(const Pose3< T > &_pose) const
Add one point to another: result = this + pose.
Definition: gz/math/Pose3.hh:263
const T Pitch() const
Get the Pitch value of the rotation.
Definition: gz/math/Pose3.hh:445
Vector3< T > & Pos()
Get a mutable reference to the position.
Definition: gz/math/Pose3.hh:368
Pose3< int > Pose3i
Definition: gz/math/Pose3.hh:494
const Quaternion< T > & Rot() const
Get the rotation.
Definition: gz/math/Pose3.hh:420
void SetX(T x)
Set X value of the position.
Definition: gz/math/Pose3.hh:383
Quaternion< T > CoordRotationSub(const Quaternion< T > &_rot) const
Subtract one rotation from another: result = this->q - rot.
Definition: gz/math/Pose3.hh:301
void Set(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Set the pose from a Vector3 and a Quaternion<T>
Definition: gz/math/Pose3.hh:92
T X() const
Get the x value.
Definition: gz/math/Vector3.hh:654
void Set(const Vector3< T > &_pos, const Vector3< T > &_rpy)
Set the pose from pos and rpy vectors.
Definition: gz/math/Pose3.hh:101
bool IsFinite() const
See if a quaternion is finite (e.g., not nan)
Definition: gz/math/Quaternion.hh:753
void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
Set the pose from a six tuple.
Definition: gz/math/Pose3.hh:114
const Pose3< T > & operator+=(const Pose3< T > &_pose)
Add-Equals operator.
Definition: gz/math/Pose3.hh:160
void SetY(T y)
Set the Y value of the position.
Definition: gz/math/Pose3.hh:398
void Correct()
Correct any nan values in this quaternion.
Definition: gz/math/Quaternion.hh:764
void Round(int _precision)
Round all values to _precision decimal places.
Definition: gz/math/Quaternion.hh:839
const T Y() const
Get the Y value of the position.
Definition: gz/math/Pose3.hh:392
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: gz/math/Pose3.hh:172
virtual ~Pose3()
Destructor.
Definition: gz/math/Pose3.hh:85
void Normalize()
Normalize the quaternion.
Definition: gz/math/Quaternion.hh:224
Pose3< double > Pose3d
Definition: gz/math/Pose3.hh:495
bool operator!=(const Pose3< T > &_pose) const
Inequality operator.
Definition: gz/math/Pose3.hh:211
const Vector3< T > & Pos() const
Get the position.
Definition: gz/math/Pose3.hh:361
Pose3< float > Pose3f
Definition: gz/math/Pose3.hh:496
T Z() const
Get the z value.
Definition: gz/math/Vector3.hh:668
const T & Y() const
Get the y component.
Definition: gz/math/Quaternion.hh:979
STL class.
const T X() const
Get the X value of the position.
Definition: gz/math/Pose3.hh:377
T setf(T... args)
T Yaw() const
Get the Euler yaw angle in radians.
Definition: gz/math/Quaternion.hh:446
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
Pose3< T > RotatePositionAboutOrigin(const Quaternion< T > &_q) const
Rotate vector part of a pose about the origin.
Definition: gz/math/Pose3.hh:336
const T & Z() const
Get the z component.
Definition: gz/math/Quaternion.hh:986
Pose3< T > & operator=(const Pose3< T > &_pose)
Assignment operator.
Definition: gz/math/Pose3.hh:238
void Round(int _precision)
Round all values to _precision decimal places.
Definition: gz/math/Pose3.hh:353
void Euler(const Vector3< T > &_vec)
Set the quaternion from Euler angles. The order of operations is roll, pitch, yaw around a fixed body...
Definition: gz/math/Quaternion.hh:313
T Pitch() const
Get the Euler pitch angle in radians.
Definition: gz/math/Quaternion.hh:439
void Reset()
Reset the pose.
Definition: gz/math/Pose3.hh:326
Pose3(const Vector3< T > &_pos, const Quaternion< T > &_rot)
Constructor.
Definition: gz/math/Pose3.hh:47
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: gz/math/Quaternion.hh:132
const T Roll() const
Get the Roll value of the rotation.
Definition: gz/math/Pose3.hh:436
const T & X() const
Get the x component.
Definition: gz/math/Quaternion.hh:972
const Pose3< T > & operator-=(const Pose3< T > &_pose)
Subtraction operator.
Definition: gz/math/Pose3.hh:192
const Pose3< T > & operator*=(const Pose3< T > &_pose)
Multiplication assignment operator. This pose will become equal to this * _pose.
Definition: gz/math/Pose3.hh:230
STL class.
bool IsFinite() const
See if a pose is finite (e.g., not nan)
Definition: gz/math/Pose3.hh:121
T Y() const
Get the y value.
Definition: gz/math/Vector3.hh:661
const T Z() const
Get the Z value of the position.
Definition: gz/math/Pose3.hh:407
static const Pose3< T > Zero
math::Pose3<T>(0, 0, 0, 0, 0, 0)
Definition: gz/math/Pose3.hh:37
Pose3()
Default constructors.
Definition: gz/math/Pose3.hh:40
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: gz/math/Pose3.hh:183
friend std::istream & operator>>(std::istream &_in, Pose3< T > &_pose)
Stream extraction operator.
Definition: gz/math/Pose3.hh:474
const T & W() const
Get the w component.
Definition: gz/math/Quaternion.hh:965
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: gz/math/Pose3.hh:147
Pose3(const Pose3< T > &_pose)
Copy constructor.
Definition: gz/math/Pose3.hh:79
Quaternion< T > & Rot()
Get a mutable reference to the rotation.
Definition: gz/math/Pose3.hh:427