@@ -421,7 +421,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
421421
422422 // Using a decaying acceleration model. Consult the README for more information about the model.
423423 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_;
425425
426426 auto have_same_sign = [](double a, double b) -> bool {
427427 return (a >= 0.0 && b >= 0.0 ) || (a < 0.0 && b < 0.0 );
@@ -434,7 +434,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
434434 return v;
435435 }
436436 // 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));
438438
439439 // If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at
440440 // most stop, not reverse its direction)
@@ -443,15 +443,16 @@ FrenetPoint PathGenerator::getFrenetPoint(
443443 // if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed ->
444444 // time t_stop
445445
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);
451451
452452 // Calculate the distance traveled until stopping
453453 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);
455456 // Output an equivalent constant speed
456457 return distance_to_reach_zero_speed / t_h;
457458 }
@@ -461,17 +462,18 @@ FrenetPoint PathGenerator::getFrenetPoint(
461462 // assume it will continue accelerating (reckless driving)
462463 const bool object_has_surpassed_limit_already = v > speed_limit;
463464 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);
465466
466467 // It is assumed the vehicle accelerates until final_speed is reached and
467468 // then continues at constant speed for the rest of the time horizon
468469 // So, we calculate the time it takes to reach the speed limit and compute how far the vehicle
469470 // would go if it accelerated until reaching the speed limit, and then continued at a constant
470471 // 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);
472473 const double distance_covered =
473474 // 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 +
475477 // Distance covered at constant speed for the rest of the horizon time
476478 speed_limit * (t_h - t_f);
477479 return distance_covered / t_h;
0 commit comments