Merge the refactor/sergio+ed branch for robot_skills