Klamp't  0.8.1
urdf_pose.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Wim Meeussen */
36 
37 #ifndef URDF_INTERFACE_POSE_H
38 #define URDF_INTERFACE_POSE_H
39 
40 #include <string>
41 #include <sstream>
42 #include <vector>
43 #include <math.h>
44 #include <KrisLibrary/utils/stringutils.h>
45 #include <KrisLibrary/utils/AnyValue.h>
46 #include "urdf_exception.h"
47 
48 #ifndef M_PI
49 #define M_PI 3.14159265358979323846
50 #endif
51 
52 namespace urdf{
53 
54 class Vector3
55 {
56 public:
57  Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
58  Vector3() {this->clear();};
59  double x;
60  double y;
61  double z;
62 
63  void set(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
64  void set(const Vector3& vec) {this->x=vec.x;this->y=vec.y;this->z=vec.z;};
65  void clear() {this->x=this->y=this->z=0.0;};
66  void init(const std::string &vector_str)
67  {
68  this->clear();
69  std::vector<std::string> pieces;
70  std::vector<double> xyz;
71  pieces = Split( vector_str, " ");
72  for (unsigned int i = 0; i < pieces.size(); ++i){
73  if (pieces[i] != ""){
74  double val;
75  if(LexicalCast(pieces[i],val))
76  xyz.push_back(val);
77  else
78  throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
79  }
80  }
81 
82  if (xyz.size() != 3)
83  throw ParseError("Parser found " + LexicalCast(xyz.size()) + " elements but 3 expected while parsing vector [" + vector_str + "]");
84 
85  this->x = xyz[0];
86  this->y = xyz[1];
87  this->z = xyz[2];
88  }
89 
90  Vector3 operator+(Vector3 vec)
91  {
92  return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
93  };
94 };
95 
96 class Rotation
97 {
98 public:
99  Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
100  Rotation() {this->clear();};
101 
102  void set(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
103  void set(const Rotation& rot) {this->x=rot.x;this->y=rot.y;this->z=rot.z;this->w=rot.w;};
104  void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const
105  {
106  quat_x = this->x;
107  quat_y = this->y;
108  quat_z = this->z;
109  quat_w = this->w;
110  };
111  void getRPY(double &roll,double &pitch,double &yaw) const
112  {
113  double sqw;
114  double sqx;
115  double sqy;
116  double sqz;
117 
118  sqx = this->x * this->x;
119  sqy = this->y * this->y;
120  sqz = this->z * this->z;
121  sqw = this->w * this->w;
122 
123  roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
124  double sarg = -2 * (this->x*this->z - this->w*this->y);
125  pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
126  yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
127 
128  };
129  void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
130  {
131  this->x = quat_x;
132  this->y = quat_y;
133  this->z = quat_z;
134  this->w = quat_w;
135  this->normalize();
136  };
137  void setFromRPY(double roll, double pitch, double yaw)
138  {
139  double phi, the, psi;
140 
141  phi = roll / 2.0;
142  the = pitch / 2.0;
143  psi = yaw / 2.0;
144 
145  this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
146  this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
147  this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
148  this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
149 
150  this->normalize();
151  };
152 
153  double x,y,z,w;
154 
155  void init(const std::string &rotation_str)
156  {
157  this->clear();
158  Vector3 rpy;
159  rpy.init(rotation_str);
160  setFromRPY(rpy.x, rpy.y, rpy.z);
161  }
162 
163  void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
164 
165  void normalize()
166  {
167  double s = sqrt(this->x * this->x +
168  this->y * this->y +
169  this->z * this->z +
170  this->w * this->w);
171  if (s == 0.0)
172  {
173  this->x = 0.0;
174  this->y = 0.0;
175  this->z = 0.0;
176  this->w = 1.0;
177  }
178  else
179  {
180  this->x /= s;
181  this->y /= s;
182  this->z /= s;
183  this->w /= s;
184  }
185  };
186 
187  // Multiplication operator (copied from gazebo)
188  Rotation operator*( const Rotation &qt ) const
189  {
190  Rotation c;
191 
192  c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y;
193  c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x;
194  c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w;
195  c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z;
196 
197  return c;
198  };
201  {
202  Rotation tmp;
203  Vector3 result;
204 
205  tmp.w = 0.0;
206  tmp.x = vec.x;
207  tmp.y = vec.y;
208  tmp.z = vec.z;
209 
210  tmp = (*this) * (tmp * this->GetInverse());
211 
212  result.x = tmp.x;
213  result.y = tmp.y;
214  result.z = tmp.z;
215 
216  return result;
217  };
218  // Get the inverse of this quaternion
219  Rotation GetInverse() const
220  {
221  Rotation q;
222 
223  double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z;
224 
225  if (norm > 0.0)
226  {
227  q.w = this->w / norm;
228  q.x = -this->x / norm;
229  q.y = -this->y / norm;
230  q.z = -this->z / norm;
231  }
232 
233  return q;
234  };
235 
236 
237 };
238 
239 class Pose
240 {
241 public:
242  Pose() { this->clear(); };
243 
244  Vector3 position;
245  Rotation rotation;
246 
247  void clear()
248  {
249  this->position.clear();
250  this->rotation.clear();
251  };
252 };
253 
254 }
255 
256 #endif
Definition: urdf_exception.h:11
Definition: urdf_pose.h:96
Definition: urdf_color.h:46
Vector3 operator*(Vector3 vec) const
Rotate a vector using the quaternion.
Definition: urdf_pose.h:200
Definition: urdf_pose.h:239
Definition: urdf_pose.h:54