Skip to content

Commit

Permalink
Binning work for rrt_star_fn
Browse files Browse the repository at this point in the history
  • Loading branch information
@olzhas
olzhas committed Oct 18, 2013
1 parent ed8747d commit 98dbd6c
Show file tree
Hide file tree
Showing 3 changed files with 111 additions and 12 deletions.
10 changes: 2 additions & 8 deletions FNRedundantManipulator.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -506,14 +506,12 @@ function update_link_position(this, ind)

function cost = calc_cost_euclidian(this, from_ind, dest_node)
% square root is not taken
temp_delta = this.tree.position(:, :, from_ind) - dest_node;
cost = sum((sum(temp_delta.^2, 2)));
cost = norm(this.tree.position(:, :, from_ind) - dest_node, 'fro');
end

function cost = calc_cost_angle(this, from_ind, dest_node)
% square root is not taken
temp_delta = this.tree.angle(:, from_ind) - dest_node;
cost = sum((sum(temp_delta.^2, 2)));
cost = norm(this.tree.angle(:, from_ind) - dest_node, 'fro');
end


Expand Down Expand Up @@ -661,13 +659,9 @@ function forced_removal(this)
this.cost(reused_node_ind) = this.calc_cost_euclidian(parent_node_ind, this.tree.position(:, :, reused_node_ind));
this.cumcost(reused_node_ind) = this.cumcost(parent_node_ind) + this.cost(reused_node_ind);
end

end

methods(Static)
function dist = euclidian_distance(src_pos, dest_pos)
dist = norm(src_pos - dest_pos);
end
function hCirc = plot_circle(x, y, r)
t = 0:0.001:2*pi;
cir_x = r*cos(t) + x;
Expand Down
111 changes: 108 additions & 3 deletions FNSimple2D.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -435,9 +435,6 @@ function rewire(this, new_node_ind, neighbors, min_node_ind)
this.parent(neighbors(ind)) = new_node_ind;
this.children(new_node_ind) = this.children(new_node_ind) + 1;
this.num_rewired = this.num_rewired + 1;


kids = this.list(this.children == neighbors(ind));
end
end
end
Expand Down Expand Up @@ -485,6 +482,12 @@ function forced_removal(this)
this.tree(:, node_to_remove) = [intmax; intmax];
this.free_nodes(this.free_nodes_ind) = node_to_remove;
this.free_nodes_ind = this.free_nodes_ind + 1;

for ind=1:this.bin_ind(end,node_to_remove)
upd = setdiff(this.bin(this.bin_ind(ind, node_to_remove)).nodes(1:this.bin(this.bin_ind(ind, node_to_remove)).last), node_to_remove);
this.bin(this.bin_ind(ind, node_to_remove)).nodes(1:numel(upd)) = upd;
end
this.bin_ind(end,node_to_remove) = 0;
end

function reused_node_ind = reuse_node(this, nearest_node, new_node_position)
Expand All @@ -500,6 +503,108 @@ function forced_removal(this)
this.children(nearest_node) = this.children(nearest_node) + 1;
this.cost(reused_node_ind) = norm(this.tree(:, nearest_node) - new_node_position);
this.cumcost(reused_node_ind) = this.cumcost(nearest_node) + this.cost(reused_node_ind);

radius = this.delta_near;

x_comp = int32(new_node_position(1) / this.bin_size - 0.5);
y_comp = int32(new_node_position(2) / this.bin_size - 0.5);

cur_bin = x_comp + y_comp*this.bin_x + this.bin_offset;

this.bin(cur_bin).last = this.bin(cur_bin).last + 1;
this.bin(cur_bin).nodes(this.bin(cur_bin).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end, reused_node_ind), reused_node_ind) = cur_bin;

%% placing nodes in additional bins
x_left = x_comp;
x_right = x_comp;
y_top = y_comp;
y_bottom = y_comp;
if new_node_position(1) - radius >= this.XY_BOUNDARY(1)
x_left = int32((new_node_position(1) - radius)/this.bin_size - 0.5);
end
if new_node_position(1) + radius <= this.XY_BOUNDARY(2)
x_right = int32((new_node_position(1) + radius)/this.bin_size - 0.5);
end
if new_node_position(2) - radius >= this.XY_BOUNDARY(3)
y_top = int32((new_node_position(2) + radius)/this.bin_size - 0.5);
end
if new_node_position(2) + radius <= this.XY_BOUNDARY(4)
y_bottom = int32((new_node_position(2) - radius)/this.bin_size - 0.5);
end

if x_comp > x_left && cur_bin - 1 > 0
this.bin(cur_bin-1).last = this.bin(cur_bin-1).last + 1;
this.bin(cur_bin-1).nodes(this.bin(cur_bin-1).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end,reused_node_ind),reused_node_ind) = cur_bin-1;
end

if x_comp < x_right && cur_bin + 1 < this.nbins
this.bin(cur_bin+1).last = this.bin(cur_bin+1).last + 1;
this.bin(cur_bin+1).nodes(this.bin(cur_bin+1).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end, reused_node_ind), reused_node_ind) = cur_bin+1;
end

if y_comp < y_top
if cur_bin+this.bin_x <= this.nbins
this.bin(cur_bin+this.bin_x).last = this.bin(cur_bin+this.bin_x).last + 1;
this.bin(cur_bin+this.bin_x).nodes(this.bin(cur_bin+this.bin_x).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end,reused_node_ind),reused_node_ind) = cur_bin+this.bin_x;
if x_comp > x_left
this.bin(cur_bin-1+this.bin_x).last = this.bin(cur_bin-1+this.bin_x).last + 1;
this.bin(cur_bin-1+this.bin_x).nodes(this.bin(cur_bin-1+this.bin_x).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end,reused_node_ind),reused_node_ind) = cur_bin-1+this.bin_x;
end
if x_comp < x_right && cur_bin+this.bin_x+1 <= this.nbins
this.bin(cur_bin+1+this.bin_x).last = this.bin(cur_bin+1+this.bin_x).last + 1;
this.bin(cur_bin+1+this.bin_x).nodes(this.bin(cur_bin+1+this.bin_x).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end,reused_node_ind),reused_node_ind) = cur_bin+1+this.bin_x;
end
end
end

if y_comp > y_bottom
if cur_bin-this.bin_x > 0
this.bin(cur_bin-this.bin_x).last = this.bin(cur_bin-this.bin_x).last + 1;
this.bin(cur_bin-this.bin_x).nodes(this.bin(cur_bin-this.bin_x).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end,reused_node_ind),reused_node_ind) = cur_bin-this.bin_x;

if x_comp > x_left && cur_bin-1-this.bin_x > 0
this.bin(cur_bin-1-this.bin_x).last = this.bin(cur_bin-1-this.bin_x).last + 1;
this.bin(cur_bin-1-this.bin_x).nodes(this.bin(cur_bin-1-this.bin_x).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end,reused_node_ind),reused_node_ind) = cur_bin-1-this.bin_x;
end
if x_comp < x_right
this.bin(cur_bin+1-this.bin_x).last = this.bin(cur_bin+1-this.bin_x).last + 1;
this.bin(cur_bin+1-this.bin_x).nodes(this.bin(cur_bin+1-this.bin_x).last) = reused_node_ind;

this.bin_ind(end,reused_node_ind) = this.bin_ind(end, reused_node_ind) + 1;
this.bin_ind(this.bin_ind(end,reused_node_ind),reused_node_ind) = cur_bin+1-this.bin_x;
end
end
end

queue = zeros(1, int32(this.max_nodes/5));
for ind = 1:



end

%%%%%%%%%%%%%%%%%%%%%%%%%
Expand Down
2 changes: 1 addition & 1 deletion configure_FNSimple2D.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,4 @@
conf.delta_goal_point = 1; % Radius of goal point
conf.delta_near = 1.5; % Radius for neighboring nodes
conf.max_step = 0.5; % Maximum position change when we add a new node to the tree
conf.bin_size = 8;
conf.bin_size = 7;

0 comments on commit 98dbd6c

Please sign in to comment.