Results
Although we weren't able to get a zero error model, we were still satisfied with our end product and what our system was able to achieve. From constant toppling to 90% accuracy, this was a big win for us.
This first video is a simple demo where all blocks start off straight. In this video, you can see the re-adjustment of the tower happening. (May need to fullsize).
The second video is a full run of the robot in better quality.
In the end, our system was able to:
Use the Sawyer wrist camera to perform a scan of the playing field at 3 locations and identify/record any AR tag positions and orientations it is able to find.
Using these recorded positions and orientations, our system would then use transformations to calculate and determine the required angle and orientation to pick up the next block.
Upon picking up and placing the next block, our system would then enact our feedback system to ensure that the block is placed at an adequate position to ensure the stability of our tower.
Note: If video player does not work, see final demo here: https://www.youtube.com/watch?v=yDIczhh5lXk