function exampleCommandBuildWorldRRT(coordinator)

%CommandBuildWorld Construct the world used for visualization and collision detection
%   This command constructs the environment, consisting of the workstation,
%   belts, shelves, parts to be moved, and other obstacles. The robot is constructed
%   separately. These objects are created using collision primitives and
%   placed in the right spots. In the workflow, this world is used during
%   path planning for collision-checking, and during
%   the visualization step.
%

% Copyright 2020 The MathWorks, Inc.

    % Construct the Workstation 
    bench = collisionBox(0.5, 0.6, 0.05);
    belt1 = collisionBox(1.3, 0.4, 0.05);

    % Place the bench and belt in the proper locations
    TBench = trvec2tform([0.4 0.1 0.18]);
    TBelt1 = trvec2tform([0 0.6 0.18]);

    bench.Pose = TBench;
    belt1.Pose = TBelt1;   
    
    % Construct and place the pillar
    pillar = collisionCylinder(0.19, 0.6 );
    Tobs1v = trvec2tform([0.4 0.5 0.5]);
    pillar.Pose = Tobs1v;    
    
    % Shelves collision meshes     
    vert1 = collisionBox(0.03, 0.3, 0.4);
    vert2 = collisionBox(0.03, 0.3, 0.4);
    hor1 = collisionBox(0.47, 0.3, 0.03);
    hor2 = collisionBox(0.47, 0.3, 0.03);

    vert1.Pose = trvec2tform([0.02, 0.65, 0.4]);
    vert2.Pose = trvec2tform([-0.45, 0.65, 0.4]); 
    hor1.Pose = trvec2tform([-0.22, 0.65, 0.4]);
    hor2.Pose = trvec2tform([-0.22, 0.65, 0.6]); 
    
    % Add the parts, which are only used for visualization and
    % simulation. A separate tool ensures that when a part is
    % gripped, it is included in the collision detection stage of
    % the trajectory optimization workflow.
    box2 = collisionBox(0.06, 0.06, 0.1);
    box3 = collisionBox(0.06, 0.06, 0.1);
    box1 = collisionBox(0.06, 0.06, 0.1);

    % Move the parts into position
    TBox2 = trvec2tform([0.5 -0.15 0.26]);
    TBox3 = trvec2tform([0.52 0 0.26]);
    TBox1 = trvec2tform([0.32 -0.08 0.26]);

    box2.Pose = TBox2;
    box3.Pose = TBox3;
    box1.Pose = TBox1;

    % Set the part mesh and color
    part1.mesh = box2;
    part2.mesh = box3;
    part3.mesh = box1;

    part1.color = 'y';
    part2.color = 'g';
    part3.color = 'g';

    part1.centerPoint = tform2trvec(part1.mesh.Pose);
    part2.centerPoint = tform2trvec(part2.mesh.Pose);
    part3.centerPoint = tform2trvec(part3.mesh.Pose);

    part1.plot = [];
    part2.plot = [];
    part3.plot = [];

    coordinator.Parts = {part1, part2, part3};
    coordinator.World = {bench, belt1, pillar, vert1, vert2, hor1, hor2, part1.mesh, part2.mesh, part3.mesh};  

    % Visualize world and parts
    visualizeWorld(coordinator)
    visualizeParts(coordinator)

   % Trigger Stateflow chart Event
   coordinator.FlowChart.worldBuilt;
end

