Open yuan0623 opened 9 years ago
jdelacroix
commented
9 years ago 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
commented
9 years ago 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
commented
9 years ago 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
commented
9 years ago 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 = obj@simiam.controller.Supervisor();
% 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
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