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.

Multidimensional Arrays: https://www.mathworks.com/help/matlab/math/multidimensional-arrays.html

Cell Arrays: https://www.mathworks.com/help/matlab/cell-arrays.html

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

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