utils.h
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 1996-2004 Richard Gourdeau
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 Richard Gourdeau
23 Professeur Agrege
24 Departement de genie electrique
25 Ecole Polytechnique de Montreal
26 C.P. 6079, Succ. Centre-Ville
27 Montreal, Quebec, H3C 3A7
28 
29 email: richard.gourdeau@polymtl.ca
30 
31 -------------------------------------------------------------------------------
32 Revision_history:
33 
34 2004/07/01: Etienne Lachance
35  -Added doxygen documentation.
36 
37 2004/07/01: Ethan Tira-Thompson
38  -Added support for newmat's use_namespace #define, using ROBOOP namespace
39 
40 2004/09/18: Etienne Lachance
41  -Added deg2rad rad2deg
42 
43 2005/06/10: Carmine Lia
44  -Added pinv
45 -------------------------------------------------------------------------------
46 */
47 
48 #ifndef __cplusplus
49 #error Must use C++ for the type Robot
50 #endif
51 #ifndef UTILS_H
52 #define UTILS_H
53 
59 static const char header_utils_rcsid[] = "$Id: utils.h,v 1.10 2006/05/16 16:11:15 gourdeau Exp $";
61 
62 #ifdef _MSC_VER // Microsoft
63 #pragma warning (disable:4786) /* Disable decorated name truncation warnings */
64 #endif
65 #include <stdio.h>
66 #include <limits>
67 #define WANT_STRING /* include.h will get string fns */
68 #define WANT_STREAM /* include.h will get stream fns */
69 #define WANT_FSTREAM /* include.h will get fstream fns */
70 #define WANT_MATH /* include.h will get math fns */
71  /* newmatap.h will get include.h */
72 
73 #include "newmatap.h" /* need matrix applications */
74 
75 #include "newmatio.h" /* need matrix output routines */
76 
77 #ifdef use_namespace
78 namespace ROBOOP {
79  using namespace NEWMAT;
80 #endif
81 
82 #ifndef M_PI
83 #define M_PI 3.14159265358979
84 #endif
85 
86 #define GRAVITY 9.81
87 
88 // global variables
89 extern Real fourbyfourident[];
90 extern Real threebythreeident[];
91 
92 // angle conversion
93 inline double deg2rad(const double angle_deg){ return angle_deg*M_PI/180; }
94 inline double rad2deg(const double angle_rad){ return angle_rad*180/M_PI; }
95 
96 // vector operation
97 
99 
100 ReturnMatrix pinv(const Matrix & M);
101 
102 // numerical analysis tools
103 
104 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past, const Real dt);
105 
106 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
107  const Matrix & xo, Real to, Real tf, int nsteps,
108  RowVector & tout, Matrix & xout);
109 
110 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
111  const Matrix & xo, Real to, Real tf, int nsteps);
112 
113 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
114  bool & exit, bool & init),
115  const Matrix & xo, Real to, Real tf, int nsteps);
116 
117 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
118  Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
119  int & nok, int & nbad,
120  RowVector & tout, Matrix & xout, Real dtsav);
121 
122 ReturnMatrix sign(const Matrix & x);
123 
124 short sign(const Real x);
125 
126 const double epsilon = 0.0000001;
127 
128 inline bool isZero(const double x)
129 {
130  if ( fabs(x) < epsilon)
131  {
132  return true;
133  }
134  return false;
135 }
136 
137 
138 // translation
140 
141 // rotation matrices
142 ReturnMatrix rotx(const Real alpha);
143 ReturnMatrix roty(const Real beta);
144 ReturnMatrix rotz(const Real gamma);
145 ReturnMatrix rotk(const Real theta, const ColumnVector & k);
146 
147 ReturnMatrix rpy(const ColumnVector & a);
148 ReturnMatrix eulzxz(const ColumnVector & a);
149 ReturnMatrix rotd(const Real theta, const ColumnVector & k1, const ColumnVector & k2);
150 
151 
152 // inverse on rotation matrices
153 ReturnMatrix irotk(const Matrix & R);
154 ReturnMatrix irpy(const Matrix & R);
155 ReturnMatrix ieulzxz(const Matrix & R);
156 
157 #ifdef use_namespace
158 }
159 #endif
160 
161 #endif
162 
ReturnMatrix x_prod_matrix(const ColumnVector &x)
Cross product matrix.
Definition: utils.cpp:88
ReturnMatrix rotx(const Real alpha)
Rotation around x axis.
Definition: homogen.cpp:87
static const char header_utils_rcsid[]
RCS/CVS version.
Definition: utils.h:60
void odeint(ReturnMatrix(*xdot)(Real time, const Matrix &xin), Matrix &xo, Real to, Real tf, Real eps, Real h1, Real hmin, int &nok, int &nbad, RowVector &tout, Matrix &xout, Real dtsav)
Integrate the ordinary differential equation xdot from time to to time tf using an adaptive step size...
Definition: utils.cpp:347
ReturnMatrix roty(const Real beta)
Rotation around x axis.
Definition: homogen.cpp:107
ReturnMatrix rotz(const Real gamma)
Rotation around z axis.
Definition: homogen.cpp:127
ReturnMatrix eulzxz(const ColumnVector &a)
Euler ZXZ rotation.
Definition: homogen.cpp:211
ReturnMatrix sign(const Matrix &x)
Sign of a matrix.
Definition: utils.cpp:406
ReturnMatrix irpy(const Matrix &R)
Obtain Roll, Pitch and Yaw from a rotation matrix.
Definition: homogen.cpp:271
double Real
Definition: include.h:307
bool isZero(const double x)
Definition: utils.h:128
ReturnMatrix irotk(const Matrix &R)
Obtain axis from a rotation matrix.
Definition: homogen.cpp:253
ReturnMatrix rpy(const ColumnVector &a)
Roll Pitch Yaw rotation.
Definition: homogen.cpp:182
void Runge_Kutta4(ReturnMatrix(*xdot)(Real time, const Matrix &xin), const Matrix &xo, Real to, Real tf, int nsteps, RowVector &tout, Matrix &xout)
Fixed step size fourth-order Runge-Kutta integrator.
Definition: utils.cpp:232
ReturnMatrix rotd(const Real theta, const ColumnVector &k1, const ColumnVector &k2)
Rotation around an arbitrary line.
Definition: homogen.cpp:240
void Runge_Kutta4_Real_time(ReturnMatrix(*xdot)(Real time, const Matrix &xin), const Matrix &xo, Real to, Real tf, int nsteps)
ReturnMatrix trans(const ColumnVector &a)
Translation.
Definition: homogen.cpp:68
The usual rectangular matrix.
Definition: newmat.h:625
FloatVector FloatVector * a
FloatVector FloatVector FloatVector * alpha
ReturnMatrix xdot(Real t, const Matrix &x)
Definition: demo.cpp:231
ReturnMatrix ieulzxz(const Matrix &R)
Obtain Roll, Pitch and Yaw from a rotation matrix.
Definition: homogen.cpp:294
Row vector.
Definition: newmat.h:953
double rad2deg(const double angle_rad)
Definition: utils.h:94
Real threebythreeident[]
Used to initialize a matrix.
Definition: robot.cpp:136
#define M_PI
Definition: utils.h:83
Column vector.
Definition: newmat.h:1008
ReturnMatrix pinv(const Matrix &M)
Matrix pseudo inverse using SVD.
Definition: utils.cpp:99
ReturnMatrix rotk(const Real theta, const ColumnVector &k)
Rotation around arbitrary axis.
Definition: homogen.cpp:149
double deg2rad(const double angle_deg)
Definition: utils.h:93
const double epsilon
Definition: utils.h:126
ReturnMatrix Integ_Trap(const ColumnVector &present, ColumnVector &past, const Real dt)
Trapezoidal integration.
Definition: utils.cpp:165
Real fourbyfourident[]
Used to initialize a matrix.
Definition: robot.cpp:133


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:17