c++ control system library for 3D position and orientation control