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

ROBOOP v1.21a
Generated Tue Jan 4 15:42:24 2005 by Doxygen 1.4.0