Gazebo Math
API Reference
8.0.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
src
gz-math
include
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
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
211
public
:
Vector3<T>
CoordPositionAdd
(
const
Vector3<T>
&
_pos
)
const
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
226
public
:
Vector3<T>
CoordPositionAdd
(
const
Pose3<T>
&
_pose
)
const
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
{
244
Quaternion<T>
tmp
(0,
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
256
public
:
Quaternion<T>
CoordRotationAdd
(
const
Quaternion<T>
&
_rot
)
const
257
{
258
return
Quaternion<T>
(
_rot
* this->q);
259
}
260
264
public
:
inline
Quaternion<T>
CoordRotationSub
(
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
{
278
Quaternion<T>
qt
;
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
301
public
:
Pose3<T>
RotatePositionAboutOrigin
(
const
Quaternion<T>
&
_q
)
const
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<<
(
429
std::ostream
&
_out
,
const
gz::math::Pose3<T>
&
_pose
)
430
{
431
_out
<<
_pose
.Pos() <<
" "
<<
_pose
.Rot();
432
return
_out
;
433
}
434
439
public
:
friend
std::istream
&
operator>>
(
440
std::istream
&
_in
,
gz::math::Pose3<T>
&
_pose
)
441
{
442
// Skip white spaces
443
_in
.setf(std::ios_base::skipws);
444
Vector3<T>
pos
;
445
Quaternion<T>
rot
;
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
478
typedef
Pose3<double>
Pose3d
;
479
481
typedef
Pose3<float>
Pose3f
;
482
}
// namespace GZ_MATH_VERSION_NAMESPACE
483
}
// namespace gz::math
484
#endif
// GZ_MATH_POSE_HH_