All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Pose.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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 "gazebo/math/Vector3.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 
77  public: void Set(const Vector3 &_pos, const Vector3 &_rpy);
78 
86  public: void Set(double _x, double _y, double _z,
87  double _roll, double _pitch, double _yaw);
88 
90  public: bool IsFinite() const;
91 
93  public: inline void Correct()
94  {
95  this->pos.Correct();
96  this->rot.Correct();
97  }
98 
101  public: Pose GetInverse() const;
102 
109  public: Pose operator+(const Pose &_pose) const;
110 
114  public: const Pose &operator+=(const Pose &_pose);
115 
120  public: inline Pose operator-() const
121  {
122  return Pose() - *this;
123  }
124 
131  public: inline Pose operator-(const Pose &_pose) const
132  {
133  return Pose(this->CoordPositionSub(_pose),
134  this->CoordRotationSub(_pose.rot));
135  }
136 
140  public: const Pose &operator-=(const Pose &_pose);
141 
145  public: bool operator ==(const Pose &_pose) const;
146 
150  public: bool operator!=(const Pose &_pose) const;
151 
155  public: Pose operator*(const Pose &_pose);
156 
159  public: Pose &operator=(const Pose &_pose);
160 
164  public: Vector3 CoordPositionAdd(const Vector3 &_pos) const;
165 
169  public: Vector3 CoordPositionAdd(const Pose &_pose) const;
170 
174  public: inline Vector3 CoordPositionSub(const Pose &_pose) const
175  {
176  Quaternion tmp(0.0,
177  this->pos.x - _pose.pos.x,
178  this->pos.y - _pose.pos.y,
179  this->pos.z - _pose.pos.z);
180 
181  tmp = _pose.rot.GetInverse() * (tmp * _pose.rot);
182  return Vector3(tmp.x, tmp.y, tmp.z);
183  }
184 
188  public: Quaternion CoordRotationAdd(const Quaternion &_rot) const;
189 
193  public: inline Quaternion CoordRotationSub(const Quaternion &_rot) const
194  {
195  Quaternion result(_rot.GetInverse() * this->rot);
196  result.Normalize();
197  return result;
198  }
199 
203  public: Pose CoordPoseSolve(const Pose &_b) const;
204 
206  public: void Reset();
207 
211  public: Pose RotatePositionAboutOrigin(const Quaternion &_rot) const;
212 
215  public: void Round(int _precision);
216 
221  public: friend std::ostream &operator<<(std::ostream &_out,
222  const gazebo::math::Pose &_pose)
223  {
224  _out << _pose.pos << " " << _pose.rot;
225  return _out;
226  }
227 
232  public: friend std::istream &operator>>(std::istream &_in,
233  gazebo::math::Pose &_pose)
234  {
235  // Skip white spaces
236  _in.setf(std::ios_base::skipws);
237  _in >> _pose.pos >> _pose.rot;
238  return _in;
239  }
240 
242  public: Vector3 pos;
243 
245  public: Quaternion rot;
246  };
248  }
249 }
250 #endif