See my other post here for details on how to accomplish this. (Verify robot state - #2 by eric.feldmann)