cob_trajectory_controller: cob_trajectory_controller: RefValJS

00001 /********************************************************************
00002  *                                                                  *
00003  *                        RefValJS_PTP                              *
00004  *                                                                  *
00005  ********************************************************************/
00006  
00007 #ifndef _REFVALSJS_PTP_H_
00008 #define _REFVALSJS_PTP_H_
00009 
00010 #include "RefVal_JS.h"
00011 #include <cmath>
00012 
00013  
00014 class RefValJS_PTP : public RefVal_JS
00015 {
00016         public:
00017                 RefValJS_PTP(const std::vector<double>& start, const std::vector<double>& ziel, double v_rad_s, double a_rad_s2);
00018                 
00019                 virtual std::vector<double> r(double s) const;
00020                 virtual double s(double t) const;
00021 
00022                 virtual std::vector<double> dr_ds(double s) const;
00023                 virtual double ds_dt(double t) const;
00024                 
00025                 double getTotalTime() const { return m_T1 + m_T2 + m_T3; }
00026                 
00027         protected:
00028                 double norm(const std::vector<double>& j);
00029                 double norm_max(const std::vector<double>& j);
00030                 double norm_sqr(const std::vector<double>& j);
00031                 double norm_weighted(const std::vector<double>& j);
00032         
00033                 std::vector<double> m_start;
00034                 std::vector<double> m_ziel;
00035                 std::vector<double> m_direction;
00036                 
00037                 double m_length;
00038                 
00039                 double m_v_rad_s;
00040                 double m_a_rad_s2;
00041                 
00042                 double m_T1;    // Dauer der Phase konst. Beschl.
00043                 double m_T2;    // Dauer der Phase konst. Geschw.
00044                 double m_T3;    // Dauer der Phase konst. Verzög.
00045                 
00046                 double m_sa1;   // "Beschl." des Wegparameters s in Phase 1
00047                 double m_sv2;   // "Geschw." des Wegparameters s in Phase 2
00048                 double m_sa3;   // "Verzög." des Wegparameters s in Phase 3
00049                 
00050                 static const double weigths[];
00051                 
00052 };
00053 
00054 inline double RefValJS_PTP::norm(const std::vector<double>& j)
00055 {
00056         // which norm should be used?
00057         return norm_weighted(j);
00058 }
00059 
00060 inline double RefValJS_PTP::norm_max(const std::vector<double>& j)
00061 {
00062         double max = j.at(0);
00063         for     (unsigned int i = 0; i<j.size(); i++)
00064         {
00065                 if(j.at(i) < max)
00066                         max = j.at(i);
00067         }
00068         return max;
00069 }
00070 
00071 inline double RefValJS_PTP::norm_sqr(const std::vector<double>& j)
00072 {
00073         double l = 0;
00074         for (unsigned int i = 0; i < j.size(); i++)
00075         {
00076                 l += j[i] * j[i];
00077         }
00078         return sqrt(l);
00079 }
00080 
00081 inline double RefValJS_PTP::norm_weighted(const std::vector<double>& j)
00082 {
00083         double l = 0;
00084         if ( j.size() == 7 )
00085         {
00086                 for (unsigned int i = 0; i < j.size(); i++)
00087                 {
00088                         l += j[i]* weigths[i] * j[i] * weigths[i];
00089                 }
00090         }
00091         else
00092         {
00093                 for (unsigned int i = 0; i < j.size(); i++)
00094                 {
00095                         l += j[i] * j[i];
00096                 }
00097         }
00098         return sqrt(l);
00099 }
00100 
00101 
00102 #endif
00103
RefValJS_PTP.h
