First, for each block color, a corresponding mask is created to isolate the block of the desired color. The masked image is converted from color to grey-scale, then to black and white. In the resulting image, the block of the desired color is now white, and everything else is black. Next, a blob detector filters by color to determine the keypoints of the desired blocks in the form of pixel coordinates on the image. The 3D location of the block is calculated by shooting rays through the keypoints and finding the intersection on the depth map. These locations are converted from the camera frame to the world frame, and sent in a custom message to the motion planning code.

Main Planning Pseudo-Code: 

1. Calculate Nash-Equilibrium solution

2.  Pick block/goal pair from this solution

3.  Generate a collision free path to the block (Not yet Implemented)

4.  Pick the block up 

5.  Generate a collision free path to the goal 

6.  Place the block

7.  Repeat 1-6 

Nash-Equilibrium solution: 

We start by making a couple of simplifications regarding the game. Firstly, we treat station locations and blocks as point masses, which significantly simplifes our calculations. Secondly, we assume that allocating colored blocks to the nearest station requiring them results in a minimal L2 travel distance for the robots moving between blocks and goals. Given our point mass assumption, this approach seems reasonable. The code can be found here.

The pseudo-code to calculate the Nash-Equilibrium proceeds as follows:

1. Enumerate all possible combinations of stations and requirements.

2. For each permutation of station and requirements, compute the distance between each block and every station in use.

3. Iterate through the stations, and for each station's required number of blocks, allocate the nearest available blocks.

4. Calculate the total distance needed for the given permutation.

5. Apply this process to all colors, summing up the results to obtain the overall map cost.

6. After evaluating all map cost permutations, select the map configuration with the lowest cost.

We also account for the following edge cases:

• If a station has more blocks than required, add a fixed cost for relocating the extra block/blocks.

• If a block within a station cannot be moved, impose a substantial fixed cost for moving the block (resulting in the least preferable option).

All control and simulation was done by using the Robot Operating System (ROS) and the Moveit package. To control the non-holonomic robot base, instead of controlling the center point, we introduce a virtual point P, which is 10 cm away from the robot base's center in the positive x direction. This approach allows the robot to move within non-holonomic constraints.