graceful_controller: implement iterative selection of control point

[mikeferguson]

I'm already working on this - just ticketing it for visibility

[SteveMacenski]

From the wobbling in https://youtu.be/QHdrfadFhRg I was thinking about how to potentially address it. I believe you mentioned that your aim would be to back out the control point on the path for collision avoidance reasons -- which my understanding would also help with turns in confined environments where going too far ahead isn't possible. I believe that makes the controller wholesale more functional, but doesn't help with the wobbling caused by recomputing the curves constantly.

So my initial thoughts last night over a notepad:

• Only recompute the control points on occasion, not every iteration. Let the robot commit to an action for longer and recompute either every N cycles, once the robot has driven N meters, or within some N meter radius of the final control point. Possibly with a velocity multiplier so that we can preemptively recompute it sooner when moving faster (such that if a new curve cannot be found, we have enough time to stop safely at the end of the current curve)

While not related to wobbling, something I want to try and thus documenting here: Either use gradient descent or sample different spiral parameters given the anchors of the boundary conditions and use the best by some scoring metric (distance to path, cost-distance metric, etc), My preference would be gradient descent so its not something tuned on the window sizes and such -- and thus falling into less of the traps of DWA.

[SteveMacenski]

Smoothing out path before execution (greedy, spline?) -- changing gains.