Gazebo Math

API Reference

8.0.0
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::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
120 public: void Set(const Vector3<T> &_pos, const Quaternion<T> &_rot)
121 {
122 this->p = _pos;
123 this->q = _rot;
124 }
125
129 public: void Set(const Vector3<T> &_pos, const Vector3<T> &_rpy)
130 {
131 this->p = _pos;
132 this->q.SetFromEuler(_rpy);
133 }
134
143 public: void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
144 {
145 this->p.Set(_x, _y, _z);
146 this->q.SetFromEuler(math::Vector3<T>(_roll, _pitch, _yaw));
147 }
148
151 public: bool IsFinite() const
152 {
153 return this->p.IsFinite() && this->q.IsFinite();
154 }
155
157 public: inline void Correct()
158 {
159 this->p.Correct();
160 this->q.Correct();
161 }
162
165 public: Pose3<T> Inverse() const
166 {
167 Quaternion<T> inv = this->q.Inverse();
168 return Pose3<T>(inv * (this->p*-1), inv);
169 }
170
174 public: bool operator==(const Pose3<T> &_pose) const
175 {
176 return this->p == _pose.p && this->q == _pose.q;
177 }
178
182 public: bool operator!=(const Pose3<T> &_pose) const
183 {
184 return this->p != _pose.p || this->q != _pose.q;
185 }
186
192 public: Pose3<T> operator*(const Pose3<T> &_pose) const
193 {
194 return Pose3<T>(_pose.CoordPositionAdd(*this), this->q * _pose.q);
195 }
196
202 public: const Pose3<T> &operator*=(const Pose3<T> &_pose)
203 {
204 *this = *this * _pose;
205 return *this;
206 }
207
212 {
213 Quaternion<T> tmp(0.0, _pos.X(), _pos.Y(), _pos.Z());
214
215 // result = pose.q + pose.q * this->p * pose.q!
216 tmp = this->q * (tmp * this->q.Inverse());
217
218 return Vector3<T>(this->p.X() + tmp.X(),
219 this->p.Y() + tmp.Y(),
220 this->p.Z() + tmp.Z());
221 }
222
227 {
228 Quaternion<T> tmp(static_cast<T>(0),
229 this->p.X(), this->p.Y(), this->p.Z());
230
231 // result = _pose.q + _pose.q * this->p * _pose.q!
232 tmp = _pose.q * (tmp * _pose.q.Inverse());
233
234 return Vector3<T>(_pose.p.X() + tmp.X(),
235 _pose.p.Y() + tmp.Y(),
236 _pose.p.Z() + tmp.Z());
237 }
238
242 public: inline Vector3<T> CoordPositionSub(const Pose3<T> &_pose) const
243 {
245 this->p.X() - _pose.p.X(),
246 this->p.Y() - _pose.p.Y(),
247 this->p.Z() - _pose.p.Z());
248
249 tmp = _pose.q.Inverse() * (tmp * _pose.q);
250 return Vector3<T>(tmp.X(), tmp.Y(), tmp.Z());
251 }
252
257 {
258 return Quaternion<T>(_rot * this->q);
259 }
260
265 const Quaternion<T> &_rot) const
266 {
267 Quaternion<T> result(_rot.Inverse() * this->q);
268 result.Normalize();
269 return result;
270 }
271
275 // \return The inverse pose.
276 public: Pose3<T> CoordPoseSolve(const Pose3<T> &_b) const
277 {
279 Pose3<T> a;
280
281 a.q = this->q.Inverse() * _b.q;
282 qt = a.q * Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
283 qt = qt * a.q.Inverse();
284 a.p = _b.p - Vector3<T>(qt.X(), qt.Y(), qt.Z());
285
286 return a;
287 }
288
291 public: void Reset()
292 {
293 // set the position to zero
294 this->p.Set();
295 this->q = Quaternion<T>::Identity;
296 }
297
302 {
303 Pose3<T> a = *this;
304 a.p.X((1.0 - 2.0*_q.Y()*_q.Y() - 2.0*_q.Z()*_q.Z()) * this->p.X()
305 +(2.0*(_q.X()*_q.Y()+_q.W()*_q.Z())) * this->p.Y()
306 +(2.0*(_q.X()*_q.Z()-_q.W()*_q.Y())) * this->p.Z());
307 a.p.Y((2.0*(_q.X()*_q.Y()-_q.W()*_q.Z())) * this->p.X()
308 +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Z()*_q.Z()) * this->p.Y()
309 +(2.0*(_q.Y()*_q.Z()+_q.W()*_q.X())) * this->p.Z());
310 a.p.Z((2.0*(_q.X()*_q.Z()+_q.W()*_q.Y())) * this->p.X()
311 +(2.0*(_q.Y()*_q.Z()-_q.W()*_q.X())) * this->p.Y()
312 +(1.0 - 2.0*_q.X()*_q.X() - 2.0*_q.Y()*_q.Y()) * this->p.Z());
313 return a;
314 }
315
318 public: void Round(int _precision)
319 {
320 this->q.Round(_precision);
321 this->p.Round(_precision);
322 }
323
326 public: inline const Vector3<T> &Pos() const
327 {
328 return this->p;
329 }
330
333 public: inline Vector3<T> &Pos()
334 {
335 return this->p;
336 }
337
342 public: inline const T X() const
343 {
344 return this->p.X();
345 }
346
348 public: inline void SetX(T x)
349 {
350 this->p.X() = x;
351 }
352
357 public: inline const T Y() const
358 {
359 return this->p.Y();
360 }
361
363 public: inline void SetY(T y)
364 {
365 this->p.Y() = y;
366 }
367
372 public: inline const T Z() const
373 {
374 return this->p.Z();
375 }
376
378 public: inline void SetZ(T z)
379 {
380 this->p.Z() = z;
381 }
382
385 public: inline const Quaternion<T> &Rot() const
386 {
387 return this->q;
388 }
389
392 public: inline Quaternion<T> &Rot()
393 {
394 return this->q;
395 }
396
401 public: inline const T Roll() const
402 {
403 return this->q.Roll();
404 }
405
410 public: inline const T Pitch() const
411 {
412 return this->q.Pitch();
413 }
414
419 public: inline const T Yaw() const
420 {
421 return this->q.Yaw();
422 }
423
428 public: friend std::ostream &operator<<(
430 {
431 _out << _pose.Pos() << " " << _pose.Rot();
432 return _out;
433 }
434
439 public: friend std::istream &operator>>(
441 {
442 // Skip white spaces
443 _in.setf(std::ios_base::skipws);
446 _in >> pos >> rot;
447 _pose.Set(pos, rot);
448 return _in;
449 }
450
457 public: bool Equal(const Pose3 &_p, const T &_tol) const
458 {
459 return this->p.Equal(_p.p, _tol) && this->q.Equal(_p.q, _tol);
460 }
461
463 private: Vector3<T> p;
464
466 private: Quaternion<T> q;
467 };
468
469 namespace detail {
470
471 template<typename T> constexpr Pose3<T> gPose3Zero{};
472
473 } // namespace detail
474
475 template<typename T> const Pose3<T> &Pose3<T>::Zero = detail::gPose3Zero<T>;
476
479
482 } // namespace GZ_MATH_VERSION_NAMESPACE
483} // namespace gz::math
484#endif // GZ_MATH_POSE_HH_