$search
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 2005/11/06: Etienne Lachance 00044 - No need to provide a copy constructor and the assignment operator 00045 (operator=) for Spl_Quaternion, Spl_Cubic and Spl_path classes. Instead we 00046 use the one provide by the compiler. 00047 ------------------------------------------------------------------------------- 00048 */ 00049 00050 00051 #ifndef TRAJECTORY_H 00052 #define TRAJECTORY_H 00053 00059 00060 static const char header_trajectory_rcsid[] = "$Id: trajectory.h,v 1.10 2006/05/16 19:24:26 gourdeau Exp $"; 00061 00062 00063 #ifdef _MSC_VER // Microsoft 00064 //#include <string.h> 00065 //#include <iostream.h> 00066 #pragma warning (disable:4786) /* Disable decorated name truncation warnings */ 00067 #endif 00068 //#include <string> 00069 //#include <iostream> 00070 00071 //#include <fstream> 00072 #include <sstream> 00073 #include <map> 00074 #include "quaternion.h" 00075 #include "utils.h" 00076 00077 #ifdef use_namespace 00078 namespace ROBOOP { 00079 using namespace NEWMAT; 00080 #endif 00081 00082 #define K_ZER0 1 00083 #define BAD_DATA -1 00084 #define EXTRAPOLLATION -2 00085 #define NOT_IN_RANGE -3 00086 00091 class Spl_cubic 00092 { 00093 public: 00094 Spl_cubic(){}; 00095 Spl_cubic(const Matrix & pts); 00096 short interpolating(const Real t, ColumnVector & s); 00097 short first_derivative(const Real t, ColumnVector & ds); 00098 short second_derivative(const Real t, ColumnVector & dds); 00099 private: 00100 int nb_path; 00101 Matrix 00102 Ak, Bk, Ck, Dk; 00103 RowVector tk; 00104 bool bad_data; 00105 }; 00106 00107 00108 #define NONE 0 00109 #define JOINT_SPACE 1 00110 #define CARTESIAN_SPACE 2 00111 00113 typedef std::map< Real, ColumnVector, less< Real > > point_map; 00114 00115 00120 class Spl_path : public Spl_cubic 00121 { 00122 public: 00123 Spl_path():Spl_cubic(){}; 00124 Spl_path(const std::string & filename); 00125 Spl_path(const Matrix & x); 00126 short p(const Real time, ColumnVector & p); 00127 short p_pdot(const Real time, ColumnVector & p, ColumnVector & pdot); 00128 short p_pdot_pddot(const Real time, ColumnVector & p, ColumnVector & pdot, 00129 ColumnVector & pdotdot); 00130 short get_type(){ return type; } 00131 double get_final_time(){ return final_time; } 00132 00133 private: 00134 short type; 00135 double final_time; 00136 }; 00137 00138 00140 typedef std::map< Real, Quaternion, less< Real > > quat_map; 00141 00142 00147 class Spl_Quaternion 00148 { 00149 public: 00150 Spl_Quaternion(){} 00151 Spl_Quaternion(const std::string & filename); 00152 Spl_Quaternion(const quat_map & quat); 00153 short quat(const Real t, Quaternion & s); 00154 short quat_w(const Real t, Quaternion & s, ColumnVector & w); 00155 private: 00156 quat_map quat_data; 00157 }; 00158 00159 00164 class Trajectory_Select 00165 { 00166 public: 00167 Trajectory_Select(); 00168 Trajectory_Select(const std::string & filename); 00169 Trajectory_Select & operator=(const Trajectory_Select & x); 00170 00171 void set_trajectory(const std::string & filename); 00172 00173 short type; 00174 Spl_path path; 00175 Spl_Quaternion path_quat; 00176 private: 00177 bool quaternion_active; 00178 }; 00179 00180 #ifdef use_namespace 00181 } 00182 #endif 00183 00184 #endif