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