teb_local_planner
teb_local_planner copied to clipboard
I think something is wrong with penaltyBoundToInterval and penaltyBoundToIntervalDerivative
The function is for bounding \c var to the interval -a < var < a
, but the code is
if (var < -a+epsilon)
{
return (-var - (a - epsilon));
}
if (var <= a-epsilon)
{
return 0.;
}
else
{
return (var - (a - epsilon));
}
inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon)
{
if (var < -a+epsilon)
{
return -1;
}
if (var <= a-epsilon)
{
return 0.;
}
else
{
return 1;
}
}
I think it should be if (var >= a-epsilon)
, the other overload funtion has the same problem