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


orocos_kdl
Author(s):
autogenerated on Wed Jul 1 2020 03:17:39