frames.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002                         frames.cxx -  description
00003                        -------------------------
00004     begin                : June 2006
00005     copyright            : (C) 2006 Erwin Aertbelien
00006     email                : firstname.lastname@mech.kuleuven.ac.be
00007 
00008  History (only major changes)( AUTHOR-Description ) :
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 
00028 #include "frames.hpp"
00029 
00030 #define _USE_MATH_DEFINES  // For MSVC
00031 #include <math.h>
00032 
00033 namespace KDL {
00034 
00035 #ifndef KDL_INLINE
00036 #include "frames.inl"
00037 #endif
00038 
00039     void Frame::Make4x4(double * d)
00040     {
00041         int i;
00042         int j;
00043         for (i=0;i<3;i++) {
00044             for (j=0;j<3;j++)
00045                 d[i*4+j]=M(i,j);
00046             d[i*4+3] = p(i);
00047         }
00048         for (j=0;j<3;j++)
00049             d[12+j] = 0.;
00050         d[15] = 1;
00051     }
00052 
00053     Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta)
00054     // returns Modified Denavit-Hartenberg parameters (According to Craig)
00055     {
00056         double ct,st,ca,sa;
00057         ct = cos(theta);
00058         st = sin(theta);
00059         sa = sin(alpha);
00060         ca = cos(alpha);
00061         return Frame(Rotation(
00062                               ct,       -st,     0,
00063                               st*ca,  ct*ca,   -sa,
00064                               st*sa,  ct*sa,    ca   ),
00065                      Vector(
00066                             a,      -sa*d,  ca*d   )
00067                      );
00068     }
00069 
00070     Frame Frame::DH(double a,double alpha,double d,double theta)
00071     // returns Denavit-Hartenberg parameters (Non-Modified DH)
00072     {
00073         double ct,st,ca,sa;
00074         ct = cos(theta);
00075         st = sin(theta);
00076         sa = sin(alpha);
00077         ca = cos(alpha);
00078         return Frame(Rotation(
00079                               ct,    -st*ca,   st*sa,
00080                               st,     ct*ca,  -ct*sa,
00081                               0,        sa,      ca   ),
00082                      Vector(
00083                             a*ct,      a*st,  d   )
00084                      );
00085     }
00086 
00087     double Vector2::Norm() const
00088     {
00089         if (fabs(data[0]) > fabs(data[1]) ) {
00090             return data[0]*sqrt(1+sqr(data[1]/data[0]));
00091         } else {
00092             return data[1]*sqrt(1+sqr(data[0]/data[1]));
00093         }
00094     }
00095     // makes v a unitvector and returns the norm of v.
00096     // if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
00097     // if this is not good, check the return value of this method.
00098     double Vector2::Normalize(double eps) {
00099         double v = this->Norm();
00100         if (v < eps) {
00101             *this = Vector2(1,0);
00102             return v;
00103         } else {
00104             *this = (*this)/v;
00105             return v;
00106         }
00107     }
00108 
00109 
00110     // do some effort not to lose precision
00111     double Vector::Norm() const
00112     {
00113         double tmp1;
00114         double tmp2;
00115         tmp1 = fabs(data[0]);
00116         tmp2 = fabs(data[1]);
00117         if (tmp1 >= tmp2) {
00118             tmp2=fabs(data[2]);
00119             if (tmp1 >= tmp2) {
00120                 if (tmp1 == 0) {
00121                     // only to everything exactly zero case, all other are handled correctly
00122                     return 0;
00123                 }
00124                 return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0]));
00125             } else {
00126                 return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
00127             }
00128         } else {
00129             tmp1=fabs(data[2]);
00130             if (tmp2 > tmp1) {
00131                 return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1]));
00132             } else {
00133                 return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
00134             }
00135         }
00136     }
00137 
00138     // makes v a unitvector and returns the norm of v.
00139     // if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
00140     // if this is not good, check the return value of this method.
00141     double Vector::Normalize(double eps) {
00142         double v = this->Norm();
00143         if (v < eps) {
00144             *this = Vector(1,0,0);
00145             return v;
00146         } else {
00147             *this = (*this)/v;
00148             return v;
00149         }
00150     }
00151 
00152 
00153     bool Equal(const Rotation& a,const Rotation& b,double eps) {
00154         return (Equal(a.data[0],b.data[0],eps) &&
00155                 Equal(a.data[1],b.data[1],eps) &&
00156                 Equal(a.data[2],b.data[2],eps) &&
00157                 Equal(a.data[3],b.data[3],eps) &&
00158                 Equal(a.data[4],b.data[4],eps) &&
00159                 Equal(a.data[5],b.data[5],eps) &&
00160                 Equal(a.data[6],b.data[6],eps) &&
00161                 Equal(a.data[7],b.data[7],eps) &&
00162                 Equal(a.data[8],b.data[8],eps)    );
00163     }
00164 
00165 
00166 
00167     Rotation operator *(const Rotation& lhs,const Rotation& rhs)
00168     // Complexity : 27M+27A
00169     {
00170         return Rotation(
00171                         lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6],
00172                         lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7],
00173                         lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8],
00174                         lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6],
00175                         lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7],
00176                         lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8],
00177                         lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6],
00178                         lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7],
00179                         lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8]
00180                         );
00181 
00182     }
00183 
00184     Rotation Rotation::Quaternion(double x,double y,double z, double w)
00185     {
00186         double x2, y2, z2, w2;
00187         x2 = x*x;  y2 = y*y; z2 = z*z;  w2 = w*w;
00188         return Rotation(w2+x2-y2-z2, 2*x*y-2*w*z, 2*x*z+2*w*y,
00189                         2*x*y+2*w*z, w2-x2+y2-z2, 2*y*z-2*w*x,
00190                         2*x*z-2*w*y, 2*y*z+2*w*x, w2-x2-y2+z2);
00191     }
00192 
00193     /* From the following sources:
00194        http://web.archive.org/web/20041029003853/http:/www.j3d.org/matrix_faq/matrfaq_latest.html
00195        http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
00196        RobOOP::quaternion.cpp
00197     */
00198     void Rotation::GetQuaternion(double& x,double& y,double& z, double& w) const
00199     {
00200         double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2);
00201         double epsilon=1E-12;
00202         if( trace > epsilon ){
00203             double s = 0.5 / sqrt(trace + 1.0);
00204             w = 0.25 / s;
00205             x = ( (*this)(2,1) - (*this)(1,2) ) * s;
00206             y = ( (*this)(0,2) - (*this)(2,0) ) * s;
00207             z = ( (*this)(1,0) - (*this)(0,1) ) * s;
00208         }else{
00209             if ( (*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2) ){
00210                 double s = 2.0 * sqrt( 1.0 + (*this)(0,0) - (*this)(1,1) - (*this)(2,2));
00211                 w = ((*this)(2,1) - (*this)(1,2) ) / s;
00212                 x = 0.25 * s;
00213                 y = ((*this)(0,1) + (*this)(1,0) ) / s;
00214                 z = ((*this)(0,2) + (*this)(2,0) ) / s;
00215             } else if ((*this)(1,1) > (*this)(2,2)) {
00216                 double s = 2.0 * sqrt( 1.0 + (*this)(1,1) - (*this)(0,0) - (*this)(2,2));
00217                 w = ((*this)(0,2) - (*this)(2,0) ) / s;
00218                 x = ((*this)(0,1) + (*this)(1,0) ) / s;
00219                 y = 0.25 * s;
00220                 z = ((*this)(1,2) + (*this)(2,1) ) / s;
00221             }else {
00222                 double s = 2.0 * sqrt( 1.0 + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) );
00223                 w = ((*this)(1,0) - (*this)(0,1) ) / s;
00224                 x = ((*this)(0,2) + (*this)(2,0) ) / s;
00225                 y = ((*this)(1,2) + (*this)(2,1) ) / s;
00226                 z = 0.25 * s;
00227             }
00228         }    
00229     }
00230 
00231 Rotation Rotation::RPY(double roll,double pitch,double yaw)
00232     {
00233         double ca1,cb1,cc1,sa1,sb1,sc1;
00234         ca1 = cos(yaw); sa1 = sin(yaw);
00235         cb1 = cos(pitch);sb1 = sin(pitch);
00236         cc1 = cos(roll);sc1 = sin(roll);
00237         return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
00238                    sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
00239                    -sb1,cb1*sc1,cb1*cc1);
00240     }
00241 
00242 // Gives back a rotation matrix specified with RPY convention
00243 void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
00244     {
00245                 double epsilon=1E-12;
00246                 pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) )  );
00247         if ( fabs(pitch) > (M_PI/2.0-epsilon) ) {
00248             yaw = atan2(        -data[1], data[4]);
00249             roll  = 0.0 ;
00250         } else {
00251             roll  = atan2(data[7], data[8]);
00252             yaw   = atan2(data[3], data[0]);
00253         }
00254     }
00255 
00256 Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) {
00257         double sa,ca,sb,cb,sg,cg;
00258         sa  = sin(Alfa);ca = cos(Alfa);
00259         sb  = sin(Beta);cb = cos(Beta);
00260         sg  = sin(Gamma);cg = cos(Gamma);
00261         return Rotation( ca*cb*cg-sa*sg,     -ca*cb*sg-sa*cg,        ca*sb,
00262                  sa*cb*cg+ca*sg,     -sa*cb*sg+ca*cg,        sa*sb,
00263                  -sb*cg ,                sb*sg,              cb
00264                 );
00265 
00266      }
00267 
00268 
00269 void Rotation::GetEulerZYZ(double& alpha,double& beta,double& gamma) const {
00270                 double epsilon = 1E-12;
00271         if (fabs(data[8]) > 1-epsilon  ) {
00272             gamma=0.0;
00273             if (data[8]>0) {
00274                 beta = 0.0;
00275                 alpha= atan2(data[3],data[0]);
00276             } else {
00277                 beta = PI;
00278                 alpha= atan2(-data[3],-data[0]);
00279             }
00280         } else {
00281             alpha=atan2(data[5], data[2]);
00282             beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]);
00283             gamma=atan2(data[7], -data[6]);
00284         }
00285  }
00286 
00287 Rotation Rotation::Rot(const Vector& rotaxis,double angle) {
00288     // The formula is
00289     // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
00290     // can be found by multiplying it with an arbitrary vector p
00291     // and noting that this vector is rotated.
00292     Vector rotvec = rotaxis;
00293         rotvec.Normalize();
00294         return Rotation::Rot2(rotvec,angle);
00295 }
00296 
00297 Rotation Rotation::Rot2(const Vector& rotvec,double angle) {
00298     // rotvec should be normalized !
00299     // The formula is
00300     // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
00301     // can be found by multiplying it with an arbitrary vector p
00302     // and noting that this vector is rotated.
00303     double ct = cos(angle);
00304     double st = sin(angle);
00305     double vt = 1-ct;
00306     double m_vt_0=vt*rotvec(0);
00307     double m_vt_1=vt*rotvec(1);
00308     double m_vt_2=vt*rotvec(2);
00309     double m_st_0=rotvec(0)*st;
00310     double m_st_1=rotvec(1)*st;
00311     double m_st_2=rotvec(2)*st;
00312     double m_vt_0_1=m_vt_0*rotvec(1);
00313     double m_vt_0_2=m_vt_0*rotvec(2);
00314     double m_vt_1_2=m_vt_1*rotvec(2);
00315     return Rotation(
00316         ct      +  m_vt_0*rotvec(0),
00317         -m_st_2 +  m_vt_0_1,
00318         m_st_1  +  m_vt_0_2,
00319         m_st_2  +  m_vt_0_1,
00320         ct      +  m_vt_1*rotvec(1),
00321         -m_st_0 +  m_vt_1_2,
00322         -m_st_1 +  m_vt_0_2,
00323         m_st_0  +  m_vt_1_2,
00324         ct      +  m_vt_2*rotvec(2)
00325         );
00326 }
00327 
00328 
00329 
00330 Vector Rotation::GetRot() const
00331          // Returns a vector with the direction of the equiv. axis
00332          // and its norm is angle
00333      {
00334        Vector axis;
00335        double angle;
00336        angle = Rotation::GetRotAngle(axis,epsilon);
00337        return axis * angle;
00338      }
00339 
00340 
00341 
00352 double Rotation::GetRotAngle(Vector& axis,double eps) const {
00353         double ca    = (data[0]+data[4]+data[8]-1)/2.0;
00354         double t= eps*eps/2.0;
00355         if (ca>1-t) {
00356                 // undefined choose the Z-axis, and angle 0
00357                 axis = Vector(0,0,1);
00358                 return 0;
00359         }
00360         if (ca < -1+t) {
00361                 // The case of angles consisting of multiples of M_PI:
00362                 // two solutions, choose a positive Z-component of the axis
00363                 double x = sqrt( (data[0]+1.0)/2);
00364                 double y = sqrt( (data[4]+1.0)/2);
00365                 double z = sqrt( (data[8]+1.0)/2);
00366                 if ( data[2] < 0) x=-x;
00367                 if ( data[7] < 0) y=-y;
00368                 if ( x*y*data[1] < 0) x=-x;  // this last line can be necessary when z is 0
00369                 // z always >= 0 
00370                 // if z equal to zero 
00371                 axis = Vector( x,y,z  );
00372                 return PI;
00373         }
00374     double angle;
00375     double mod_axis;
00376     double axisx, axisy, axisz;
00377         axisx = data[7]-data[5];
00378     axisy = data[2]-data[6];
00379     axisz = data[3]-data[1];
00380     mod_axis = sqrt(axisx*axisx+axisy*axisy+axisz*axisz);
00381         axis  = Vector(axisx/mod_axis,
00382                    axisy/mod_axis,
00383                    axisz/mod_axis );
00384     angle = atan2(mod_axis/2,ca);
00385     return angle;
00386 }
00387 
00388 bool operator==(const Rotation& a,const Rotation& b) {
00389 #ifdef KDL_USE_EQUAL
00390     return Equal(a,b);
00391 #else
00392     return ( a.data[0]==b.data[0] &&
00393              a.data[1]==b.data[1] &&
00394              a.data[2]==b.data[2] &&
00395              a.data[3]==b.data[3] &&
00396              a.data[4]==b.data[4] &&
00397              a.data[5]==b.data[5] &&
00398              a.data[6]==b.data[6] &&
00399              a.data[7]==b.data[7] &&
00400              a.data[8]==b.data[8]  );
00401 #endif
00402 }
00403 }


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14