Consider continuing optimization when solution_callback fails but there is still time budget remaining

[sea-bass]

From @SammyRamone:

Independent from this error, I was also wondering if the current way to call this function makes sense. Currently, it is only called at the very end of searchPositionIK(). This means, during the process of searching for an IK solution, pick_ik constantly checks the solution_fn (which does not contain the callback). When this finally returns true, it will check the callback. If the callback then returns false, pick_ik will say that it did not find a valid solution, although neither time nor iteration limits have been reached. Instead it would also be possible to continue searching for a solution until either a limit has reached or the solution_fn and the callback are valid at the same time.

I saw that bio_ik was doing it also like pick_ik is doing it currently. Maybe it is how MoveIt expects the kinematics_plugins to behave. However, I think that there might be benefit in changing this behavior, so that a valid solution is found more often.

[SammyRamone]

I thought a bit more about the problem and came up with different ideas to solve it. Maybe we could discuss it a bit and then I could try to create a PR.

• First option: Just adding the solution_callback to solution_fn
  • Implementation: The callback would be added as part of the solution_fn. Then it would be tested continously and the algorithm would continue to optimize till either a valid solution is found or timelimit is reached.
  • Pro
    • Simple to implement, no big changes to the code base.
  • Contra
    • A lot more calls to the callback function which might be computational expensive
    • There is no gradient for the callback function. Therefore, the optimization has no idea in which direction to search for a valid solution.
    • If the stop_optimization_on_valid_solution parameter is used, it could happen that a valid solution is found but we continue optimizing. When the time limit is reached, the current solution might not be valid for the callback (as we had no gradient to optimize for it) and the IK would return no solution, although one was found in between.
    • Both of these issues could be mitigated if the user ensures that it provides a valid gradient for its validity callback as part of their cost function.
• Second option: Restarting the solver with new initial seed
  • Implementation: Keep the current behavior of just checking the callback if a solution was computed. If the solution is invalid, restart the optimization with a new initial seed and the remaining time budget.
  • Pro
    • Simple to implement
    • No extra callback calls during optimization
  • Contra
    • It is pure chance if we find a valid solution. There is no guidance towards a solution that is valid for the callback. Even if the user provides a guidance through the cost function, the resetting of the population might hinder finding anything valid. Especially if the algorithm quickly finds valid solutions for the goal pose but not for the callback, it will constantly be resetting itself.
    • In case of stop_optimization_on_valid_solution, we could end up with the same issue as in option 1
• Third option: Distance to next collision as goal
  • Implementation: Creation of a new goal which computes the distance to the next collision, or in case of current collision how deep it is colliding.
  • Pro
    • Provides gradient in cost function
    • The callback is typically used for collision detection
  • Contra
    • Only solves the problem for collision detection, not general callbacks
    • Might be complicated to integrate into the code as the kinematic plugin does not have knowledge about the planning scene (and thus no information about collisions).

I think that option 3 would be very user friendly, but would require somehow getting the planning scene into the kinematics plugin which might be difficult.
Option 1 (with additional documentation about the necessarity of having a gradient in the cost function that helps finding soultions that are valid for the callback) would be much easier to implement and more general.
Option 2 is also simple to implement but might not bring much improvement in quickly finding valid solutions.

[sea-bass]

I would do Option 2: implement KDL-like random restarts of the initial seed.

Option 1 could be maybe useful for the global solver, and could be used at least on the elite member selection but not every time. As you said, it would really slow things down if we're not careful, and I think asking users to provide a gradient for the solution callback may be very challenging and require interface changes at the MoveIt level. I might have some idea to explore here as well.

However, this could be combined with Option 3 (custom collision avoidance cost function) which actually would guide optimization away from collisions and reduce the probability of having to restart in the first place.

FYI @stephanie-eng this is likely the issue that made your ROS 1 pick_ik benchmarking barf on collision checking and we should try again when we have a solution.

@SammyRamone I am happy to work on a PR for Option 2 as I have some ideas, but if you have time and get to it first, go ahead and I'll review.

[SammyRamone]

I would do Option 2: implement KDL-like random restarts of the initial seed.

Fine for me. It is probably the "safest" approach which will not confuse the user too much. Everyone who uses pick_ik for more uncommon stuff (like me) can still maintain a fork with a custom implementation for option 1, if it is necessary for them.

Option 1 could be maybe useful for the global solver, and could be used at least on the elite member selection but not every time. As you said, it would really slow things down if we're not careful, and I think asking users to provide a gradient for the solution callback may be very challenging and require interface changes at the MoveIt level. I might have some idea to explore here as well.

I was more thinking of providing the gradient via the cost function. Then it would not require any changes to the MoveIt interface. However, I agree that this is not simple to do and the typical user does not want to do this.

@SammyRamone I am happy to work on a PR for Option 2 as I have some ideas, but if you have time and get to it first, go ahead and I'll review.

If you already have an idea how to nicely implement Option 2, go ahead. I have (yet another) idea for a PR that I'm currently working on 😅

[sea-bass]

If you already have an idea how to nicely implement Option 2, go ahead.

behold! #53