00001
00009
00010
00011
00012
00013 #include <ecl/linear_algebra.hpp>
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
00022
00023
00024
00025
00026
00027
00028
00029
00030 namespace ecl {
00031
00032
00033
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;
00040
00041
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
00053
00054 clearSplines();
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
00063
00064 bool splines_constrained = false;
00065 while ( !splines_constrained ) {
00066 tension_splines = generateTensionSplines(tension,0.0);
00067
00068
00069
00070 Array<double> domain = tension_splines[0].domain();
00071 splines_constrained = true;
00072
00073
00074
00075 for ( unsigned int i = 1; i < domain.size()-1; ++i ) {
00076 for ( unsigned int j = 0; j < dimension(); ++j ) {
00077 double accel = fabs(tension_splines[j].dderivative(domain[i]));
00078
00079
00080
00081
00082
00083
00084
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
00091
00092 if ( slope_before > slope_after ) {
00093 waypoints[i-1].duration(waypoints[i-1].duration()*1.1);
00094 } else {
00095 waypoints[i].duration(waypoints[i].duration()*1.1);
00096 }
00097
00098 break;
00099 }
00100 }
00101 }
00102 }
00103
00104
00105
00106
00107
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
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
00125 tension_splines = generateTensionSplines(tension,t);
00126 for ( unsigned int j = 0; j < dimension(); ++j) {
00127
00128 double y_dot = tension_splines[j].derivative(2*t);
00129 double y = tension_splines[j](2*t);
00130
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
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
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 ) {
00152 leading_quintic_time = 2*t;
00153
00154 quintic_constrained = true;
00155 i = 5;
00156 }
00157 } else {
00158 if ( i == 5 ) {
00159 quintic_constrained = false;
00160 }
00161 break;
00162 }
00163 }
00164 if ( !quintic_constrained ) {
00165
00166 waypoints[0].duration(waypoints[0].duration()*1.1);
00167 break;
00168 }
00169 }
00170 }
00171
00172
00173
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
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
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 ) {
00208 trailing_quintic_time_0 = tension_splines[j].domain().back()-t;
00209 trailing_quintic_time_f = tension_splines[j].domain().back()+t;
00210
00211 quintic_constrained = true;
00212 i = 5;
00213 }
00214 } else {
00215 if ( i == 5 ) {
00216 quintic_constrained = false;
00217 }
00218 break;
00219 }
00220 }
00221 if ( !quintic_constrained ) {
00222 waypoints[n-1].duration(waypoints[n-1].duration()*1.1);
00223 break;
00224 }
00225 }
00226 }
00227
00228
00229
00230
00231
00232
00233
00234
00235
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
00256
00257 clearSplines();
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
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
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);
00284 } else if ( e.data() == 1 ) {
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);
00289 } else if ( e.data() >= n+1 ) {
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);
00294 } else if ( e.data() >= n+2 ) {
00295
00296 waypoints[n-1].duration(waypoints[n-1].duration()*1.1);
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);
00305 waypoints[e.data()-1].duration(waypoints[e.data()-1].duration()*1.05);
00306 }
00307 }
00308 }
00309
00310
00311
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
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
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
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
00366 return spline_functions[joint][spline_functions[joint].size()-1]->dderivative(t_f);
00367 }
00368
00369
00370
00371
00372
00373
00374 bool Trajectory<JointAngles>::validateWaypoints(unsigned int min_no_waypoints) {
00375
00376 unsigned int n = waypoints.size()-1;
00377 if ( n+1 < min_no_waypoints ) {
00378 return false;
00379 }
00380
00381 for (unsigned int i = 0; i < n; ++i ) {
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;
00394
00395
00396
00397
00398
00399
00400 for (unsigned int i = 0; i < n; ++i ) {
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];
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
00434 for ( unsigned int i = 0; i < waypoints.size()-1; ++i ) {
00435 total_time += waypoints[i].duration();
00436
00437 }
00438
00439 trajectory_duration = total_time;
00440 }
00441
00442 Array<SmoothLinearSpline> Trajectory<JointAngles>::generateLinearSplines() throw(DataException<int>) {
00443
00444
00445
00446
00447
00448
00449 Array<SmoothLinearSpline> splines(dimension());
00450 unsigned int n = waypoints.size()-1;
00451 Array<double> waypoint_times(n+3), values(n+3);
00452
00453
00454
00455
00456
00457
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
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493 std::vector <LinearFunction> initial_tangents;
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;
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
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);
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;
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;
00540 }
00541 }
00542 if ( acceleration_constraint_broken ) {
00543 break;
00544 }
00545 }
00546 if ( !acceleration_constraint_broken ) {
00547 t_pullback = t;
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
00567
00568
00569
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
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
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;
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;
00617 }
00618 }
00619 if ( acceleration_constraint_broken ) {
00620 break;
00621 }
00622 }
00623 if ( !acceleration_constraint_broken ) {
00624 t_pullforward = t;
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
00644
00645
00646
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
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
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;
00697
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 }