Initial commit, works with RQT RobotControl