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