function trace = WP_run_BDI
%

clear WP_bot
clear WP_beliefs
clear WP_plan_find_floor

Tr = eye(4);
Tr(1:3,1:3) = CS5320_gen_R([0;0;1],pi/4);
Tr(1:2,4) = [3;7];

Tc_t = eye(4);
Tc_t(3,4) = 0.82;
Tc_x = eye(4);
Tc_x(1:3,1:3) = CS5320_gen_R([1;0;0],-(90+34.69)*pi/180);
Tc_z = eye(4);
%Tc_z(1:3,1:3) = CS5320_gen_R([0;0;1],pi);
Tc = Tc_t*Tc_x*Tc_z;

[world,dimens] = WP_init_world;

step = 1;
action = [0,0,0];
trace(step).action = action;
while WP_beliefs(1,7,'info',0)==0  % Quit if finished belief set
    step = step + 1;
    if action(1)==1    % first element of action says to sense
        % put call to get Kinect data and return:
        %    rgbd (x,y,z)
        %    rgb_im (r,g,b)
%         [rgbd,rgb_im] = WP_rgbd(Tr,Tc,world);
%        load rgbd;
        load DD5;
        rgbd = DD5;
        rgbd = WP_remove_Inf_NaNs(rgbd);
    else
        rgbd = [];
        rgb_im = [];
    end
    percept.rgbd = rgbd;
    percept.rgb_im = rgb_im;

    action = WP_bot(percept);  % Get action command from BDI
    trace(step).action = action;
    % if move required, call actuator controller
    if action(2)~=0|action(3)~=0
        % Put commands to move robot
        tch = 0;
    end
end
