trajectory.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010  ** Includes
00011  *****************************************************************************/
00012 
00013 #include <ecl/linear_algebra.hpp> // has to be first (eigen stdvector defn).
00014 #include "../../include/ecl/manipulators/waypoint.hpp"
00015 #include "../../include/ecl/manipulators/trajectory.hpp"
00016 #include <ecl/geometry/function_math.hpp>
00017 #include <ecl/geometry/polynomial.hpp>
00018 #include <ecl/math/simple.hpp>
00019 
00020 /*****************************************************************************
00021  ** Debug Macros
00022  *****************************************************************************/
00023 
00024 //#define DEBUG_LINEAR_INTERPOLATION
00025 /*****************************************************************************
00026  ** Namespaces
00027  *****************************************************************************/
00028 
00029 namespace ecl
00030 {
00031 
00032 /*****************************************************************************
00033  ** Implementation [Trajectory][QuinticTensionSplineInterpolation]
00034  *****************************************************************************/
00035 
00036 void Trajectory<JointAngles>::tensionSplineInterpolation(const double &tension) ecl_assert_throw_decl(StandardException)
00037 {
00038   bool result;
00039   unsigned int n = waypoints.size() - 1; // n = number of segments
00040 
00041   // who the hell made the constraint that this should be 5, shouldn't we only need 3 since we calculate 2 more pseudo points?
00042   ecl_assert_throw(
00043       n+1 >= 5,
00044       StandardException(LOC,ConfigurationError,"Not enough waypoints for a tension spline interpolation (must be >= 5)."));
00045 
00046   result = validateWaypoints(5);
00047   ecl_assert_throw(
00048       result,
00049       StandardException(LOC,ConfigurationError,"Not all the waypoint maximum rates have been specified correctly (must be > 0.0)."));
00050 
00051   result = initialiseWaypointDurations();
00052   ecl_assert_throw( result,
00053                    StandardException(LOC,ConfigurationError,"A waypoint was configured with a zero duration."));
00054 
00055   /******************************************
00056    ** Reserve Spline Storage Space
00057    *******************************************/
00058   clearSplines(); // Clean up any pre-existing definition.
00059   for (unsigned int j = 0; j < dimension(); ++j)
00060   {
00061     spline_functions[j].resize(3, NULL);
00062   }
00063   Array<TensionSpline> tension_splines(dimension());
00064   Array<QuinticPolynomial> leading_quintics(dimension()), trailing_quintics(dimension());
00065 
00066   /******************************************
00067    ** Roughly Generate Tension Spline
00068    *******************************************/
00069   bool splines_constrained = false;
00070   while (!splines_constrained)
00071   {
00072     tension_splines = generateTensionSplines(tension, 0.0);
00073     /*********************
00074      ** Check Max Accel
00075      **********************/
00076     Array<double> domain = tension_splines[0].domain();
00077     splines_constrained = true; // This changes to false if we break the max accel constraint
00078     // - tension splines have high accelerations at the joins.
00079     // - inbetween the joins, the acceleration goes towards zero.
00080     // - so we just check the size of the acceleration at the joins to find trouble spots.
00081     for (unsigned int i = 1; i < domain.size() - 1; ++i)
00082     { // Natural spline, first and last accelerations are always zero, so dont worry about them
00083       for (unsigned int j = 0; j < dimension(); ++j)
00084       {
00085         double accel = fabs(tension_splines[j].dderivative(domain[i]));
00086         /*********************
00087          ** Debugging
00088          **********************/
00089 //                std::cout << "Accel checking" << std::endl;
00090 //                std::cout << "  Domain: " << domain << std::endl;
00091 //                std::cout << "  Accel[" << i << "," << j << "]: " << accel << std::endl;
00092 //                sleep(1);
00093         if (accel > max_accelerations[j])
00094         {
00095           splines_constrained = false;
00096           double slope_before = fabs(
00097               (waypoints[i].angles()[j] - waypoints[i - 1].angles()[j]) / waypoints[i - 1].duration());
00098           double slope_after = fabs(
00099               (waypoints[i + 1].angles()[j] - waypoints[i].angles()[j]) / waypoints[i].duration());
00100 //                    std::cout << "  Slope_Before[" << i << "," << j << "]: " << slope_before << std::endl;
00101 //                    std::cout << "  Slope_After [" << i << "," << j << "]: " << slope_after << std::endl;
00102           if (slope_before > slope_after)
00103           {
00104             waypoints[i - 1].duration(waypoints[i - 1].duration() * 1.1); // 10% increase
00105           }
00106           else
00107           {
00108             waypoints[i].duration(waypoints[i].duration() * 1.1); // 10% increase
00109           }
00110 //                    i = domain.size(); // Force it to restart <-- This doesn't actually work so well - dampen the whole curve at once instead of gradually from one end!
00111           break;
00112         }
00113       }
00114     }
00115   }
00116   /******************************************
00117    ** Check rates are set for head/tail
00118    *******************************************/
00119   // Note that we may yet change values[1] and values[n+1] when fixing the pseudo points below
00120   // Set them all to zero (default option, i.e. starting at rest).
00121   if (!waypoints[0].rates_configured())
00122   {
00123     waypoints[0].rates() = WayPoint<JointAngles>::JointDataArray::Constant(waypoints[0].dimension(), 0.0);
00124   }
00125   if (!waypoints[n].rates_configured())
00126   {
00127     waypoints[n].rates() = WayPoint<JointAngles>::JointDataArray::Constant(waypoints[n].dimension(), 0.0);
00128   }
00129 
00130   /******************************************
00131    ** Make the pre pseudo point.
00132    *******************************************/
00133   double leading_quintic_time = 0.0;
00134   bool quintic_constrained = false;
00135   while (!quintic_constrained)
00136   {
00137     quintic_constrained = true;
00138     for (unsigned int i = 1; i <= 5; ++i)
00139     {
00140       double t = i * waypoints[0].duration() / 10;
00141       // move the tension spline to the right, i.e. give us some space to insert a quintic
00142       tension_splines = generateTensionSplines(tension, t); // t <-- is the initial time to start the tension spline
00143       for (unsigned int j = 0; j < dimension(); ++j)
00144       {
00145         // find the right side boundary conditions for the quintic we want to insert
00146         double y_dot = tension_splines[j].derivative(2 * t);
00147         double y = tension_splines[j](2 * t);
00148         // the left side boundary conditions for the quintic
00149         double y_0 = waypoints[0].angles()[j];
00150         double y_0_dot = waypoints[0].rates()[j];
00151         leading_quintics[j] = QuinticPolynomial::Interpolation(0.0, y_0, y_0_dot, 0.0, 2 * t, y, y_dot, 0.0);
00152 
00153         /*********************
00154          ** Debugging
00155          **********************/
00156 //                          std::cout << "Pre Pseudo Point" << std::endl;
00157 //                              std::cout << "  Domain: " << tension_splines[0].domain() << std::endl;
00158 //                              std::cout << "    y[" << i << "," << j << "]     : " << y << std::endl;
00159 //                              std::cout << "    y_dot[" << i << "," << j << "] : " << ydot << std::endl;
00160 //                              double accel_max = fabs( Maximum<CubicPolynomial>()(0.0,2*t,leading_quintics[j].derivative().derivative()));
00161 //                              if ( accel_max < fabs( Minimum<CubicPolynomial>()(0.0,2*t,leading_quintics[j].derivative().derivative()))  ) {
00162 //                                      accel_max = fabs( Minimum<CubicPolynomial>()(0.0,2*t,leading_quintics[j].derivative().derivative()));
00163 //                              }
00164 //                              std::cout << "    accel[" << i << "," << j << "] : " << accel_max << std::endl;
00165 //                              sleep(1);
00166         if ((fabs(ecl::Maximum<CubicPolynomial>()(0.0, 2 * t, leading_quintics[j].derivative().derivative()))
00167             < fabs(max_accelerations[j]))
00168             && (fabs(ecl::Minimum<CubicPolynomial>()(0.0, 2 * t, leading_quintics[j].derivative().derivative()))
00169                 < fabs(max_accelerations[j])))
00170         {
00171           if (j == dimension() - 1)
00172           { // all joints are constrained, great!
00173             leading_quintic_time = 2 * t;
00174             // Everything done, force it to completely bail out of these loops.
00175             quintic_constrained = true;
00176             i = 5;
00177           }
00178         }
00179         else
00180         { // failed to constrain for this i value.
00181           if (i == 5)
00182           { // If we get here, even up to half the length of the segment has failed.
00183             quintic_constrained = false;
00184           }
00185           break;
00186         }
00187       }
00188       if (!quintic_constrained)
00189       { // give up, go back to start of while loop with increased duration
00190 //                              std::cout << "Quintic unconstrained, increasing the first duration : " << waypoints[0].duration() << std::endl;
00191         waypoints[0].duration(waypoints[0].duration() * 1.1); // 10% increase
00192         break; // from the for loop
00193       }
00194     }
00195   }
00196 
00197   /******************************************
00198    ** Make the post pseudo point.
00199    *******************************************/
00200   double trailing_quintic_time_0 = 0.0;
00201   double trailing_quintic_time_f = 0.0;
00202   quintic_constrained = false;
00203   while (!quintic_constrained)
00204   {
00205     quintic_constrained = true;
00206     tension_splines = generateTensionSplines(tension, leading_quintic_time / 2);
00207     for (unsigned int i = 1; i <= 5; ++i)
00208     {
00209       double t = i * waypoints[n - 1].duration() / 10;
00210       double t_f = tension_splines[0].domain().back();
00211       for (unsigned int j = 0; j < dimension(); ++j)
00212       {
00213         double y_dot = tension_splines[j].derivative(t_f - t);
00214         double y = tension_splines[j](t_f - t);
00215         double y_f = waypoints[n].angles()[j];
00216         double y_f_dot = waypoints[n].rates()[j];
00217         trailing_quintics[j] = QuinticPolynomial::Interpolation(t_f - t, y, y_dot, 0.0, t_f + t, y_f, y_f_dot, 0.0);
00218 
00219         /*********************
00220          ** Debugging
00221          **********************/
00222 //                          std::cout << "Post Pseudo Point" << std::endl;
00223 //                              std::cout << "  Domain: " << tension_splines[0].domain() << std::endl;
00224 //                              std::cout << "    y[" << i << "," << j << "]     : " << y << std::endl;
00225 //                              std::cout << "    y_dot[" << i << "," << j << "] : " << ydot << std::endl;
00226 //                              double accel_max = fabs( Maximum<CubicPolynomial>()(t_f-t,t_f+t,trailing_quintics[j].derivative().derivative()));
00227 //                              if ( accel_max < fabs( Minimum<CubicPolynomial>()(t_f-t,t_f+t,trailing_quintics[j].derivative().derivative()))  ) {
00228 //                                      accel_max = fabs( Minimum<CubicPolynomial>()(t_f-t,t_f+t,trailing_quintics[j].derivative().derivative()));
00229 //                              }
00230 //                              std::cout << "    accel[" << i << "," << j << "] : " << accel_max << std::endl;
00231 //                              sleep(1);
00232         if ((fabs(Maximum<CubicPolynomial>()(t_f - t, t_f + t, trailing_quintics[j].derivative().derivative()))
00233             < fabs(max_accelerations[j]))
00234             && (fabs(Minimum<CubicPolynomial>()(t_f - t, t_f + t, trailing_quintics[j].derivative().derivative()))
00235                 < fabs(max_accelerations[j])))
00236         {
00237           if (j == dimension() - 1)
00238           { // all joints are constrained, great!
00239             trailing_quintic_time_0 = tension_splines[j].domain().back() - t;
00240             trailing_quintic_time_f = tension_splines[j].domain().back() + t;
00241             // Everything done, force it to completely bail out of these loops.
00242             quintic_constrained = true;
00243             i = 5;
00244           }
00245         }
00246         else
00247         { // failed to constrain for this i value.
00248           if (i == 5)
00249           { // If we get here, even up to half the length of the segment has failed.
00250             quintic_constrained = false;
00251           }
00252           break;
00253         }
00254       }
00255       if (!quintic_constrained)
00256       { // give up, go back to start of while loop with increased duration
00257         waypoints[n - 1].duration(waypoints[n - 1].duration() * 1.1); // 10% increase
00258         break;
00259       }
00260     }
00261   }
00262 
00263   /*********************
00264    ** Debugging
00265    **********************/
00266 //    std::cout << "Leading Quintic Time :" << leading_quintic_time << std::endl;
00267 //    std::cout << "Trailing Quintic Time:" << trailing_quintic_time_0 << " "  << trailing_quintic_time_f << std::endl;
00268   /******************************************
00269    ** Finalise
00270    *******************************************/
00271   trajectory_duration = trailing_quintic_time_f;
00272   for (unsigned int j = 0; j < dimension(); ++j)
00273   {
00274     spline_functions[j][0] = new SplineFunction<QuinticPolynomial>(0.0, leading_quintic_time, leading_quintics[j]);
00275     spline_functions[j][1] = new SplineFunction<TensionSpline>(leading_quintic_time, trailing_quintic_time_0,
00276                                                                tension_splines[j]);
00277     spline_functions[j][2] = new SplineFunction<QuinticPolynomial>(trailing_quintic_time_0, trailing_quintic_time_f,
00278                                                                    trailing_quintics[j]);
00279   }
00280 }
00281 
00282 void Trajectory<JointAngles>::linearSplineInterpolation() throw (StandardException)
00283 {
00284   if (!validateWaypoints(2))
00285   {
00286     throw StandardException(LOC, ConfigurationError,
00287                             "Not all the waypoint maximum rates have been specified correctly (must be > 0.0).");
00288   }
00289   if (!initialiseWaypointDurations())
00290   {
00291     throw StandardException(LOC, ConfigurationError, "A waypoint was configured with a zero duration.");
00292   }
00293 
00294   /******************************************
00295    ** Reserve Spline Storage Space
00296    *******************************************/
00297   clearSplines(); // Clean up any pre-existing definition.
00298   for (unsigned int j = 0; j < dimension(); ++j)
00299   {
00300     spline_functions[j].resize(1, NULL);
00301   }
00302   Array<SmoothLinearSpline> splines(dimension());
00303 
00304   /******************************************
00305    ** Generate Splines
00306    *******************************************/
00307   bool splines_constrained = false;
00308   int n = waypoints.size() - 1;
00309   while (!splines_constrained)
00310   {
00311     try
00312     {
00313       splines = generateLinearSplines();
00314       splines_constrained = true;
00315 #ifdef DEBUG_LINEAR_INTERPOLATION
00316       std::cout << "Linear Interpolation: spline generation success" << std::endl;
00317 #endif
00318     }
00319     catch (DataException<int> &e)
00320     {
00321       /*********************
00322        ** Stretch Waypoints
00323        **********************/
00324       if (e.data() <= 0)
00325       {
00326 #ifdef DEBUG_LINEAR_INTERPOLATION
00327         std::cout << "Linear Interpolation: spline generation failed trying to create pre-pseudo, stretching first segment" << std::endl;
00328 #endif
00329         waypoints[0].duration(waypoints[0].duration() * 1.1); // 10% increase to w_{0}-w_{1} duration
00330       }
00331       else if (e.data() == 1)
00332       { // error at corner prepseudo-w0-w1
00333 #ifdef DEBUG_LINEAR_INTERPOLATION
00334         std::cout << "Linear Interpolation: spline generation failed, stretching at corner " << e.data() << std::endl;
00335 #endif
00336         waypoints[0].duration(waypoints[0].duration() * 1.1); // 10% increase to w_{0}-w_{1} duration
00337       }
00338       else if (e.data() >= n + 1)
00339       { // error at corner w_{n-1}-w_{n}-postpseudo
00340 #ifdef DEBUG_LINEAR_INTERPOLATION
00341         std::cout << "Linear Interpolation: spline generation failed, stretching at corner " << e.data() << std::endl;
00342 #endif
00343         waypoints[n - 1].duration(waypoints[n - 1].duration() * 1.1); // 10% increase to w_{n-1}-w_{n} duration
00344       }
00345       else if (e.data() >= n + 2)
00346       {
00347         // occurs when trying to create the post-pseudo point
00348         waypoints[n - 1].duration(waypoints[n - 1].duration() * 1.1); // 10% increase to w_{n-1}-w_{n} duration
00349 #ifdef DEBUG_LINEAR_INTERPOLATION
00350         std::cout << "Linear Interpolation: spline generation failed trying to create post-pseudo, stretching last segment" << std::endl;
00351 #endif
00352       }
00353       else
00354       {
00355 #ifdef DEBUG_LINEAR_INTERPOLATION
00356         std::cout << "Linear Interpolation: spline generation failed, stretching at corner " << e.data() << std::endl;
00357 #endif
00358         waypoints[e.data() - 2].duration(waypoints[e.data() - 2].duration() * 1.05); // 5% increase on either side
00359         waypoints[e.data() - 1].duration(waypoints[e.data() - 1].duration() * 1.05); // 5% increase on either side
00360       }
00361     }
00362   }
00363 
00364   /******************************************
00365    ** Finalise
00366    *******************************************/
00367   trajectory_duration = splines[0].domain().back() - splines[0].domain().front();
00368   for (unsigned int j = 0; j < dimension(); ++j)
00369   {
00370     spline_functions[j][0] = new SplineFunction<SmoothLinearSpline>(0.0, splines[j].domain().back(), splines[j]);
00371   }
00372 }
00373 
00374 /*****************************************************************************
00375  ** Implementation [Trajectory][Access]
00376  *****************************************************************************/
00377 
00378 double Trajectory<JointAngles>::operator ()(const unsigned int& joint, const double& time)
00379     ecl_assert_throw_decl(StandardException)
00380 {
00381 
00382   double t_f = spline_functions[joint][spline_functions[joint].size() - 1]->domain()[1];
00383   ecl_assert_throw( ( time >= 0.0 ) && ( time <= t_f + 0.0001 ), StandardException(LOC,OutOfRangeError));
00384   unsigned int i;
00385   for (i = 0; i < spline_functions[joint].size(); ++i)
00386   {
00387     if (time <= spline_functions[joint][i]->domain()[1])
00388     {
00389       return (*(spline_functions[joint][i]))(time);
00390     }
00391   }
00392   // Sometimes machine precision can kill off some of the inequality arguments above (like e^{-16} error). Fallback:
00393   return (*(spline_functions[joint][spline_functions[joint].size() - 1]))(t_f);
00394 }
00395 
00396 double Trajectory<JointAngles>::derivative(const unsigned int& joint, const double& time)
00397     ecl_assert_throw_decl(StandardException)
00398 {
00399 
00400   double t_f = spline_functions[joint][spline_functions[joint].size() - 1]->domain()[1];
00401   ecl_assert_throw( ( time >= 0.0 ) && ( time <= t_f + 0.0001 ), StandardException(LOC,OutOfRangeError));
00402   unsigned int i;
00403   for (i = 0; i < spline_functions[joint].size(); ++i)
00404   {
00405     if (time <= spline_functions[joint][i]->domain()[1])
00406     {
00407       return spline_functions[joint][i]->derivative(time);
00408       break;
00409     }
00410   }
00411   // Sometimes machine precision can kill off some of the inequality arguments above (like e^{-16} error). Fallback:
00412   return spline_functions[joint][spline_functions[joint].size() - 1]->derivative(t_f);
00413 }
00414 
00415 double Trajectory<JointAngles>::dderivative(const unsigned int& joint, const double& time)
00416     ecl_assert_throw_decl(StandardException)
00417 {
00418 
00419   double t_f = spline_functions[joint][spline_functions[joint].size() - 1]->domain()[1];
00420   ecl_assert_throw( ( time >= 0.0 ) && ( time <= t_f + 0.0001 ), StandardException(LOC,OutOfRangeError));
00421   unsigned int i;
00422   for (i = 0; i < spline_functions[joint].size(); ++i)
00423   {
00424     if (time <= spline_functions[joint][i]->domain()[1])
00425     {
00426       return spline_functions[joint][i]->dderivative(time);
00427       break;
00428     }
00429   }
00430   // Sometimes machine precision can kill off some of the inequality arguments above (like e^{-16} error). Fallback:
00431   return spline_functions[joint][spline_functions[joint].size() - 1]->dderivative(t_f);
00432 }
00433 
00434 /*****************************************************************************
00435  ** Implementation [Trajectory][Private]
00436  *****************************************************************************/
00437 
00438 bool Trajectory<JointAngles>::validateWaypoints(unsigned int min_no_waypoints)
00439 {
00440 
00441   unsigned int n = waypoints.size() - 1; // n = number of segments
00442   if (n + 1 < min_no_waypoints)
00443   {
00444     return false;
00445   }
00446 
00447   for (unsigned int i = 0; i < n; ++i)
00448   { // Dont care about the last waypoint w_n
00449     for (unsigned int j = 0; j < waypoints[i].nominalRates().size(); ++j)
00450     {
00451       if (waypoints[i].nominalRates()[j] <= 0.0)
00452       {
00453         return false;
00454       }
00455     }
00456   }
00457   return true;
00458 }
00459 
00460 bool Trajectory<JointAngles>::initialiseWaypointDurations()
00461 {
00462 
00463   unsigned int n = waypoints.size() - 1; // n = number of segments
00464 
00465   // Waypoint durations are often not configured and default to 0.0
00466 
00467   // Here we check the nominal rates and angle differences to automatically guess a
00468   // duration. If the checks are ok, and this is bigger than the configured
00469   // waypoint duration, then replace the waypoint duration with this calculation.
00470   for (unsigned int i = 0; i < n; ++i)
00471   { // Dont need to worry about the w_n's duration.
00472     double max_duration = 0.0;
00473     for (unsigned int j = 0; j < dimension(); ++j)
00474     {
00475       double distance = fabs(waypoints[i + 1].angles()[j] - waypoints[i].angles()[j]);
00476       if (waypoints[i].nominalRates()[j] != 0.0)
00477       {
00478         double segment_duration = distance / waypoints[i].nominalRates()[j]; // Time = Distance/Velocity
00479         if (segment_duration > max_duration)
00480         {
00481           max_duration = segment_duration;
00482         }
00483       }
00484     }
00485     if (max_duration > waypoints[i].duration())
00486     {
00487       waypoints[i].duration(max_duration);
00488     }
00489     if (waypoints[i].duration() == 0.0)
00490     {
00491       return false;
00492     }
00493   }
00494   return true;
00495 }
00496 
00497 void Trajectory<JointAngles>::clearSplines()
00498 {
00499   for (unsigned int joint = 0; joint < dimension(); ++joint)
00500   {
00501     for (unsigned int function = 0; function < spline_functions[joint].size(); ++function)
00502     {
00503       if (spline_functions[joint][function] != NULL)
00504       {
00505         delete spline_functions[joint][function];
00506         spline_functions[joint][function] = NULL;
00507       }
00508     }
00509     spline_functions[joint].clear();
00510   }
00511 }
00512 
00513 void Trajectory<JointAngles>::updateDuration()
00514 {
00515   double total_time = 0.0;
00516 //    std::cout << "Durations: ";
00517   for (unsigned int i = 0; i < waypoints.size() - 1; ++i)
00518   {
00519     total_time += waypoints[i].duration();
00520 //        std::cout << waypoints[i].duration() << " ";
00521   }
00522 //    std::cout << std::endl;
00523   trajectory_duration = total_time;
00524 }
00525 
00526 Array<SmoothLinearSpline> Trajectory<JointAngles>::generateLinearSplines() throw (DataException<int> )
00527 {
00528   // n = number of segments
00529   // n+1 = number of input waypoints
00530   // n+3 = number of waypoints (inc. pre-post pseudo)
00531 
00532   // This is a bit different from the cubic as we don't  use this in a combo interpolater.
00533   Array<SmoothLinearSpline> splines(dimension());
00534   unsigned int n = waypoints.size() - 1; // n = number of segments
00535   Array<double> waypoint_times(n + 3), values(n + 3); // Including pre and post points
00536 
00537   /******************************************
00538    ** Check rates are set for head/tail
00539    *******************************************/
00540   // Note that we may yet change values[1] and values[n+1] when fixing the pseudo points below
00541   // Set them all to zero (default option, i.e. starting at rest).
00542   if (!waypoints[0].rates_configured())
00543   {
00544     waypoints[0].rates() = WayPoint<JointAngles>::JointDataArray::Constant(waypoints[0].dimension(), 0.0);
00545   }
00546   if (!waypoints[n].rates_configured())
00547   {
00548     waypoints[n].rates() = WayPoint<JointAngles>::JointDataArray::Constant(waypoints[n].dimension(), 0.0);
00549   }
00550 
00551   /******************************************
00552    ** Make the pre pseudo point.
00553    *******************************************/
00554   /*
00555    * Pull back the first waypoint, find intersection of initial tangent line and the nominal rate line.
00556    * Why? This lets us ensure the first segment is of the specified velocity, and the second segment will
00557    * fall the correct between nominal rates required to get to w_1.
00558    *
00559    *    x_
00560    *   ^  \__
00561    *  /      \
00562          * o ------ o
00563    *         /
00564    *        /
00565    *       x
00566    *
00567    * - diagonal lines : nominal rate slopes
00568    * - x : possible pseudo waypoints (at intersection of initial velocity line and nominal rate line)
00569    *
00570    * Basic process:
00571    *
00572    *  - pullback a bit
00573    *  - get intersections as above
00574    *  - find minimum time intersection time (t_first_duration)
00575    *  - synchronise a pseudo waypoint at that time on the initial tangents for each joint (only 1 of these will be an actual intersection point)
00576    *  - check acceleration constraints of quintics for this waypoint at each joint
00577    *  - if fail, pullback again, if success, done!
00578    */
00579   std::vector<LinearFunction> initial_tangents; // these are the lines from potential pseudo point to w_1, our eventual pseudo point must lie on it.
00580   std::vector<double> initial_angles, initial_rates;
00581   for (unsigned int j = 0; j < dimension(); ++j)
00582   {
00583     initial_angles.push_back(waypoints[0].angles()[j]);
00584     initial_rates.push_back(waypoints[0].rates()[j]);
00585     initial_tangents.push_back(LinearFunction::PointSlopeForm(0, initial_angles[j], initial_rates[j]));
00586   }
00587   std::vector<CartesianPoint2d, Eigen::aligned_allocator<CartesianPoint2d> > pseudo_points(dimension());
00588   std::vector<LinearFunction> nominal_rate_lines(dimension());
00589   WayPoint<JointAngles> pre_pseudo_waypoint(dimension());
00590   double t_pullback = 0.001; // start with 1ms
00591   double t_pullback_max_ = waypoints[0].duration();
00592   double t_pre_intersect_duration = 0.0; // duration from the pulled back wp0 to the intersection point
00593   bool acceleration_constraint_broken = true;
00594   while (acceleration_constraint_broken && (t_pullback <= t_pullback_max_))
00595   {
00596     // establish t_pre_intersect_duration as the minimum time to the intersection of initial tangent
00597     // and nominal rate line
00598     t_pre_intersect_duration = t_pullback_max_ / 2;
00599     for (unsigned int j = 0; j < dimension(); ++j)
00600     {
00601       LinearFunction nominal_rate_line = LinearFunction::PointSlopeForm(
00602           t_pullback, initial_angles[j], -1 * ecl::psign(initial_rates[j]) * waypoints[0].nominalRates()[j]);
00603       CartesianPoint2d pseudo_point = LinearFunction::Intersection(initial_tangents[j], nominal_rate_line);
00604       if (pseudo_point.x() < t_pre_intersect_duration)
00605       {
00606         t_pre_intersect_duration = pseudo_point.x();
00607       }
00608       nominal_rate_lines[j] = nominal_rate_line;
00609     }
00610     acceleration_constraint_broken = false;
00611     for (unsigned int j = 0; j < dimension(); ++j)
00612     {
00613       CartesianPoint2d pseudo_point(t_pre_intersect_duration, initial_tangents[j](t_pre_intersect_duration));
00614       // from the pseudo way point to the next way point (wp1)
00615       double pseudo_point_duration = t_pullback + waypoints[0].duration() - t_pre_intersect_duration;
00616       // Will test 5 positions, which cover all of t_pre_intersect_duration
00617       // and half of pseudo_point_duration
00618       for (unsigned int i = 1; i <= 5; ++i)
00619       {
00620         double t_l = t_pre_intersect_duration - i * t_pre_intersect_duration / 5.0;
00621         double t_r = t_pre_intersect_duration + i * pseudo_point_duration / 10.0;
00622         LinearFunction segment = LinearFunction::Interpolation(pseudo_point.x(), pseudo_point.y(),
00623                                                                t_pullback + waypoints[0].duration(),
00624                                                                waypoints[1].angles()[j]);
00625         double y_0 = initial_tangents[j](t_l);
00626         double y_0_dot = initial_tangents[j].derivative(t_l);
00627         double y = segment(t_r);
00628         double y_dot = segment.derivative(t_r); // slope
00629         QuinticPolynomial quintic = QuinticPolynomial::Interpolation(t_l, y_0, y_0_dot, 0.0, t_r, y, y_dot, 0.0);
00630         if ((fabs(CubicPolynomial::Maximum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j]))
00631             && (fabs(CubicPolynomial::Minimum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j])))
00632         {
00633           acceleration_constraint_broken = false;
00634           break; // we're good, get out and continue through all the joints
00635         }
00636         if (i == 5)
00637         {
00638 #ifdef DEBUG_LINEAR_INTERPOLATION
00639           std::cout << "Linear Interpolation: pre psuedo point failed with acceleration checks [" << t_pre_intersect_duration << "][" << t_pullback << "]" << std::endl;
00640 #endif
00641           acceleration_constraint_broken = true;
00642         }
00643       }
00644     }
00645     t_pullback = t_pullback * 2; // takes 11 runs to get from 1ms to 1s; adjust, if this is too much
00646   }
00647   if (acceleration_constraint_broken)
00648   {
00649     throw DataException<int>(LOC, ConstructorError, "Max acceleration bound broken by pre pseudo point.", 0);
00650   }
00651   pre_pseudo_waypoint.duration(t_pullback + waypoints[0].duration() - t_pre_intersect_duration);
00652   for (unsigned int j = 0; j < dimension(); ++j)
00653   {
00654     pre_pseudo_waypoint.angles()[j] = initial_tangents[j](t_pre_intersect_duration);
00655   }
00656 #ifdef DEBUG_LINEAR_INTERPOLATION
00657   std::cout << "Linear Interpolation: pre psuedo point done!" << std::endl;
00658   std::cout << "                    : angles " << pre_pseudo_waypoint.angles() << std::endl;
00659   std::cout << "                    : pre pseudo duration " << t_pre_intersect_duration << std::endl;
00660   std::cout << "                    : modified first waypoint duration " << pre_pseudo_waypoint.duration() << std::endl;
00661 #endif
00662 
00663   /******************************************
00664    ** Make the post pseudo point.
00665    *******************************************/
00666   std::vector<double> final_angles, final_rates;
00667   for (unsigned int j = 0; j < dimension(); ++j)
00668   {
00669     final_angles.push_back(waypoints[n].angles()[j]);
00670     final_rates.push_back(waypoints[n].rates()[j]);
00671     nominal_rate_lines[j] = LinearFunction::PointSlopeForm(
00672         0, final_angles[j], -1 * ecl::psign(final_rates[j]) * waypoints[n - 1].nominalRates()[j]);
00673   }
00674   std::vector<LinearFunction> final_tangents(dimension());
00675   WayPoint<JointAngles> post_pseudo_waypoint(dimension());
00676   double t_pullforward = 0.001; // start with 1ms
00677   double t_pullforward_max_ = waypoints[n - 1].duration();
00678   double t_post_intersect_duration = 0.0; // duration from the pulled back wp0 to the intersection point
00679   acceleration_constraint_broken = true;
00680   while (acceleration_constraint_broken && (t_pullforward <= t_pullforward_max_))
00681   {
00682     // establish t_final as the maximum time to the intersection of final tangent and nominal rate line
00683     t_post_intersect_duration = 0.0;
00684     for (unsigned int j = 0; j < dimension(); ++j)
00685     {
00686       LinearFunction final_tangent = LinearFunction::PointSlopeForm(t_pullforward, final_angles[j], final_rates[j]);
00687       final_tangents[j] = final_tangent;
00688       CartesianPoint2d pseudo_point = LinearFunction::Intersection(final_tangents[j], nominal_rate_lines[j]);
00689       if (pseudo_point.x() > t_post_intersect_duration)
00690       {
00691         t_post_intersect_duration = pseudo_point.x();
00692       }
00693     }
00694     // check acceleration constraints
00695     acceleration_constraint_broken = false;
00696     for (unsigned int j = 0; j < dimension(); ++j)
00697     {
00698       CartesianPoint2d pseudo_point(t_post_intersect_duration, final_tangents[j](t_post_intersect_duration));
00699       double pseudo_point_duration = t_pullforward - t_post_intersect_duration;
00700       for (unsigned int i = 1; i <= 5; ++i)
00701       {
00702         double t_l = t_post_intersect_duration - i * (waypoints[n - 1].duration() + t_post_intersect_duration) / 10.0;
00703         double t_r = t_post_intersect_duration + i * pseudo_point_duration / 5.0;
00704         LinearFunction segment = LinearFunction::Interpolation(-waypoints[n - 1].duration(),
00705                                                                waypoints[n - 1].angles()[j], pseudo_point.x(),
00706                                                                pseudo_point.y());
00707         double y_0 = segment(t_l);
00708         double y_0_dot = segment.derivative(t_l);
00709         double y = final_tangents[j](t_r);
00710         double y_dot = final_tangents[j].derivative(t_r);
00711         QuinticPolynomial quintic = QuinticPolynomial::Interpolation(t_l, y_0, y_0_dot, 0.0, t_r, y, y_dot, 0.0);
00712         if ((fabs(CubicPolynomial::Maximum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j]))
00713             && (fabs(CubicPolynomial::Minimum(t_l, t_r, quintic.derivative().derivative())) < fabs(max_accelerations[j])))
00714         {
00715           acceleration_constraint_broken = false;
00716           break; // we're good, get out and continue through all the joints
00717         }
00718         if (i == 5)
00719         {
00720 #ifdef DEBUG_LINEAR_INTERPOLATION
00721           std::cout << "Linear Interpolation: post psuedo point failed with acceleration checks [" << t_post_intersect_duration << "][" << t_pullforward << "]" << std::endl;
00722 #endif
00723           acceleration_constraint_broken = true;
00724         }
00725       }
00726     }
00727     t_pullforward = t_pullforward * 2; // takes 12 runs to get from 1ms to 1s, adjust, if this is too much
00728   }
00729   if (acceleration_constraint_broken)
00730   {
00731     throw DataException<int>(LOC, ConstructorError, "Max acceleration bound broken by pre pseudo point.", 0);
00732   }
00733   post_pseudo_waypoint.duration(t_pullforward - t_post_intersect_duration);
00734   for (unsigned int j = 0; j < dimension(); ++j)
00735   {
00736     post_pseudo_waypoint.angles()[j] = final_tangents[j](t_post_intersect_duration);
00737   }
00738 #ifdef DEBUG_LINEAR_INTERPOLATION
00739   std::cout << "Linear Interpolation: post psuedo point done!" << std::endl;
00740   std::cout << "                    : angles " << post_pseudo_waypoint.angles() << std::endl;
00741   std::cout << "                    : modified last point duration " << waypoints[n-1].duration() + t_post_intersect_duration << std::endl;
00742   std::cout << "                    : post pseudo duration " << post_pseudo_waypoint.duration() << std::endl;
00743 #endif
00744 
00745   /*********************
00746    ** Waypoint Times
00747    **********************/
00748   // n+3 points (w_0...w_n + pre and post pseudos)
00749   // n+3 waypoint_times
00750   waypoint_times[0] = 0.0;
00751   waypoint_times[1] = t_pre_intersect_duration;
00752   waypoint_times[2] = t_pre_intersect_duration + pre_pseudo_waypoint.duration();
00753   for (unsigned int i = 2; i < n; ++i)
00754   {
00755     waypoint_times[i + 1] = waypoint_times[i] + waypoints[i - 1].duration();
00756   }
00757   waypoint_times[n + 1] = waypoint_times[n] + waypoints[n - 1].duration() + t_post_intersect_duration;
00758   waypoint_times[n + 2] = waypoint_times[n + 1] + post_pseudo_waypoint.duration();
00759 
00760 #ifdef DEBUG_LINEAR_INTERPOLATION
00761   std::cout << "Linear Interpolation: waypoint time estimates before interpolation: " << waypoint_times << std::endl;
00762 #endif
00763 
00764   for (unsigned int j = 0; j < dimension(); ++j)
00765   {
00766     /******************************************
00767      ** Set Values
00768      *******************************************/
00769     values[0] = waypoints[0].angles()[j];
00770     values[1] = pre_pseudo_waypoint.angles()[j];
00771     for (unsigned int i = 2; i <= n; ++i)
00772     {
00773       values[i] = waypoints[i - 1].angles()[j];
00774     }
00775     values[n + 1] = post_pseudo_waypoint.angles()[j];
00776     values[n + 2] = waypoints[n].angles()[j];
00777 #ifdef DEBUG_LINEAR_INTERPOLATION
00778     std::cout << "Linear Interpolation: values[" << j << "]: " << values << std::endl;
00779 #endif
00780 
00781     /******************************************
00782      ** Generate Spline
00783      *******************************************/
00784     try
00785     {
00786       splines[j] = SmoothLinearSpline::Interpolation(waypoint_times, values, max_accelerations[j]);
00787     }
00788     catch (DataException<int> &e)
00789     {
00790       throw DataException<int>(LOC, e);
00791     }
00792   }
00793 #ifdef DEBUG_LINEAR_INTERPOLATION
00794   for ( unsigned int j = 0; j < dimension(); ++j)
00795   {
00796     std::cout << "Linear Interpolation: discretised domain [" << j << "]: " << splines[j].domain() << std::endl;
00797   }
00798 #endif
00799   return splines;
00800 }
00801 
00802 Array<TensionSpline> Trajectory<JointAngles>::generateTensionSplines(const double& tension, const double initial_time)
00803 {
00804 
00805   Array<TensionSpline> tension_splines(dimension());
00806   unsigned int n = waypoints.size() - 1; // n = number of segments
00807   // Include all waypoints (even w_0 and w_n). We set up pre and post pseudo points later.
00808   Array<double> waypoint_times(n + 1), values(n + 1);
00809   waypoint_times[0] = initial_time;
00810   for (unsigned int i = 1; i < n + 1; ++i)
00811   {
00812     waypoint_times[i] = waypoint_times[i - 1] + waypoints[i - 1].duration();
00813   }
00814 
00815   for (unsigned int j = 0; j < dimension(); ++j)
00816   {
00817     for (unsigned int i = 0; i < n + 1; ++i)
00818     {
00819       values[i] = waypoints[i].angles()[j];
00820     }
00821     tension_splines[j] = TensionSpline::Natural(waypoint_times, values, tension);
00822   }
00823   return tension_splines;
00824 }
00825 
00826 } // namespace ecl


ecl_manipulators
Author(s): Daniel Stonier
autogenerated on Sat Mar 5 2016 15:23:53