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

ROBOOP v1.21a
Generated Thu Nov 22 00:51:28 2007 by Doxygen 1.5.4