utils.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 2003/02/03: Etienne Lachance
00035    -Added functions x_prod_matrix, vec_dot_prod, Integ_Trap and sign.
00036    -Working on Runge_Kutta4_etienne.
00037 
00038 2004/07/01: Etienne Lachance
00039    -Removed function vec_x_prod and vec_dot_prod. Now we use CrossProduct
00040     and DotProduct, from Newmat library.
00041    -Added doxygen documentation.
00042 
00043 2004/07/01: Ethan Tira-Thompson
00044     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00045 
00046 2005/06/10: Carmine Lia
00047    -Added pinv
00048 
00049 2005/06/29 Richard Gourdeau
00050    -Fixed max() bug for VC++ 6.0
00051 -------------------------------------------------------------------------------
00052 */
00053 
00059 
00060 static const char rcsid[] = "$Id: utils.cpp,v 1.26 2006/05/16 16:11:15 gourdeau Exp $";
00061 
00062 #include "utils.h"
00063 
00064 #ifdef _MSC_VER
00065 #if _MSC_VER  < 1300                       // Microsoft
00066 #ifndef GUARD_minmax_H
00067 #define GUARD_minmax_H
00068 // needed to cope with bug in MS library:
00069 // it fails to define min/max
00070 template <class T> inline T max(const T& a, const T& b)
00071 {
00072    return (a > b) ? a : b;
00073 }
00074 template <class T> inline T min(const T& a, const T& b)
00075 {
00076    return (a < b) ? a : b;
00077 }
00078 #endif
00079 #endif
00080 #endif
00081 
00082 #ifdef use_namespace
00083 namespace ROBOOP {
00084   using namespace NEWMAT;
00085 #endif
00086 
00087 
00088 ReturnMatrix x_prod_matrix(const ColumnVector & x)
00090 {
00091    Matrix S(3,3); S = 0.0;
00092    S(1,2) = -x(3); S(1,3) =  x(2);
00093    S(2,1) =  x(3);                 S(2,3) = -x(1);
00094    S(3,1) = -x(2); S(3,2) =  x(1);
00095 
00096    S.Release(); return (S);
00097 }
00098 
00099 ReturnMatrix pinv(const Matrix & M)
00121 {
00122   /* floating point eps */
00123   const Real eps = numeric_limits<Real>::epsilon(); 
00124   
00125   int m = M.nrows();
00126   int n = M.ncols();
00127   
00128   if(n > m)
00129   {
00130     Matrix X = pinv(M.t()).t();
00131     return X;
00132    }
00133   
00134   Matrix U, V;
00135   
00136   DiagonalMatrix Q;
00137   
00138   Matrix X(n, m);
00139   
00140   SVD(M, Q, U, V);
00141  //cout << "Q\n" << Q << "U\n" << U << "V\n" << V << endl;
00142   Real tol = max(m,n)*Q(1)*eps;
00143  // cout << "\ntol\n" << tol << "\neps\n" << eps << endl; 
00144   // conteggio dei sv != 0
00145   int r = 0;
00146   for(int i = 1; i <= Q.size(); i++)
00147       if(Q(i) > tol)
00148          r++;
00149   
00150   if(r != 0)
00151   {
00152      DiagonalMatrix D(r);
00153      for(int i = 1; i <= r; i++)
00154         D(i) = 1/Q(i);
00155      //cout << V.SubMatrix(1,V.nrows(),1,r) << endl;
00156      //cout << D << endl;
00157      //cout << U.SubMatrix(1,U.nrows(),1,r).t() << endl;
00158      X = V.SubMatrix(1,V.nrows(),1,r)*D*U.SubMatrix(1,U.nrows(),1,r).t();
00159   
00160   }
00161   X.Release();
00162   return X;
00163 }  
00164 
00165 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past,
00166                         const Real dt)
00168 {
00169    ColumnVector integration(present.Nrows());
00170    integration = (past+present)*0.5*dt;
00171    past = present;
00172 
00173    integration.Release();
00174    return ( integration );
00175 }
00176 
00177 
00178 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00179                                                  bool & exit, bool & init),
00180                             const Matrix & xo, Real to, Real tf, int nsteps)
00182 {
00183     static Real h, h2, t;
00184     static Matrix k1, k2, k3, k4, x;
00185     static bool first_pass = true, init = false, exit = false;
00186 
00187     if(first_pass || init)
00188     {
00189         h = (tf-to)/nsteps;
00190         h2 = h/2.0;
00191         t = to;
00192         x = xo;
00193         first_pass = false;
00194     }
00195       k1 = (*xdot)(t, x, exit, init) * h;
00196       if(exit) return;
00197       k2 = (*xdot)(t+h2, x+k1*0.5, exit, init)*h;
00198       if(exit) return;
00199       k3 = (*xdot)(t+h2, x+k2*0.5, exit, init)*h;
00200       if(exit) return;
00201       k4 = (*xdot)(t+h, x+k3, exit, init)*h;
00202       if(exit) return;
00203       x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00204       t += h;
00205 }
00206 
00207 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(const Real time, const Matrix & xin),
00208                             const Matrix & xo, const Real to, const Real tf, const int nsteps)
00209 {
00210     static Real h, h2, t;
00211     static Matrix k1, k2, k3, k4, x;
00212     static bool first_pass = true;
00213 
00214     if(first_pass)
00215     {
00216         h = (tf-to)/nsteps;
00217         h2 = h/2.0;
00218         t = to;
00219         x = xo;
00220         first_pass = false;
00221     }
00222       k1 = (*xdot)(t, x) * h;
00223       t += h2;
00224       k2 = (*xdot)(t, x+k1*0.5)*h;
00225       k3 = (*xdot)(t, x+k2*0.5)*h;
00226       t += h2;
00227       k4 = (*xdot)(t, x+k3)*h;
00228       x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00229 }
00230 
00231 
00232 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00233             const Matrix & xo, Real to, Real tf, int nsteps,
00234             RowVector & tout, Matrix & xout)
00236 {
00237    Real h, h2, t;
00238    Matrix k1, k2, k3, k4, x;
00239 
00240    h = (tf-to)/nsteps;
00241    h2 = h/2.0;
00242    t = to;
00243    x = xo;
00244    xout = Matrix(xo.Nrows(),(nsteps+1)*xo.Ncols());
00245    xout.SubMatrix(1,xo.Nrows(),1,xo.Ncols()) = x;
00246    tout = RowVector(nsteps+1);
00247    tout(1) = to;
00248    for (int i = 1; i <= nsteps; i++) {
00249       k1 = (*xdot)(t, x) * h;
00250       k2 = (*xdot)(t+h2, x+k1*0.5)*h;
00251       k3 = (*xdot)(t+h2, x+k2*0.5)*h;
00252       k4 = (*xdot)(t+h, x+k3)*h;
00253       x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00254       t += h;
00255       tout(i+1) = t;
00256       xout.SubMatrix(1,xo.Nrows(),i*xo.Ncols()+1,(i+1)*xo.Ncols()) = x;
00257    }
00258 }
00259 
00260 ReturnMatrix rk4(const Matrix & x, const Matrix & dxdt, Real t, Real h,
00261                  ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00270 {
00271    Matrix xt, xout, dxm, dxt;
00272    Real th, hh, h6;
00273 
00274    hh = h*0.5;
00275    h6 = h/6.0;
00276    th = t + hh;
00277    xt = x + hh*dxdt;
00278    dxt = (*xdot)(th,xt);
00279    xt = x + hh*dxt;
00280    dxm = (*xdot)(th,xt);
00281    xt = x + h*dxm;
00282    dxm += dxt;
00283    dxt = (*xdot)(t+h,xt);
00284    xout = x+h6*(dxdt+dxt+2.0*dxm);
00285 
00286    xout.Release(); return xout;
00287 }
00288 
00289 #define PGROW -0.20
00290 #define PSHRNK -0.25
00291 #define FCOR 0.06666666
00292 #define SAFETY 0.9
00293 #define ERRCON 6.0E-4
00294 
00295 void rkqc(Matrix & x, Matrix & dxdt, Real & t, Real htry,
00296           Real eps, Matrix & xscal, Real & hdid, Real & hnext,
00297           ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00306 {
00307    Real tsav, hh, h, temp, errmax;
00308    Matrix dxsav, xsav, xtemp;
00309 
00310    tsav = t;
00311    xsav = x;
00312    dxsav = dxdt;
00313    h = htry;
00314    for(;;) {
00315       hh = 0.5*h;
00316       xtemp = rk4(xsav,dxsav,tsav,hh,xdot);
00317       t = tsav+hh;
00318       dxdt = (*xdot)(t,xtemp);
00319       x = rk4(xtemp,dxdt,t,hh,xdot);
00320       t = tsav+h;
00321       if(t == tsav) {
00322          cerr << "Step size too small in routine RKQC\n";
00323          exit(1);
00324       }
00325       xtemp = rk4(xsav,dxsav,tsav,h,xdot);
00326       errmax = 0.0;
00327       xtemp = x-xtemp;
00328       for(int i = 1; i <= x.Nrows(); i++) {
00329          temp = fabs(xtemp(i,1)/xscal(i,1));
00330          if(errmax < temp) errmax = temp;
00331       }
00332       errmax /= eps;
00333       if(errmax <= 1.0) {
00334          hdid = h;
00335          hnext = (errmax > ERRCON ?
00336                   SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
00337          break;
00338       }
00339       h = SAFETY*h*exp(PSHRNK*log(errmax));
00340    }
00341    x += xtemp*FCOR;
00342 }
00343 
00344 #define MAXSTP 10000
00345 #define TINY 1.0e-30
00346 
00347 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00348             Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00349             int & nok, int & nbad,
00350             RowVector & tout, Matrix & xout, Real dtsav)
00360 {
00361    Real tsav, t, hnext, hdid, h;
00362    RowVector tv(1);
00363 
00364    Matrix xscal, x, dxdt;
00365 
00366    tv = to;
00367    tout = tv;
00368    xout = xo;
00369    xscal = xo;
00370    t = to;
00371    h = (tf > to) ? fabs(h1) : -fabs(h1);
00372    nok = (nbad) = 0;
00373    x = xo;
00374    tsav = t;
00375    for(int nstp = 1; nstp <= MAXSTP; nstp++){
00376       dxdt = (*xdot)(t,x);
00377       for(int i = 1; i <= x.Nrows(); i++)
00378          xscal(i,1) = fabs(x(i,1))+fabs(dxdt(i,1)*h)+TINY;
00379       if((t+h-tf)*(t+h-to) > 0.0) h = tf-t;
00380       rkqc(x,dxdt,t,h,eps,xscal,hdid,hnext,xdot);
00381       if(hdid == h) ++(nok); else ++(nbad);
00382       if((t-tf)*(tf-to) >= 0.0) {
00383          xo = x;
00384          tv = t;
00385          tout = tout | tv;
00386          xout = xout | x;
00387          return;
00388       }
00389       if(fabs(t-tsav) > fabs(dtsav)) {
00390          tv = t;
00391          tout = tout | tv;
00392          xout = xout | x;
00393          tsav = t;
00394       }
00395       if(fabs(hnext) <= hmin) {
00396          cerr << "Step size too small in ODEINT\n";
00397          cerr << setw(7) << setprecision(3) << (tout & xout).t();
00398          exit(1);
00399       }
00400       h = hnext;
00401    }
00402    cerr << "Too many step in routine ODEINT\n";
00403    exit(1);
00404 }
00405 
00406 ReturnMatrix sign(const Matrix & x)
00408 {
00409    Matrix out = x;
00410 
00411    for(int i = 1; i <= out.Ncols(); i++) {
00412       for(int j = 1; j <= out.Nrows(); j++) {
00413          if(out(j,i) > 0.0) {
00414             out(j,i) = 1.0;
00415          } else if(out(j,i) < 0.0) {
00416             out(j,i) = -1.0;
00417          }
00418       }
00419    }
00420 
00421    out.Release(); return out;
00422 }
00423 
00424 
00425 short sign(const Real x)
00427 {
00428    short i = 1;
00429    if(x > 0.0)
00430       i = 1;
00431    else
00432       i = -1;
00433 
00434    return (i);
00435 }
00436 
00437 #ifdef use_namespace
00438 }
00439 #endif
00440 


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