Function hateb_local_planner::penaltyBoundToInterval(const double&, const double&, const double&)

• Defined in File penalties.h

Function Documentation

inline double hateb_local_planner::penaltyBoundToInterval(const double &var, const double &a, const double &epsilon)

Linear penalty function for bounding var to the interval \( -a < var < a \).

See also

penaltyBoundToIntervalDerivative

Parameters:

• var – The scalar that should be bounded

• a – lower and upper absolute bound

• epsilon – safty margin (move bound to the interior of the interval)

Returns:

Penalty / cost value that is nonzero if the constraint is not satisfied