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

rst-tu-dortmund / teb_local_planner

I think something is wrong with penaltyBoundToInterval and penaltyBoundToIntervalDerivative #400