Collection of math utilities for robotics applications