RefValJS_PTP_Trajectory.cpp
Go to the documentation of this file.
00001 #include <cob_trajectory_controller/RefValJS_PTP_Trajectory.h>
00002 #include <stdexcept>
00003 #include <algorithm>
00004 #include <vector>
00005 
00006 //#define PTP_TRAJ_DEBUG
00007 
00008 const double RefValJS_PTP_Trajectory::weigths[] = { 1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.7 };
00009 
00010 
00011 inline double sqr(double x)
00012 {
00013         return x*x;
00014 }
00015 
00016 RefValJS_PTP_Trajectory::RefValJS_PTP_Trajectory(const trajectory_msgs::JointTrajectory& trajectory, double v_rad_s, double a_rad_s2, bool smooth)
00017 {       
00018         m_trajectory = trajectory;
00019         m_length = 0;
00020         //m_stepSize = 0.4;
00021         m_stepSize = 0.0175; // 0.0175 rad = 1°
00022         m_length_parts.clear();
00023         m_s_parts.clear();
00024         
00025         if ( m_trajectory.points.size() < 1 )
00026         {
00027                 throw std::runtime_error("Tried to create reference values for empty trajectory!");
00028         }
00029         
00030         std::vector<std::vector<double> > zwischenPunkte;
00031         
00032         if (smooth == false)
00033         {
00034                 // Maß dafür, wo groß die Rundung wird
00035 //              double between_stepsize = 2.5; // 0.2 rad = 11.5°
00036                 //uhr-messmerf:
00037                 //double between_stepsize = 0.1; //verkleinert wegen Exception (s.u.)
00038                 double between_stepsize = 0.8; //AUB
00039                 for (unsigned int i=0; i < m_trajectory.points.size()-1; i++)
00040                 {
00041                         std::vector<double> direction;
00042                         direction.resize( m_trajectory.points[i+1].positions.size());
00043                         double len = 0;
00044                         for(unsigned int j = 0; j <  m_trajectory.points[i+1].positions.size(); j++)
00045                         {
00046                                 direction.at(j) = m_trajectory.points[i+1].positions.at(j)-m_trajectory.points[i].positions.at(j);
00047                                 len += direction.at(j) * direction.at(j);
00048                         }
00049                         double dist = sqrt(len);
00050                         int num_segs = ceil(dist/between_stepsize);
00051                         zwischenPunkte.resize(num_segs);
00052                         for (int j=0; j < num_segs; j++)
00053                         {
00054                                 std::vector<double> betw_joint;
00055                                 betw_joint.resize(direction.size());
00056                                 for(unsigned int d=0; d < direction.size(); d++)
00057                                 {
00058                                         betw_joint.at(d) = m_trajectory.points[i].positions.at(d) + direction.at(d) * ( (double)j / (double)num_segs );
00059                                 }
00060                                 zwischenPunkte.at(j) = betw_joint;
00061                         }
00062                 }
00063                 zwischenPunkte.push_back( m_trajectory.points.back().positions );
00064         } else 
00065         {
00066                 zwischenPunkte.resize(trajectory.points.size());
00067                 for(unsigned int i = 0; i < trajectory.points.size(); i++)
00068                 {
00069                         zwischenPunkte.at(i) = trajectory.points.at(i).positions;
00070                 }
00071         }
00072         ROS_INFO("Calculated %lu zwischenPunkte", zwischenPunkte.size());
00073         
00074         m_TrajectorySpline.setCtrlPoints(zwischenPunkte);
00075         
00076         // Note: unlike the name of the function suggests, the distance between any two neigbouring points
00077         // is not always the same!
00078         if ( m_TrajectorySpline.ipoWithConstSampleDist(m_stepSize, m_SplinePoints)==false )
00079         {
00080                 //uhr-messmerf: diese exception wird geworfen, wenn nicht genügend Stützpunkte vorhanden sind (kleiner m_iGrad=3)
00081                 //uhr-messmerf: dies tritt auf, wenn die Punkte der Trajectory pfad zu nah beieinander liegen (dist < between_stepsize=2.5)
00082                 throw std::runtime_error("Error in BSplineND::ipoWithConstSampleDist!");
00083         }
00084         m_SplinePoints = zwischenPunkte;
00085         ROS_INFO("Calculated %lu splinepoints", m_SplinePoints.size());
00086         
00087         m_length_cumulated.clear();
00088         m_length_cumulated.push_back(0.0);
00089         
00090         for (unsigned int i=0; i < m_SplinePoints.size()-1; i++)
00091         {
00092                 std::vector<double> dist;
00093                 dist.resize(m_SplinePoints[i].size());
00094                 for(unsigned int j=0; j < m_SplinePoints[i].size(); j++)
00095                 {
00096                         dist.at(j) = m_SplinePoints[i+1].at(j) - m_SplinePoints[i].at(j);
00097                 }
00098                 //double max = fabs(direction.getMax());
00099                 //double min = fabs(direction.getMin());
00100                 m_length_parts.push_back( norm(dist) );
00101                 m_length += norm(dist);
00102                 m_length_cumulated.push_back(m_length);
00103         }
00104         
00105         m_param_length = m_TrajectorySpline.getMaxdPos();
00106 
00107         for (unsigned int i=0; i < m_length_parts.size(); i++)
00108         {
00109                 m_s_parts.push_back( m_length_parts[i] / m_length );
00110         }
00111         
00112         m_v_rad_s = v_rad_s;
00113         m_a_rad_s2 = a_rad_s2;
00114         
00115         /* Parameter für Beschl.begrenzte Fahrt: */    
00116         double a = fabs(m_a_rad_s2);
00117         double v = fabs(m_v_rad_s);
00118         
00119         if (m_length > v*v/a)
00120         {
00121                 // Phase mit konst. Geschw. wird erreicht:
00122                 m_T1 = m_T3 = v / a;
00123                 m_T2 = (m_length - v*v/a) / v;
00124                 
00125                 // Wegparameter s immer positiv, deswegen keine weitere Fallunterscheidung nötig:
00126                 m_sv2 = 1.0 / (m_T1 + m_T2);
00127                 m_sa1 = m_sv2 / m_T1;
00128                 m_sa3 = -m_sa1;
00129         } 
00130         else {
00131                 // Phase konst. Geschw. wird nicht erreicht:
00132                 m_T2 = 0.0;
00133                 m_T1 = m_T3 = sqrt(m_length / a);
00134 
00135                 m_sv2 = 1.0 / m_T1;
00136                 m_sa1 = m_sv2 / m_T1;
00137                 m_sa3 = -m_sa1;
00138                 
00139         }
00140         // Bewegung vollständig charakterisiert.
00141                 
00142 }
00143 
00144 std::vector<double> RefValJS_PTP_Trajectory::r(double s) const
00145 {
00146         //printw("Line %d\ts: %f\n", __LINE__, s);
00147         std::vector<double> soll;
00148         if (s <= 0)
00149         {
00150                 soll = m_trajectory.points.front().positions;
00151         } else
00152         if (s < 1)
00153         {
00154                 // since the Distances between the points of m_SplinePoints are not equal,
00155                 // we first need to find the last i where m_length_cumulated[i] is smaller than s*m_length
00156                 
00157                 // return the first index i, where m_length_cumulated[i] is not smaller s * m_length
00158                 // more information under: http://www.cplusplus.com/reference/algorithm/lower_bound/
00159                 //typedef std::vector<double> dvec;
00160                 
00161                 
00162                 vecd_it start = m_length_cumulated.begin();
00163                 vecd_it end = m_length_cumulated.end();
00164                 vecd_it it = upper_bound(start, end, s * m_length);
00165                 
00166                 int i = int(it-start) - 1;
00167                 double frac = (s * m_length - m_length_cumulated[i]) / m_length_parts[i];
00168                 
00169 
00170                 /*
00171                 int i = floor( s * m_param_length / m_stepSize);
00172                 double frac = s * m_param_length / m_stepSize - (double) i;
00173                 */
00174                 // interpolate
00175                 soll.resize(m_SplinePoints[i].size());
00176                 for(unsigned int j = 0; j < m_SplinePoints[i].size(); j++)
00177                 {
00178                         soll.at(j) = m_SplinePoints[i].at(j) + (m_SplinePoints[i+1].at(j)-m_SplinePoints[i].at(j))*frac;
00179                 }
00180         }
00181         else
00182         {
00183                 soll = m_trajectory.points.back().positions;
00184         }
00185         return soll;
00186 }
00187 
00188 double RefValJS_PTP_Trajectory::s(double t) const
00189 {
00190         if (t >= m_T1 + m_T2 + m_T3)
00191                 return 1.0;
00192         else if (t >= m_T1 + m_T2)
00193         {
00194                 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);
00195         } 
00196         else if (t >= m_T1)
00197         {
00198                 return 0.5*m_sa1*m_T1*m_T1 + m_sv2*(t-m_T1);
00199         }
00200         else if (t > 0.0)
00201         {
00202                 return 0.5*m_sa1*t*t;
00203         }
00204         else return 0.0;
00205 }
00206 
00207 double RefValJS_PTP_Trajectory::ds_dt(double t) const
00208 {
00209         if (t >= m_T1 + m_T2 + m_T3)
00210                 return 0.0;
00211         else if (t >= m_T1 + m_T2)
00212         {
00213                 return m_sv2 + m_sa3*(t-m_T1-m_T2);
00214         } 
00215         else if (t >= m_T1)
00216         {
00217                 return m_sv2;
00218         }
00219         else if (t > 0.0)
00220         {
00221                 return m_sa1*t;
00222         }
00223         else return 0.0;
00224 }
00225 
00226 
00227 std::vector<double> RefValJS_PTP_Trajectory::dr_ds(double s) const
00228 {
00229         //printw("Line %d\ts: %f\n", __LINE__, s);
00230         std::vector<double> result = m_trajectory.points.front().positions;
00231         if (s < 0.0 || s >= 1.0)
00232         {
00233                 for(unsigned int j=0; j < result.size(); j++)
00234                         result.at(j) = 0.0;
00235         }
00236         else
00237         {
00238                 
00239                 vecd_it start = m_length_cumulated.begin();
00240                 vecd_it end = m_length_cumulated.end();
00241                 vecd_it it = upper_bound(start, end, s * m_length);
00242                 
00243                 int i = int(it-start) - 1;      
00244                 double frac = (s * m_length - m_length_cumulated[i]) / m_length_parts[i];
00245                 
00246                 
00247                 /*
00248                 int i = floor( s * m_param_length / m_stepSize);
00249                 double frac = s * m_param_length / m_stepSize - (double) i;
00250                 */
00251                 
00252                 std::vector<double> vi; // vi
00253                 std::vector<double> vii;// vi+1
00254                 
00255                 double step_s = m_stepSize / m_param_length;
00256 
00257                 if ( i == 0 )
00258                 {
00259                         // vi rechtsseitig approximieren
00260                         step_s = m_length_parts[0] / m_length;
00261                         vi.resize(m_SplinePoints[i].size());
00262                         for(unsigned int k = 0; k < m_SplinePoints[i].size(); k++)
00263                         {
00264                                 vi.at(k) = (m_SplinePoints[1].at(k) - m_SplinePoints[0].at(k)) / step_s;
00265                         }
00266                         vii = vi;
00267                         // vi+1 zentrisch approximieren
00268                         //step_s = (m_length_parts[i]+m_length_parts[i+1]) / m_length;
00269                         //vii = (m_SplinePoints[i+2] - m_SplinePoints[i]) / (2.0 * step_s);
00270                 } else
00271                 if ( i == (int) m_SplinePoints.size() - 2 )
00272                 {
00273                         // vi zentrisch:
00274                         //step_s = (m_length_parts[i]+m_length_parts[i-1]) / m_length;
00275                         //vi = (m_SplinePoints[i+1] - m_SplinePoints[i-1]) / (2.0 * step_s);
00276                         // vi+1 linksseitig:
00277                         step_s = (m_length_parts[i]) / m_length;
00278                         vii.resize(m_SplinePoints[i].size());
00279                         for(unsigned int k = 0; k < m_SplinePoints[i].size(); k++)
00280                                 vii.at(k) = (m_SplinePoints[i+1].at(k) - m_SplinePoints[i].at(k)) / step_s;
00281                         vi = vii;
00282                 } else
00283                 {
00284                         // beide zentrisch:
00285                         step_s = (m_length_parts[i]+m_length_parts[i-1]) / m_length;
00286                         vi.resize(m_SplinePoints[i].size());
00287                         for(unsigned int k = 0; k < m_SplinePoints[i].size(); k++)
00288                                 vi.at(k) = (m_SplinePoints[i+1].at(k) - m_SplinePoints[i-1].at(k)) / step_s;
00289                         step_s = (m_length_parts[i]+m_length_parts[i+1]) / m_length;
00290                         vii.resize(m_SplinePoints[i].size());
00291                         for(unsigned int k = 0; k < m_SplinePoints[i].size(); k++)
00292                                 vii.at(k) = (m_SplinePoints[i+2].at(k) - m_SplinePoints[i].at(k)) / step_s;
00293                 }
00294 
00295                 // linear interpolieren:
00296                 result.resize(m_SplinePoints[i].size());
00297                 for(unsigned int k = 0; k < m_SplinePoints[i].size(); k++)
00298                         result.at(k) = vi.at(k) + (vii.at(k)-vi.at(k))*frac;
00299         }
00300 
00301         return result;
00302 }
00303 
00304 
00305 
00306 


cob_trajectory_controller
Author(s): Alexander Bubeck
autogenerated on Tue Mar 3 2015 15:12:41