Homepage Demos Overview Downloads Tutorials Reference
Credits

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 */
00047 
00048 /*!
00049   @file utils.cpp
00050   @brief Utility functions.
00051 */
00052 
00053 //! @brief RCS/CVS version.
00054 static const char rcsid[] = "$Id: utils.cpp,v 1.4 2004/07/14 02:32:12 ejt Exp $";
00055 
00056 #include "robot.h"
00057 
00058 #ifdef use_namespace
00059 namespace ROBOOP {
00060   using namespace NEWMAT;
00061 #endif
00062 
00063 
00064 ReturnMatrix x_prod_matrix(const ColumnVector & x)
00065 //!  @brief Cross product matrix
00066 {
00067    Matrix S(3,3); S = 0.0;
00068    S(1,2) = -x(3); S(1,3) =  x(2);
00069    S(2,1) =  x(3);                 S(2,3) = -x(1);
00070    S(3,1) = -x(2); S(3,2) =  x(1);
00071 
00072    S.Release(); return (S);
00073 }
00074 
00075 
00076 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past,
00077                         const Real dt)
00078   //!  @brief Trapezoidal integration.
00079 {
00080    ColumnVector integration(present.Nrows());
00081    integration = (past+present)*0.5*dt;
00082    past = present;
00083 
00084    integration.Release();
00085    return ( integration );
00086 }
00087 
00088 
00089 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00090              bool & exit, bool & init),
00091           const Matrix & xo, Real to, Real tf, int nsteps)
00092 //!  @brief Fixed step size fourth-order Runge-Kutta integrator.
00093 {
00094     static Real h, h2, t;
00095     static Matrix k1, k2, k3, k4, x;
00096     static bool first_pass = true, init = false, exit = false;
00097 
00098     if(first_pass || init)
00099     {
00100   h = (tf-to)/nsteps;
00101   h2 = h/2.0;
00102   t = to;
00103   x = xo;
00104   first_pass = false;
00105     }
00106       k1 = (*xdot)(t, x, exit, init) * h;
00107       if(exit) return;
00108       k2 = (*xdot)(t+h2, x+k1*0.5, exit, init)*h;
00109       if(exit) return;
00110       k3 = (*xdot)(t+h2, x+k2*0.5, exit, init)*h;
00111       if(exit) return;
00112       k4 = (*xdot)(t+h, x+k3, exit, init)*h;
00113       if(exit) return;
00114       x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00115       t += h;
00116 }
00117 
00118 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(const Real time, const Matrix & xin),
00119           const Matrix & xo, const Real to, const Real tf, const int nsteps)
00120 {
00121     static Real h, h2, t;
00122     static Matrix k1, k2, k3, k4, x;
00123     static bool first_pass = true;
00124 
00125     if(first_pass)
00126     {
00127   h = (tf-to)/nsteps;
00128   h2 = h/2.0;
00129   t = to;
00130   x = xo;
00131   first_pass = false;
00132     }
00133       k1 = (*xdot)(t, x) * h;
00134       t += h2;
00135       k2 = (*xdot)(t, x+k1*0.5)*h;
00136       k3 = (*xdot)(t, x+k2*0.5)*h;
00137       t += h2;
00138       k4 = (*xdot)(t, x+k3)*h;
00139       x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00140 }
00141 
00142 
00143 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00144             const Matrix & xo, Real to, Real tf, int nsteps,
00145             RowVector & tout, Matrix & xout)
00146 //!  @brief Fixed step size fourth-order Runge-Kutta integrator.
00147 {
00148    Real h, h2, t;
00149    Matrix k1, k2, k3, k4, x;
00150 
00151    h = (tf-to)/nsteps;
00152    h2 = h/2.0;
00153    t = to;
00154    x = xo;
00155    xout = Matrix(xo.Nrows(),(nsteps+1)*xo.Ncols());
00156    xout.SubMatrix(1,xo.Nrows(),1,xo.Ncols()) = x;
00157    tout = RowVector(nsteps+1);
00158    tout(1) = to;
00159    for (int i = 1; i <= nsteps; i++) {
00160       k1 = (*xdot)(t, x) * h;
00161       k2 = (*xdot)(t+h2, x+k1*0.5)*h;
00162       k3 = (*xdot)(t+h2, x+k2*0.5)*h;
00163       k4 = (*xdot)(t+h, x+k3)*h;
00164       x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00165       t += h;
00166       tout(i+1) = t;
00167       xout.SubMatrix(1,xo.Nrows(),i*xo.Ncols()+1,(i+1)*xo.Ncols()) = x;
00168    }
00169 }
00170 
00171 ReturnMatrix rk4(const Matrix & x, const Matrix & dxdt, Real t, Real h,
00172                  ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00173 /*!
00174   @brief Compute one Runge-Kutta fourth order step
00175 
00176   adapted from:
00177   Numerical Recipes in C, The Art of Scientific Computing,
00178   Press, William H. and Flannery, Brian P. and Teukolsky, Saul A.
00179   and Vetterling, William T., Cambridge University Press, 1988.
00180 */
00181 {
00182    Matrix xt, xout, dxm, dxt;
00183    Real th, hh, h6;
00184 
00185    hh = h*0.5;
00186    h6 = h/6.0;
00187    th = t + hh;
00188    xt = x + hh*dxdt;
00189    dxt = (*xdot)(th,xt);
00190    xt = x + hh*dxt;
00191    dxm = (*xdot)(th,xt);
00192    xt = x + h*dxm;
00193    dxm += dxt;
00194    dxt = (*xdot)(t+h,xt);
00195    xout = x+h6*(dxdt+dxt+2.0*dxm);
00196 
00197    xout.Release(); return xout;
00198 }
00199 
00200 #define PGROW -0.20
00201 #define PSHRNK -0.25
00202 #define FCOR 0.06666666
00203 #define SAFETY 0.9
00204 #define ERRCON 6.0E-4
00205 
00206 void rkqc(Matrix & x, Matrix & dxdt, Real & t, Real htry,
00207           Real eps, Matrix & xscal, Real & hdid, Real & hnext,
00208           ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00209 /*!
00210   @brief Compute one adaptive step based on two rk4.
00211 
00212   adapted from:
00213   Numerical Recipes in C, The Art of Scientific Computing,
00214   Press, William H. and Flannery, Brian P. and Teukolsky, Saul A.
00215   and Vetterling, William T., Cambridge University Press, 1988.
00216 */
00217 {
00218    Real tsav, hh, h, temp, errmax;
00219    Matrix dxsav, xsav, xtemp;
00220 
00221    tsav = t;
00222    xsav = x;
00223    dxsav = dxdt;
00224    h = htry;
00225    for(;;) {
00226       hh = 0.5*h;
00227       xtemp = rk4(xsav,dxsav,tsav,hh,xdot);
00228       t = tsav+hh;
00229       dxdt = (*xdot)(t,xtemp);
00230       x = rk4(xtemp,dxdt,t,hh,xdot);
00231       t = tsav+h;
00232       if(t == tsav) {
00233          cerr << "Step size too small in routine RKQC\n";
00234          exit(1);
00235       }
00236       xtemp = rk4(xsav,dxsav,tsav,h,xdot);
00237       errmax = 0.0;
00238       xtemp = x-xtemp;
00239       for(int i = 1; i <= x.Nrows(); i++) {
00240          temp = fabs(xtemp(i,1)/xscal(i,1));
00241          if(errmax < temp) errmax = temp;
00242       }
00243       errmax /= eps;
00244       if(errmax <= 1.0) {
00245          hdid = h;
00246          hnext = (errmax > ERRCON ?
00247                   SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
00248          break;
00249       }
00250       h = SAFETY*h*exp(PSHRNK*log(errmax));
00251    }
00252    x += xtemp*FCOR;
00253 }
00254 
00255 #define MAXSTP 10000
00256 #define TINY 1.0e-30
00257 
00258 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00259             Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00260             int & nok, int & nbad,
00261             RowVector & tout, Matrix & xout, Real dtsav)
00262 /*!
00263   @brief Integrate the ordinary differential equation xdot from time to
00264    to time tf using an adaptive step size strategy 
00265 
00266    adapted from:
00267    Numerical Recipes in C, The Art of Scientific Computing,
00268    Press, William H. and Flannery, Brian P. and Teukolsky, Saul A.
00269    and Vetterling, William T., Cambridge University Press, 1988.
00270 */
00271 {
00272    Real tsav, t, hnext, hdid, h;
00273    RowVector tv(1);
00274 
00275    Matrix xscal, x, dxdt;
00276 
00277    tv = to;
00278    tout = tv;
00279    xout = xo;
00280    xscal = xo;
00281    t = to;
00282    h = (tf > to) ? fabs(h1) : -fabs(h1);
00283    nok = (nbad) = 0;
00284    x = xo;
00285    tsav = t;
00286    for(int nstp = 1; nstp <= MAXSTP; nstp++){
00287       dxdt = (*xdot)(t,x);
00288       for(int i = 1; i <= x.Nrows(); i++)
00289          xscal(i,1) = fabs(x(i,1))+fabs(dxdt(i,1)*h)+TINY;
00290       if((t+h-tf)*(t+h-to) > 0.0) h = tf-t;
00291       rkqc(x,dxdt,t,h,eps,xscal,hdid,hnext,xdot);
00292       if(hdid == h) ++(nok); else ++(nbad);
00293       if((t-tf)*(tf-to) >= 0.0) {
00294          xo = x;
00295          tv = t;
00296          tout = tout | tv;
00297          xout = xout | x;
00298          return;
00299       }
00300       if(fabs(t-tsav) > fabs(dtsav)) {
00301          tv = t;
00302          tout = tout | tv;
00303          xout = xout | x;
00304          tsav = t;
00305       }
00306       if(fabs(hnext) <= hmin) {
00307          cerr << "Step size too small in ODEINT\n";
00308          cerr << setw(7) << setprecision(3) << (tout & xout).t();
00309          exit(1);
00310       }
00311       h = hnext;
00312    }
00313    cerr << "Too many step in routine ODEINT\n";
00314    exit(1);
00315 }
00316 
00317 ReturnMatrix sign(const Matrix & x)
00318 //!  @brief Sign of a matrix.
00319 {
00320    Matrix out = x;
00321 
00322    for(int i = 1; i <= out.Ncols(); i++) {
00323       for(int j = 1; j <= out.Nrows(); j++) {
00324          if(out(j,i) > 0.0) {
00325             out(j,i) = 1.0;
00326          } else if(out(j,i) < 0.0) {
00327             out(j,i) = -1.0;
00328          }
00329       }
00330    }
00331 
00332    out.Release(); return out;
00333 }
00334 
00335 
00336 short sign(const Real x)
00337   //!  @brief Sign of real.
00338 {
00339    short i = 1;
00340    if(x > 0.0)
00341       i = 1;
00342    else
00343       i = -1;
00344 
00345    return (i);
00346 }
00347 
00348 #ifdef use_namespace
00349 }
00350 #endif
00351 

ROBOOP v1.21a
Generated Tue Nov 23 16:35:51 2004 by Doxygen 1.3.9.1