Gazebo Math
API Reference
7.4.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
include
gz
math
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
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
332
public
:
Quaternion<T>
CoordRotationAdd
(
const
Quaternion<T>
&_rot)
const
333
{
334
return
Quaternion<T>
(_rot * this->q);
335
}
336
340
public
:
inline
Quaternion<T>
CoordRotationSub
(
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
377
public
:
Pose3<T>
RotatePositionAboutOrigin
(
const
Quaternion<T>
&_q)
const
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
554
typedef
Pose3<double>
Pose3d
;
555
557
typedef
Pose3<float>
Pose3f
;
558
}
559
}
560
}
561
#endif