00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #ifndef TRAJECTORY_H
00047 #define TRAJECTORY_H
00048
00049
00050
00051
00052
00053
00054
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
00060
00061 #pragma warning (disable:4786)
00062 #endif
00063
00064
00065
00066
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
00088
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;
00102 Matrix
00103 Ak, Bk, Ck, Dk;
00104 RowVector tk;
00105 bool bad_data;
00106 };
00107
00108
00109 #define NONE 0
00110 #define JOINT_SPACE 1
00111 #define CARTESIAN_SPACE 2
00112
00113
00114 typedef map< Real, ColumnVector, less< Real > > point_map;
00115
00116
00117
00118
00119
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;
00138 double final_time;
00139 };
00140
00141
00142
00143 typedef map< Real, Quaternion, less< Real > > quat_map;
00144
00145
00146
00147
00148
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;
00162 };
00163
00164
00165
00166
00167
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;
00180 Spl_path path;
00181 Spl_Quaternion path_quat;
00182 private:
00183 bool quaternion_active;
00184 };
00185
00186 #ifdef use_namespace
00187 }
00188 #endif
00189
00190 #endif