Gazebo Math

API Reference

7.5.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 gz::math
25 {
26  // Inline bracket to help doxygen filtering.
27  inline namespace GZ_MATH_VERSION_NAMESPACE {
28  //
70  template<typename T>
71  class Pose3
72  {
75  public: static const Pose3<T> &Zero;
76 
79  public: Pose3() = default;
80 
84  public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
85  : p(_pos), q(_rot)
86  {
87  }
88 
97  public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
98  : p(_x, _y, _z), q(_roll, _pitch, _yaw)
99  {
100  }
101 
112  public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
113  : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
114  {
115  }
116 
119  public: Pose3(const Pose3<T> &_pose) = default;
120 
122  public: ~Pose3() = default;
123 
127  public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
128  {
129  this->p = _pos;
130  this->q = _rot;
131  }
132 
136  public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
137  {
138  this->p = _pos;
139  this->q.SetFromEuler(_rpy);
140  }
141 
150  public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
151  {
152  this->p.Set(_x, _y, _z);
153  this->q.SetFromEuler(math::Vector3<T>(_roll, _pitch, _yaw));
154  }
155 
158  public: bool IsFinite() const
159  {
160  return this->p.IsFinite() && this->q.IsFinite();
161  }
162 
164  public: inline void Correct()
165  {
166  this->p.Correct();
167  this->q.Correct();
168  }
169 
172  public: Pose3<T> Inverse() const
173  {
174  Quaternion<T> inv = this->q.Inverse();
175  return Pose3<T>(inv * (this->p*-1), inv);
176  }
177 
184  public: GZ_DEPRECATED(7) Pose3<T> operator+(const Pose3<T> &_pose) const
185  {
186  Pose3<T> result;
187 
188  result.p = this->CoordPositionAdd(_pose);
189  result.q = this->CoordRotationAdd(_pose.q);
190 
191  return result;
192  }
193 
198  public: GZ_DEPRECATED(7) const Pose3<T> &
199  operator+=(const Pose3<T> &_pose)
200  {
201  this->p = this->CoordPositionAdd(_pose);
202  this->q = this->CoordRotationAdd(_pose.q);
203 
204  return *this;
205  }
206 
211  public: GZ_DEPRECATED(7) Pose3<T> operator-() const
212  {
213  return this->Inverse();
214  }
215 
222  public: GZ_DEPRECATED(7) Pose3<T> operator-(const Pose3<T> &_pose) const
223  {
224  return Pose3<T>(this->CoordPositionSub(_pose),
225  this->CoordRotationSub(_pose.q));
226  }
227 
232  public: GZ_DEPRECATED(7) const Pose3<T> &
233  operator-=(const Pose3<T> &_pose)
234  {
235  this->p = this->CoordPositionSub(_pose);
236  this->q = this->CoordRotationSub(_pose.q);
237 
238  return *this;
239  }
240 
244  public: bool operator==(const Pose3<T> &_pose) const
245  {
246  return this->p == _pose.p && this->q == _pose.q;
247  }
248 
252  public: bool operator!=(const Pose3<T> &_pose) const
253  {
254  return this->p != _pose.p || this->q != _pose.q;
255  }
256 
262  public: Pose3<T> operator*(const Pose3<T> &_pose) const
263  {
264  return Pose3<T>(_pose.CoordPositionAdd(*this), this->q * _pose.q);
265  }
266 
272  public: const Pose3<T> &operator*=(const Pose3<T> &_pose)
273  {
274  *this = *this * _pose;
275  return *this;
276  }
277 
280  public: Pose3<T> &operator=(const Pose3<T> &_pose) = default;
281 
285  public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
286  {
287  Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
288 
289  // result = pose.q + pose.q * this->p * pose.q!
290  tmp = this->q * (tmp * this->q.Inverse());
291 
292  return Vector3<T>(this->p.X() + tmp.X(),
293  this->p.Y() + tmp.Y(),
294  this->p.Z() + tmp.Z());
295  }
296 
300  public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
301  {
302  Quaternion<T> tmp(static_cast<T>(0),
303  this->p.X(), this->p.Y(), this->p.Z());
304 
305  // result = _pose.q + _pose.q * this->p * _pose.q!
306  tmp = _pose.q * (tmp * _pose.q.Inverse());
307 
308  return Vector3<T>(_pose.p.X() + tmp.X(),
309  _pose.p.Y() + tmp.Y(),
310  _pose.p.Z() + tmp.Z());
311  }
312 
316  public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
317  {
318  Quaternion<T> tmp(0,
319  this->p.X() - _pose.p.X(),
320  this->p.Y() - _pose.p.Y(),
321  this->p.Z() - _pose.p.Z());
322 
323  tmp = _pose.q.Inverse() * (tmp * _pose.q);
324  return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
325  }
326 
330  public: Quaternion<T> CoordRotationAdd(const Quaternion<T> &_rot) const
331  {
332  return Quaternion<T>(_rot * this->q);
333  }
334 
339  const Quaternion<T> &_rot) const
340  {
341  Quaternion<T> result(_rot.Inverse() * this->q);
342  result.Normalize();
343  return result;
344  }
345 
349  // \return The inverse pose.
350  public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
351  {
352  Quaternion<T> qt;
353  Pose3<T> a;
354 
355  a.q = this->q.Inverse() * _b.q;
356  qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
357  qt = qt * a.q.Inverse();
358  a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
359 
360  return a;
361  }
362 
365  public: void Reset()
366  {
367  // set the position to zero
368  this->p.Set();
369  this->q = Quaternion<T>::Identity;
370  }
371 
376  {
377  Pose3<T> a = *this;
378  a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
379  +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
380  +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
381  a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
382  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
383  +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
384  a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
385  +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
386  +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
387  return a;
388  }
389 
392  public: void Round(int _precision)
393  {
394  this->q.Round(_precision);
395  this->p.Round(_precision);
396  }
397 
400  public: inline const Vector3<T> &Pos() const
401  {
402  return this->p;
403  }
404 
407  public: inline Vector3<T> &Pos()
408  {
409  return this->p;
410  }
411 
416  public: inline const T X() const
417  {
418  return this->p.X();
419  }
420 
422  public: inline void SetX(T x)
423  {
424  this->p.X() = x;
425  }
426 
431  public: inline const T Y() const
432  {
433  return this->p.Y();
434  }
435 
437  public: inline void SetY(T y)
438  {
439  this->p.Y() = y;
440  }
441 
446  public: inline const T Z() const
447  {
448  return this->p.Z();
449  }
450 
452  public: inline void SetZ(T z)
453  {
454  this->p.Z() = z;
455  }
456 
459  public: inline const Quaternion<T> &Rot() const
460  {
461  return this->q;
462  }
463 
466  public: inline Quaternion<T> &Rot()
467  {
468  return this->q;
469  }
470 
475  public: inline const T Roll() const
476  {
477  return this->q.Roll();
478  }
479 
484  public: inline const T Pitch() const
485  {
486  return this->q.Pitch();
487  }
488 
493  public: inline const T Yaw() const
494  {
495  return this->q.Yaw();
496  }
497 
502  public: friend std::ostream &operator<<(
503  std::ostream &_out, const gz::math::Pose3<T> &_pose)
504  {
505  _out << _pose.Pos() << " " << _pose.Rot();
506  return _out;
507  }
508 
513  public: friend std::istream &operator>>(
514  std::istream &_in, gz::math::Pose3<T> &_pose)
515  {
516  // Skip white spaces
517  _in.setf(std::ios_base::skipws);
518  Vector3<T> pos;
519  Quaternion<T> rot;
520  _in >> pos >> rot;
521  _pose.Set(pos, rot);
522  return _in;
523  }
524 
531  public: bool Equal(const Pose3 &_p, const T &_tol) const
532  {
533  return this->p.Equal(_p.p, _tol) && this->q.Equal(_p.q, _tol);
534  }
535 
537  private: Vector3<T> p;
538 
540  private: Quaternion<T> q;
541  };
542 
543  namespace detail {
544 
545  template<typename T> constexpr Pose3<T> gPose3Zero{};
546 
547  } // namespace detail
548 
549  template<typename T> const Pose3<T> &Pose3<T>::Zero = detail::gPose3Zero<T>;
550 
553 
556  } // namespace GZ_MATH_VERSION_NAMESPACE
557 } // namespace gz::math
558 #endif // GZ_MATH_POSE_HH_