Class
List
Hierarchy
Modules
Common
Events
Math
Messages
Physics
Rendering
Sensors
Transport
Links
Gazebo Website
Wiki
Tutorials
Download
All
Classes
Namespaces
Files
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Macros
Groups
Pages
gazebo
math
Pose.hh
Go to the documentation of this file.
1
/*
2
* Copyright 2012 Nate Koenig
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
/* Desc: External interfaces for Gazebo
18
* Author: Nate Koenig
19
* Date: 03 Apr 2007
20
*/
21
22
#ifndef _POSE_HH_
23
#define _POSE_HH_
24
25
#include <iostream>
26
27
#include "
math/Vector3.hh
"
28
#include "
math/Quaternion.hh
"
29
30
namespace
gazebo
31
{
32
namespace
math
33
{
36
39
class
Pose
40
{
42
public
:
static
const
Pose
Zero
;
43
45
public
:
Pose
();
46
50
public
:
Pose
(
const
Vector3
&_pos,
const
Quaternion
&_rot);
51
59
public
:
Pose
(
double
_x,
double
_y,
double
_z,
60
double
_roll,
double
_pitch,
double
_yaw);
61
64
public
:
Pose
(
const
Pose
&_pose);
65
67
public
:
virtual
~Pose
();
68
72
public
:
void
Set
(
const
Vector3
&_pos,
const
Quaternion
&_rot);
73
81
public
:
void
Set
(
double
_x,
double
_y,
double
_z,
82
double
_roll,
double
_pitch,
double
_yaw);
83
85
public
:
bool
IsFinite
()
const
;
86
88
public
:
inline
void
Correct
()
89
{
90
this->
pos
.
Correct
();
91
this->
rot
.
Correct
();
92
}
93
96
public
:
Pose
GetInverse
()
const
;
97
101
public
:
Pose
operator+
(
const
Pose
&_pose)
const
;
102
106
public
:
const
Pose
&
operator+=
(
const
Pose
&_pose);
107
111
public
:
inline
Pose
operator-
(
const
Pose
&_pose)
const
112
{
113
return
Pose
(this->
CoordPositionSub
(_pose),
114
this->
CoordRotationSub
(_pose.
rot
));
115
}
116
120
public
:
const
Pose
&
operator-=
(
const
Pose
&_pose);
121
125
public
:
bool
operator ==
(
const
Pose
&_pose)
const
;
126
130
public
:
bool
operator!=
(
const
Pose
&_pose)
const
;
131
135
public
:
Pose
operator*
(
const
Pose
&_pose);
136
140
public
:
Vector3
CoordPositionAdd
(
const
Vector3
&_pos)
const
;
141
145
public
:
Vector3
CoordPositionAdd
(
const
Pose
&_pose)
const
;
146
150
public
:
inline
Vector3
CoordPositionSub
(
const
Pose
&_pose)
const
151
{
152
Quaternion
tmp(0.0,
153
this->
pos
.
x
- _pose.
pos
.
x
,
154
this->pos.y - _pose.
pos
.
y
,
155
this->pos.z - _pose.
pos
.
z
);
156
157
tmp = _pose.
rot
.
GetInverse
() * (tmp * _pose.
rot
);
158
return
Vector3
(tmp.
x
, tmp.
y
, tmp.
z
);
159
}
160
164
public
:
Quaternion
CoordRotationAdd
(
const
Quaternion
&_rot)
const
;
165
169
public
:
inline
Quaternion
CoordRotationSub
(
const
Quaternion
&_rot)
const
170
{
171
Quaternion
result(_rot.
GetInverse
() * this->
rot
);
172
result.
Normalize
();
173
return
result;
174
}
175
176
180
public
:
Pose
CoordPoseSolve
(
const
Pose
&_b)
const
;
181
183
public
:
void
Reset
();
184
188
public
:
Pose
RotatePositionAboutOrigin
(
const
Quaternion
&_rot)
const
;
189
192
public
:
void
Round
(
int
_precision);
193
195
public
:
Vector3
pos
;
196
198
public
:
Quaternion
rot
;
199
204
public
:
friend
std::ostream &
operator<<
(std::ostream &_out,
205
const
gazebo::math::Pose
&_pose)
206
{
207
_out << _pose.
pos
<<
" "
<< _pose.
rot
;
208
return
_out;
209
}
210
215
public
:
friend
std::istream &
operator>>
(std::istream &_in,
216
gazebo::math::Pose
&_pose)
217
{
218
// Skip white spaces
219
_in.setf(std::ios_base::skipws);
220
_in >> _pose.
pos
>> _pose.
rot
;
221
return
_in;
222
}
223
};
225
}
226
}
227
#endif