Gazebo Math

API Reference

7.4.0
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
24namespace gz
25{
26 namespace math
27 {
28 // Inline bracket to help doxygen filtering.
29 inline namespace GZ_MATH_VERSION_NAMESPACE {
30 //
72 template<typename T>
73 class Pose3
74 {
77 public: static const Pose3<T> &Zero;
78
81 public: Pose3() = default;
82
86 public: Pose3(const Vector3<T> &_pos, const Quaternion<T> &_rot)
87 : p(_pos), q(_rot)
88 {
89 }
90
99 public: Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
100 : p(_x, _y, _z), q(_roll, _pitch, _yaw)
101 {
102 }
103
114 public: Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
115 : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
116 {
117 }
118
121 public: Pose3(const Pose3<T> &_pose) = default;
122
124 public: ~Pose3() = default;
125
129 public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
130 {
131 this->p = _pos;
132 this->q = _rot;
133 }
134
138 public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
139 {
140 this->p = _pos;
141 this->q.SetFromEuler(_rpy);
142 }
143
152 public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
153 {
154 this->p.Set(_x, _y, _z);
155 this->q.SetFromEuler(math::Vector3<T>(_roll, _pitch, _yaw));
156 }
157
160 public: bool IsFinite() const
161 {
162 return this->p.IsFinite() && this->q.IsFinite();
163 }
164
166 public: inline void Correct()
167 {
168 this->p.Correct();
169 this->q.Correct();
170 }
171
174 public: Pose3<T> Inverse() const
175 {
176 Quaternion<T> inv = this->q.Inverse();
177 return Pose3<T>(inv * (this->p*-1), inv);
178 }
179
186 public: GZ_DEPRECATED(7) Pose3<T> operator+(const Pose3<T> &_pose) const
187 {
188 Pose3<T> result;
189
190 result.p = this->CoordPositionAdd(_pose);
191 result.q = this->CoordRotationAdd(_pose.q);
192
193 return result;
194 }
195
200 public: GZ_DEPRECATED(7) const Pose3<T> &
201 operator+=(const Pose3<T> &_pose)
202 {
203 this->p = this->CoordPositionAdd(_pose);
204 this->q = this->CoordRotationAdd(_pose.q);
205
206 return *this;
207 }
208
213 public: GZ_DEPRECATED(7) Pose3<T> operator-() const
214 {
215 return this->Inverse();
216 }
217
224 public: GZ_DEPRECATED(7) Pose3<T> operator-(const Pose3<T> &_pose) const
225 {
226 return Pose3<T>(this->CoordPositionSub(_pose),
227 this->CoordRotationSub(_pose.q));
228 }
229
234 public: GZ_DEPRECATED(7) const Pose3<T> &
235 operator-=(const Pose3<T> &_pose)
236 {
237 this->p = this->CoordPositionSub(_pose);
238 this->q = this->CoordRotationSub(_pose.q);
239
240 return *this;
241 }
242
246 public: bool operator==(const Pose3<T> &_pose) const
247 {
248 return this->p == _pose.p && this->q == _pose.q;
249 }
250
254 public: bool operator!=(const Pose3<T> &_pose) const
255 {
256 return this->p != _pose.p || this->q != _pose.q;
257 }
258
264 public: Pose3<T> operator*(const Pose3<T> &_pose) const
265 {
266 return Pose3<T>(_pose.CoordPositionAdd(*this), this->q * _pose.q);
267 }
268
274 public: const Pose3<T> &operator*=(const Pose3<T> &_pose)
275 {
276 *this = *this * _pose;
277 return *this;
278 }
279
282 public: Pose3<T> &operator=(const Pose3<T> &_pose) = default;
283
287 public: Vector3<T> CoordPositionAdd(const Vector3<T> &_pos) const
288 {
289 Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
290
291 // result = pose.q + pose.q * this->p * pose.q!
292 tmp = this->q * (tmp * this->q.Inverse());
293
294 return Vector3<T>(this->p.X() + tmp.X(),
295 this->p.Y() + tmp.Y(),
296 this->p.Z() + tmp.Z());
297 }
298
302 public: Vector3<T> CoordPositionAdd(const Pose3<T> &_pose) const
303 {
304 Quaternion<T> tmp(static_cast<T>(0),
305 this->p.X(), this->p.Y(), this->p.Z());
306
307 // result = _pose.q + _pose.q * this->p * _pose.q!
308 tmp = _pose.q * (tmp * _pose.q.Inverse());
309
310 return Vector3<T>(_pose.p.X() + tmp.X(),
311 _pose.p.Y() + tmp.Y(),
312 _pose.p.Z() + tmp.Z());
313 }
314
318 public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
319 {
320 Quaternion<T> tmp(0,
321 this->p.X() - _pose.p.X(),
322 this->p.Y() - _pose.p.Y(),
323 this->p.Z() - _pose.p.Z());
324
325 tmp = _pose.q.Inverse() * (tmp * _pose.q);
326 return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
327 }
328
333 {
334 return Quaternion<T>(_rot * this->q);
335 }
336
341 const Quaternion<T> &_rot) const
342 {
343 Quaternion<T> result(_rot.Inverse() * this->q);
344 result.Normalize();
345 return result;
346 }
347
351 // \return The inverse pose.
352 public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
353 {
354 Quaternion<T> qt;
355 Pose3<T> a;
356
357 a.q = this->q.Inverse() * _b.q;
358 qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
359 qt = qt * a.q.Inverse();
360 a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
361
362 return a;
363 }
364
367 public: void Reset()
368 {
369 // set the position to zero
370 this->p.Set();
371 this->q = Quaternion<T>::Identity;
372 }
373
378 {
379 Pose3<T> a = *this;
380 a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
381 +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
382 +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
383 a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
384 +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
385 +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
386 a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
387 +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
388 +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
389 return a;
390 }
391
394 public: void Round(int _precision)
395 {
396 this->q.Round(_precision);
397 this->p.Round(_precision);
398 }
399
402 public: inline const Vector3<T> &Pos() const
403 {
404 return this->p;
405 }
406
409 public: inline Vector3<T> &Pos()
410 {
411 return this->p;
412 }
413
418 public: inline const T X() const
419 {
420 return this->p.X();
421 }
422
424 public: inline void SetX(T x)
425 {
426 this->p.X() = x;
427 }
428
433 public: inline const T Y() const
434 {
435 return this->p.Y();
436 }
437
439 public: inline void SetY(T y)
440 {
441 this->p.Y() = y;
442 }
443
448 public: inline const T Z() const
449 {
450 return this->p.Z();
451 }
452
454 public: inline void SetZ(T z)
455 {
456 this->p.Z() = z;
457 }
458
461 public: inline const Quaternion<T> &Rot() const
462 {
463 return this->q;
464 }
465
468 public: inline Quaternion<T> &Rot()
469 {
470 return this->q;
471 }
472
477 public: inline const T Roll() const
478 {
479 return this->q.Roll();
480 }
481
486 public: inline const T Pitch() const
487 {
488 return this->q.Pitch();
489 }
490
495 public: inline const T Yaw() const
496 {
497 return this->q.Yaw();
498 }
499
504 public: friend std::ostream &operator<<(
505 std::ostream &_out, const gz::math::Pose3<T> &_pose)
506 {
507 _out << _pose.Pos() << " " << _pose.Rot();
508 return _out;
509 }
510
515 public: friend std::istream &operator>>(
516 std::istream &_in, gz::math::Pose3<T> &_pose)
517 {
518 // Skip white spaces
519 _in.setf(std::ios_base::skipws);
520 Vector3<T> pos;
521 Quaternion<T> rot;
522 _in >> pos >> rot;
523 _pose.Set(pos, rot);
524 return _in;
525 }
526
533 public: bool Equal(const Pose3 &_p, const T &_tol) const
534 {
535 return this->p.Equal(_p.p, _tol) && this->q.Equal(_p.q, _tol);
536 }
537
539 private: Vector3<T> p;
540
542 private: Quaternion<T> q;
543 };
544
545 namespace detail {
546
547 template<typename T> constexpr Pose3<T> gPose3Zero{};
548
549 } // namespace detail
550
551 template<typename T> const Pose3<T> &Pose3<T>::Zero = detail::gPose3Zero<T>;
552
555
558 }
559 }
560}
561#endif