Imported from https://github.com/nutintin/Robotic_SP2019