00001 /******************************************************************** 00002 * * 00003 * RefValJS_PTP_Trajectory * 00004 * * 00005 ********************************************************************/ 00006 00007 #ifndef _REFVALJS_PTP_TRAJECTORY_H_ 00008 #define _REFVALJS_PTP_TRAJECTORY_H_ 00009 #include <ros/console.h> 00010 #include <cob_trajectory_controller/RefVal_JS.h> 00011 #include <vector> 00012 #include <cob_trajectory_controller/BSplineND.h> 00013 #include <trajectory_msgs/JointTrajectory.h> 00014 00015 00016 class RefValJS_PTP_Trajectory : public RefVal_JS 00017 { 00018 public: 00019 /* smooth = false: 00020 additional knots between the trajectory points will be generated to make the spline follow straight lines 00021 and just have the corners rounded (default) 00022 smooth = true: 00023 no additional points will be generated, path will be lot smoother, but will have significant differences 00024 between a path were the trajectory points are connected by straight lines. Do not use in conjunction with 00025 a path planner! 00026 */ 00027 RefValJS_PTP_Trajectory(const trajectory_msgs::JointTrajectory& trajectory, double v_rad_s, double a_rad_s2, bool smooth=false); 00028 //RefValJS_PTP_Trajectory(const std::vector<Jointd>& trajectory, Jointd start, Jointd startvel, double v_rad_s, double a_rad_s2, bool smooth=false); 00029 00030 std::vector<double> r(double s) const; 00031 double s(double t) const; 00032 00033 std::vector<double> dr_ds(double s) const; 00034 double ds_dt(double t) const; 00035 00036 double getTotalTime() const { return m_T1 + m_T2 + m_T3; } 00037 00038 00039 std::vector<double> getLengthParts() const { return m_length_parts; } 00040 00041 protected: 00042 double norm(const std::vector<double>& j); 00043 double norm_max(const std::vector<double>& j); 00044 double norm_sqr(const std::vector<double>& j); 00045 double norm_weighted(const std::vector<double>& j); 00046 trajectory_msgs::JointTrajectory m_trajectory; 00047 00048 typedef std::vector<double> vecd; 00049 typedef std::vector<double>::const_iterator vecd_it; 00050 vecd m_length_parts; 00051 vecd m_length_cumulated; 00052 vecd m_s_parts; 00053 00054 BSplineND< std::vector<double> > m_TrajectorySpline; 00055 std::vector< std::vector<double> > m_SplinePoints; 00056 00057 00058 double m_stepSize; 00059 double m_length; 00060 double m_param_length; 00061 00062 double m_v_rad_s; 00063 double m_a_rad_s2; 00064 00065 double m_T1; // Dauer der Phase konst. Beschl. 00066 double m_T2; // Dauer der Phase konst. Geschw. 00067 double m_T3; // Dauer der Phase konst. Verzög. 00068 00069 double m_sa1; // "Beschl." des Wegparameters s in Phase 1 00070 double m_sv2; // "Geschw." des Wegparameters s in Phase 2 00071 double m_sa3; // "Verzög." des Wegparameters s in Phase 3 00072 00073 static const double weigths[]; 00074 00075 }; 00076 00077 00078 inline double RefValJS_PTP_Trajectory::norm(const std::vector<double>& j) 00079 { 00080 // which norm should be used? 00081 return norm_weighted(j); 00082 } 00083 00084 inline double RefValJS_PTP_Trajectory::norm_max(const std::vector<double>& j) 00085 { 00086 double max = j.at(0); 00087 for (unsigned int i = 0; i<j.size(); i++) 00088 { 00089 if(j.at(i) < max) 00090 max = j.at(i); 00091 } 00092 return max; 00093 } 00094 00095 inline double RefValJS_PTP_Trajectory::norm_sqr(const std::vector<double>& j) 00096 { 00097 double l = 0; 00098 for (unsigned int i = 0; i < j.size(); i++) 00099 { 00100 l += j[i] * j[i]; 00101 } 00102 return sqrt(l); 00103 } 00104 00105 inline double RefValJS_PTP_Trajectory::norm_weighted(const std::vector<double>& j) 00106 { 00107 double l = 0; 00108 if ( j.size() == 7 ) 00109 { 00110 for (unsigned int i = 0; i < j.size(); i++) 00111 { 00112 l += j[i]* weigths[i] * j[i] * weigths[i]; 00113 } 00114 } 00115 else 00116 { 00117 for (unsigned int i = 0; i < j.size(); i++) 00118 { 00119 l += j[i] * j[i]; 00120 } 00121 } 00122 return sqrt(l); 00123 } 00124 00125 00126 00127 #endif 00128