orocos_kdl
frames.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  frames.cxx - description
3  -------------------------
4  begin : June 2006
5  copyright : (C) 2006 Erwin Aertbelien
6  email : firstname.lastname@mech.kuleuven.ac.be
7 
8  History (only major changes)( AUTHOR-Description ) :
9 
10  ***************************************************************************
11  * This library is free software; you can redistribute it and/or *
12  * modify it under the terms of the GNU Lesser General Public *
13  * License as published by the Free Software Foundation; either *
14  * version 2.1 of the License, or (at your option) any later version. *
15  * *
16  * This library is distributed in the hope that it will be useful, *
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
19  * Lesser General Public License for more details. *
20  * *
21  * You should have received a copy of the GNU Lesser General Public *
22  * License along with this library; if not, write to the Free Software *
23  * Foundation, Inc., 59 Temple Place, *
24  * Suite 330, Boston, MA 02111-1307 USA *
25  * *
26  ***************************************************************************/
27 
28 #include "frames.hpp"
29 #include "utilities/utility.h"
30 
31 #include <algorithm>
32 
33 namespace KDL {
34 
35 #ifndef KDL_INLINE
36 #include "frames.inl"
37 #endif
38 
39  void Frame::Make4x4(double * d)
40  {
41  int i;
42  int j;
43  for (i=0;i<3;i++) {
44  for (j=0;j<3;j++)
45  d[i*4+j]=M(i,j);
46  d[i*4+3] = p(i);
47  }
48  for (j=0;j<3;j++)
49  d[12+j] = 0.;
50  d[15] = 1;
51  }
52 
53  Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta)
54  // returns Modified Denavit-Hartenberg parameters (According to Craig)
55  {
56  double ct,st,ca,sa;
57  ct = cos(theta);
58  st = sin(theta);
59  sa = sin(alpha);
60  ca = cos(alpha);
61  return Frame(Rotation(
62  ct, -st, 0,
63  st*ca, ct*ca, -sa,
64  st*sa, ct*sa, ca ),
65  Vector(
66  a, -sa*d, ca*d )
67  );
68  }
69 
70  Frame Frame::DH(double a,double alpha,double d,double theta)
71  // returns Denavit-Hartenberg parameters (Non-Modified DH)
72  {
73  double ct,st,ca,sa;
74  ct = cos(theta);
75  st = sin(theta);
76  sa = sin(alpha);
77  ca = cos(alpha);
78  return Frame(Rotation(
79  ct, -st*ca, st*sa,
80  st, ct*ca, -ct*sa,
81  0, sa, ca ),
82  Vector(
83  a*ct, a*st, d )
84  );
85  }
86 
87  double Vector2::Norm(double eps) const
88  {
89  double tmp1 = fabs(data[0]);
90  double tmp2 = fabs(data[1]);
91 
92  if (tmp1 < eps && tmp2 < eps)
93  return 0;
94 
95  if (tmp1 > tmp2) {
96  return tmp1*sqrt(1+sqr(data[1]/data[0]));
97  } else {
98  return tmp2*sqrt(1+sqr(data[0]/data[1]));
99  }
100  }
101  // makes v a unitvector and returns the norm of v.
102  // if v is smaller than eps, Vector(1,0) is returned with norm 0.
103  // if this is not good, check the return value of this method.
104  double Vector2::Normalize(double eps) {
105  double v = this->Norm();
106  if (v < eps) {
107  *this = Vector2(1,0);
108  return 0;
109  } else {
110  *this = (*this)/v;
111  return v;
112  }
113  }
114 
115 
116  // do some effort not to lose precision
117  double Vector::Norm(double eps) const
118  {
119  double tmp1;
120  double tmp2;
121  tmp1 = fabs(data[0]);
122  tmp2 = fabs(data[1]);
123  if (tmp1 >= tmp2) {
124  tmp2=fabs(data[2]);
125  if (tmp1 >= tmp2) {
126  if (tmp1 < eps) {
127  // only to everything exactly zero case, all other are handled correctly
128  return 0;
129  }
130  return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0]));
131  } else {
132  return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
133  }
134  } else {
135  tmp1=fabs(data[2]);
136  if (tmp2 > tmp1) {
137  return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1]));
138  } else {
139  return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
140  }
141  }
142  }
143 
144  // makes v a unitvector and returns the norm of v.
145  // if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
146  // if this is not good, check the return value of this method.
147  double Vector::Normalize(double eps) {
148  double v = this->Norm();
149  if (v < eps) {
150  *this = Vector(1,0,0);
151  return 0;
152  } else {
153  *this = (*this)/v;
154  return v;
155  }
156  }
157 
158 
159  bool Equal(const Rotation& a,const Rotation& b,double eps) {
160  return (Equal(a.data[0],b.data[0],eps) &&
161  Equal(a.data[1],b.data[1],eps) &&
162  Equal(a.data[2],b.data[2],eps) &&
163  Equal(a.data[3],b.data[3],eps) &&
164  Equal(a.data[4],b.data[4],eps) &&
165  Equal(a.data[5],b.data[5],eps) &&
166  Equal(a.data[6],b.data[6],eps) &&
167  Equal(a.data[7],b.data[7],eps) &&
168  Equal(a.data[8],b.data[8],eps) );
169  }
170 
171 
172 
173  Rotation operator *(const Rotation& lhs,const Rotation& rhs)
174  // Complexity : 27M+27A
175  {
176  return Rotation(
177  lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6],
178  lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7],
179  lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8],
180  lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6],
181  lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7],
182  lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8],
183  lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6],
184  lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7],
185  lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8]
186  );
187 
188  }
189 
190  Rotation Rotation::Quaternion(double x,double y,double z, double w)
191  {
192  double x2, y2, z2, w2;
193  x2 = x*x; y2 = y*y; z2 = z*z; w2 = w*w;
194  return Rotation(w2+x2-y2-z2, 2*x*y-2*w*z, 2*x*z+2*w*y,
195  2*x*y+2*w*z, w2-x2+y2-z2, 2*y*z-2*w*x,
196  2*x*z-2*w*y, 2*y*z+2*w*x, w2-x2-y2+z2);
197  }
198 
199  /* From the following sources:
200  http://web.archive.org/web/20041029003853/http:/www.j3d.org/matrix_faq/matrfaq_latest.html
201  http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
202  RobOOP::quaternion.cpp
203  */
204  void Rotation::GetQuaternion(double& x,double& y,double& z, double& w) const
205  {
206  double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2);
207  double epsilon=1E-12;
208  if( trace > epsilon ){
209  double s = 0.5 / sqrt(trace + 1.0);
210  w = 0.25 / s;
211  x = ( (*this)(2,1) - (*this)(1,2) ) * s;
212  y = ( (*this)(0,2) - (*this)(2,0) ) * s;
213  z = ( (*this)(1,0) - (*this)(0,1) ) * s;
214  }else{
215  if ( (*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2) ){
216  double s = 2.0 * sqrt( 1.0 + (*this)(0,0) - (*this)(1,1) - (*this)(2,2));
217  w = ((*this)(2,1) - (*this)(1,2) ) / s;
218  x = 0.25 * s;
219  y = ((*this)(0,1) + (*this)(1,0) ) / s;
220  z = ((*this)(0,2) + (*this)(2,0) ) / s;
221  } else if ((*this)(1,1) > (*this)(2,2)) {
222  double s = 2.0 * sqrt( 1.0 + (*this)(1,1) - (*this)(0,0) - (*this)(2,2));
223  w = ((*this)(0,2) - (*this)(2,0) ) / s;
224  x = ((*this)(0,1) + (*this)(1,0) ) / s;
225  y = 0.25 * s;
226  z = ((*this)(1,2) + (*this)(2,1) ) / s;
227  }else {
228  double s = 2.0 * sqrt( 1.0 + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) );
229  w = ((*this)(1,0) - (*this)(0,1) ) / s;
230  x = ((*this)(0,2) + (*this)(2,0) ) / s;
231  y = ((*this)(1,2) + (*this)(2,1) ) / s;
232  z = 0.25 * s;
233  }
234  }
235  }
236 
237 Rotation Rotation::RPY(double roll,double pitch,double yaw)
238  {
239  double ca1,cb1,cc1,sa1,sb1,sc1;
240  ca1 = cos(yaw); sa1 = sin(yaw);
241  cb1 = cos(pitch);sb1 = sin(pitch);
242  cc1 = cos(roll);sc1 = sin(roll);
243  return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
244  sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
245  -sb1,cb1*sc1,cb1*cc1);
246  }
247 
248 // Gives back a rotation matrix specified with RPY convention
249 void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
250  {
251  double epsilon=1E-12;
252  pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) );
253  if ( fabs(pitch) > (PI_2-epsilon) ) {
254  yaw = atan2( -data[1], data[4]);
255  roll = 0.0 ;
256  } else {
257  roll = atan2(data[7], data[8]);
258  yaw = atan2(data[3], data[0]);
259  }
260  }
261 
262 Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) {
263  double sa,ca,sb,cb,sg,cg;
264  sa = sin(Alfa);ca = cos(Alfa);
265  sb = sin(Beta);cb = cos(Beta);
266  sg = sin(Gamma);cg = cos(Gamma);
267  return Rotation( ca*cb*cg-sa*sg, -ca*cb*sg-sa*cg, ca*sb,
268  sa*cb*cg+ca*sg, -sa*cb*sg+ca*cg, sa*sb,
269  -sb*cg , sb*sg, cb
270  );
271 
272  }
273 
274 
275 void Rotation::GetEulerZYZ(double& alpha,double& beta,double& gamma) const {
276  double epsilon = 1E-12;
277  if (fabs(data[8]) > 1-epsilon ) {
278  gamma=0.0;
279  if (data[8]>0) {
280  beta = 0.0;
281  alpha= atan2(data[3],data[0]);
282  } else {
283  beta = PI;
284  alpha= atan2(-data[3],-data[0]);
285  }
286  } else {
287  alpha=atan2(data[5], data[2]);
288  beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]);
289  gamma=atan2(data[7], -data[6]);
290  }
291  }
292 
293 Rotation Rotation::Rot(const Vector& rotaxis,double angle) {
294  // The formula is
295  // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
296  // can be found by multiplying it with an arbitrary vector p
297  // and noting that this vector is rotated.
298  Vector rotvec = rotaxis;
299  rotvec.Normalize();
300  return Rotation::Rot2(rotvec,angle);
301 }
302 
303 Rotation Rotation::Rot2(const Vector& rotvec,double angle) {
304  // rotvec should be normalized !
305  // The formula is
306  // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
307  // can be found by multiplying it with an arbitrary vector p
308  // and noting that this vector is rotated.
309  double ct = cos(angle);
310  double st = sin(angle);
311  double vt = 1-ct;
312  double m_vt_0=vt*rotvec(0);
313  double m_vt_1=vt*rotvec(1);
314  double m_vt_2=vt*rotvec(2);
315  double m_st_0=rotvec(0)*st;
316  double m_st_1=rotvec(1)*st;
317  double m_st_2=rotvec(2)*st;
318  double m_vt_0_1=m_vt_0*rotvec(1);
319  double m_vt_0_2=m_vt_0*rotvec(2);
320  double m_vt_1_2=m_vt_1*rotvec(2);
321  return Rotation(
322  ct + m_vt_0*rotvec(0),
323  -m_st_2 + m_vt_0_1,
324  m_st_1 + m_vt_0_2,
325  m_st_2 + m_vt_0_1,
326  ct + m_vt_1*rotvec(1),
327  -m_st_0 + m_vt_1_2,
328  -m_st_1 + m_vt_0_2,
329  m_st_0 + m_vt_1_2,
330  ct + m_vt_2*rotvec(2)
331  );
332 }
333 
334 
335 
336 Vector Rotation::GetRot() const
337  // Returns a vector with the direction of the equiv. axis
338  // and its norm is angle
339  {
340  Vector axis;
341  double angle;
342  angle = Rotation::GetRotAngle(axis,epsilon);
343  return axis * angle;
344  }
345 
346 
347 
358 double Rotation::GetRotAngle(Vector& axis,double eps) const {
359  double angle,x,y,z; // variables for result
360  double epsilon = eps; // margin to allow for rounding errors
361  double epsilon2 = eps*10; // margin to distinguish between 0 and 180 degrees
362 
363  // optional check that input is pure rotation, 'isRotationMatrix' is defined at:
364  // http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/
365 
366  if ((std::abs(data[1] - data[3]) < epsilon)
367  && (std::abs(data[2] - data[6])< epsilon)
368  && (std::abs(data[5] - data[7]) < epsilon))
369  {
370  // singularity found
371  // first check for identity matrix which must have +1 for all terms
372  // in leading diagonal and zero in other terms
373  if ((std::abs(data[1] + data[3]) < epsilon2)
374  && (std::abs(data[2] + data[6]) < epsilon2)
375  && (std::abs(data[5] + data[7]) < epsilon2)
376  && (std::abs(data[0] + data[4] + data[8]-3) < epsilon2))
377  {
378  // this singularity is identity matrix so angle = 0, axis is arbitrary
379  // Choose 0, 0, 1 to pass orocos tests
380  axis = KDL::Vector(0,0,1);
381  angle = 0.0;
382  return angle;
383  }
384 
385  // otherwise this singularity is angle = 180
386  angle = PI;
387  double xx = (data[0] + 1) / 2;
388  double yy = (data[4] + 1) / 2;
389  double zz = (data[8] + 1) / 2;
390  double xy = (data[1] + data[3]) / 4;
391  double xz = (data[2] + data[6]) / 4;
392  double yz = (data[5] + data[7]) / 4;
393 
394  if ((xx > yy) && (xx > zz))
395  {
396  // data[0] is the largest diagonal term
397  x = sqrt(xx);
398  y = xy/x;
399  z = xz/x;
400  }
401  else if (yy > zz)
402  {
403  // data[4] is the largest diagonal term
404  y = sqrt(yy);
405  x = xy/y;
406  z = yz/y;
407  }
408  else
409  {
410  // data[8] is the largest diagonal term so base result on this
411  z = sqrt(zz);
412  x = xz/z;
413  y = yz/z;
414  }
415  axis = KDL::Vector(x, y, z);
416  return angle; // return 180 deg rotation
417  }
418 
419  double f = (data[0] + data[4] + data[8] - 1) / 2;
420 
421  x = (data[7] - data[5]);
422  y = (data[2] - data[6]);
423  z = (data[3] - data[1]);
424  axis = KDL::Vector(x, y, z);
425  angle = atan2(axis.Norm()/2,f);
426  axis.Normalize();
427  return angle;
428 }
429 
430 bool operator==(const Rotation& a,const Rotation& b) {
431 #ifdef KDL_USE_EQUAL
432  return Equal(a,b);
433 #else
434  return ( a.data[0]==b.data[0] &&
435  a.data[1]==b.data[1] &&
436  a.data[2]==b.data[2] &&
437  a.data[3]==b.data[3] &&
438  a.data[4]==b.data[4] &&
439  a.data[5]==b.data[5] &&
440  a.data[6]==b.data[6] &&
441  a.data[7]==b.data[7] &&
442  a.data[8]==b.data[8] );
443 #endif
444 }
445 }
KDL::Vector2::Normalize
double Normalize(double eps=epsilon)
Definition: frames.cpp:129
KDL::Rotation::Rot2
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:328
KDL::Vector::data
double data[3]
Definition: frames.hpp:165
frames.hpp
KDL::operator*
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Definition: articulatedbodyinertia.cpp:49
std::fabs
T fabs(T... args)
frames.inl
KDL::Rotation::GetRot
Vector GetRot() const
Definition: frames.cpp:361
KDL::Rotation::Quaternion
static Rotation Quaternion(double x, double y, double z, double w)
Definition: frames.cpp:215
KDL::Frame::Frame
Frame()
Definition: frames.hpp:586
KDL::sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:391
KDL::Rotation::GetEulerZYZ
void GetEulerZYZ(double &alpha, double &beta, double &gamma) const
Definition: frames.cpp:300
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::Rotation::Rotation
Rotation()
Definition: frames.hpp:308
KDL::Vector::Norm
double Norm(double eps=epsilon) const
Definition: frames.cpp:142
KDL::Rotation::GetRPY
void GetRPY(double &roll, double &pitch, double &yaw) const
Definition: frames.cpp:274
utility.h
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
KDL::Vector::Normalize
double Normalize(double eps=epsilon)
Definition: frames.cpp:172
KDL::Frame::Make4x4
void Make4x4(double *d)
Reads data from an double array.
Definition: frames.cpp:64
algorithm
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::operator==
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:455
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:453
KDL::PI_2
const double PI_2
the value of pi/2
Definition: utility.cxx:17
KDL::Rotation::GetQuaternion
void GetQuaternion(double &x, double &y, double &z, double &w) const
Definition: frames.cpp:229
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::PI
const double PI
the value of pi
Definition: utility.cxx:16
KDL::Rotation::GetRotAngle
double GetRotAngle(Vector &axis, double eps=epsilon) const
Definition: frames.cpp:383
KDL::Rotation::EulerZYZ
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Definition: frames.cpp:287
KDL::Frame::DH
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:95
KDL::Frame::DH_Craig1989
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:78
KDL::Rotation::data
double data[9]
Definition: frames.hpp:306
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotvec, double angle)
Definition: frames.cpp:318
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
KDL::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Definition: frames.cpp:262
KDL::Vector2::Vector2
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:965
KDL::Vector2::Norm
double Norm(double eps=epsilon) const
Definition: frames.cpp:112
KDL::Vector::Vector
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
Definition: frames.hpp:167
KDL::Vector2::data
double data[2]
Definition: frames.hpp:962
KDL::sqr
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:375
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303
KDL::abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
Definition: rall1d.h:431