Homepage Demos Overview Downloads Tutorials Reference
Credits

trajectory.h

Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2002-2004  Etienne Lachance
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00022 
00023 Reference:
00024 
00025 [1] J. Angeles, Fundamentals of Robotic Mechanical Systems: Theory, Methods and Algorithms,
00026     Springer-Verlag, Mechanical Engineering Series, 1997, ISBN: 0-387-94540-7.
00027 
00028 [2] E.B. Dam, M. Koch and M. Lillholm, "Quaternions, Interpolation and Animation",
00029     tech report of University of Copenhagen, number: DIKU-TR-98/5, 1998.
00030 
00031 -------------------------------------------------------------------------------
00032 Revision_history:
00033 
00034 2004/05/22: Etienne Lachance
00035    -Added class Trajectory_Select.
00036 
00037 2004/07/01: Etienne Lachance
00038    -Added doxygen documentation.
00039 
00040 2004/07/01: Ethan Tira-Thompson
00041     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00042 -------------------------------------------------------------------------------
00043 */
00044 
00045 
00046 #ifndef TRAJECTORY_H
00047 #define TRAJECTORY_H
00048 
00049 /*!
00050   @file trajectory.h
00051   @brief Header file for trajectory generation class.
00052 */
00053 
00054 //! @brief RCS/CVS version.
00055 static const char header_trajectory_rcsid[] = "$Id: trajectory.h,v 1.4 2004/07/14 02:32:12 ejt Exp $";
00056 
00057 
00058 #ifdef _MSC_VER                         // Microsoft
00059 //#include <string.h>
00060 //#include <iostream.h>
00061 #pragma warning (disable:4786)  /* Disable decorated name truncation warnings */
00062 #endif
00063 //#include <string>
00064 //#include <iostream>
00065 
00066 //#include <fstream>
00067 #ifdef __WATCOMC__
00068 #include <strstrea.h>
00069 #else
00070 #include <sstream>
00071 #endif
00072 #include <map>
00073 #include "quaternion.h"
00074 #include "robot.h"
00075 
00076 #ifdef use_namespace
00077 namespace ROBOOP {
00078   using namespace NEWMAT;
00079 #endif
00080 
00081 #define K_ZER0          1
00082 #define BAD_DATA       -1
00083 #define EXTRAPOLLATION -2
00084 #define NOT_IN_RANGE   -3
00085 
00086 /*!
00087   @class Spl_cubic
00088   @brief Natural cubic splines class.
00089 */
00090 class Spl_cubic
00091 {
00092 public:
00093    Spl_cubic(){};
00094    Spl_cubic(const Matrix & pts);
00095    Spl_cubic(const Spl_cubic & x);
00096    Spl_cubic & operator=(const Spl_cubic & x);
00097    short interpolating(const Real t, ColumnVector & s);
00098    short first_derivative(const Real t, ColumnVector & ds);
00099    short second_derivative(const Real t, ColumnVector & dds);
00100 private:
00101    int nb_path;    //!< Number of path, i.e: path in x,y,z nb_path=3
00102    Matrix
00103    Ak, Bk, Ck, Dk;
00104    RowVector tk;   //!< Time at control points.
00105    bool bad_data;  //!< Status flag.
00106 };
00107 
00108 
00109 #define NONE            0
00110 #define JOINT_SPACE     1
00111 #define CARTESIAN_SPACE 2
00112 
00113 //! @brief Data at control points.
00114 typedef map< Real, ColumnVector, less< Real > > point_map;
00115 
00116 
00117 /*!
00118   @class Spl_path
00119   @brief Cartesian or joint space trajectory
00120 */
00121 class Spl_path : public Spl_cubic
00122 {
00123 public:
00124    Spl_path():Spl_cubic(){};
00125    Spl_path(const string & filename);
00126    Spl_path(const Matrix & x);
00127    Spl_path(const Spl_path & x);
00128    Spl_path & operator=(const Spl_path & x);
00129    short p(const Real time, ColumnVector & p);
00130    short p_pdot(const Real time, ColumnVector & p, ColumnVector & pdot);
00131    short p_pdot_pddot(const Real time, ColumnVector & p, ColumnVector & pdot,
00132                       ColumnVector & pdotdot);
00133    short get_type(){ return type; }
00134    double get_final_time(){ return final_time; }
00135    
00136 private:
00137    short type;        //!< Cartesian space or joint space.
00138    double final_time; //!< Spline final time.
00139 };
00140 
00141 
00142 //! @brief Data at control points.
00143 typedef map< Real, Quaternion, less< Real > > quat_map;
00144 
00145 
00146 /*!
00147   @class Spl_Quaternion
00148   @brief Cubic quaternions spline.
00149 */
00150 class Spl_Quaternion
00151 {
00152 public:
00153    Spl_Quaternion(){}
00154    Spl_Quaternion(const string & filename);
00155    Spl_Quaternion(const quat_map & quat);
00156    Spl_Quaternion(const Spl_Quaternion & x);
00157    Spl_Quaternion & operator=(const Spl_Quaternion & x);
00158    short quat(const Real t, Quaternion & s);
00159    short quat_w(const Real t, Quaternion & s, ColumnVector & w);
00160 private:
00161    quat_map quat_data;  //!< Data at control points.
00162 };
00163 
00164 
00165 /*!
00166   @class Trajectory_Select
00167   @brief Trajectory class selection.
00168 */
00169 class Trajectory_Select
00170 {
00171 public:
00172     Trajectory_Select();
00173     Trajectory_Select(const string & filename);
00174     Trajectory_Select(const Trajectory_Select & x);
00175     Trajectory_Select & operator=(const Trajectory_Select & x);
00176 
00177     void set_trajectory(const string & filename);
00178 
00179     short type;               //!< Cartesian or joint space
00180     Spl_path path;            //!< Spl_path instance.
00181     Spl_Quaternion path_quat; //!< Spl_Quaternion instance.
00182 private:
00183     bool quaternion_active;   //!< Using Spl_Quaternion.
00184 };
00185 
00186 #ifdef use_namespace
00187 }
00188 #endif
00189 
00190 #endif

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