Looks like a fun and interesting problem. Without knowing anything more regarding your app I would advice you to look a bit at AI and gaming AI for inspiration. First of all I would try to create perception layer whose purpose is to present the world in a a simpler format in which your robot knows its options and operate at full speed. This may also give you as a developer ideas of how to interpret robots world and to enhance your world parsing algorithm. Best of luck /U
What if the world was your spoon. What if was in your power to bend it to your liking. What if it could provide for you whatever you asked of it.