Margin Parameter to Overcome Numerical Issues

[jayalatn]

Because of floating point number accuracy, sometimes, the state.satisfiesBounds(group) fails in the stomp_planner.cpp. Currently, there is no external way (via the configuration file) to set a margin for satisfiesBounds(group, margin). Can we add this feature and what is the best way to add it unless via config file?

[jrgnicho]

If we don't expect this margin value to change we could set it up as a global const variable with a very small value

[jayalatn]

Yes that would be sufficient for now if configurable parameter is a hassle. I can implement one of the other if we can create a ticket for this.

[jrgnicho]

@jayalatn feel free to create a PR with the global const variable

[jayalatn]

will do!