Multi-dimensional Arrays and Cell Data Structure in Matlab
The Purpose of this live script is to show you examples of Matlab Multi-Dimensional Array and Cell data structure.
These examples are generated in the context of a decomposition and search motion planning.
Note that the decomposition given here is done manually.
Consider the partitioned space, road map and the corresponding dual planning graph in the example shown below.
Let's consider a coordiate system for the workspace and plot the workspace and the partitions.
Note: A better practice will be writing a function to plot the polygons.
We use this less professional way of programming to make this live script stand alone without a need for exernal functions.
clear all
close all
h1=figure();
number_of_Obst=2; %define the number of obstacles
O={}; %this tells matlab that O is a cell; O is used to store the obstacles
O(1)={[2 5; %vertex 1
3.5 6; %vertex 2
4.5 4.8 %vertex 3
2 5]}; %repeating vertex 1
O(2)={[8 4.5; %vertex 1
10 6; %vertex 2
12.5 4.3; %vertex 3
11 3.5; %vertex 4
8 4.5]}; %repeating vertex 1
xlim([0 14])
ylim([0 8])
hold on
%Now lets draw the polygon (to draw a polygon we draw its line segments
for j=1:number_of_Obst
m(j)=length(O{j}(:,:))-1; %getting the number of segments
for i=1:m(j)
plot(O{j}(i,1), O{j}(i,2),'o');
hold on
for t=0:0.01:1
x_segment=(1-t)*O{j}(i,1)+t*O{j}(i+1,1);
y_segment=(1-t)*O{j}(i,2)+t*O{j}(i+1,2);
plot(x_segment,y_segment,'.r');
end
end
end
box on
%Now let's draw the partitions. This is manually done (try doing using the trapezoidation algorithm
line([O{1}(1,1),O{1}(1,1)],[0 8],'LineWidth',2)
line([O{1}(2,1),O{1}(2,1)],[O{1}(2,2) 8],'LineWidth',2)
line([O{1}(3,1),O{1}(3,1)],[0 8],'LineWidth',2)
line([O{2}(1,1),O{2}(1,1)],[0 8],'LineWidth',2)
line([O{2}(2,1),O{2}(2,1)],[O{2}(2,2) 8],'LineWidth',2)
line([O{2}(4,1),O{2}(4,1)],[O{2}(4,2) 0],'LineWidth',2)
line([O{2}(3,1),O{2}(3,1)],[0 8],'LineWidth',2)
%Now let's draw the center points (manual)
%first let's define the center points as cell
C(1)={([0 4]+O{1}(1,:))/2};
C(2)={([O{1}(1,1) O{1}(1,2)+2]+O{1}(2,:))/2};
C(3)={([O{1}(1,1) O{1}(1,2)-2]+[O{1}(3,1) O{1}(3,2)-2])/2};
C(4)={([O{1}(2,1) O{1}(1,2)+2]+[O{1}(3,1) O{1}(3,2)+2])/2};
C(5)={([O{1}(3,1) O{1}(3,2)]+[O{2}(1,1) O{2}(1,2)])/2};
C(6)={([O{2}(1,1) O{2}(1,2)+3]+O{2}(2,:))/2};
C(7)={([O{2}(1,1) O{1}(1,2)-3]+[O{2}(4,1) O{2}(4,2)-3])/2};
C(8)={([O{2}(2,1) O{2}(1,2)+2]+[O{2}(3,1) O{2}(3,2)+2])/2};
C(9)={([O{2}(4,1) O{2}(4,2)-2.5]+[O{2}(3,1) O{2}(3,2)-2.5])/2};
C(10)={(O{2}(3,:)+[14 4])/2};
for i=1:10
plot(C{i}(1,1),C{i}(1,2),'rs')
end
%Now let's plot the boundary points (manual)
B(1,1,:)=[O{1}(1,1),6.5];
B(2,1,:)=[O{1}(1,1),2.5];
B(3,1,:)=[O{1}(2,1),7];
B(4,1,:)=[O{1}(3,1),7];
B(5,1,:)=[O{1}(3,1),3];
B(6,1,:)=[O{2}(1,1),6.5];
B(7,1,:)=[O{2}(1,1),2.5];
B(8,1,:)=[O{2}(2,1),7];
B(9,1,:)=[O{2}(4,1),2];
B(10,1,:)=[O{2}(3,1),6.5];
B(11,1,:)=[O{2}(3,1),2.5];
for i=1:11
plot(B(i,1,1),B(i,1,2),'rd')
end
%Lets now plot the roadmap (manual)
%partitions are numbersed from 1 to 10
line([C{1}(1,1) B(1,1,1)],[C{1}(1,2) B(1,1,2)])
line([C{1}(1,1) B(2,1,1)],[C{1}(1,2) B(2,1,2)])
line([C{2}(1,1) B(1,1,1)],[C{2}(1,2) B(1,1,2)])
line([C{3}(1,1) B(2,1,1)],[C{3}(1,2) B(2,1,2)])
line([C{2}(1,1) B(3,1,1)],[C{2}(1,2) B(3,1,2)])
line([C{3}(1,1) B(5,1,1)],[C{3}(1,2) B(5,1,2)])
line([C{4}(1,1) B(3,1,1)],[C{4}(1,2) B(3,1,2)])
line([C{4}(1,1) B(4,1,1)],[C{4}(1,2) B(4,1,2)])
line([C{5}(1,1) B(4,1,1)],[C{5}(1,2) B(4,1,2)])
line([C{5}(1,1) B(5,1,1)],[C{5}(1,2) B(5,1,2)])
line([C{5}(1,1) B(6,1,1)],[C{5}(1,2) B(6,1,2)])
line([C{5}(1,1) B(7,1,1)],[C{5}(1,2) B(7,1,2)])
line([C{6}(1,1) B(6,1,1)],[C{6}(1,2) B(6,1,2)])
line([C{6}(1,1) B(8,1,1)],[C{6}(1,2) B(8,1,2)])
line([C{7}(1,1) B(7,1,1)],[C{7}(1,2) B(7,1,2)])
line([C{7}(1,1) B(9,1,1)],[C{7}(1,2) B(9,1,2)])
line([C{8}(1,1) B(8,1,1)],[C{8}(1,2) B(8,1,2)])
line([C{8}(1,1) B(10,1,1)],[C{8}(1,2) B(10,1,2)])
line([C{9}(1,1) B(9,1,1)],[C{9}(1,2) B(9,1,2)])
line([C{9}(1,1) B(11,1,1)],[C{9}(1,2) B(11,1,2)])
line([C{10}(1,1) B(10,1,1)],[C{10}(1,2) B(10,1,2)])
line([C{10}(1,1) B(11,1,1)],[C{10}(1,2) B(11,1,2)])
%Now let's add the start and goal points
S=[1 1];
G=[13.3 7];
plot(S(1,1),S(1,2),'ro','MarkerSize',8)
plot(G(1,1),G(1,2),'ro','MarkerSize',8)
%Now let's connect the start and the goal to the roadmap
line([S(1,1), C{1}(1,1)],[S(1,2) C{1}(1,2)])
line([G(1,1), C{10}(1,1)],[G(1,2) C{10}(1,2)])
Some programming tricks, for copy content of one figure to another. we will use h3 later to plot the roadmap
h3=figure();
h1Chil = h1.Children;
copyobj(h1Chil, h3);
Now let's draw the dual graph of the decomposed free workspace.
We will define the graph using the adjacency table. Becasue Adjacency table at each row has variable size, we will use Matlab cell data structure; see https://www.mathworks.com/help/matlab/ref/cell.html for more information on Matlab Cell.
AdjTab={};% this tells matlab that this is a cell
AdjTab(1)={[2]};
AdjTab(2)={[1,3,4]};
AdjTab(3)={[2,5]};
AdjTab(4)={[2,6]};
AdjTab(5)={[3,6]};
AdjTab(6)={[5,4,7,8]};
AdjTab(7)={[6,9]};
AdjTab(8)={[6,10]};
AdjTab(9)={[7,11]};
AdjTab(10)={[8,11]};
AdjTab(11)={[9 10 12]};
AdjTab(12)={[11]};
%Now let's define the location of the nodes for visualization
for i=1:10
N(i+1,1,1)=C{i}(1,1);
N(i+1,1,2)=C{i}(1,2);
end
N(1,1,1)=S(1,1);
N(1,1,2)=S(1,2);
N(12,1,1)=G(1,1);
N(12,1,2)=G(1,2);
Now let's plot the dual graph. We will place the nodes at the center point of the partions.
h2=figure;
for i=1:12
plot(N(i,1,1),N(i,1,2),'bo','MarkerSize',20)
hold on
end
xlim([0 14])
ylim([0 8])
%Now let's connect the neighboring nodes to draw the edges
for i=1:12
text(N(i,1,1),N(i,1,2),num2str(i))%adding labels to the nodes
for k=AdjTab{i}(1,:)
line([N(i,1,1) N(k,1,1)],[N(i,1,2) N(k,1,2)])%ploting the edges
end
end
pathh=[3 5 6 7 9];
for i=1:length(pathh)-1
line([N(pathh(i),1,1) N(pathh(i+1),1,1)],[N(pathh(i),1,2) N(pathh(i+1),1,2)],'Color','m')
end
Now let's say your planner has given you a path from start to goal as a sequence of interconnected nodes on the path.
Path starts at node 1 (the start node) and ends at node 12 (the goal node).
we know that node 1 belong to cell 1 and node 12 belongs to cell 11.
Path=[1 2 4 6 8 10 11 12];
Now we want to plot this on the roadmap.
To simplify this plotting, we are going to use Matlab's higher dimensional array to encode whar center points are in two sides of a boundary point.
B_informed(2,3,1,1)=B(1,1,1); %this is useful for when the path is from 2 to 3
B_informed(2,3,1,2)=B(1,1,2);
B_informed(3,2,1,1)=B(1,1,1); %this is useful for when the path is from 3 to 2
B_informed(3,2,1,2)=B(1,1,2);
B_informed(2,4,1,1)=B(2,1,1);
B_informed(2,4,1,2)=B(2,1,2);
B_informed(4,2,1,1)=B(2,1,1);
B_informed(4,2,1,2)=B(2,1,2);
B_informed(3,5,1,1)=B(3,1,1);
B_informed(3,5,1,2)=B(3,1,2);
B_informed(5,3,1,1)=B(3,1,1);
B_informed(5,3,1,2)=B(3,1,2);
B_informed(5,6,1,1)=B(4,1,1);
B_informed(5,6,1,2)=B(4,1,2);
B_informed(6,5,1,1)=B(4,1,1);
B_informed(6,5,1,2)=B(4,1,2);
B_informed(4,6,1,1)=B(5,1,1);
B_informed(4,6,1,2)=B(5,1,2);
B_informed(6,4,1,1)=B(5,1,1);
B_informed(6,4,1,2)=B(5,1,2);
B_informed(6,7,1,1)=B(6,1,1);
B_informed(6,7,1,2)=B(6,1,2);
B_informed(7,8,1,1)=B(6,1,1);
B_informed(7,8,1,2)=B(6,1,2);
B_informed(6,8,1,1)=B(7,1,1);
B_informed(6,8,1,2)=B(7,1,2);
B_informed(8,6,1,1)=B(7,1,1);
B_informed(8,6,1,2)=B(7,1,2);
B_informed(7,9,1,1)=B(8,1,1);
B_informed(7,9,1,2)=B(8,1,2);
B_informed(9,7,1,1)=B(8,1,1);
B_informed(9,7,1,2)=B(8,1,2);
B_informed(8,10,1,1)=B(9,1,1);
B_informed(8,10,1,2)=B(9,1,2);
B_informed(10,8,1,1)=B(9,1,1);
B_informed(10,8,1,2)=B(9,1,2);
B_informed(9,11,1,1)=B(10,1,1);
B_informed(9,11,1,2)=B(10,1,2);
B_informed(11,9,1,1)=B(10,1,1);
B_informed(11,9,1,2)=B(10,1,2);
B_informed(10,11,1,1)=B(11,1,1);
B_informed(10,11,1,2)=B(11,1,2);
B_informed(11,10,1,1)=B(11,1,1);
B_informed(11,10,1,2)=B(11,1,2);
%you complete the rest
First we copy content of the first figure we plotted into a new figure so I can plot the path on top of the roadmap.
figure(h1)
hold on
Then we plot the path. Notice that how using an informed boundary point indexing helps with automating the path plot.
line([N(1,1,1),N(2,1,1)],[N(1,1,2),N(2,1,2)],'Color','m','LineWidth',2)% first node connects to the center of the cell 1
for i=2:length(Path)-2
k=Path(i);
j=Path(i+1);
line([N(k,1,1),B_informed(k,j,1,1)],[N(k,1,2),B_informed(k,j,1,2)],'Color','m','LineWidth',2)
line([B_informed(k,j,1,1),N(j,1,1)],[B_informed(k,j,1,2),N(j,1,2)],'Color','m','LineWidth',2)
end
k=Path(end-1);
line([N(k,1,1),N(12,1,1)],[N(k,1,2),N(12,1,2)],'Color','m','LineWidth',2)% from center of last cell to the goal node
@Solmaz Kia, UCI, April 24th, 2023