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

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