@@ -421,7 +421,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
421
421
422
422
// Using a decaying acceleration model. Consult the README for more information about the model.
423
423
const double t_h = duration;
424
- const float λ = std::log (2 ) / acceleration_exponential_half_life_;
424
+ const float lambda = std::log (2 ) / acceleration_exponential_half_life_;
425
425
426
426
auto have_same_sign = [](double a, double b) -> bool {
427
427
return (a >= 0.0 && b >= 0.0 ) || (a < 0.0 && b < 0.0 );
@@ -434,7 +434,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
434
434
return v;
435
435
}
436
436
// Get velocity after time horizon
437
- const auto terminal_velocity = v + a * (1.0 / λ ) * (1 - std::exp (-λ * t_h));
437
+ const auto terminal_velocity = v + a * (1.0 / lambda ) * (1 - std::exp (-lambda * t_h));
438
438
439
439
// If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at
440
440
// most stop, not reverse its direction)
@@ -443,15 +443,16 @@ FrenetPoint PathGenerator::getFrenetPoint(
443
443
// if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed ->
444
444
// time t_stop
445
445
446
- // 0 = Vo + acc(1/λ )(1-e^(-λt_stop ))
447
- // e^(-λt_stop ) = 1 - (-Vo* λ )/acc
448
- // t_stop = (-1/λ )*ln(1 - (-Vo* λ )/acc)
449
- // t_stop = (-1/λ )*ln(1 + (Vo* λ )/acc)
450
- auto t_stop = (-1.0 / λ ) * std::log ( 1 + ( v * λ / a) );
446
+ // 0 = Vo + acc(1/lambda )(1-e^(-lambda t_stop ))
447
+ // e^(-lambda t_stop ) = 1 - (-Vo* lambda )/acc
448
+ // t_stop = (-1/lambda )*ln(1 - (-Vo* lambda )/acc)
449
+ // t_stop = (-1/lambda )*ln(1 + (Vo* lambda )/acc)
450
+ auto t_stop = (-1.0 / lambda ) * std::log1p ( v * lambda / a);
451
451
452
452
// Calculate the distance traveled until stopping
453
453
auto distance_to_reach_zero_speed =
454
- v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow (λ, 2 )) * (std::exp (-λ * t_h) - 1 );
454
+ v * t_stop + a * t_stop * (1.0 / lambda) +
455
+ a * (1.0 / std::pow (lambda, 2 )) * std::expm1 (-lambda * t_h);
455
456
// Output an equivalent constant speed
456
457
return distance_to_reach_zero_speed / t_h;
457
458
}
@@ -461,17 +462,18 @@ FrenetPoint PathGenerator::getFrenetPoint(
461
462
// assume it will continue accelerating (reckless driving)
462
463
const bool object_has_surpassed_limit_already = v > speed_limit;
463
464
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
464
- return v + a * (1.0 / λ ) + (a / (t_h * std::pow (λ , 2 ))) * ( std::exp (-λ * t_h) - 1 );
465
+ return v + a * (1.0 / lambda ) + (a / (t_h * std::pow (lambda , 2 ))) * std::expm1 (-lambda * t_h);
465
466
466
467
// It is assumed the vehicle accelerates until final_speed is reached and
467
468
// then continues at constant speed for the rest of the time horizon
468
469
// So, we calculate the time it takes to reach the speed limit and compute how far the vehicle
469
470
// would go if it accelerated until reaching the speed limit, and then continued at a constant
470
471
// speed.
471
- const double t_f = (-1.0 / λ ) * std::log (1 - ((speed_limit - v) * λ ) / a);
472
+ const double t_f = (-1.0 / lambda ) * std::log (1 - ((speed_limit - v) * lambda ) / a);
472
473
const double distance_covered =
473
474
// Distance covered while accelerating
474
- a * (1.0 / λ) * t_f + a * (1.0 / std::pow (λ, 2 )) * (std::exp (-λ * t_f) - 1 ) + v * t_f +
475
+ a * (1.0 / lambda) * t_f + a * (1.0 / std::pow (lambda, 2 )) * std::expm1 (-lambda * t_f) +
476
+ v * t_f +
475
477
// Distance covered at constant speed for the rest of the horizon time
476
478
speed_limit * (t_h - t_f);
477
479
return distance_covered / t_h;
0 commit comments