Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

homogen.cpp

Go to the documentation of this file.
00001 /*
00002 ROBOOP -- A robotics object oriented package in C++
00003 Copyright (C) 1996-2004  Richard Gourdeau
00004 
00005 This library is free software; you can redistribute it and/or modify
00006 it under the terms of the GNU Lesser General Public License as
00007 published by the Free Software Foundation; either version 2.1 of the
00008 License, or (at your option) any later version.
00009 
00010 This library is distributed in the hope that it will be useful,
00011 but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 GNU Lesser General Public License for more details.
00014 
00015 You should have received a copy of the GNU Lesser General Public
00016 License along with this library; if not, write to the Free Software
00017 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 
00019 
00020 Report problems and direct all questions to:
00021 
00022 Richard Gourdeau
00023 Professeur Agrege
00024 Departement de genie electrique
00025 Ecole Polytechnique de Montreal
00026 C.P. 6079, Succ. Centre-Ville
00027 Montreal, Quebec, H3C 3A7
00028 
00029 email: richard.gourdeau@polymtl.ca
00030 
00031 -------------------------------------------------------------------------------
00032 Revision_history:
00033 
00034 2004/07/01: Etienne Lachance
00035    -Added protection on trans function.
00036    -Added doxygen documentation.
00037 
00038 2004/07/01: Ethan Tira-Thompson
00039     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00040 -------------------------------------------------------------------------------
00041 */
00042 
00043 /*!
00044   @file homogen.cpp
00045   @brief Homogen transformation functions.
00046 */
00047 
00048 #include "robot.h"
00049 
00050 using namespace std;
00051 
00052 #ifdef use_namespace
00053 namespace ROBOOP {
00054   using namespace NEWMAT;
00055 #endif
00056   //! @brief RCS/CVS version.
00057   static const char rcsid[] __UNUSED__ = "$Id: homogen.cpp,v 1.6 2007/11/11 23:57:24 ejt Exp $";
00058 
00059 
00060 ReturnMatrix trans(const ColumnVector & a)
00061 //!  @brief Translation.
00062 {
00063    Matrix translation(4,4);
00064 
00065    translation << fourbyfourident; // identity matrix 
00066 
00067    if (a.Nrows() < 3) 
00068      {
00069        translation(1,4) = a(1);
00070        translation(2,4) = a(2);
00071        translation(3,4) = a(3);
00072      }
00073    else
00074        cerr << "trans: wrong size in input vector." << endl;
00075 
00076    translation.Release(); return translation;
00077 }
00078 
00079 
00080 ReturnMatrix rotx(const Real alpha)
00081 //!  @brief Rotation around x axis.
00082 {
00083    Matrix rot(4,4);
00084    Real c, s;
00085 
00086    rot << fourbyfourident; // identity matrix 
00087 
00088    c = cos(alpha);
00089    s = sin(alpha);
00090 
00091    rot(2,2) = c;
00092    rot(3,3) = c;
00093    rot(2,3) = -s;
00094    rot(3,2) = s;
00095 
00096    rot.Release(); return rot;
00097 }
00098 
00099 
00100 ReturnMatrix roty(const Real beta)
00101 //!  @brief Rotation around x axis.
00102 {
00103    Matrix rot(4,4);
00104    Real c, s;
00105 
00106    rot << fourbyfourident; // identity matrix
00107 
00108    c = cos(beta);
00109    s = sin(beta);
00110 
00111    rot(1,1) = c;
00112    rot(3,3) = c;
00113    rot(1,3) = s;
00114    rot(3,1) = -s;
00115 
00116    rot.Release(); return rot;
00117 }
00118 
00119 
00120 ReturnMatrix rotz(const Real gamma)
00121 //!  @brief Rotation around z axis.
00122 {
00123    Matrix rot(4,4);
00124    Real c, s;
00125 
00126    rot << fourbyfourident; // identity matrix
00127 
00128    c = cos(gamma);
00129    s = sin(gamma);
00130 
00131    rot(1,1) = c;
00132    rot(2,2) = c;
00133    rot(1,2) = -s;
00134    rot(2,1) = s;
00135 
00136    rot.Release(); return rot;
00137 }
00138 
00139 // compound rotations 
00140 
00141 
00142 ReturnMatrix rotk(const Real theta, const ColumnVector & k)
00143 //! @brief Rotation around arbitrary axis.
00144 {
00145    Matrix rot(4,4);
00146    Real c, s, vers, kx, ky, kz;
00147 
00148    rot << fourbyfourident; // identity matrix
00149 
00150    vers = SumSquare(k.SubMatrix(1,3,1,1));
00151    if (vers != 0.0) { // compute the rotation if the vector norm is not 0.0
00152       vers = sqrt(1/vers);
00153       kx = k(1)*vers;
00154       ky = k(2)*vers;
00155       kz = k(3)*vers;
00156       s = sin(theta);
00157       c = cos(theta);
00158       vers = 1-c;
00159 
00160       rot(1,1) = kx*kx*vers+c;
00161       rot(1,2) = kx*ky*vers-kz*s;
00162       rot(1,3) = kx*kz*vers+ky*s;
00163       rot(2,1) = kx*ky*vers+kz*s;
00164       rot(2,2) = ky*ky*vers+c;
00165       rot(2,3) = ky*kz*vers-kx*s;
00166       rot(3,1) = kx*kz*vers-ky*s;
00167       rot(3,2) = ky*kz*vers+kx*s;
00168       rot(3,3) = kz*kz*vers+c;
00169    }
00170 
00171    rot.Release(); return rot;
00172 }
00173 
00174 
00175 ReturnMatrix rpy(const ColumnVector & a)
00176 //!  @brief Roll Pitch Yaw rotation.
00177 {
00178    Matrix rot(4,4);
00179    Real ca, sa, cb, sb, cc, sc;
00180 
00181    rot << fourbyfourident; // identity matrix
00182 
00183    ca = cos(a(1));
00184    sa = sin(a(1));
00185    cb = cos(a(2));
00186    sb = sin(a(2));
00187    cc = cos(a(3));
00188    sc = sin(a(3));
00189 
00190    rot(1,1) = cb*cc;
00191    rot(1,2) = sa*sb*cc-ca*sc;
00192    rot(1,3) = ca*sb*cc+sa*sc;
00193    rot(2,1) = cb*sc;
00194    rot(2,2) = sa*sb*sc+ca*cc;
00195    rot(2,3) = ca*sb*sc-sa*cc;
00196    rot(3,1) = -sb;
00197    rot(3,2) = sa*cb;
00198    rot(3,3) = ca*cb;
00199 
00200    rot.Release(); return rot;
00201 }
00202 
00203 
00204 ReturnMatrix eulzxz(const ColumnVector & a)
00205 //!  @brief Euler ZXZ rotation
00206 {
00207    Matrix rot(4,4);
00208    Real c1, s1, ca, sa, c2, s2;
00209 
00210    rot << fourbyfourident; // identity matrix
00211 
00212    c1 = cos(a(1));
00213    s1 = sin(a(1));
00214    ca = cos(a(2));
00215    sa = sin(a(2));
00216    c2 = cos(a(3));
00217    s2 = sin(a(3));
00218 
00219    rot(1,1) = c1*c2-s1*ca*s2;
00220    rot(1,2) = -c1*s2-s1*ca*c2;
00221    rot(1,3) = sa*s1;
00222    rot(2,1) = s1*c2+c1*ca*s2;
00223    rot(2,2) = -s1*s2+c1*ca*c2;
00224    rot(2,3) = -sa*c1;
00225    rot(3,1) = sa*s2;
00226    rot(3,2) = sa*c2;
00227    rot(3,3) = ca;
00228 
00229    rot.Release(); return rot;
00230 }
00231 
00232 
00233 ReturnMatrix rotd(const Real theta, const ColumnVector & k1,
00234                   const ColumnVector & k2)
00235 //!  @brief Rotation around an arbitrary line.
00236 {
00237    Matrix rot;
00238 
00239    rot = trans(k1)*rotk(theta,k2-k1)*trans(-k1);
00240 
00241    rot.Release(); return rot;
00242 }
00243 
00244 // inverse problem for compound rotations
00245 
00246 ReturnMatrix irotk(const Matrix & R)
00247 //!  @brief Obtain axis from a rotation matrix.
00248 {
00249    ColumnVector k(4);
00250    Real a, b, c;
00251 
00252    a = (R(3,2)-R(2,3));
00253    b = (R(1,3)-R(3,1));
00254    c = (R(2,1)-R(1,2));
00255    k(4) = atan(sqrt(a*a + b*b + c*c)/(R(1,1) + R(2,2) + R(3,3)-1));
00256    k(1) = (R(3,2)-R(2,3))/(2*sin(k(4)));
00257    k(2) = (R(1,3)-R(3,1))/(2*sin(k(4)));
00258    k(3) = (R(2,1)-R(1,2))/(2*sin(k(4)));
00259 
00260    k.Release(); return k;
00261 }
00262 
00263 
00264 ReturnMatrix irpy(const Matrix & R)
00265   //!  @brief Obtain Roll, Pitch and Yaw from a rotation matrix.
00266 {
00267    ColumnVector k(3);
00268 
00269    if (R(3,1)==1) {
00270       k(1) = atan2(-R(1,2),-R(1,3));
00271       k(2) = -M_PI/2;
00272       k(3) = 0.0;
00273    } else if (R(3,1)==-1) {
00274       k(1) = atan2(R(1,2),R(1,3));
00275       k(2) = M_PI/2;
00276       k(3) = 0.0;
00277    } else {
00278       k(1) = atan2(R(3,2), R(3,3));
00279       k(2) = atan2(-R(3,1), sqrt(R(1,1)*R(1,1) + R(2,1)*R(2,1)));
00280       k(3) = atan2(R(2,1), R(1,1));
00281    }
00282 
00283    k.Release(); return k;
00284 }
00285 
00286 
00287 ReturnMatrix ieulzxz(const Matrix & R)
00288   //!  @brief Obtain Roll, Pitch and Yaw from a rotation matrix.
00289 {
00290    ColumnVector  a(3);
00291 
00292    if ((R(3,3)==1)  || (R(3,3)==-1)) {
00293       a(1) = 0.0;
00294       a(2) = ((R(3,3) == 1) ? 0.0 : M_PI);
00295       a(3) = atan2(R(2,1),R(1,1));
00296    } else {
00297       a(1) = atan2(R(1,3), -R(2,3));
00298       a(2) = atan2(sqrt(R(1,3)*R(1,3) + R(2,3)*R(2,3)), R(3,3));
00299       a(3) = atan2(R(3,1), R(3,2));
00300    }
00301 
00302    a.Release(); return a;
00303 }
00304 
00305 #ifdef use_namespace
00306 }
00307 #endif

ROBOOP v1.21a
Generated Thu Nov 22 00:51:28 2007 by Doxygen 1.5.4