GraspBlocks fails because of tolerance on re-checking TSR

[ClintLiddick]

In https://github.com/personalrobotics/herbpy/blob/master/src/herbpy/action/blocks.py#L48-L54 we execute PlanToTSR, and then use tsr_chain.contains(ee_pose) in a loop to see which block we moved to. However, TsrChain::contains uses a tolerance EPSILON = 0.001 (meant to avoid precision errors), and we will never be that exactly on the planned target, we should find another way to find which block TSR we actually grabbed.