Reachability validation

[mpomarlan]

When using costmaps to resolve a location designator, and this designator is a reachability one, then the robot-pose-gaussian-costmap::reachable-location-validator function is used.

This function performs a simple check that the distance between the candidate solution pose and the target pose in the designator is between a min and a max threshold. However, these thresholds are the same as the ones used to create the costmap from which the solution is sampled; we are pretty much guaranteed this distance will never be outside the threshholds because the costmap itself is hard cut off at them.

Suggestion: perform a call to ik to actually check reachability (I have code for this in the Bolzano branch of my fork). A couple of notes about the suggestion:

• service calls tend to spam the console with debug messages, especially when the IK fails for several candidates; they're written that way in roslisp, presumably, but maybe some messages might be silenced when required
• this makes designator validation dependent on some sort of tf. For example, if one wants to quickly check that designators in the projection world are solvable, and wants to do this in the REPL command line, it's better to use a (pr2-proj:with-projected-robot (desig:reference ...)) than a simple (desig:reference ...)

[gaya-]

Lorenz had this kind of validation. I found that it was too strict and even created false negatives. Additionally, I was creating a lot of computational overhead (or rather it would take too long) as IK queries are not completely for free. Because of that, I personally rely on good failure handling and resampling.

[mpomarlan]

I'll be gathering experience about the change, of course, until we can get to March or whenever some development time can be scheduled. The little experience I have so far is positive in terms of speed, and I didn't get false negatives.

Rather, the reason for the suggested change is false positives created by the previous approach. It comes down to how big the max distance threshold is for the costmap. Too large, and there is a chance that the robot will try to grip an object from too far away, which will result in a failure.

As to why it's a good idea to increase the max distance threshold for the costmap, it is precisely to avoid false negatives. The robot "should" reach more often than it does when that threshold is low (I say this having tried various pick-place plans and really there should have been fewer cases where it gave up). Increasing the max distance threshold for the costmap will work, as long as the reachability test actually tests reachability, rather than trivially being true for the costmap.