Anki Cozmo Kidnapping using Monte Carlo Localization

Nicholas Stach, Quan Nguyen, Brayton Alkinburgh, Douglas Harsha
Spring 2023
Src code ( github, zip^[1])

Goal:

• Implement MCL with Anki Cozmo
• Create Panorama of collected images from which the Cozmo bot can use to find its position and relocate itself.
• Create means of displaying belief probabilities.
• Docs: Cozmo API
• Helpful link for taking pictures with cozmo and saving them.

Table of Contents

1. Setup
2. Understanding algorithm
3. Tasks
4. Files and Dirs
5. Approaches
6. Running
7. Working log

Setup

Following the installation guide from Cozmo

• Setup on computer + phone:

  • Computer Setup

    1. Follow the Initial setup:
       • Install Anki Cozmo SDK
       • SDK Example Programs: download and extract to any directory
    2. Install Android Debug Bridge, follow that to Final installation.
    3. Setup environment (recommend using Anaconda): Python 3.8 + required packages in requirements.txt:
       • Create environment:

         conda create -n envname python=3.8
         

       • Activate environment:

         conda activate envname
         

       • Install all required packages:

         pip install -r requirements.txt
         

  • Mobile Phone (Android) Setup

    1. Install official Cozmo app on phone (Android/iOS)
    2. Enable USB debugging on your device. This step may vary based on your phone, so check the android instruction if needed.
    3. Those steps are required everytime working on project: i. Connect USB cable from phone to computer
       1. Run adb devices on CommandPrompt on computer to check and authorize your phone. (unil it appears "device" instead of "unauthorized")
       2. Open Cozmo app and turn Cozmo bot on
       3. Connect to Cozmo bot’s wifi network.
       4. Enable SDK on Cozmo app
  • After setting up on both machines, download the SDK Examples, open Cozmo app, connect with cozmo bot (ensure the robot does not fall off the desk) -> setting -> enable SDK. Then try running the examples.
• Website Publishing

  1. Generate html file:

     • Find your Chrome's download folder in your local machine. It will be asked in your terminal.
     • Run the generator:

       python html-generator.py
       

  2. Upload html file and assets (if any) to /public_html/ folder on lab machine at Gettysburg College.

     chmod -R 755 ~/public_html/folder_to_publish
     chmod -R 755 .
     

[Back to top]

Understanding algorithm

In our code, there are comments that refer to table, etc. Those refer to the algorithms from the "Probabilistic Robotics" by Sebastian Thrun, Wolfram Burgard and Dieter Fox.

• Pseudo code for MCL

  

• Table 5.2

  

• Table 5.4

  

  The formula above, however, is a bit different from the one from Prof. Neller's lecture:

  

[Back to top]

Tasks

• Future:
  • How to improve accuracy
  • How to make it works in dark environment (may use edge detection??)
  • How to localize with just 4 images (0, 90, 180, 270 degrees) (divide imgs into $n$ cols then compare w/ $n$ cols of each img for all $m$ ims?)
• Get robot to turn to location it has highest belief prob after MCL done
• Polish borrowed code: make more succint, use methods we have already coded.
• Read MCL slides + Java demo code (convert to python) (Bray & Doug)
• Try running MCL w/ pano
• How to rotate robot
• How to get image from cozmo’s camera (file picture-collection.py)
• Fix MCL code for images (check Youtube and github) (file my-MCL.py)
• Create a panorama (optional)
  • Check openCV Stitching (e.g from prev group: L.Attai,R.Fuller,C.Rhodes)
• Crop pano img + sensor img in MCL algo (Nick)
• Recreate MCL as per supplied examples (based on L.Attai,R.Fuller,C.Rhodes))

[Back to top]

Files and Dirs

• /cozmo-images-kidnap/: images for kidnap problem (collected data that is updated for each run)
  • x.jpg: images captured when running kidnap.py for localization.
  • data.csv: result of localization, used to generate histogram.
  • hist.png: histogram of collected belief probablities after MCL is run
• /model/: directory for training/inference neural network model.
• cozmo_MCL.py: MCL implementation, based on previous group's work (stitching method).
• kidnap.py: main file to run localization (stitching method).
• cozmo_MCL_nn.py: MCL ver. for neural-net w/ much cleaner code.
• kidnap_nn.py: main file to run localization (neural-net).
• MCL_simple.py: simple version of MCL (not the MCL described in Prof. Neller's lecture -> not accurate)
• requirements.txt: has all required installs for use with our code and Cozmo
• html-generator.py: convert md to html file using codebeautify.org. This yields much better html than python library like python-markdown
• index.html: result of html-generator.py.

[Back to top]

Approaches

• Stitching method:

  • Functionality

    Our Cozmo MCL was able to localize with reasonable accuracy in some environments (in cases where the Stitching works well). Locations with repetative patterns or extreme amounts of light greatly reduced accuracy of the localization.

    Our group was able to improve upon a past group's MCL and make it give Cozmo a command to turn to home accordingly toward from its most believed location. Our group also created a histogram with clustered probablities and implemented the highest belief into the MCL to serve as the location to turn to.

  • Results

    Building off of the work of Leah Attai, Casey Rhodes, and Rachel Fuller, as well as examples provided by Mohammad Altaleb, we were able to construct a Monte Carlo Localization System that successfully localized the Anki Cozmo robot in some environments. Our system centered around a set of 30 photos taken 12 degrees apart to form a panorama. The panorama mapped the 360 degree area around the robot prior to its "kidnapping." By rotating the robot an arbitrary number of degrees, we then "kidnapped" the robot. The robot would then take a single picture, and we performed a set of pixel-matching operations to determine which section of the panorama most likely corresponded with the robot's kidnapped heading. We would repeat this process ten times, rotating the robot slighty each time to collect varied data and increase the accuracy of our final identification. Thus having localized, we turned the robot to turn towards its initial "home" position, represented as the starting point of the panorama, reversing the kidnapping.

    Our system's performance was generally dependent upon the success of two functions. Firstly, in order to create the panorama it was necessary to stitch together the 30 initial photos. Our stitching function, based on an example from OpenCV, struggled in certain environments that reduced the distinctions between certain photos. Too much or too little light, or a lack of identifiable landmarks would stymie the algorithm, and thus produce a low-confidence panorama that induced difficulties in our localization attempts. Secondly, out pixel-matching comparison algorithm would struggle from much the same issues, running into situations were a large area of the panorama (such as a white wall) appeared as candidates for localizations. This issue was likely the cause of several failed tests where the Cozmo robot localized close but not exactly to its home position, within 20 degrees or so.

    The following table shows examples of test images taken in sequence that were and were not conducive to panorama-based localization. The top two, taken in the CS Lounge, are visually distinct with enough "landmarks" such as the striped couch, trash can, person, and chair to allow our algorithm to successfully localize. The bottom two, taken next to the CS Lounge printer, are too similar for the algorithm to extract any meaningful data -- localizations in this area appeared to be little more than guesses.

    This Histogram shows the results of one test of our localization system. The X-Axis is the width of the panorama corresponding to the 360 degree area in pixels, the Y-Axis the total probabilistic likelihood that the Cozmo is facing that area of the panorama. Blue lines represent the Cozmo's confidence in its location before localization, orange represents its confidence after localization. The point with the highest probability, here just after pixel 1400, was selected as the kidnapped location.

    

• NeuralNet method:

  • Functionality

    Theoretically, this approach works well when we tested on Google Colab. The advantage of this method is that it compares the images based on the similarity, which should be more accurate than compare pixel-by-pixel like in stitching method.

    In MCL function for this approach, the return degree for Cozmo bot after determining current position is not yet verified.

  • Result

    N/A

[Back to top]

Running

• Stitching method: After code cleaning, errors were found and the it got worse while trying to fix them. Running the program (cleaned ver.):

  python kidnap.py
  

  If it does not work well, try the one under /working_ver/, which is believed to contain the working code. Remember to ensure that the file paths work.
• NeuralNet method:
  • Install extra packages. Note that this installs PyTorch, so make sure GPU is enabled:

    pip install -r ./model/requirements.txt
    

  • Run kidnapping problem:

    python kidnap_nn.py
    

[Back to top]

Future Goals

[Back to top]

Working log

[Back to top]