function AddRobot(this, varargin)

Name      = 'Cozmo_1';
RobotPose = {Pose()};
HeadAngle = 0;
CameraResolution = [320 240];

parseVarargin(varargin{:});

if ~iscell(RobotPose)
    RobotPose = {RobotPose};
end

if isscalar(HeadAngle)
    HeadAngle = HeadAngle * length(RobotPose);
else
    assert(length(HeadAngle) == length(RobotPose), ...
        ['Number of head angles should be one or the same as the ' ...
        'number of poses.']);
end

underscoreIndex = strfind(Name, '_');
if isempty(underscoreIndex) || ...
        isnan(str2double(Name((underscoreIndex+1):end)))
    error('Name must end in _<ID>!');
end

fid = fopen(this.worldFile, 'at');
cleanup = onCleanup(@()fclose(fid));

% Add the cozmo bot
fprintf(fid, [...
    'DEF %s CozmoBot {\n' ...
    '  name "%s"\n' ...
    '  translation  %f %f %f\n' ...  % initial translation
    '  rotation     %f %f %f %f\n' ... % initial rotation
    '  usbChannel   %d\n' ...
    '  cameraWidth  %d\n' ...
    '  cameraHeight %d\n' ...
    '  controller "visionUnitTestController"\n' ...
    '  controllerArgs "', ...    
    ], Name, Name, ...
    RobotPose{1}.T(1)/1000, RobotPose{1}.T(2)/1000, RobotPose{1}.T(3)/1000, ...
    RobotPose{1}.axis(1), RobotPose{1}.axis(2), RobotPose{1}.axis(3),  RobotPose{1}.angle, ...
    this.usbChannel, ...
    CameraResolution(1), CameraResolution(2));


% Pass the list of poses and head angles as controllerArgs
for i_pose = 1:length(RobotPose)
    T = RobotPose{i_pose}.T/1000;
    fprintf(fid, '%f %f %f %f %f %f %f %f ', ...
        RobotPose{i_pose}.axis(1), RobotPose{i_pose}.axis(2), RobotPose{i_pose}.axis(3), ...
        RobotPose{i_pose}.angle, ...
        T(1), T(2), T(3), HeadAngle(i_pose));
end

fprintf(fid, '"\n}\n\n'); 

if this.useOffboardVision
    % Add an offboard vision processor for this robot
    fprintf(fid, [ ...
        'DEF OffboardVisionProcessor USBConnection {\n' ...
        '  controller "offboard_vision_processor"\n' ...
        '  usbChannel %d\n' ...
        '  robotName "%s"\n' ...
        '}\n\n'], this.usbChannel, Name);
end % IF useOffboardVision

end % function AddRobot() 

    
