Updated to new RoboLibJ Structure