Class
List
Hierarchy
Modules
Common
Events
Math
Messages
Physics
Rendering
Sensors
Transport
Links
Gazebo Website
Wiki
Tutorials
Download
Report Documentation Issues
•
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 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
/* 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
110
public
:
inline
Pose
operator-
()
const
111
{
112
return
Pose
() - *
this
;
113
}
114
118
public
:
inline
Pose
operator-
(
const
Pose
&_pose)
const
119
{
120
return
Pose
(this->
CoordPositionSub
(_pose),
121
this->
CoordRotationSub
(_pose.
rot
));
122
}
123
127
public
:
const
Pose
&
operator-=
(
const
Pose
&_pose);
128
132
public
:
bool
operator ==
(
const
Pose
&_pose)
const
;
133
137
public
:
bool
operator!=
(
const
Pose
&_pose)
const
;
138
142
public
:
Pose
operator*
(
const
Pose
&_pose);
143
147
public
:
Vector3
CoordPositionAdd
(
const
Vector3
&_pos)
const
;
148
152
public
:
Vector3
CoordPositionAdd
(
const
Pose
&_pose)
const
;
153
157
public
:
inline
Vector3
CoordPositionSub
(
const
Pose
&_pose)
const
158
{
159
Quaternion
tmp(0.0,
160
this->
pos
.
x
- _pose.
pos
.
x
,
161
this->pos.y - _pose.
pos
.
y
,
162
this->pos.z - _pose.
pos
.
z
);
163
164
tmp = _pose.
rot
.
GetInverse
() * (tmp * _pose.
rot
);
165
return
Vector3
(tmp.
x
, tmp.
y
, tmp.
z
);
166
}
167
171
public
:
Quaternion
CoordRotationAdd
(
const
Quaternion
&_rot)
const
;
172
176
public
:
inline
Quaternion
CoordRotationSub
(
const
Quaternion
&_rot)
const
177
{
178
Quaternion
result(_rot.
GetInverse
() * this->
rot
);
179
result.
Normalize
();
180
return
result;
181
}
182
183
187
public
:
Pose
CoordPoseSolve
(
const
Pose
&_b)
const
;
188
190
public
:
void
Reset
();
191
195
public
:
Pose
RotatePositionAboutOrigin
(
const
Quaternion
&_rot)
const
;
196
199
public
:
void
Round
(
int
_precision);
200
202
public
:
Vector3
pos
;
203
205
public
:
Quaternion
rot
;
206
211
public
:
friend
std::ostream &
operator<<
(std::ostream &_out,
212
const
gazebo::math::Pose
&_pose)
213
{
214
_out << _pose.
pos
<<
" "
<< _pose.
rot
;
215
return
_out;
216
}
217
222
public
:
friend
std::istream &
operator>>
(std::istream &_in,
223
gazebo::math::Pose
&_pose)
224
{
225
// Skip white spaces
226
_in.setf(std::ios_base::skipws);
227
_in >> _pose.
pos
>> _pose.
rot
;
228
return
_in;
229
}
230
};
232
}
233
}
234
#endif