add utils.py in main branche and update README.md to fix bugs for visualization

[SiyuChen1]

Description

How I Tested

Do not merge before

• link to dependent pull request

I fulfilled the following requirements

• All new code is formatted according to our style guide (for C++ run clang-format, for Python, run flake8 and fix all warnings).
• All new functions/classes are documented and existing documentation is updated according to changes.
• No commented code from testing/debugging is kept (unless there is a good reason to keep it).