Add option to keep optimizing after a valid solution was found

[SammyRamone]

Currently, pick_ik will try to find an IK solution that optimizes for the given goal pose and the additional goals. As soon as the current best solution satisfies the pose threshold and the cost threshold, it will return the solution.

pick_ik/src/ik_memetic.cpp

Lines 247 to 249 in 807b04e

	if (solution_fn(ik.best().genes)) {
	if (print_debug) fmt::print("Found solution!\n");
	return ik.best();

This makes sense in a lot of applications, as you often want to get a (for your context) valid IK solution as fast as possible. However, there are also scenarios where you are not in a hurry and might want to keep optimizing the IK solution until you reach a certain time or iteration limit.
This is currently not easily possible in pick_ik. It is possible to reduce the cost_threshold, so that the optimization runs longer. However, this comes with the risk that no solution is acepted as correct.
I first thought that the memetic_stop_on_first_solution would do this, but it only regards the stopping of other threads as soon as one has found a solution.
Therefore, I added the optimize_solution option which will keep optimizing the solution till either time or iteration limit is reached. Indpependend on the usage of threads and IK mode.

If you don't want to merge this upstream it is, of course, also fine.

[sea-bass]

Thanks for the PR -- I generally like this idea, but a few comments:

1. The name optimize_solution probably needs to be more specific, as really anything this plugin does is optimizing a solution. Maybe we could flip the logic of the parameter and call it something like stop_optimization_on_valid_solution which defaults to true, but can be set to false?
2. (Optional but appreciated) If you have changes to the parameters, consider adding some more information in the doc page. Same holds for your idea on the position scaling in the other issue.

[SammyRamone]

1. The name `optimize_solution` probably needs to be more specific, as really anything this plugin does is optimizing a solution. Maybe we could flip the logic of the parameter and call it something like `stop_optimization_on_valid_solution` which defaults to `true`, but can be set to `false`?

That is true. I was aiming to make the name distinguishable from memetic_stop_on_first_solution, which already sounds close to what I want to achieve. However, I ended up with too generic name. I also don't have a better idea than yours, so I will change it accordingly.

2. (Optional but appreciated) If you have changes to the parameters, consider adding some more information in the [doc page](https://github.com/PickNikRobotics/pick_ik/blob/main/doc/USAGE.md). Same holds for your idea on the position scaling in the other issue.

Very good point, I completly forgot about that. I will also information to the doc page.

[sea-bass]

Thank you!