RefValJS_PTP.cpp
Go to the documentation of this file.
00001 #include <cob_trajectory_controller/RefValJS_PTP.h>
00002 
00003 inline double sqr(double x)
00004 {
00005         return x*x;
00006 }
00007 
00008 const double RefValJS_PTP::weigths[] = { 1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.7 };
00009 
00010 
00011 RefValJS_PTP::RefValJS_PTP(const std::vector<double>& start, const std::vector<double>& ziel, double v_rad_s, double a_rad_s2)
00012 {       
00013         m_start = start;
00014         m_ziel = ziel;
00015         
00016         m_direction.resize(start.size());
00017         for(unsigned int i = 0; i < start.size(); i++)
00018                 m_direction.at(i) = ziel.at(i) - start.at(i);
00019         double max = m_direction.at(0);
00020         double min = m_direction.at(0);
00021         for(unsigned int i = 0; i < m_direction.size(); i++)
00022         {
00023                 if(max < m_direction.at(i))
00024                         max = m_direction.at(i);
00025                 if(min > m_direction.at(i))
00026                         min = m_direction.at(i);
00027         }
00028         max = fabs(max);
00029         min = fabs(min);
00030 
00031         //m_length = (max>min)?max:min;
00032         m_length = norm(m_direction);
00033         
00034         m_v_rad_s = v_rad_s;
00035         m_a_rad_s2 = a_rad_s2;
00036         
00037         /* Parameter für Beschl.begrenzte Fahrt: */    
00038         double a = fabs(m_a_rad_s2);
00039         double v = fabs(m_v_rad_s);
00040         
00041         if (m_length > v*v/a)
00042         {
00043                 // Phase mit konst. Geschw. wird erreicht:
00044                 m_T1 = m_T3 = v / a;
00045                 m_T2 = (m_length - v*v/a) / v;
00046                 
00047                 // Wegparameter s immer positiv, deswegen keine weitere Fallunterscheidung nötig:
00048                 m_sv2 = 1.0 / (m_T1 + m_T2);
00049                 m_sa1 = m_sv2 / m_T1;
00050                 m_sa3 = -m_sa1;
00051         } 
00052         else {
00053                 // Phase konst. Geschw. wird nicht erreicht:
00054                 m_T2 = 0.0;
00055                 m_T1 = m_T3 = sqrt(m_length / a);
00056 
00057                 m_sv2 = 1.0 / m_T1;
00058                 m_sa1 = m_sv2 / m_T1;
00059                 m_sa3 = -m_sa1;
00060                 
00061         }
00062         // Bewegung vollständig charakterisiert.
00063                 
00064 }
00065 
00066         
00067 std::vector<double> RefValJS_PTP::r(double s) const
00068 {
00069         std::vector<double> soll;
00070         soll.resize(m_start.size());
00071         if (s <= 0)
00072         {
00073                 soll = m_start;
00074         } else
00075         if (s < 1)
00076         {
00077                 for(unsigned int i = 0; i < m_start.size(); i++)
00078                         soll.at(i) = m_start.at(i) + m_direction.at(i) * s;
00079         }
00080         else
00081         {
00082                 soll = m_ziel;
00083         }
00084         return soll;
00085 }
00086 
00087 double RefValJS_PTP::s(double t) const
00088 {
00089         if (t >= m_T1 + m_T2 + m_T3)
00090                 return 1.0;
00091         else if (t >= m_T1 + m_T2)
00092         {
00093                 return 0.5*m_sa1*m_T1*m_T1 + m_sv2*m_T2 + m_sv2*(t-m_T1-m_T2) + 0.5*m_sa3*sqr(t-m_T1-m_T2);
00094         } 
00095         else if (t >= m_T1)
00096         {
00097                 return 0.5*m_sa1*m_T1*m_T1 + m_sv2*(t-m_T1);
00098         }
00099         else if (t > 0.0)
00100         {
00101                 return 0.5*m_sa1*t*t;
00102         }
00103         else return 0.0;
00104 }
00105 
00106 double RefValJS_PTP::ds_dt(double t) const
00107 {
00108         if (t >= m_T1 + m_T2 + m_T3)
00109                 return 0.0;
00110         else if (t >= m_T1 + m_T2)
00111         {
00112                 return m_sv2 + m_sa3*(t-m_T1-m_T2);
00113         } 
00114         else if (t >= m_T1)
00115         {
00116                 return m_sv2;
00117         }
00118         else if (t > 0.0)
00119         {
00120                 return m_sa1*t;
00121         }
00122         else return 0.0;
00123 }
00124 
00125 std::vector<double> RefValJS_PTP::dr_ds(double s) const
00126 {
00127         std::vector<double> result = m_start;
00128         if (s < 0.0 || s >= 1.0)
00129         {
00130                 for(unsigned int i = 0; i<result.size(); i++)
00131                         result.at(i) = 0.0;
00132         }
00133         else
00134         {
00135                 result = m_direction;
00136         }
00137 
00138         return result;
00139 }
00140 
00141 


cob_trajectory_controller
Author(s): Alexander Bubeck
autogenerated on Sun Oct 5 2014 23:02:43