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 #include "gazebo/util/system.hh"
30 
31 namespace gazebo
32 {
33  namespace math
34  {
37 
41  {
43  public: static const Pose Zero;
44 
46  public: Pose();
47 
51  public: Pose(const Vector3 &_pos, const Quaternion &_rot);
52 
60  public: Pose(double _x, double _y, double _z,
61  double _roll, double _pitch, double _yaw);
62 
65  public: Pose(const Pose &_pose);
66 
68  public: virtual ~Pose();
69 
73  public: void Set(const Vector3 &_pos, const Quaternion &_rot);
74 
78  public: void Set(const Vector3 &_pos, const Vector3 &_rpy);
79 
87  public: void Set(double _x, double _y, double _z,
88  double _roll, double _pitch, double _yaw);
89 
91  public: bool IsFinite() const;
92 
94  public: inline void Correct()
95  {
96  this->pos.Correct();
97  this->rot.Correct();
98  }
99 
102  public: Pose GetInverse() const;
103 
110  public: Pose operator+(const Pose &_pose) const;
111 
115  public: const Pose &operator+=(const Pose &_pose);
116 
121  public: inline Pose operator-() const
122  {
123  return Pose() - *this;
124  }
125 
132  public: inline Pose operator-(const Pose &_pose) const
133  {
134  return Pose(this->CoordPositionSub(_pose),
135  this->CoordRotationSub(_pose.rot));
136  }
137 
141  public: const Pose &operator-=(const Pose &_pose);
142 
146  public: bool operator ==(const Pose &_pose) const;
147 
151  public: bool operator!=(const Pose &_pose) const;
152 
156  public: Pose operator*(const Pose &_pose);
157 
160  public: Pose &operator=(const Pose &_pose);
161 
165  public: Vector3 CoordPositionAdd(const Vector3 &_pos) const;
166 
170  public: Vector3 CoordPositionAdd(const Pose &_pose) const;
171 
175  public: inline Vector3 CoordPositionSub(const Pose &_pose) const
176  {
177  Quaternion tmp(0.0,
178  this->pos.x - _pose.pos.x,
179  this->pos.y - _pose.pos.y,
180  this->pos.z - _pose.pos.z);
181 
182  tmp = _pose.rot.GetInverse() * (tmp * _pose.rot);
183  return Vector3(tmp.x, tmp.y, tmp.z);
184  }
185 
189  public: Quaternion CoordRotationAdd(const Quaternion &_rot) const;
190 
194  public: inline Quaternion CoordRotationSub(const Quaternion &_rot) const
195  {
196  Quaternion result(_rot.GetInverse() * this->rot);
197  result.Normalize();
198  return result;
199  }
200 
204  public: Pose CoordPoseSolve(const Pose &_b) const;
205 
207  public: void Reset();
208 
212  public: Pose RotatePositionAboutOrigin(const Quaternion &_rot) const;
213 
216  public: void Round(int _precision);
217 
222  public: friend std::ostream &operator<<(std::ostream &_out,
223  const gazebo::math::Pose &_pose)
224  {
225  _out << _pose.pos << " " << _pose.rot;
226  return _out;
227  }
228 
233  public: friend std::istream &operator>>(std::istream &_in,
234  gazebo::math::Pose &_pose)
235  {
236  // Skip white spaces
237  _in.setf(std::ios_base::skipws);
238  _in >> _pose.pos >> _pose.rot;
239  return _in;
240  }
241 
243  public: Vector3 pos;
244 
246  public: Quaternion rot;
247  };
249  }
250 }
251 #endif