Simulated RRT and MPC planning on an indoor agent and implemented collision cone multi-agent obstacle avoidance