(moveit_utils) add geometry helpers