coursera program assignment 7

[yuan0623]

Hi JP
is it possible for you to release the solution of the program assignment 7.. I've struggled for a long time..... If you don't want to release the solution, can you please tell me how to make sure that set_progress_point(obj) just run once before following the wall. The issue is that if set_progress_point loop just as other function, I can never make progress...... Please help me with that....Thx

[jdelacroix]

I can't seem to find my solution to Week 7; however, here is the solution from the first version of the course: https://github.com/jdelacroix/simiam/blob/coursera-standalone-sp13/%2Bsimiam/%2Bcontroller/%2Bkhepera3/K3Supervisor.m#L164

A copy&paste may not work, because it is code for an older version of the simulator, but you should get a good idea for how this "state machine" solves the navigation problem in Week 7.

[yuan0623]

Hi
Thanks for your help, but there is still some issue... When the robot switch into follow-wall state, how to maintain that state until it made progress. My problem is the robot switch into follow the wall, and it switch avoid-obstacle or go-to-goal..... so unstable, and there is no way for the robot to finish the task...

[yuan0623]

Well, if I comment the unsafe-avoid_obstacle line, then my robot can directly switch into follow wall and go to the goal.... The problem is that if put avoid_obstacle in to the code, then the robot will switch between the follow_wall and avoid_obstacle, so it can not do the task..........is it possible to make the follow_wall robot be free from the avoid_obstacle. How to do that?

[tweil]

Here is my solution to 2nd version of PA7, QBSupervisor.m, based on the solution that JP just posted above. I needed to make several changes to the code to make the simulation succeed. I had to change inputs.direction to obj.fw.direction in a couple of places in the Finite State Machine section. I also needed to change the constant values of obj.d_at_obs , obj.d_unsafe and obj.d_fw.

After successfully completing the simulation, I tried changing the robot starting location and angle bin settings.xml. I also changed the location of the goal in QBSupervisor.m. In all cases the robot crashed. If I tweaked the constant values of obj.d_at_obs , obj.d_unsafe and obj.d_fw, I was able to get some of the simulations to complete successfully. Some of the ones that failed might need a better finite state machine.

QBSupervisor.m

classdef QBSupervisor < simiam.controller.Supervisor
%% SUPERVISOR switches between controllers and handles their inputs/outputs.
%
% Properties:
%   current_controller      - Currently selected controller
%   controllers             - List of available controllers
%   goal_points             - Set of goal points
%   goal_index              - Pointer to current goal point
%   v                       - Robot velocity
%
% Methods:
%   execute - Selects and executes the current controller.

% Copyright (C) 2013, Georgia Tech Research Corporation
% see the LICENSE file included with this software

properties
%% PROPERTIES

    states
    eventsd

    current_state

    prev_ticks          % Previous tick count on the left and right wheels

    v
    theta_d
    goal

    d_stop
    d_at_obs
    d_unsafe
    d_fw
    d_prog

    p

    v_max_w0            % QuickBot's max linear velocity when w=0
    w_max_v0            % QuickBot's max angular velocity when v=0
    v_min_w0
    w_min_v0

    switch_count

    fw_direction
end

methods
%% METHODS

    function obj = QBSupervisor()
    %% SUPERVISOR Constructor
        obj = [email protected]();

        % initialize the controllers
        obj.controllers{1} = simiam.controller.Stop();
        obj.controllers{2} = simiam.controller.GoToAngle();
        obj.controllers{3} = simiam.controller.GoToGoal();
        obj.controllers{4} = simiam.controller.AvoidObstacles();
        obj.controllers{5} = simiam.controller.AOandGTG();
        obj.controllers{6} = simiam.controller.FollowWall();
        obj.controllers{7} = simiam.controller.SlidingMode();

        % set the initial controller
        obj.current_controller = obj.controllers{3};
        obj.current_state = 3;

        % generate the set of states
        for i = 1:length(obj.controllers)
            obj.states{i} = struct('state', obj.controllers{i}.type, ...
                                   'controller', obj.controllers{i});
        end

        % define the set of eventsd
        obj.eventsd{1} = struct('event', 'at_obstacle', ...
                               'callback', @at_obstacle);

        obj.eventsd{2} = struct('event', 'at_goal', ...
                               'callback', @at_goal);

        obj.eventsd{3} = struct('event', 'obstacle_cleared', ...
                                'callback', @obstacle_cleared);

        obj.eventsd{4} = struct('event', 'unsafe', ...
                                'callback', @unsafe);

        obj.eventsd{5} = struct('event', 'progress_made', ...
                                'callback', @progress_made);

        obj.eventsd{6} = struct('event', 'sliding_left', ...
                                'callback', @sliding_left);

        obj.eventsd{7} = struct('event', 'sliding_right', ...
                                'callback', @sliding_right);

        obj.prev_ticks = struct('left', 0, 'right', 0);

        obj.theta_d     = pi/4;

        %% START CODE BLOCK %%
        obj.v           = 0.15;
        obj.goal        = [1.1, 1.1];  %1.1, 1.1
        obj.d_stop      = 0.05;
        obj.d_at_obs    = 0.13; %0.1   0.2           
        obj.d_unsafe    = 0.03; %0.05  0.1

        obj.d_fw        = 0.175; %0.17
        obj.fw_direction   = 'left';


        %% END CODE BLOCK %%

        obj.p = simiam.util.Plotter();
        obj.current_controller.p = obj.p;

        obj.d_prog = 10;

        obj.switch_count = 0;
    end

    function execute(obj, dt)
    %% EXECUTE Selects and executes the current controller.
    %   execute(obj, robot) will select a controller from the list of
    %   available controllers and execute it.
    %
    %   See also controller/execute

        obj.update_odometry();

        inputs = obj.controllers{7}.inputs; 
        inputs.v = obj.v;
        inputs.d_fw = obj.d_fw;
        inputs.x_g = obj.goal(1);
        inputs.y_g = obj.goal(2);

        %% START CODE BLOCK %%

        ir_distances = obj.robot.get_ir_distances();

       if (obj.check_event('at_goal'))
            obj.switch_to_state('stop');
            [x,y,theta] = obj.state_estimate.unpack();
            fprintf('stopped at (%0.3f,%0.3f)\n', x, y);
        elseif(obj.check_event('unsafe'))
            obj.switch_to_state('avoid_obstacles');                
        else
            if (obj.is_in_state('go_to_goal'))
                if(obj.check_event('at_obstacle') && obj.check_event('sliding_left'))
                    obj.fw_direction = 'left'
                    fprintf('now following to the left\n');
                    obj.switch_to_state('follow_wall');
                    obj.set_progress_point();
                elseif(obj.check_event('at_obstacle') && obj.check_event('sliding_right'))
                    obj.fw_direction = 'right';
                    fprintf('now following to the right\n');
                    obj.switch_to_state('follow_wall');
                    obj.set_progress_point();
                end
            elseif (obj.is_in_state('follow_wall') && strcmp(inputs.direction,'left'))
                if(obj.check_event('progress_made') && ~obj.check_event('sliding_left'))
                    obj.switch_to_state('go_to_goal');
                    fprintf('go_to_goal Progress_made left\n');
                end
            elseif (obj.is_in_state('follow_wall') && strcmp(inputs.direction, 'right'))
                if(obj.check_event('progress_made') && ~obj.check_event('sliding_right'))
                    obj.switch_to_state('go_to_goal');
                    fprintf('go_to_goal Progress_made right\n');
                end
            elseif (obj.is_in_state('avoid_obstacles'))
                if(obj.check_event('obstacle_cleared'))
                    if(obj.check_event('sliding_left'))
                        obj.fw_direction = 'left';
                        obj.switch_to_state('follow_wall');
                    elseif(obj.check_event('sliding_right'))
                        obj.fw_direction = 'right';
                        obj.switch_to_state('follow_wall');
                    else
                        obj.switch_to_state('go_to_goal');
                    end
                end
            end

        end 

        %% END CODE BLOCK %%

        inputs.direction = obj.fw_direction;
        outputs = obj.current_controller.execute(obj.robot, obj.state_estimate, inputs, dt);

        [vel_r, vel_l] = obj.ensure_w(obj.robot, outputs.v, outputs.w);
        obj.robot.set_wheel_speeds(vel_r, vel_l);

%[x, y, theta] = obj.state_estimate.unpack();
%fprintf('current_pose: (%0.3f,%0.3f,%0.3f)\n', x, y, theta);

     end

    function set_progress_point(obj)
        [x, y, theta] = obj.state_estimate.unpack();
        obj.d_prog = min(norm([x-obj.goal(1);y-obj.goal(2)]), obj.d_prog);
        fprintf('Event: set_progress_point %f\n',obj.d_prog);
    end

    %% Events %%

    function rc = sliding_left(obj, state, robot)
        inputs = obj.controllers{7}.inputs;
        inputs.x_g = obj.goal(1);
        inputs.y_g = obj.goal(2);
        inputs.direction = 'left';

        obj.controllers{7}.execute(obj.robot, obj.state_estimate, inputs, 0);

        u_gtg = obj.controllers{7}.u_gtg;
        u_ao = obj.controllers{7}.u_ao;
        u_fw = obj.controllers{7}.u_fw;

        %% START CODE BLOCK %%
        %sigma = [0;0];
        %sigma1 = (u_ao(1)*u_fw(2) - u_ao(2)*u_fw(1))/(u_ao(1)*u_gtg(2) - u_ao(2)*u_gtg(1));
        %sigma2 = (u_fw(1)*u_gtg(2) - u_fw(2)*u_gtg(1))/(u_ao(1)*u_gtg(2) - u_ao(2)*u_gtg(1));
        %sigma = [sigma1;sigma2];
        sigma = [u_gtg,u_ao]\u_fw;
        %% END CODE BLOCK %%
        fprintf('sliding_left sigma = %f %f\n', sigma(1), sigma(2));
        rc = false;
        if sigma(1) > 0 && sigma(2) > 0

        fprintf('now sliding left\n');
            rc = true;
        end
    end

    function rc = sliding_right(obj, state, robot)
        inputs = obj.controllers{7}.inputs;
        inputs.x_g = obj.goal(1);
        inputs.y_g = obj.goal(2);
        inputs.direction = 'right';

        obj.controllers{7}.execute(obj.robot, obj.state_estimate, inputs, 0);

        u_gtg = obj.controllers{7}.u_gtg;
        u_ao = obj.controllers{7}.u_ao;
        u_fw = obj.controllers{7}.u_fw;

        %% START CODE BLOCK
        %sigma = [0;0];
        %sigma1 = (u_ao(1)*u_fw(2) - u_ao(2)*u_fw(1))/(u_ao(1)*u_gtg(2) - u_ao(2)*u_gtg(1));
        %sigma2 = (u_fw(1)*u_gtg(2) - u_fw(2)*u_gtg(1))/(u_ao(1)*u_gtg(2) - u_ao(2)*u_gtg(1));
        %sigma = [sigma1;sigma2];

        sigma = [u_gtg,u_ao]\u_fw;
        %% END CODE BLOCK
        fprintf('sliding_right sigma = %f %f\n', sigma(1), sigma(2));
        rc = false;
        if sigma(1) > 0 && sigma(2) > 0
        fprintf('now sliding right\n');
            rc = true;
        end
    end

    function rc = progress_made(obj, state, robot)

        % Check for any progress
        [x, y, theta] = obj.state_estimate.unpack();            
        rc = false;

        %% START CODE BLOCK %% DONE

        epsilon = 0.2;
        if norm([x-obj.goal(1);y-obj.goal(2)]) < (obj.d_prog - epsilon)
            rc = true;
            fprintf('Event: progress_made %f\n',norm([x-obj.goal(1);y-obj.goal(2)]));
        end

        %% END CODE BLOCK %%

    end

    function rc = at_goal(obj, state, robot)
        [x,y,theta] = obj.state_estimate.unpack();
        rc = false;

        % Test distance from goal
        if norm([x - obj.goal(1); y - obj.goal(2)]) < obj.d_stop
        fprintf('Event: at_goal \n');
            rc = true;
        end
    end

    function rc = at_obstacle(obj, state, robot)
        ir_distances = obj.robot.get_ir_distances();
        rc = false; % Assume initially that the robot is clear of obstacle

        % Loop through and test the sensors (only the front set)
        if any(ir_distances(1:5) < obj.d_at_obs)
        fprintf('Event: at_obstacle \n');
            rc = true;
        end
    end

    function rc = unsafe(obj, state, robot)
        ir_distances = obj.robot.get_ir_distances();              
        rc = false; % Assume initially that the robot is clear of obstacle

        % Loop through and test the sensors (only the front set)
        if any(ir_distances(1:5) < obj.d_unsafe)
        fprintf('Event: unsafe \n');
                rc = true;
        end
    end

    function rc = obstacle_cleared(obj, state, robot)
        ir_distances = obj.robot.get_ir_distances();
        rc = false;              % Assume initially that the robot is clear of obstacle

        % Loop through and test the sensors (only front set)
        if all(ir_distances(1:5) > obj.d_at_obs)
            fprintf('Event: obstacle_cleared \n');
            rc = true;
        end
    end



    %% Output shaping

    function [vel_r, vel_l] = ensure_w(obj, robot, v, w)

        % This function ensures that w is respected as best as possible
        % by scaling v.

        R = robot.wheel_radius;
        L = robot.wheel_base_length;

        vel_max = robot.max_vel;
        vel_min = robot.min_vel;

%     fprintf('IN (v,w) = (%0.3f,%0.3f)\n', v, w);

        if (abs(v) > 0)
            % 1. Limit v,w to be possible in the range [vel_min, vel_max]
            % (avoid stalling or exceeding motor limits)
            v_lim = max(min(abs(v), (R/2)*(2*vel_max)), (R/2)*(2*vel_min));
            w_lim = max(min(abs(w), (R/L)*(vel_max-vel_min)), 0);

            % 2. Compute the desired curvature of the robot's motion

            [vel_r_d, vel_l_d] = robot.dynamics.uni_to_diff(v_lim, w_lim);

            % 3. Find the max and min vel_r/vel_l
            vel_rl_max = max(vel_r_d, vel_l_d);
            vel_rl_min = min(vel_r_d, vel_l_d);

            % 4. Shift vel_r and vel_l if they exceed max/min vel
            if (vel_rl_max > vel_max)
                vel_r = vel_r_d - (vel_rl_max-vel_max);
                vel_l = vel_l_d - (vel_rl_max-vel_max);
            elseif (vel_rl_min < vel_min)
                vel_r = vel_r_d + (vel_min-vel_rl_min);
                vel_l = vel_l_d + (vel_min-vel_rl_min);
            else
                vel_r = vel_r_d;
                vel_l = vel_l_d;
            end

            % 5. Fix signs (Always either both positive or negative)
            [v_shift, w_shift] = robot.dynamics.diff_to_uni(vel_r, vel_l);

            v = sign(v)*v_shift;
            w = sign(w)*w_shift;

        else
            % Robot is stationary, so we can either not rotate, or
            % rotate with some minimum/maximum angular velocity
            w_min = R/L*(2*vel_min);
            w_max = R/L*(2*vel_max);

            if abs(w) > w_min
                w = sign(w)*max(min(abs(w), w_max), w_min);
            else
                w = 0;
            end


        end

%     fprintf('OUT (v,w) = (%0.3f,%0.3f)\n', v, w);
        [vel_r, vel_l] = robot.dynamics.uni_to_diff(v, w);
    end


    %% State machine support functions

    function set_current_controller(obj, ctrl)
        % save plots
        obj.current_controller = ctrl;
        obj.p.switch_2d_ref();
        obj.current_controller.p = obj.p;
    end

    function rc = is_in_state(obj, name)
        rc = strcmp(name, obj.states{obj.current_state}.state);
    end

    function switch_to_state(obj, name)

        if(~obj.is_in_state(name))
            for i=1:numel(obj.states)
                if(strcmp(obj.states{i}.state, name))
                    obj.set_current_controller(obj.states{i}.controller);
                    obj.current_state = i;
                    fprintf('switching to state %s\n', name);
                    obj.states{i}.controller.reset();
                    obj.switch_count = obj.switch_count + 1;
                    return;
                end
            end
        else
% fprintf('already in state %s\n', name);
            return
        end

        fprintf('no state exists with name %s\n', name);
    end

    function rc = check_event(obj, name)
       for i=1:numel(obj.eventsd)
           if(strcmp(obj.eventsd{i}.event, name))
               rc = obj.eventsd{i}.callback(obj, obj.states{obj.current_state}, obj.robot);
               return;
           end
       end

       % return code (rc)
       fprintf('no event exists with name %s\n', name);
       rc = false;
    end

    %% Odometry

    function update_odometry(obj)
    %% UPDATE_ODOMETRY Approximates the location of the robot.
    %   obj = obj.update_odometry(robot) should be called from the
    %   execute function every iteration. The location of the robot is
    %   updated based on the difference to the previous wheel encoder
    %   ticks. This is only an approximation.
    %
    %   state_estimate is updated with the new location and the
    %   measured wheel encoder tick counts are stored in prev_ticks.

        % Get wheel encoder ticks from the robot
        right_ticks = obj.robot.encoders(1).ticks;
        left_ticks = obj.robot.encoders(2).ticks;

        % Recal the previous wheel encoder ticks
        prev_right_ticks = obj.prev_ticks.right;
        prev_left_ticks = obj.prev_ticks.left;

        % Previous estimate 
        [x, y, theta] = obj.state_estimate.unpack();

        % Compute odometry here
        R = obj.robot.wheel_radius;
        L = obj.robot.wheel_base_length;
        m_per_tick = (2*pi*R)/obj.robot.encoders(1).ticks_per_rev;

        d_right = (right_ticks-prev_right_ticks)*m_per_tick;
        d_left = (left_ticks-prev_left_ticks)*m_per_tick;

        d_center = (d_right + d_left)/2;
        phi = (d_right - d_left)/L;

        x_dt = d_center*cos(theta);
        y_dt = d_center*sin(theta);
        theta_dt = phi;

        theta_new = theta + theta_dt;
        x_new = x + x_dt;
        y_new = y + y_dt;                           
% fprintf('Estimated pose (x,y,theta): (%0.3g,%0.3g,%0.3g)\n', x_new, y_new, theta_new);

        % Save the wheel encoder ticks for the next estimate
        obj.prev_ticks.right = right_ticks;
        obj.prev_ticks.left = left_ticks;

        % Update your estimate of (x,y,theta)
        obj.state_estimate.set_pose([x_new, y_new, atan2(sin(theta_new), cos(theta_new))]);
    end

    %% Utility functions

    function attach_robot(obj, robot, pose)
        obj.robot = robot;
        [x, y, theta] = pose.unpack();
        obj.state_estimate.set_pose([x, y, theta]);

        [v_0, obj.w_max_v0] = robot.dynamics.diff_to_uni(obj.robot.max_vel, -obj.robot.max_vel);
        [obj.v_max_w0, w_0] = robot.dynamics.diff_to_uni(obj.robot.max_vel, obj.robot.max_vel);

        [v_0, obj.w_min_v0] = robot.dynamics.diff_to_uni(obj.robot.min_vel, -obj.robot.min_vel);
        [obj.v_min_w0, w_0] = robot.dynamics.diff_to_uni(obj.robot.min_vel, obj.robot.min_vel);
    end
  end
end