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


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32