Skip to content

Commit

Permalink
youbot working in matlab
Browse files Browse the repository at this point in the history
  • Loading branch information
@ahtamjidi
ahtamjidi committed Jan 13, 2014
1 parent 1eda6e5 commit f801ef0
Show file tree
Hide file tree
Showing 28 changed files with 2,462 additions and 2,705 deletions.
3,281 changes: 1,645 additions & 1,636 deletions Aji_V-rep/remApi.m
View file

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion All_FIRM_graph_classes/@FIRM_graph_interface/FIRM_graph_interface.m
View file
Expand Up @@ -127,7 +127,7 @@
% in the following we plot the feedback pi on the graph.
selected_nodes = user_data_class.par.selected_nodes_for_plotting_feedback_pi;
% if ~isempty(selected_nodes )
obj.plot_handle_feedback_pi = obj.PRM.draw_feedback_pi(obj.feedback_pi, obj.Edges, selected_nodes);
% obj.plot_handle_feedback_pi = obj.PRM.draw_feedback_pi(obj.feedback_pi, obj.Edges, selected_nodes);
% end
end
end
Expand Down
119 changes: 60 additions & 59 deletions All_PRM_classes/Point_PRM_class.m
View file
Expand Up @@ -135,65 +135,66 @@
end

function feedback_plot_handle = draw_feedback_pi(obj, feedback_pi, selected_node_indices)
error('This function is obsolete. But it can be updated beautifully with the new was of looking to feedback!')
% This function has not been updated after the latest changes.

% the selected nodes are a set of nodes that the feedback pi is
% only drawn for them. This is for uncluttering the figure.
if ~exist('selected_node_indices', 'var') % if there is no "selected nodes", we draw the "feedback pi" for all nodes.
selected_node_indices = 1:obj.num_nodes;
elseif isvector(selected_node_indices) % the "selected_node_indices" has to be a row vector. So, here we make the code robust to column vector as well.
selected_node_indices = reshape(selected_node_indices,1,length(selected_node_indices));
end
feedback_plot_handle = [];
num_edges = length(obj.edges_list);
feedback_text_handle = zeros(1,num_edges);
for i = selected_node_indices
start = obj.nodes(i).val(1:2); % from now on, in this function, we only consider 2D position of the nodes.
j = feedback_pi(i); % j is the next node for node i, based on "feedback pi".
if isnan(j) % the feedback_pi on the goal node return "nan"
continue
end
[~,edge_num] = intersect(obj.edges_list,[i,j],'rows'); % this function returns the number of PRM edge, whose start and end nodes are i and j, respectively.
edge_num =obj.corresponding_2D_edges(edge_num); % this line returns the number of corresponding 2D edge.
% in the following we draw a paraller line to the edge,
% through which we want to illustrate the feedback "pi".
final = obj.nodes(j).val(1:2);
parallel_vector_normalized = (final - start)/norm(final - start);
perpendicular_vector_normalized = [ parallel_vector_normalized(2); - parallel_vector_normalized(1)];
shiftet_dist = 1;
length_of_parallel = norm(final - start)/3; % length of the dotted parallel line
offset_from_start = (norm(final - start) - length_of_parallel)/2;
start_new = start + shiftet_dist * perpendicular_vector_normalized + offset_from_start * parallel_vector_normalized;
final_new = final + shiftet_dist * perpendicular_vector_normalized - offset_from_start * parallel_vector_normalized;
plot([start_new(1), final_new(1)],[start_new(2), final_new(2)], '--r');

% in the following we plot the small triangle (called arrow
% here) on the dotted parallel line
middle = (start_new + final_new)/2;
arrow_size = length_of_parallel/5;
arrow_head = middle + parallel_vector_normalized*arrow_size/2;
bottom_mid = middle - parallel_vector_normalized*arrow_size/2;
bottom_vertices_outer = bottom_mid + perpendicular_vector_normalized*arrow_size/4;
bottom_vertices_inner = bottom_mid - perpendicular_vector_normalized*arrow_size/4;
verts = [arrow_head';bottom_vertices_outer';bottom_vertices_inner'];
faces = [1 2 3]; % this tells the number of vertices in the patch object. In our case, we have only three vertices and all of them are among the patch vertices.
patch_handle = patch('Faces',faces,'Vertices',verts,'FaceColor','g','EdgeColor','r');
feedback_plot_handle = [feedback_plot_handle, patch_handle]; %#ok<AGROW>

% in the following, we write the number of the node on the
% corresponding parallel lines.
text_dist = shiftet_dist/2;
text_position = bottom_vertices_outer + text_dist*perpendicular_vector_normalized;
text_position(1) = text_position(1) - 0.45; % for some reason MATLAB shifts the starting point of the text a little bit to the right. So, here we return it back.
if feedback_text_handle (edge_num) ~= 0 % which means some text has already been written for this edge
current_text = get(feedback_text_handle (edge_num), 'String');
set(feedback_text_handle (edge_num), 'String', [current_text, ', ', num2str(i)])
else
feedback_text_handle (edge_num) = text(text_position(1), text_position(2), num2str(i), 'fontsize',10,'color','r','EdgeColor','g');
feedback_plot_handle = [feedback_plot_handle, feedback_text_handle (edge_num)]; %#ok<AGROW>
end
end
feedback_plot_handle =[];
% % % error('This function is obsolete. But it can be updated beautifully with the new was of looking to feedback!')
% % % % This function has not been updated after the latest changes.
% % %
% % % % the selected nodes are a set of nodes that the feedback pi is
% % % % only drawn for them. This is for uncluttering the figure.
% % % if ~exist('selected_node_indices', 'var') % if there is no "selected nodes", we draw the "feedback pi" for all nodes.
% % % selected_node_indices = 1:obj.num_nodes;
% % % elseif isvector(selected_node_indices) % the "selected_node_indices" has to be a row vector. So, here we make the code robust to column vector as well.
% % % selected_node_indices = reshape(selected_node_indices,1,length(selected_node_indices));
% % % end
% % % feedback_plot_handle = [];
% % % num_edges = length(obj.edges_list);
% % % feedback_text_handle = zeros(1,num_edges);
% % % for i = selected_node_indices
% % % start = obj.nodes(i).val(1:2); % from now on, in this function, we only consider 2D position of the nodes.
% % % j = feedback_pi(i); % j is the next node for node i, based on "feedback pi".
% % % if isnan(j) % the feedback_pi on the goal node return "nan"
% % % continue
% % % end
% % % [~,edge_num] = intersect(obj.edges_list,[i,j],'rows'); % this function returns the number of PRM edge, whose start and end nodes are i and j, respectively.
% % % edge_num =obj.corresponding_2D_edges(edge_num); % this line returns the number of corresponding 2D edge.
% % % % in the following we draw a paraller line to the edge,
% % % % through which we want to illustrate the feedback "pi".
% % % final = obj.nodes(j).val(1:2);
% % % parallel_vector_normalized = (final - start)/norm(final - start);
% % % perpendicular_vector_normalized = [ parallel_vector_normalized(2); - parallel_vector_normalized(1)];
% % % shiftet_dist = 1;
% % % length_of_parallel = norm(final - start)/3; % length of the dotted parallel line
% % % offset_from_start = (norm(final - start) - length_of_parallel)/2;
% % % start_new = start + shiftet_dist * perpendicular_vector_normalized + offset_from_start * parallel_vector_normalized;
% % % final_new = final + shiftet_dist * perpendicular_vector_normalized - offset_from_start * parallel_vector_normalized;
% % % plot([start_new(1), final_new(1)],[start_new(2), final_new(2)], '--r');
% % %
% % % % in the following we plot the small triangle (called arrow
% % % % here) on the dotted parallel line
% % % middle = (start_new + final_new)/2;
% % % arrow_size = length_of_parallel/5;
% % % arrow_head = middle + parallel_vector_normalized*arrow_size/2;
% % % bottom_mid = middle - parallel_vector_normalized*arrow_size/2;
% % % bottom_vertices_outer = bottom_mid + perpendicular_vector_normalized*arrow_size/4;
% % % bottom_vertices_inner = bottom_mid - perpendicular_vector_normalized*arrow_size/4;
% % % verts = [arrow_head';bottom_vertices_outer';bottom_vertices_inner'];
% % % faces = [1 2 3]; % this tells the number of vertices in the patch object. In our case, we have only three vertices and all of them are among the patch vertices.
% % % patch_handle = patch('Faces',faces,'Vertices',verts,'FaceColor','g','EdgeColor','r');
% % % feedback_plot_handle = [feedback_plot_handle, patch_handle]; %#ok<AGROW>
% % %
% % % % in the following, we write the number of the node on the
% % % % corresponding parallel lines.
% % % text_dist = shiftet_dist/2;
% % % text_position = bottom_vertices_outer + text_dist*perpendicular_vector_normalized;
% % % text_position(1) = text_position(1) - 0.45; % for some reason MATLAB shifts the starting point of the text a little bit to the right. So, here we return it back.
% % % if feedback_text_handle (edge_num) ~= 0 % which means some text has already been written for this edge
% % % current_text = get(feedback_text_handle (edge_num), 'String');
% % % set(feedback_text_handle (edge_num), 'String', [current_text, ', ', num2str(i)])
% % % else
% % % feedback_text_handle (edge_num) = text(text_position(1), text_position(2), num2str(i), 'fontsize',10,'color','r','EdgeColor','g');
% % % feedback_plot_handle = [feedback_plot_handle, feedback_text_handle (edge_num)]; %#ok<AGROW>
% % % end
% % % end
end
function nearest_node_ind = compute_nearest_node_ind(obj,current_node_ind)
% This function computes the nearest node to the "current_node_ind"
Expand Down
4 changes: 4 additions & 0 deletions All_controller_classes/Finite_time_LQG_class.m
View file
Expand Up @@ -83,6 +83,10 @@
% True state propagation
next_Xg_val = MotionModel_class.f_discrete(Xg.val,u,w);

<<<<<<< Updated upstream
=======

>>>>>>> Stashed changes
% generating observation noise
if ~exist('noise_mode','var')
Vg = ObservationModel_class.generate_observation_noise(next_Xg_val);
Expand Down
16 changes: 16 additions & 0 deletions All_controller_classes/SLQG_class.m
View file
Expand Up @@ -7,6 +7,7 @@
lnr_sys;

Big_lnr_sys;
Stationary_belief;
Stationary_Gaussian_Hb;
end

Expand Down Expand Up @@ -58,6 +59,21 @@
stationGHb_val = obj.Stationary_Gaussian_Hb;
end
end
function stationary_belief_val = get.Stationary_belief(obj)
error('This function has not been changed from periodic to stationary yet')
% the property "periodic_belief" is computed only once, the
% first time it is needed.
if isempty(obj.periodic_belief)
periodic_belief_val = belief.empty;
periodic_pest = obj.estimator.periodicCov;
for k = 1 : obj.T
periodic_belief_val(k) = belief( state(obj.lnr_pts(k).x) , periodic_pest(:,:,k) );
end
obj.periodic_belief = periodic_belief_val; % this line works because the class is a subclass of the "handle" class. otherwise, we had to output the "obj".
else
periodic_belief_val = obj.periodic_belief;
end
end
function [next_Hstate, reliable] = propagate_Hstate(obj,old_Hstate,noise_mode)
% propagates the Hstate using Stationary Kalman Filter and
% Stationary LQR.
Expand Down

0 comments on commit f801ef0

Please sign in to comment.