Geometric fundamentals of robotics (Springer, 2005)