Utilities

Common utilities for PythonRobotics.