Pose.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 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 _GAZEBO_POSE_HH_
18 #define _GAZEBO_POSE_HH_
19 
20 #include <iostream>
21 
22 #include <ignition/math/Pose3.hh>
23 
24 #include "gazebo/math/Vector3.hh"
26 #include "gazebo/util/system.hh"
27 
28 namespace gazebo
29 {
30  namespace math
31  {
34 
38  {
40  public: static const Pose Zero;
41 
43  public: Pose();
44 
48  public: Pose(const Vector3 &_pos, const Quaternion &_rot);
49 
57  public: Pose(double _x, double _y, double _z,
58  double _roll, double _pitch, double _yaw);
59 
62  public: Pose(const Pose &_pose);
63 
66  public: Pose(const ignition::math::Pose3d &_pose);
67 
69  public: virtual ~Pose();
70 
74  public: void Set(const Vector3 &_pos, const Quaternion &_rot);
75 
79  public: void Set(const Vector3 &_pos, const Vector3 &_rpy);
80 
88  public: void Set(double _x, double _y, double _z,
89  double _roll, double _pitch, double _yaw);
90 
92  public: bool IsFinite() const;
93 
95  public: inline void Correct()
96  {
97  this->pos.Correct();
98  this->rot.Correct();
99  }
100 
103  public: Pose GetInverse() const;
104 
111  public: Pose operator+(const Pose &_pose) const;
112 
116  public: const Pose &operator+=(const Pose &_pose);
117 
122  public: inline Pose operator-() const
123  {
124  return Pose() - *this;
125  }
126 
133  public: inline Pose operator-(const Pose &_pose) const
134  {
135  return Pose(this->CoordPositionSub(_pose),
136  this->CoordRotationSub(_pose.rot));
137  }
138 
142  public: const Pose &operator-=(const Pose &_pose);
143 
147  public: bool operator ==(const Pose &_pose) const;
148 
152  public: bool operator!=(const Pose &_pose) const;
153 
157  public: Pose operator*(const Pose &_pose);
158 
161  public: Pose &operator=(const Pose &_pose);
162 
165  public: Pose &operator=(const ignition::math::Pose3d &_pose);
166 
170  public: Vector3 CoordPositionAdd(const Vector3 &_pos) const;
171 
175  public: Vector3 CoordPositionAdd(const Pose &_pose) const;
176 
180  public: inline Vector3 CoordPositionSub(const Pose &_pose) const
181  {
182  Quaternion tmp(0.0,
183  this->pos.x - _pose.pos.x,
184  this->pos.y - _pose.pos.y,
185  this->pos.z - _pose.pos.z);
186 
187  tmp = _pose.rot.GetInverse() * (tmp * _pose.rot);
188  return Vector3(tmp.x, tmp.y, tmp.z);
189  }
190 
194  public: Quaternion CoordRotationAdd(const Quaternion &_rot) const;
195 
199  public: inline Quaternion CoordRotationSub(const Quaternion &_rot) const
200  {
201  Quaternion result(_rot.GetInverse() * this->rot);
202  result.Normalize();
203  return result;
204  }
205 
209  public: Pose CoordPoseSolve(const Pose &_b) const;
210 
212  public: void Reset();
213 
217  public: Pose RotatePositionAboutOrigin(const Quaternion &_rot) const;
218 
221  public: void Round(int _precision);
222 
225  public: ignition::math::Pose3d Ign() const;
226 
231  public: friend std::ostream &operator<<(std::ostream &_out,
232  const gazebo::math::Pose &_pose)
233  {
234  _out << _pose.pos << " " << _pose.rot;
235  return _out;
236  }
237 
242  public: friend std::istream &operator>>(std::istream &_in,
243  gazebo::math::Pose &_pose)
244  {
245  // Skip white spaces
246  _in.setf(std::ios_base::skipws);
247  _in >> _pose.pos >> _pose.rot;
248  return _in;
249  }
250 
252  public: Vector3 pos;
253 
255  public: Quaternion rot;
256  };
258  }
259 }
260 #endif
double z
z value of the quaternion
Definition: Quaternion.hh:386
Encapsulates a position and rotation in three space.
Definition: Pose.hh:37
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
Pose operator-() const
Negation operator A is the transform from O to P in frame O then -A is transform from P to O specifie...
Definition: Pose.hh:122
double x
X location.
Definition: Vector3.hh:311
Quaternion CoordRotationSub(const Quaternion &_rot) const
Subtract one rotation from another: result = this->rot - rot.
Definition: Pose.hh:199
double z
Z location.
Definition: Vector3.hh:317
Pose operator-(const Pose &_pose) const
Subtraction operator A is the transform from O to P in frame O B is the transform from O to Q in fram...
Definition: Pose.hh:133
#define GZ_MATH_VISIBLE
Definition: system.hh:134
Vector3 pos
The position.
Definition: Pose.hh:252
friend std::ostream & operator<<(std::ostream &_out, const gazebo::math::Pose &_pose)
Stream insertion operator.
Definition: Pose.hh:231
A quaternion class.
Definition: Quaternion.hh:42
friend std::istream & operator>>(std::istream &_in, gazebo::math::Pose &_pose)
Stream extraction operator.
Definition: Pose.hh:242
Quaternion rot
The rotation.
Definition: Pose.hh:255
GAZEBO_VISIBLE void Set(common::Image &_img, const msgs::Image &_msg)
Convert a msgs::Image to a common::Image.
double y
y value of the quaternion
Definition: Quaternion.hh:383
Quaternion GetInverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:100
double x
x value of the quaternion
Definition: Quaternion.hh:380
Vector3 CoordPositionSub(const Pose &_pose) const
Subtract one position from another: result = this - pose.
Definition: Pose.hh:180
static const Pose Zero
math::Pose(0, 0, 0, 0, 0, 0)
Definition: Pose.hh:40
void Correct()
Fix any nan values.
Definition: Pose.hh:95
double y
Y location.
Definition: Vector3.hh:314
void Normalize()
Normalize the quaternion.