Implement A State Machine/Behavior Tree For Tool Retrieval Demo

[dcolli23]

Currently, the control logic for the tool retrieval demo is hard coded with conditionals. This should be replaced with a somewhat general approach using a state machine/behavior tree. However, we don't want to reinvent the wheel here. We should use a state machine or behavior tree library that makes this much simpler for us. This GitHub page that lists some libraries might be a good first place to look.

[dcolli23]

@JemuelStanley47 should probably be involved with this in some capacity since they'll be working on improving the robustness of the grasping/manipulation. We can discuss more on this during the intro meeting on Tuesday or sometime afterwards