Raspberry Pi Controlled Robotic Swarm System
Evan Kravitz (esk95), Haiyang Wang (hw656)
Have 2 Cozmo robots autonomously assemble a 1-level structure out of cubes. The cubes and 2 Cozmo robots will initially be placed in a designated area on the floor. At the start, the robots will begin autonomously picking up 1 cube each, and then place the cubes on the floor in order to build a 1-level contiguous structure. The robots will keep placing cubes on the floor until no more unplaced cubes remain.
Robotic swarm systems are becoming increasingly popular for accomplishing sophisticated tasks. One area where robotic swarm systems seem to have potential is in construction. This project involves using Raspberry Pis to control a network of Cozmo robots in order to build a structure out of blocks. This was accomplished by writing a Python script to control the rovers via the Cozmo SDK, and a controller script to manage the different race conditions involved in the group construction (who gets to pick up which cubes, where cubes can be placed down). The communication between the rovers and the controller is managed by the Linux operating system, which provides a robust and reliable means of asynchronous data transfer between Raspberry Pis. Our system design allows for a variable number of cubes and rovers to be used.
We intended to design a system involving 2 Cozmo robots (can be scaled to include more) that are connected to 1 Amazon Kindle Fire each, which are then connected to 1 Raspberry Pi each. In addition, there is a Raspberry Pi running a controller script. We intend to have the Cozmos operate on a course built on a white board, and place the cubes and Cozmos in designated areas of the board. The tags on the walls will also be placed at fixed locations.
Fig. 1: Schematic depicting the Cozmo and Raspberry Pi communication network
In order to have multiple cubes used at a time, we needed to create custom cubes. Since we were already 3D printing cubes, we decided to change the geometry of the cubes to make it easy for Cozmo to pick up the cube. This involved expanding the width of the cube and area where the Cozmo forklift can latch onto. Although this was a first step, it became an ongoing process while we experimented with different geometries and 3D printer settings.
Testing the custom cubes mainly consisted of seeing if the Cozmo can pick-up the cubes given the cube geometry (is the cube too heavy, is the place for the forklift to latch into not large enough). We would run Python scripts to pick up the cubes and then observe how Cozmo performs at these tasks. We also tested out different tags to see if the tag design affects the performance.
This step was perhaps the most challenging aspect of the entire project. The task must have a high success rate since the robots will be acting in a crowded environment with many other robots. Our algorithm has two main components: 1) getting Cozmo to get the correct approach angle and direction; and b) getting Comzo to handle pickup errors.
Fig. 2: Diagram showing orientation angles of cube and Cozmo
In order to get Cozmo to have the correct approach angle and direction, we use the Cozmo engine to get the angle and location of the cube (using custom object tag markers). From this information, we can calculate a tangent line that starts from the face of the cube (see Fig. 2). Then, we tell Cozmo to move to a point along the tangent line and then face the same direction as the cube. We use the Cozmo SDK to move Cozmo to a specific pose, which is an x, y direction on the cartesian plane and an orientation with respect to the starting point. Once Cozmo is at the right approach angle and position, we tell Cozmo to move forward around 70% of the way to the cube. If the cube is very far away from the Cozmo at this point, the robot must recheck for the cube and repeat the process. If the cube is close enough to the Cozmo, the Cozmo can move forward around 5cm into the cube to pick it up.
The testing procedure for the cube pickup algorithm was closely tied with the testing of the cube designs, but was more focused on increasing the accuracy of the approach angle. We had to verify trigonometry coded in software by doing manual tests of the robot picking up the cube using an array of placement configurations. This was to ensure that the algorithm works in many environments.
The next step in the project was to develop the inter-Raspberry Pi communication protocol. For this project, since each Cozmo requires one Raspberry Pi, and the Python scripts running on each Raspberry Pi cannot multiplex between different Cozmos connected to the Raspberry Pi, we chose to allot 1 Raspberry Pi per Cozmo. In order to get communication working, we decided to use ssh, which is a built-in Linux service used for remote command execution. ssh works quite reliably, and can be used without keyboard interaction with the sshpass utility. Since all the Raspberry Pis had identical file structures and login credentials (all are copies of each other) and all software was shared via github, we could run the shell scripts on arbitrary Raspberry Pis. The only thing that had to be hard-coded is the network IP address of the controller. During the project, we found that a Raspberry Pi can ssh into itself, so we can actually get by with using a single Raspberry Pi to be both the controller and a rover.
We decided to use fifo data structures in order to transmit information between Raspberry Pis. In essence, to send a message, a Raspberry Pi echo’s a command to a fifo on a controller, and then the Raspberry Pi that wants to read the message cat’s the message from the fifo. Our system has 3 different types of communications: 0) cube pickup requests; 1) cube dropoff requests; 2) cube dropoff acknowledgements. Each message from a rover Raspberry Pi to a controller is sent to the fifo fifo_controller_read and consists of three numbers robot_id message_type column_id/cube_id. message_type of 0 corresponds to a pickup request, 1 to a dropoff request, and 2 if there’s a dropoff acknowledgement. Each robot has a unique robot_id which is used by the controller. column_id is used for message_type of 1 in order to specify the column at which to drop-off the cube. cube_id is used for message_type of 0 in order to specify the cube to pick up.
message_type’s of 0 and 1 require a response to the rover Raspberry Pi, which involves the fifo fifo_[robot_id]_read, where robot_id corresponds to the robot id of the robot which sent the original message and is waiting for a response.
Testing the controller logic started with verifying that the fifos work as intended simply by using a couple console windows. Then, the tests involved writing shell scripts to interact with the fifo via sshpass, which would eventually be called from within Python scripts.
The controller script, which can run on a standalone Raspberry Pi or on a Raspberry Pi concurrently running a rover script, is essential for ensuring that Cozmos don’t fight over the same cube pickup/dropoff spot. The controller script is essentially one infinite loop that continually reads from fifo_controller_read and sends appropriate response information. If the cube a robot wishes to pick up is free, the controller sends a message of 1 to the rover’s fifo. If another robot is in the process of picking up the cube, the response will be 0 (and the rover will search for another cube to pick up). Similarly, if a robot wishes to place a cube in a certain column, the controller will respond with a 1 if the spot is free for dropping off the cube and 0 otherwise. The acknowledgement message is used to mark that certain columns are now free for placing additional cubes.
Testing the controller consisted of confirming that the messages it sends to the fifos are correct given the messages it receives. This involved using another Raspberry Pi to communicate with the controller.
The rover logic was designed to work with a variable number of Cozmo robots. The only hard-coded parameter that must be set is the robot_id of each Raspberry Pi-Cozmo pair, which is used for communicating to the controller, and the IP address of the controller Raspberry Pi.
The initialization software consists of registering the Cozmo robot device and the tags so that the software can reference them. The main rover logic is structured as a finite state machine. The states are structured in a way to allow for future expansion, which will happen in the CEI Lab. There is a mission, which is an overall objective that the robot wishes to achieve. Currently, the missions we have are DO_NOTHING and FLOOR_CUBE_PLACEMENT. Within the missions, there are actions that describe the status of the mission (NOT_SET, GET_CUBE, PLACE_CUBE). For each action, there is a different function called and the current state in the FSM is changed. We use an object-oriented design approach to easily share global variables across function calls.
If the current action is NOT_SET, the action gets changed to GET_CUBE and the rover is commanded to pick up the cube. If the current action is GET_CUBE, then the rover will change its action to PLACE_CUBE and then command the rover to place a cube. If the current action is PLACE_CUBE, the rover moves back to its original position and then changes its action to NOT_SET.
To retrieve the cube, the software repeatedly tells the Cozmo to move forward, and then scan either the left or right direction for a tag. If a tag is found while scanning, the robot asks the controller for permission to pick up the cube. If the controller grants permission, the rover will proceed to pick up the cube. If the controller denies permission, then the rover will continue scanning for other cubes. The code for picking up the cubes follows the aforementioned discussion of the cube pickup algorithm.
To place the cubes, the software first waits 5 seconds in order to prevent potential collisions with robots on the other side of the course. After, the software commands the rover to go to a hardcoded point in the Cozmo coordinate system that is defined relative to the starting point. This point corresponds a rough position of where the Cozmo should start looking for tags that indicate the dropoff locations on the walls or behind cubes. This position is also used to determine the column id at which a Cozmo is located, which is used by the controller. To place a cube down, the software follows a similar routine as the cube pick up method in order to get an approach angle and position. However, the arms move down at the end of dropping off the cube rather than moving up, and the distance that the rover moves forward to drop off the cube is reduced.
To test the system during this phase of the project, we did a lot of trials with different wall placements and initial robot placements, in order to determine initial configurations that minimize collisions and maximize success. We also did full-scale integration tests at this stage in order to identify areas of the code that did not work.
Overall, we were fairly satisfied with the results of the project. We were able to get multiple Cozmo robots running concurrently on a course while communicating to a controller. Furthermore, we were successful in demonstrating Cozmo’s ability to pick up cubes and place them down accurately. Unfortunately, one of the Cozmo’s lift arms got seriously injured and is unable to lift cubes anymore. Since this occurred shortly before the project deadline, we were not able to get a replacement Cozmo and demonstrate the system working at peak level with 2 Cozmos.
We believe we have completed most of our initial goals successfully. We learned a lot about how to control the Cozmo with the Raspberry Pi. We were able to demonstrate that Cozmos can be used to achieve group activities.
Anki, the maker of Cozmo, shut down days before the project deadline. This means that future support for Cozmo will be limited. However, we are still happy with our final product and believe that many of the protocols can be converted for use with different robotic systems, including our FSM, controller, and communication protocols.
3 x Raspberry Pi Model 3 B+
Free (provided by CEI Lab)
3D printed cubes
Free (3D printing material provided by CEI Lab)
2 x Cozmo Robot
Free (provided by CEI Lab)
2 x Amazon Kindle Fire
Free (provided by CEI Lab)
3 x 32GB microSD
Free (provided by CEI Lab)
3D printed Raspberry Pi holder
Free (3D printing material provided by CEI Lab)
$0.20 (Cornell printing services)
$5.34 (Cornell Store)
Total project cost: $5.54
If we had more time, we would explore better collision detection technology. Besides using the video feed from the front camera on the Cozmo, there is no way for Cozmo to directly detect collisions. Since the Cozmo is designed to be used with the SDK and supported hardware/software, it is very difficult to “hack” the Cozmo to work with different hardware other than what comes out of the box. Furthermore, using the camera feed to detect collisions is an expensive operation since collision detection has to constantly occur, and there is a lot of latency involved in sending camera data from the Cozmo to the Raspberry Pi, and the time needed to process the images for potential collisions is significant.
Unfortunately, we experienced systematic errors in cube localization. This is due to the fact that our needs required a higher degree of precision than what the Cozmo engine could provide. In addition, the Cozmo rover would routinely get de-localized from its original position, meaning that it would have trouble getting back to the cube placement and drop-off spots. If we had more time, we would improve the localization accuracy by “re-localizing” the Cozmo with a large tag on, say, a wall. In addition, since the software is designed with portability in mind, we believe that newer and more performant robots will be able to carry out the tasks that our system demands much more easily than the Cozmos we used.
See our github repository: https://github.com/evankravitz/Cozmo_swarm
We would like to acknowledge the contributions of Jingwen Du, who is participating in research in the Collective Embodied Intelligence Lab at Cornell under Prof. Kirstin Petersen. She worked on the project with Evan and Haiyang, but did not contribute to this report since she is not enrolled in ECE 5725. She helped develop the cube design, tag placement/size, and cube pickup algorithm.
We would also like to thank Prof. Petersen for allowing us to use CEI lab materials for this project, and for funding some of the project parts. In addition, we extend our gratitude to Prof. Joe Skovira and the course staff for their help this semester.