utils.h
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 doxygen documentation.
00036 
00037 2004/07/01: Ethan Tira-Thompson
00038     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00039 
00040 2004/09/18: Etienne Lachance
00041    -Added deg2rad rad2deg
00042 
00043 2005/06/10: Carmine Lia
00044    -Added pinv
00045 -------------------------------------------------------------------------------
00046 */
00047 
00048 #ifndef __cplusplus
00049 #error Must use C++ for the type Robot
00050 #endif
00051 #ifndef UTILS_H
00052 #define UTILS_H
00053 
00059 
00060 static const char header_utils_rcsid[] = "$Id: utils.h,v 1.10 2006/05/16 16:11:15 gourdeau Exp $";
00061 
00062 #ifdef _MSC_VER                         // Microsoft
00063 #pragma warning (disable:4786)  /* Disable decorated name truncation warnings */
00064 #endif
00065 #include <stdio.h>
00066 #include <limits>
00067 #define WANT_STRING                  /* include.h will get string fns */
00068 #define WANT_STREAM                  /* include.h will get stream fns */
00069 #define WANT_FSTREAM                 /* include.h will get fstream fns */
00070 #define WANT_MATH                    /* include.h will get math fns */
00071                                      /* newmatap.h will get include.h */
00072 
00073 #include "newmatap.h"                /* need matrix applications */
00074 
00075 #include "newmatio.h"                /* need matrix output routines */
00076 
00077 #ifdef use_namespace
00078 namespace ROBOOP {
00079   using namespace NEWMAT;
00080 #endif
00081 
00082 #ifndef M_PI
00083 #define M_PI 3.14159265358979
00084 #endif
00085 
00086 #define GRAVITY 9.81
00087 
00088 // global variables
00089 extern Real fourbyfourident[];
00090 extern Real threebythreeident[];
00091 
00092 // angle conversion
00093 inline double deg2rad(const double angle_deg){ return angle_deg*M_PI/180; }
00094 inline double rad2deg(const double angle_rad){ return angle_rad*180/M_PI; }
00095 
00096 // vector operation 
00097 
00098 ReturnMatrix x_prod_matrix(const ColumnVector & x);
00099 
00100 ReturnMatrix pinv(const Matrix & M);
00101 
00102 // numerical analysis tools
00103 
00104 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past, const Real dt);
00105 
00106 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00107                   const Matrix & xo, Real to, Real tf, int nsteps,
00108                   RowVector & tout, Matrix & xout);
00109 
00110 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00111                             const Matrix & xo, Real to, Real tf, int nsteps);
00112 
00113 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00114                             bool & exit, bool & init),
00115                             const Matrix & xo, Real to, Real tf, int nsteps);
00116 
00117 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00118             Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00119             int & nok, int & nbad,
00120             RowVector & tout, Matrix & xout, Real dtsav);
00121 
00122 ReturnMatrix sign(const Matrix & x);
00123 
00124 short sign(const Real x);
00125 
00126 const double epsilon = 0.0000001;
00127 
00128 inline bool isZero(const double x)
00129 {
00130     if ( fabs(x) < epsilon)
00131     {
00132         return true;
00133     }
00134     return false;
00135 }
00136 
00137 
00138 // translation 
00139 ReturnMatrix trans(const ColumnVector & a);
00140 
00141 // rotation matrices 
00142 ReturnMatrix rotx(const Real alpha);
00143 ReturnMatrix roty(const Real beta);
00144 ReturnMatrix rotz(const Real gamma);
00145 ReturnMatrix rotk(const Real theta, const ColumnVector & k);
00146 
00147 ReturnMatrix rpy(const ColumnVector & a);
00148 ReturnMatrix eulzxz(const ColumnVector & a);
00149 ReturnMatrix rotd(const Real theta, const ColumnVector & k1, const ColumnVector & k2);
00150 
00151 
00152 // inverse on rotation matrices 
00153 ReturnMatrix irotk(const Matrix & R);
00154 ReturnMatrix irpy(const Matrix & R);
00155 ReturnMatrix ieulzxz(const Matrix & R);
00156 
00157 #ifdef use_namespace
00158 }
00159 #endif
00160 
00161 #endif
00162 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:55