Homepage Demos Overview Downloads Tutorials 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 #ifdef use_namespace
00051 namespace ROBOOP {
00052   using namespace NEWMAT;
00053 #endif
00054   //! @brief RCS/CVS version.
00055   static const char rcsid[] __UNUSED__ = "$Id: homogen.cpp,v 1.5 2005/07/26 03:22:09 ejt Exp $";
00056 
00057 
00058 ReturnMatrix trans(const ColumnVector & a)
00059 //!  @brief Translation.
00060 {
00061    Matrix translation(4,4);
00062 
00063    translation << fourbyfourident; // identity matrix 
00064 
00065    if (a.Nrows() < 3) 
00066      {
00067        translation(1,4) = a(1);
00068        translation(2,4) = a(2);
00069        translation(3,4) = a(3);
00070      }
00071    else
00072        cerr << "trans: wrong size in input vector." << endl;
00073 
00074    translation.Release(); return translation;
00075 }
00076 
00077 
00078 ReturnMatrix rotx(const Real alpha)
00079 //!  @brief Rotation around x axis.
00080 {
00081    Matrix rot(4,4);
00082    Real c, s;
00083 
00084    rot << fourbyfourident; // identity matrix 
00085 
00086    c = cos(alpha);
00087    s = sin(alpha);
00088 
00089    rot(2,2) = c;
00090    rot(3,3) = c;
00091    rot(2,3) = -s;
00092    rot(3,2) = s;
00093 
00094    rot.Release(); return rot;
00095 }
00096 
00097 
00098 ReturnMatrix roty(const Real beta)
00099 //!  @brief Rotation around x axis.
00100 {
00101    Matrix rot(4,4);
00102    Real c, s;
00103 
00104    rot << fourbyfourident; // identity matrix
00105 
00106    c = cos(beta);
00107    s = sin(beta);
00108 
00109    rot(1,1) = c;
00110    rot(3,3) = c;
00111    rot(1,3) = s;
00112    rot(3,1) = -s;
00113 
00114    rot.Release(); return rot;
00115 }
00116 
00117 
00118 ReturnMatrix rotz(const Real gamma)
00119 //!  @brief Rotation around z axis.
00120 {
00121    Matrix rot(4,4);
00122    Real c, s;
00123 
00124    rot << fourbyfourident; // identity matrix
00125 
00126    c = cos(gamma);
00127    s = sin(gamma);
00128 
00129    rot(1,1) = c;
00130    rot(2,2) = c;
00131    rot(1,2) = -s;
00132    rot(2,1) = s;
00133 
00134    rot.Release(); return rot;
00135 }
00136 
00137 // compound rotations 
00138 
00139 
00140 ReturnMatrix rotk(const Real theta, const ColumnVector & k)
00141 //! @brief Rotation around arbitrary axis.
00142 {
00143    Matrix rot(4,4);
00144    Real c, s, vers, kx, ky, kz;
00145 
00146    rot << fourbyfourident; // identity matrix
00147 
00148    vers = SumSquare(k.SubMatrix(1,3,1,1));
00149    if (vers != 0.0) { // compute the rotation if the vector norm is not 0.0
00150       vers = sqrt(1/vers);
00151       kx = k(1)*vers;
00152       ky = k(2)*vers;
00153       kz = k(3)*vers;
00154       s = sin(theta);
00155       c = cos(theta);
00156       vers = 1-c;
00157 
00158       rot(1,1) = kx*kx*vers+c;
00159       rot(1,2) = kx*ky*vers-kz*s;
00160       rot(1,3) = kx*kz*vers+ky*s;
00161       rot(2,1) = kx*ky*vers+kz*s;
00162       rot(2,2) = ky*ky*vers+c;
00163       rot(2,3) = ky*kz*vers-kx*s;
00164       rot(3,1) = kx*kz*vers-ky*s;
00165       rot(3,2) = ky*kz*vers+kx*s;
00166       rot(3,3) = kz*kz*vers+c;
00167    }
00168 
00169    rot.Release(); return rot;
00170 }
00171 
00172 
00173 ReturnMatrix rpy(const ColumnVector & a)
00174 //!  @brief Roll Pitch Yaw rotation.
00175 {
00176    Matrix rot(4,4);
00177    Real ca, sa, cb, sb, cc, sc;
00178 
00179    rot << fourbyfourident; // identity matrix
00180 
00181    ca = cos(a(1));
00182    sa = sin(a(1));
00183    cb = cos(a(2));
00184    sb = sin(a(2));
00185    cc = cos(a(3));
00186    sc = sin(a(3));
00187 
00188    rot(1,1) = cb*cc;
00189    rot(1,2) = sa*sb*cc-ca*sc;
00190    rot(1,3) = ca*sb*cc+sa*sc;
00191    rot(2,1) = cb*sc;
00192    rot(2,2) = sa*sb*sc+ca*cc;
00193    rot(2,3) = ca*sb*sc-sa*cc;
00194    rot(3,1) = -sb;
00195    rot(3,2) = sa*cb;
00196    rot(3,3) = ca*cb;
00197 
00198    rot.Release(); return rot;
00199 }
00200 
00201 
00202 ReturnMatrix eulzxz(const ColumnVector & a)
00203 //!  @brief Euler ZXZ rotation
00204 {
00205    Matrix rot(4,4);
00206    Real c1, s1, ca, sa, c2, s2;
00207 
00208    rot << fourbyfourident; // identity matrix
00209 
00210    c1 = cos(a(1));
00211    s1 = sin(a(1));
00212    ca = cos(a(2));
00213    sa = sin(a(2));
00214    c2 = cos(a(3));
00215    s2 = sin(a(3));
00216 
00217    rot(1,1) = c1*c2-s1*ca*s2;
00218    rot(1,2) = -c1*s2-s1*ca*c2;
00219    rot(1,3) = sa*s1;
00220    rot(2,1) = s1*c2+c1*ca*s2;
00221    rot(2,2) = -s1*s2+c1*ca*c2;
00222    rot(2,3) = -sa*c1;
00223    rot(3,1) = sa*s2;
00224    rot(3,2) = sa*c2;
00225    rot(3,3) = ca;
00226 
00227    rot.Release(); return rot;
00228 }
00229 
00230 
00231 ReturnMatrix rotd(const Real theta, const ColumnVector & k1,
00232                   const ColumnVector & k2)
00233 //!  @brief Rotation around an arbitrary line.
00234 {
00235    Matrix rot;
00236 
00237    rot = trans(k1)*rotk(theta,k2-k1)*trans(-k1);
00238 
00239    rot.Release(); return rot;
00240 }
00241 
00242 // inverse problem for compound rotations
00243 
00244 ReturnMatrix irotk(const Matrix & R)
00245 //!  @brief Obtain axis from a rotation matrix.
00246 {
00247    ColumnVector k(4);
00248    Real a, b, c;
00249 
00250    a = (R(3,2)-R(2,3));
00251    b = (R(1,3)-R(3,1));
00252    c = (R(2,1)-R(1,2));
00253    k(4) = atan(sqrt(a*a + b*b + c*c)/(R(1,1) + R(2,2) + R(3,3)-1));
00254    k(1) = (R(3,2)-R(2,3))/(2*sin(k(4)));
00255    k(2) = (R(1,3)-R(3,1))/(2*sin(k(4)));
00256    k(3) = (R(2,1)-R(1,2))/(2*sin(k(4)));
00257 
00258    k.Release(); return k;
00259 }
00260 
00261 
00262 ReturnMatrix irpy(const Matrix & R)
00263   //!  @brief Obtain Roll, Pitch and Yaw from a rotation matrix.
00264 {
00265    ColumnVector k(3);
00266 
00267    if (R(3,1)==1) {
00268       k(1) = atan2(-R(1,2),-R(1,3));
00269       k(2) = -M_PI/2;
00270       k(3) = 0.0;
00271    } else if (R(3,1)==-1) {
00272       k(1) = atan2(R(1,2),R(1,3));
00273       k(2) = M_PI/2;
00274       k(3) = 0.0;
00275    } else {
00276       k(1) = atan2(R(3,2), R(3,3));
00277       k(2) = atan2(-R(3,1), sqrt(R(1,1)*R(1,1) + R(2,1)*R(2,1)));
00278       k(3) = atan2(R(2,1), R(1,1));
00279    }
00280 
00281    k.Release(); return k;
00282 }
00283 
00284 
00285 ReturnMatrix ieulzxz(const Matrix & R)
00286   //!  @brief Obtain Roll, Pitch and Yaw from a rotation matrix.
00287 {
00288    ColumnVector  a(3);
00289 
00290    if ((R(3,3)==1)  || (R(3,3)==-1)) {
00291       a(1) = 0.0;
00292       a(2) = ((R(3,3) == 1) ? 0.0 : M_PI);
00293       a(3) = atan2(R(2,1),R(1,1));
00294    } else {
00295       a(1) = atan2(R(1,3), -R(2,3));
00296       a(2) = atan2(sqrt(R(1,3)*R(1,3) + R(2,3)*R(2,3)), R(3,3));
00297       a(3) = atan2(R(3,1), R(3,2));
00298    }
00299 
00300    a.Release(); return a;
00301 }
00302 
00303 #ifdef use_namespace
00304 }
00305 #endif

ROBOOP v1.21a
Generated Tue Aug 16 16:32:14 2005 by Doxygen 1.4.4