├── README.md
└── Shape assembly code
    ├── Fcn_CalEnteringCmd.m
    ├── Fcn_CalExplorationCmd.m
    ├── Fcn_CalInteractionCmd.m
    ├── Fcn_GetNeighborSet.m
    ├── Fcn_GetPerformMetric.m
    ├── Fcn_NegotOrientState.m
    ├── Fcn_NegotPositState.m
    ├── Lib_AnimatFunctions.m
    ├── Lib_DrawingFunctions.m
    ├── Lib_InitialFunctions.m
    ├── Lib_RecordFunctions.m
    ├── Lib_ShapeFunctions.m
    ├── Lib_UpdateFunctions.m
    ├── MainLoop.m
    └── ShapeImage
        ├── Image_geom_snow.mat
        ├── Image_geom_starfish.mat
        ├── Image_letter_B.mat
        ├── Image_letter_O.mat
        └── Image_letter_R.mat


/README.md:
--------------------------------------------------------------------------------
 1 | <div align="center">
 2 |   <h1>Mean-shift exploration in shape assembly of robot swarms</h1>
 3 | <p align="center">
 4 |   <a href="https://www.nature.com/articles/s41467-023-39251-5">
 5 |     <img src="https://img.shields.io/badge/Paper-blue?logo=googledocs&logoColor=white&labelColor=grey&color=blue"></a>
 6 |   <a href="https://www.bilibili.com/video/BV1Pk4y1H7A3/?spm_id_from=333.999.0.0&vd_source=288648f5b920459d12ebbcfd2da00a19">
 7 |     <img src="https://img.shields.io/badge/Video-blue?logo=bilibili&logoColor=white&labelColor=grey&color=blue"></a>
 8 |   <a href="https://www.youtube.com/watch?v=inoifg2tcJM&feature=youtu.be">
 9 |     <img src="https://img.shields.io/badge/Video-blue?logo=youtube&logoColor=white&labelColor=grey&color=blue"></a>
10 |   <a href="https://opensource.org/licenses/MIT">
11 |     <img src="https://img.shields.io/badge/License-MIT-yellow.svg"></a>
12 | </p>
13 | 
14 | ![image](https://github.com/WestlakeIntelligentRobotics/Shape-assembly-code/assets/125523389/257a4227-ac3e-4f8e-8f2a-49e666366dde)
15 | 
16 | </div>
17 | 
18 | This file is the source code of our paper "Mean-shift exploration in shape assembly of robot swarms". It is coded by Matlab (R2020a). 
19 | 
20 | 
21 | This file contains 16 m files and 1 folder.
22 | 
23 | - MainLoop.m is the main program, which is used to set simulation parameters, start/stop simulation and animation.
24 | - Fcn_NegotPositState.m and Fcn_NegotOrientState are used to negotiate position and orientation of the target formation.
25 | - Fcn_CalEnteringCmd, Fcn_CalExplorationCmd, and Fcn_CalInteractionCmd are used to calculate shape-entering, shape-exploring, and interaction velocity commands, respectively.
26 | - Fcn_GetNeighborSet is used to calculate neighboring set, and Fcn_GetPerformMetric is used to calculate performance metrics.
27 | - Lib_InitialFunctions, Lib_ShapeFunctions, Lib_UpdateFunctions, Lib_AnimatFunctions, Lib_DrawingFunctions, and Lib_RecordFunctions are used for simulation initialization, shape processing, robot motion update, animated simulation, curve drawing, and data record.
28 | - The folder "ShapeImage" contains 4 different shapes, including snowflake, starfish, letter "R", letter "O", and letter "B".
29 | 
30 | Note that simulation parameters may need to be readjusted depending on the swarm size and shape type.
31 | 
32 | Please send all bug reports to authors. 
33 | 
34 | The authors may be contacted via:
35 | 
36 | Shiyu Zhao
37 | Associate Professor
38 | Director of the Intelligent Unmanned Systems Laboratory
39 | School of Engineering, Westlake University
40 | Email: zhaoshiyu@westlake.edu.cn
41 | 
42 | and
43 | 
44 | Guibin Sun
45 | Postdoctoral Fellow
46 | School of Automation Science and Electrical Engineering
47 | Beihang University
48 | Email: sunguibinx@buaa.edu.cn
49 | 
50 | @Copyright: If using this procedure for publications, please cite the article 
51 | "Mean-shift selfless exploration in shape assembly of robot swarms". 
52 | 
53 | ## Citing
54 | 
55 | If you find our work useful, please consider citing:
56 | 
57 | ```BibTeX
58 | Sun, G., Zhou, R., Ma, Z. et al. Mean-shift exploration in shape assembly of robot swarms. Nat Commun 14, 3476 (2023). https://doi.org/10.1038/s41467-023-39251-5
59 | ```
60 | 
61 | 
62 | 
63 | 


--------------------------------------------------------------------------------
/Shape assembly code/Fcn_CalEnteringCmd.m:
--------------------------------------------------------------------------------
  1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | % FUNCTION: calculate shape-entering velocity command
  3 | function [cmd_set,grid_set]=Fcn_CalEnteringCmd(sim_param,shape_mtr,shape_info,robot_state,shape_state)
  4 |     % set proportional gain
  5 |     kappa=10.0;
  6 |     % robot-variable substitution
  7 |     t=sim_param.t;
  8 |     vel_max=sim_param.vel_max;
  9 |     swarm_size=sim_param.swarm_size;
 10 |     pos_set=robot_state.pos_set;
 11 |     % shape-variable substitution
 12 |     rn=shape_info.rn; 
 13 |     cn=shape_info.cn; 
 14 |     grid=shape_info.grid; 
 15 |     cen_x=shape_info.cen_x; 
 16 |     cen_y=shape_info.cen_y; 
 17 |     shape_x=shape_mtr.shape_x; 
 18 |     shape_y=shape_mtr.shape_y; 
 19 |     shape_value=shape_mtr.shape_value; 
 20 |     fpos_set=shape_state.pos_set;
 21 |     fvel_set=shape_state.vel_set;
 22 |     fhead_set=shape_state.head_set; 
 23 |     % calculate local target position
 24 |     grid_set=zeros(size(pos_set));
 25 |     gpos_set=zeros(size(pos_set));
 26 |     gray_set=zeros(1,swarm_size);
 27 |     for index=1:1:swarm_size
 28 |         % locate robot in the local coordinate system
 29 |         location=TransGoalToLocal(pos_set(:,index),fpos_set(:,index),fhead_set(:,index),grid,cen_x,cen_y,rn,cn);
 30 |         % get gray color of the above location
 31 |         gray_color=GetGrayValue(location,shape_value); 
 32 |         % calculate the local target point
 33 |         if gray_color==1 % robot is not in the gray grid
 34 |             goal_local=GetLocalTargetOut(location,pos_set(:,index),shape_x(:,:,index),shape_y(:,:,index),shape_value,rn);   
 35 |         else             % robot is in the gray grid
 36 |             delta=vel_max*t;
 37 |             range=ceil(delta/grid)+3;
 38 |             goal_local=GetLocalTargetIn(location,pos_set(:,index),shape_x(:,:,index),shape_y(:,:,index),shape_value,rn,cn,range);  
 39 |         end
 40 |         grid_set(:,index)=[location(1),location(2)]';
 41 |         gpos_set(:,index)=goal_local;
 42 |         gray_set(:,index)=gray_color;
 43 |     end
 44 |     % calculate shape-entering command
 45 |     dist=sqrt((gpos_set(1,:)-pos_set(1,:)).^2+(gpos_set(2,:)-pos_set(2,:)).^2); 
 46 |     dist(dist<=0)=1;    
 47 |     cmd_set=kappa.*(gpos_set-pos_set).*(gray_set./dist)+fvel_set;
 48 | end
 49 | 
 50 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 51 | % FUNCTION: get local target location when robot is in the gray grid
 52 | function goal_set=GetLocalTargetIn(location,pos_set,shape_x,shape_y,shape_value,rn,cn,range)
 53 |     % get local matrix for searching
 54 |     min_r=max(1,location(1)-range); 
 55 |     max_r=min(rn,location(1)+range);
 56 |     min_c=max(1,location(2)-range); 
 57 |     max_c=min(cn,location(2)+range);
 58 |     local_mtr=shape_value(min_r:max_r,min_c:max_c);    
 59 |     % find the cell whose gray value is less than current gray value
 60 |     cen_r=location(1)-min_r+1; 
 61 |     cen_c=location(2)-min_c+1; 
 62 |     valid_mtr=zeros(size(local_mtr));
 63 |     gray_value=local_mtr(cen_r,cen_c);
 64 |     valid_mtr(local_mtr<gray_value)=1;  
 65 |     temp_x=shape_x(min_r:max_r,min_c:max_c)-pos_set(1);
 66 |     temp_y=shape_y(min_r:max_r,min_c:max_c)-pos_set(2);
 67 |     dist_mtr=sqrt(temp_x.^2+temp_y.^2).*valid_mtr;    
 68 |     dist_mtr=dist_mtr.*valid_mtr;
 69 |     dist_mtr(dist_mtr==0)=Inf;
 70 |     [~,index]=min(dist_mtr,[],'all','linear');
 71 |     [r,~]=size(dist_mtr);
 72 |     c_temp=fix(index/r)+1; 
 73 |     r_temp=rem(index,r); 
 74 |     if r_temp==0 
 75 |         r_temp=r;
 76 |     end
 77 |     r_index=(r_temp-cen_r)+location(1);
 78 |     c_index=(c_temp-cen_c)+location(2);
 79 |     goal_x=shape_x(r_index,c_index);
 80 |     goal_y=shape_y(r_index,c_index);
 81 |     goal_set=[goal_x,goal_y]';
 82 | end
 83 | 
 84 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 85 | % FUNCTION: get local target location when robot is not in the gray grid
 86 | function goal_set=GetLocalTargetOut(location,pos_set,shape_x,shape_y,shape_value,rn)
 87 |     valid_mtr=shape_value; 
 88 |     valid_mtr(valid_mtr<1)=0;
 89 |     temp_x=shape_x-pos_set(1); 
 90 |     temp_y=shape_y-pos_set(2); 
 91 |     dist_mtr=sqrt(temp_x.^2+temp_y.^2).*(~valid_mtr);
 92 |     dist_mtr(dist_mtr==0)=Inf; 
 93 |     if location(3)
 94 |         dist_mtr(location(1),location(2))=Inf;
 95 |     end
 96 |     [~,index]=min(dist_mtr,[],'all','linear');
 97 |     c_index=fix(index/rn)+1; 
 98 |     r_index=rem(index,rn);
 99 |     if r_index==0
100 |         r_index=rn;
101 |     end
102 |     goal_x=shape_x(r_index,c_index);
103 |     goal_y=shape_y(r_index,c_index);
104 |     goal_set=[goal_x,goal_y]';
105 | end
106 | 
107 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
108 | % FUNCTION: get gray color of the given location
109 | function value=GetGrayValue(location,shape_value)
110 |     if location(3)==0
111 |         value=1;
112 |     else
113 |         value=shape_value(location(1),location(2));
114 |     end
115 | end
116 | 
117 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
118 | % FUNCTION: transform global position to local locations
119 | function location=TransGoalToLocal(pos_set,fpos_set,fhead_set,grid,cen_x,cen_y,rn,cn)
120 |     % calculate the azimuth angle of robot to the formation center
121 |     head_set=atan2(pos_set(2,:)-fpos_set(2),pos_set(1,:)-fpos_set(1));
122 |     azim_set=head_set-fhead_set; 
123 |     azim_set=LimitAngle(azim_set);
124 |     % calculate the distance between robot and formation center
125 |     x_temp=(pos_set(1,:)-fpos_set(1)).^2; 
126 |     y_temp=(pos_set(2,:)-fpos_set(2)).^2;    
127 |     dist=sqrt(x_temp+y_temp);
128 |     % get relative position of robot to the formation center
129 |     x_coor=round(dist.*cos(azim_set)./grid);
130 |     y_coor=round(dist.*sin(azim_set)./grid);
131 |     % calculate the position of robot in the formation matrix
132 |     x_grid=x_coor+cen_x;
133 |     y_grid=rn-(y_coor+cen_y)+1;
134 |     % determine whether the robot is in the formation matrix
135 |     inside=zeros(1,length(x_grid));
136 |     index_x=find(x_grid>=1&x_grid<=cn);
137 |     index_y=find(y_grid>=1&y_grid<=rn);
138 |     inside(intersect(index_x,index_y))=1;
139 |     location=[y_grid;x_grid;inside];    
140 | end
141 | 
142 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
143 | % FUNCTION: limit angle
144 | function angle_s=LimitAngle(angle)
145 |     angle(angle>pi)=angle(angle>pi)-2*pi;
146 |     angle(angle<-pi)=angle(angle<-pi)+2*pi;
147 |     angle_s=angle;
148 | end
149 | 


--------------------------------------------------------------------------------
/Shape assembly code/Fcn_CalExplorationCmd.m:
--------------------------------------------------------------------------------
  1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | % FUNCTION: calculate shape-exploration velocity command
  3 | function cmd_set=Fcn_CalExplorationCmd(sim_param,shape_mtr,shape_info,grid_set,neigh_mtr,robot_state,shape_state)
  4 |     % set proportional gain
  5 |     kappa_1=5.0; 
  6 |     kappa_2=15.0; 
  7 |     % robot variable substitution
  8 |     a_mtr=neigh_mtr.a_mtr;
  9 |     swarm_size=sim_param.swarm_size;
 10 |     r_body=sim_param.r_body; 
 11 |     r_avoid=sim_param.r_avoid; 
 12 |     r_sense=sim_param.r_sense;    
 13 |     pos_set=robot_state.pos_set;
 14 |     % shape-variable substitution
 15 |     rn=shape_info.rn; 
 16 |     cn=shape_info.cn; 
 17 |     cen_x=shape_info.cen_x; 
 18 |     cen_y=shape_info.cen_y; 
 19 |     grid=shape_info.grid; 
 20 |     shape_x=shape_mtr.shape_x; 
 21 |     shape_y=shape_mtr.shape_y; 
 22 |     shape_value=shape_mtr.shape_value;  
 23 |     fpos_set=shape_state.pos_set;
 24 |     fhead_set=shape_state.head_set; 
 25 |     % calculate mean point  
 26 |     gfpos_set=zeros(size(pos_set));
 27 |     gepos_set=zeros(size(pos_set));
 28 |     robot_range=max(1,floor(max(r_avoid/2,r_body*2)/grid));
 29 |     neigh_range=ceil(r_sense/grid);
 30 |     for index=1:1:swarm_size
 31 |         % get mean point for filling action when there are both black and non-black cells around robot
 32 |         gfpos_set(:,index)=GetMeanPointFill(grid_set(:,index),pos_set(:,index),shape_x(:,:,index),shape_y(:,:,index),...
 33 |             shape_value,rn,cn,grid,neigh_range);
 34 |         % get mean point for exploring action when all the cells around robot are black 
 35 |         gepos_set(:,index)=GetMeanPointExpl(index,grid_set(:,index),pos_set,shape_x(:,:,index),shape_y(:,:,index),...
 36 |             shape_value,rn,cn,cen_x,cen_y,grid,neigh_range,robot_range,a_mtr(index,:),fpos_set(:,index),fhead_set(:,index));
 37 |     end
 38 |     % calculate attractive force
 39 |     cmd_set=kappa_1.*(gfpos_set-pos_set)+kappa_2.*(gepos_set-pos_set);
 40 | end
 41 | 
 42 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 43 | % FUNCTION: get mean point for exploring action
 44 | function goal_set=GetMeanPointExpl(index,grid_set,pos_set,shape_x,shape_y,shape_value,rn,cn,cen_x,cen_y,grid,neigh_range,robot_range,a_mtr,fpos_set,fhead_set)
 45 |     % get neighboring position
 46 |     pos_curr=pos_set(:,index);
 47 |     pos_neigh=pos_set(:,a_mtr==1);
 48 |     % initialize valid matrix
 49 |     valid_mtr=shape_value; 
 50 |     valid_mtr(valid_mtr>0)=1;
 51 |     % determine the occupied cells by neigbors
 52 |     neigh_num=sum(a_mtr,2);
 53 |     for i=1:1:neigh_num
 54 |         % locate agent in the local coordinate system
 55 |         location=TransGoalToLocal(pos_neigh(:,i),fpos_set,fhead_set,grid,cen_x,cen_y,rn,cn);
 56 |         min_r=max(1,location(1)-robot_range); 
 57 |         max_r=min(rn,location(1)+robot_range);
 58 |         min_c=max(1,location(2)-robot_range); 
 59 |         max_c=min(cn,location(2)+robot_range);
 60 |         valid_mtr(min_r:max_r,min_c:max_c)=ones(max_r-min_r+1,max_c-min_c+1);
 61 |     end
 62 |     % get local matrix for searching
 63 |     min_r=max(1,grid_set(1)-neigh_range); 
 64 |     max_r=min(rn,grid_set(1)+neigh_range);
 65 |     min_c=max(1,grid_set(2)-neigh_range); 
 66 |     max_c=min(cn,grid_set(2)+neigh_range);
 67 |     local_mtr=valid_mtr(min_r:max_r,min_c:max_c);  
 68 |     % get local matrix for searching
 69 |     [r_temp,c_temp]=find(local_mtr==0);
 70 |     r_index=r_temp+min_r-1;
 71 |     c_index=c_temp+min_c-1;
 72 |     gx_set=shape_x(sub2ind(size(shape_x),r_index',c_index'));
 73 |     gy_set=shape_y(sub2ind(size(shape_y),r_index',c_index'));    
 74 |     temp_x=gx_set-pos_curr(1);
 75 |     temp_y=gy_set-pos_curr(2);    
 76 |     dist_set=sqrt(temp_x.^2+temp_y.^2);
 77 |     weight=CalWeightFunction(dist_set,grid*neigh_range,0);
 78 |     temp=sum(weight,2);
 79 |     if temp==0
 80 |         goal_set=pos_curr;
 81 |         return
 82 |     end
 83 |     goal_x=sum(gx_set.*weight,2)/temp;
 84 |     goal_y=sum(gy_set.*weight,2)/temp;
 85 |     goal_set=[goal_x,goal_y]';
 86 |     if size(goal_set)==0
 87 |         goal_set=pos_set(:,index);
 88 |     end
 89 | end
 90 | 
 91 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 92 | % FUNCTION: get mean point for filling action
 93 | function goal_set=GetMeanPointFill(grid_set,pos_set,shape_x,shape_y,shape_value,rn,cn,grid,neigh_range)
 94 |     % get local matrix for searching
 95 |     min_r=max(1,grid_set(1)-neigh_range); 
 96 |     max_r=min(rn,grid_set(1)+neigh_range);
 97 |     min_c=max(1,grid_set(2)-neigh_range); 
 98 |     max_c=min(cn,grid_set(2)+neigh_range);
 99 |     local_mtr=shape_value(min_r:max_r,min_c:max_c); 
100 |     local_mtr(local_mtr>0)=1; 
101 |     % calculate local grid in local matrix
102 |     cen_r=grid_set(1)-min_r+1; 
103 |     cen_c=grid_set(2)-min_c+1; 
104 |     % get local matrix for searching
105 |     [r_temp,c_temp]=find(local_mtr==0);
106 |     r_index=r_temp-cen_r+grid_set(1);
107 |     c_index=c_temp-cen_c+grid_set(2);
108 |     gx_set=shape_x(sub2ind(size(shape_x),r_index',c_index'));
109 |     gy_set=shape_y(sub2ind(size(shape_y),r_index',c_index'));
110 |     temp_x=gx_set-pos_set(1);
111 |     temp_y=gy_set-pos_set(2);    
112 |     dist_set=sqrt(temp_x.^2+temp_y.^2);
113 |     weight=CalWeightFunction(dist_set,grid*neigh_range,0);
114 |     temp=sum(weight,2);
115 |     if temp==0
116 |         goal_set=pos_set;
117 |         return
118 |     end
119 |     goal_x=sum(gx_set.*weight,2)/temp;
120 |     goal_y=sum(gy_set.*weight,2)/temp;
121 |     goal_set=[goal_x,goal_y]';
122 |     if size(goal_set)==0
123 |         goal_set=pos_set;
124 |     end
125 | end
126 | 
127 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
128 | % FUNCTION: weight function 
129 | function y=CalWeightFunction(x,r,s)
130 |     y=(1+cos(pi.*(x-2*s)./(r-2*s)))/2;
131 |     y(x<2*s)=1;
132 |     y(x>r)=0;
133 | end
134 | 
135 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
136 | % FUNCTION: transform global position to local locations
137 | function location=TransGoalToLocal(pos_set,fpos_set,fhead_set,grid,cen_x,cen_y,rn,cn)
138 |     % calculate the azimuth angle of robot to the formation center
139 |     head_set=atan2(pos_set(2,:)-fpos_set(2),pos_set(1,:)-fpos_set(1));
140 |     azim_set=head_set-fhead_set; 
141 |     azim_set=LimitAngle(azim_set);
142 |     % calculate the distance between robot and formation center
143 |     x_temp=(pos_set(1,:)-fpos_set(1)).^2; 
144 |     y_temp=(pos_set(2,:)-fpos_set(2)).^2;    
145 |     dist=sqrt(x_temp+y_temp);
146 |     % get relative position of robot to the formation center
147 |     x_coor=round(dist.*cos(azim_set)./grid);
148 |     y_coor=round(dist.*sin(azim_set)./grid);
149 |     % calculate the position of robot in the formation matrix
150 |     x_grid=x_coor+cen_x;
151 |     y_grid=rn-(y_coor+cen_y)+1;
152 |     % determine whether the robot is in the formation matrix
153 |     inside=zeros(1,length(x_grid));
154 |     index_x=find(x_grid>=1&x_grid<=cn);
155 |     index_y=find(y_grid>=1&y_grid<=rn);
156 |     inside(intersect(index_x,index_y))=1;
157 |     location=[y_grid;x_grid;inside];    
158 | end
159 | 
160 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
161 | % FUNCTION: limit angle
162 | function angle_s=LimitAngle(angle)
163 |     angle(angle>pi)=angle(angle>pi)-2*pi;
164 |     angle(angle<-pi)=angle(angle<-pi)+2*pi;
165 |     angle_s=angle;
166 | end
167 | 


--------------------------------------------------------------------------------
/Shape assembly code/Fcn_CalInteractionCmd.m:
--------------------------------------------------------------------------------
 1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | % FUNCTION: calculate interaction velocity command
 3 | function cmd_set=Fcn_CalInteractionCmd(sim_param,neigh_mtr,robot_state)
 4 |     % set proportional gain
 5 |     kappa_1=25;
 6 |     kappa_2=1;
 7 |     % calculate avoidance command
 8 |     cmd_avoid=CalAvoidanceCmd(sim_param,neigh_mtr);
 9 |     % calculate consensus command
10 |     cmd_conse=CalConsensusCmd(sim_param,neigh_mtr,robot_state);
11 |     % calculate interaction command
12 |     cmd_set=kappa_1.*cmd_avoid-kappa_2.*cmd_conse;
13 | end
14 | 
15 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
16 | % FUNCTION: calculate avoidance command
17 | function cmd_set=CalAvoidanceCmd(sim_param,neigh_mtr)
18 |     % variable substitution
19 |     r_avoid=sim_param.r_avoid; 
20 |     a_mtr=neigh_mtr.a_mtr; 
21 |     d_mtr=neigh_mtr.d_mtr; 
22 |     rx_mtr=neigh_mtr.rx_mtr; 
23 |     ry_mtr=neigh_mtr.ry_mtr; 
24 |     % calculate command
25 |     dis_mtr=d_mtr+(~a_mtr).*r_avoid;
26 |     temp=r_avoid-d_mtr; 
27 |     temp(temp<=0)=0;
28 |     spr_mtr=temp./dis_mtr;
29 |     unit_x=-rx_mtr./dis_mtr;
30 |     unit_y=-ry_mtr./dis_mtr;
31 |     cmd_x=sum(spr_mtr.*unit_x,2);
32 |     cmd_y=sum(spr_mtr.*unit_y,2);    
33 |     cmd_set=[cmd_x';cmd_y']; 
34 | end
35 | 
36 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
37 | % FUNCTION: calculate consensus command
38 | function cmd_set=CalConsensusCmd(sim_param,neigh_mtr,robot_state)
39 |     % variable substitution
40 |     a_mtr=neigh_mtr.a_mtr;     
41 |     vel_set=robot_state.vel_set;
42 |     swarm_size=sim_param.swarm_size;
43 |     % calculate the relative velocity matrix
44 |     vx_mtr=(vel_set(1,:)'*ones(1,swarm_size));
45 |     vy_mtr=(vel_set(2,:)'*ones(1,swarm_size));
46 |     vx_rel=(vx_mtr-vx_mtr').*a_mtr;
47 |     vy_rel=(vy_mtr-vy_mtr').*a_mtr;
48 |     % calculate control force
49 |     cmd_x=sum(vx_rel,2);
50 |     cmd_y=sum(vy_rel,2);
51 |     neigh_num=sum(a_mtr,2); 
52 |     neigh_num(neigh_num==0)=1;    
53 |     cmd_set=[cmd_x';cmd_y']./neigh_num';
54 | end


--------------------------------------------------------------------------------
/Shape assembly code/Fcn_GetNeighborSet.m:
--------------------------------------------------------------------------------
 1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | % FUNCTION: get neighboring set 
 3 | function output=Fcn_GetNeighborSet(sim_param,robot_state)
 4 |     % variable substitution
 5 |     r_sense=sim_param.r_sense; 
 6 |     swarm_size=sim_param.swarm_size;
 7 |     pos_set=robot_state.pos_set;
 8 |     % calculate distance matrix
 9 |     x_mtr=(pos_set(1,:)'*ones(1,swarm_size));
10 |     y_mtr=(pos_set(2,:)'*ones(1,swarm_size));
11 |     x_rel=x_mtr'-x_mtr;
12 |     y_rel=y_mtr'-y_mtr;
13 |     d_mtr=sqrt((x_rel).^2+(y_rel).^2);
14 |     % calculate adjacent matrix
15 |     a_temp=d_mtr<=(ones(swarm_size,swarm_size).*r_sense);
16 |     a_mtr=a_temp-diag(ones(1,swarm_size));
17 |     % output  substitution    
18 |     output.a_mtr=a_mtr;
19 |     output.d_mtr=d_mtr;
20 |     output.rx_mtr=x_rel.*a_mtr;
21 |     output.ry_mtr=y_rel.*a_mtr;
22 | end


--------------------------------------------------------------------------------
/Shape assembly code/Fcn_GetPerformMetric.m:
--------------------------------------------------------------------------------
  1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | % FUNCTION: get performance metric
  3 | function metric=Fcn_GetPerformMetric(sim_param,neigh_mtr,robot_state,rcent_state,gray_mtr,gray_info)
  4 |     % variable substitution
  5 |     rn=gray_info.rn; 
  6 |     cn=gray_info.cn; 
  7 |     cen_x=gray_info.cen_x;
  8 |     cen_y=gray_info.cen_y;
  9 |     grid_size=gray_info.grid; 
 10 |     gray_scale=gray_info.gray_scale;
 11 |     r_sense=sim_param.r_sense;
 12 |     swarm_size=sim_param.swarm_size;
 13 |     a_mtr=neigh_mtr.a_mtr;
 14 |     d_mtr=neigh_mtr.d_mtr;
 15 |     shape_value=gray_mtr(:,:,3);
 16 |     pos_set=robot_state.pos_set;
 17 |     vel_set=robot_state.vel_set;
 18 |     fpos_set=rcent_state.pos_set;
 19 |     fhead_set=rcent_state.head_set;
 20 |     % calculate the number of cells that have been covered
 21 |     num_ent=0;
 22 |     shape_value(shape_value>0)=1;
 23 |     cell_num=length(find(shape_value==0));
 24 |     robot_range=floor(sim_param.r_avoid/grid_size);
 25 |     for i=1:1:swarm_size
 26 |         % locate robot in the local coordinate system
 27 |         location=TransGoalToLocal(pos_set(:,i),fpos_set,fhead_set,grid_size,cen_x,cen_y,rn,cn);
 28 |         % get the distribution of robots in shape
 29 |         if location(3)            
 30 |             min_r=max(1,location(1)-robot_range); 
 31 |             max_r=min(rn,location(1)+robot_range);
 32 |             min_c=max(1,location(2)-robot_range); 
 33 |             max_c=min(cn,location(2)+robot_range);
 34 |             shape_value(min_r:max_r,min_c:max_c)=ones(max_r-min_r+1,max_c-min_c+1);                
 35 |         end 
 36 |         % get gray level of this location
 37 |         shape_value_temp=gray_mtr(:,:,3);
 38 |         gray_value=GetGrayValue(location,shape_value_temp); 
 39 |         % update the number of the entered robots
 40 |         if abs(gray_value)<=1.0/gray_scale
 41 |             num_ent=num_ent+1;
 42 |         else
 43 |             num_ent=num_ent+0;
 44 |         end
 45 |     end
 46 |      % calculate coverage rate M1
 47 |     void_num=length(find(shape_value==0));    
 48 |     metric.cover_rate=(cell_num-void_num)/cell_num;
 49 |     % calculate entering rate M2
 50 |     metric.ent_rate=num_ent./swarm_size;
 51 |     % calculate distance variance M3
 52 |     ds_mtr=d_mtr.*a_mtr+r_sense.*(~a_mtr);
 53 |     min_set=min(ds_mtr');
 54 |     mean=sum(min_set,2)./swarm_size;
 55 |     derr_set=(min_set-mean).^2;
 56 |     metric.dist_var=sum(derr_set,2);
 57 |     % calculate mean velocity M4
 58 |     temp=sqrt(vel_set(1,:).^2+vel_set(2,:).^2); 
 59 |     vel_norm=sum(temp,2); 
 60 |     vel_norm(vel_norm==0)=1;
 61 |     temp=sum(vel_set,2); 
 62 |     vel_sum=sqrt(temp'*temp);    
 63 |     metric.mean_vel=vel_sum./vel_norm;
 64 | end
 65 | 
 66 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 67 | % FUNCTION: get gray color of the given location
 68 | function value=GetGrayValue(location,shape_value)
 69 |     if location(3)==0
 70 |         value=1;
 71 |     else
 72 |         value=shape_value(location(1),location(2));
 73 |     end
 74 | end
 75 | 
 76 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 77 | % FUNCTION: transform global position to local locations
 78 | function location=TransGoalToLocal(pos_set,fpos_set,fhead_set,grid,cen_x,cen_y,rn,cn)
 79 |     % calculate the azimuth angle of robot to the formation center
 80 |     head_set=atan2(pos_set(2,:)-fpos_set(2),pos_set(1,:)-fpos_set(1));
 81 |     azim_set=head_set-fhead_set; 
 82 |     azim_set=LimitAngle(azim_set);
 83 |     % calculate the distance between robot and formation center
 84 |     x_temp=(pos_set(1,:)-fpos_set(1)).^2; 
 85 |     y_temp=(pos_set(2,:)-fpos_set(2)).^2;    
 86 |     dist=sqrt(x_temp+y_temp);
 87 |     % get relative position of robot to the formation center
 88 |     x_coor=round(dist.*cos(azim_set)./grid);
 89 |     y_coor=round(dist.*sin(azim_set)./grid);
 90 |     % calculate the position of robot in the formation matrix
 91 |     x_grid=x_coor+cen_x;
 92 |     y_grid=rn-(y_coor+cen_y)+1;
 93 |     % determine whether the robot is in the formation matrix
 94 |     inside=zeros(1,length(x_grid));
 95 |     index_x=find(x_grid>=1&x_grid<=cn);
 96 |     index_y=find(y_grid>=1&y_grid<=rn);
 97 |     inside(intersect(index_x,index_y))=1;
 98 |     location=[y_grid;x_grid;inside];    
 99 | end
100 | 
101 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
102 | % FUNCTION: limit angle
103 | function angle_s=LimitAngle(angle)
104 |     angle(angle>pi)=angle(angle>pi)-2*pi;
105 |     angle(angle<-pi)=angle(angle<-pi)+2*pi;
106 |     angle_s=angle;
107 | end


--------------------------------------------------------------------------------
/Shape assembly code/Fcn_NegotOrientState.m:
--------------------------------------------------------------------------------
 1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | % FUNCTION: negotiate formation orientation
 3 | function state=Fcn_NegotOrientState(sim_param,neigh_mtr,shape_state,refer_state,inform_index)
 4 |     % set proportional gain
 5 |     kappa_conse=1.6;
 6 |     kappa_track=3.0;
 7 |     alpha=0.8;
 8 |     % variable substitution
 9 |     t=sim_param.t;
10 |     a_mtr=neigh_mtr.a_mtr;
11 |     head_set=shape_state.head_set;
12 |     hvel_set=shape_state.hvel_set;
13 |     ref_head=refer_state.head_set;
14 |     ref_hvel=refer_state.hvel_set;
15 |     swarm_size=sim_param.swarm_size;
16 |     % calculate consensus command for all the robots
17 |     err_set=GetConsensusError(swarm_size,a_mtr,head_set);
18 |     [sign_set,pabs_set]=SepSymbAndValue(err_set);
19 |     cmd_cons=kappa_conse.*sign_set.*(pabs_set.^alpha);
20 |     % calculate reference-tracking command for informed robots
21 |     cmd_track=(kappa_track.*(ref_head-head_set)+ref_hvel);
22 |     % calculate alignment command for uninformed robots
23 |     cmd_align=GetAlignConsensus(swarm_size,a_mtr,hvel_set);
24 |     % get valid set of informed robots
25 |     inform_set=zeros(1,swarm_size);
26 |     inform_set(inform_index)=1;
27 |     % calculate velocity command    
28 |     hvel_max=pi/2;
29 |     cmd_set=-cmd_cons+inform_set.*cmd_track+(~inform_set).*cmd_align;
30 |     cmd_set(cmd_set>hvel_max)=hvel_max; 
31 |     cmd_set(cmd_set<-hvel_max)=-hvel_max;
32 |     % update negotiation movements
33 |     state=shape_state;
34 |     state.head_set=state.head_set+state.hvel_set.*t;
35 |     state.hvel_set=cmd_set;    
36 | end
37 | 
38 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
39 | % FUNCTION: calculate alignment consensus
40 | function cons_set=GetAlignConsensus(swarm_size,a_mtr,state_set)
41 |     h_mtr=kron(state_set,ones(swarm_size,1));
42 |     h_set=sum(h_mtr.*a_mtr,2);
43 |     neigh_num=sum(a_mtr,2)';
44 |     neigh_num(neigh_num==0)=1;
45 |     cons_set=h_set'./neigh_num;
46 | end
47 | 
48 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
49 | % FUNCTION: calculate consensus errors
50 | function [sign_set,abs_set]=SepSymbAndValue(value_set)
51 |     sign_set=sign(value_set);
52 |     abs_set=abs(value_set);
53 | end
54 | 
55 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
56 | % FUNCTION: calculate consensus errors
57 | function err_set=GetConsensusError(swarm_size,a_mtr,state_set)
58 |     h_mtr=kron(state_set,ones(swarm_size,1));
59 |     h_rel=(h_mtr'-h_mtr).*a_mtr;
60 |     neigh_num=sum(a_mtr,2)';
61 |     neigh_num(neigh_num==0)=1;
62 |     err_set=sum(h_rel,2)'./neigh_num;
63 | end


--------------------------------------------------------------------------------
/Shape assembly code/Fcn_NegotPositState.m:
--------------------------------------------------------------------------------
 1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | % FUNCTION: negotiate formation position
 3 | function state=Fcn_NegotPositState(sim_param,neigh_mtr,shape_state,refer_state,inform_index)
 4 |     % set proportional gain
 5 |     kappa_conse=1.6;
 6 |     kappa_track=3.0;
 7 |     alpha=0.8;
 8 |     % variable substitution
 9 |     t=sim_param.t;
10 |     a_mtr=neigh_mtr.a_mtr;
11 |     pos_set=shape_state.pos_set;
12 |     vel_set=shape_state.vel_set;
13 |     ref_pos=refer_state.pos_set;
14 |     ref_vel=refer_state.vel_set;
15 |     swarm_size=sim_param.swarm_size;
16 |     % calculate consensus command for all the robots
17 |     err_set=GetConsensusError(swarm_size,a_mtr,pos_set);
18 |     [sign_set,pabs_set]=SepSymbAndValue(err_set);
19 |     cmd_cons=kappa_conse.*sign_set.*(pabs_set.^alpha);
20 |     % calculate reference-tracking command for informed robots
21 |     cmd_track=(kappa_track.*(ref_pos-pos_set)+ref_vel);
22 |     % calculate alignment command for uninformed robots
23 |     cmd_align=GetAlignConsensus(swarm_size,a_mtr,vel_set);
24 |     % get valid set of informed robots
25 |     inform_set=zeros(1,swarm_size);
26 |     inform_set(inform_index)=1;
27 |     % calculate velocity command    
28 |     cmd_set=-cmd_cons+inform_set.*cmd_track+(~inform_set).*cmd_align;
29 |     % update negotiation movements
30 |     state=shape_state;
31 |     state.pos_set=state.pos_set+state.vel_set.*t;
32 |     state.vel_set=cmd_set;
33 | end
34 | 
35 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36 | % FUNCTION: calculate alignment consensus
37 | function cons_set=GetAlignConsensus(swarm_size,a_mtr,state_set)
38 |     x_mtr=kron(state_set(1,:),ones(swarm_size,1));
39 |     y_mtr=kron(state_set(2,:),ones(swarm_size,1));
40 |     x_set=sum(x_mtr.*a_mtr,2);
41 |     y_set=sum(y_mtr.*a_mtr,2);
42 |     neigh_num=sum(a_mtr,2)';
43 |     neigh_num(neigh_num==0)=1;
44 |     cons_set=[x_set,y_set]'./neigh_num;
45 | end
46 | 
47 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
48 | % FUNCTION: calculate consensus errors
49 | function [sign_set,abs_set]=SepSymbAndValue(value_set)
50 |     sign_set=sign(value_set);
51 |     abs_set=abs(value_set);
52 | end
53 | 
54 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
55 | % FUNCTION: calculate consensus errors
56 | function err_set=GetConsensusError(swarm_size,a_mtr,state_set)
57 |     x_mtr=kron(state_set(1,:),ones(swarm_size,1));
58 |     y_mtr=kron(state_set(2,:),ones(swarm_size,1));
59 |     x_rel=(x_mtr'-x_mtr).*a_mtr;
60 |     y_rel=(y_mtr'-y_mtr).*a_mtr;
61 |     neigh_num=sum(a_mtr,2)';
62 |     neigh_num(neigh_num==0)=1;
63 |     err_set=[sum(x_rel,2),sum(y_rel,2)]'./neigh_num;
64 | end
65 | 


--------------------------------------------------------------------------------
/Shape assembly code/Lib_AnimatFunctions.m:
--------------------------------------------------------------------------------
  1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | % FUNCTION: animated simulation functions
  3 | function lib=Lib_AnimatFunctions
  4 |     global scale; scale=0.2;
  5 |     lib.InitAnimation=@InitAnimation;
  6 |     lib.UpadateAnimation=@UpadateAnimation;
  7 | end
  8 | 
  9 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 10 | % FUNCTION: initialize animated simulation
 11 | function hand_set=InitAnimation(draw_mode,sim_param,sim_range,robot_state,refer_state,shape_state,rcent_state,inform_index,shape_mtr,shape_info)
 12 |     % no drawing
 13 |     if draw_mode==0
 14 |         hand_set=false;
 15 |         return;
 16 |     end
 17 |     % initialize animation
 18 |     figure(1);
 19 |     box on; hold on; grid on; axis equal;
 20 |     set(gcf,'units','normalized','position',[0.05,0.05,0.9,0.85]);
 21 |     % draw objects
 22 |     switch draw_mode
 23 |         case 1 % draw robot swarm
 24 |             [hand_set.hr_pos,hand_set.hr_vel]=DrawReferMotion(sim_param.r_body,refer_state.pos_set,refer_state.vel_set);
 25 |             [hand_set.ha_pos,hand_set.ha_vel]=DrawRobotMotionA(sim_param.swarm_size,sim_param.r_body,robot_state.pos_set,robot_state.vel_set,inform_index);
 26 |         case 2 % draw negotiation
 27 |             hand_set.hs_pos=plot(shape_state.pos_set(1,:),shape_state.pos_set(2,:),'+','color',[0.9290 0.6940 0.1250],'markersize',3);
 28 |             hand_set.hc_pos=plot(rcent_state.pos_set(1,:),rcent_state.pos_set(2,:),'rs','markersize',8);
 29 |             [hand_set.ha_pos,hand_set.ha_vel]=DrawRobotMotionA(sim_param.swarm_size,sim_param.r_body,robot_state.pos_set,robot_state.vel_set,inform_index);
 30 |         case 3 % draw static shape
 31 |             hand_set.h_shape=DrawTargetShape(shape_mtr.size,shape_info.rn,shape_info.cn,shape_info.grid,shape_mtr);
 32 |             [hand_set.ha_pos,hand_set.ha_vel]=DrawRobotMotionB(sim_param.swarm_size,sim_param.r_body,robot_state.pos_set,robot_state.vel_set);
 33 |     end
 34 |     % set drawing properties
 35 |     xlabel('x axis (m)');
 36 |     ylabel('y axis (m)');
 37 |     set(gca,'FontSize',10,'Fontname','Times New Roman');
 38 |     axis(sim_range);
 39 |     set(gca,'xtick',sim_range(1):(sim_range(2)-sim_range(1))/4:sim_range(2));
 40 |     set(gca,'ytick',sim_range(3):(sim_range(4)-sim_range(3))/4:sim_range(4));  
 41 | end
 42 | 
 43 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 44 | % FUNCTION: update animated simulation
 45 | function hand_set=UpadateAnimation(hand_set,draw_mode,sim_param,robot_state,refer_state,shape_state,rcent_state)
 46 |     % no drawing
 47 |     if draw_mode==0
 48 |         return;
 49 |     end
 50 |     % update objects
 51 |     switch draw_mode
 52 |         case 1 % update robot swarm
 53 |             UpdateReferMotion(hand_set.hr_pos,hand_set.hr_vel,sim_param.r_body,refer_state.pos_set,refer_state.vel_set);
 54 |             UpdateRobotMotion(hand_set.ha_pos,hand_set.ha_vel,sim_param.swarm_size,sim_param.r_body,robot_state.pos_set,robot_state.vel_set);
 55 |         case 2 % update negotiation
 56 |             set(hand_set.hs_pos,'XData',shape_state.pos_set(1,:),'YData',shape_state.pos_set(2,:));
 57 |             set(hand_set.hc_pos,'XData',rcent_state.pos_set(1,:),'YData',rcent_state.pos_set(2,:));
 58 |             UpdateRobotMotion(hand_set.ha_pos,hand_set.ha_vel,sim_param.swarm_size,sim_param.r_body,robot_state.pos_set,robot_state.vel_set);
 59 |         case 3 % update static shape
 60 |             UpdateRobotMotion(hand_set.ha_pos,hand_set.ha_vel,sim_param.swarm_size,sim_param.r_body,robot_state.pos_set,robot_state.vel_set);
 61 |     end
 62 | end
 63 | 
 64 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 65 | % FUNCTION: draw robots' movements
 66 | function [ha_pos,ha_vel]=DrawRobotMotionA(swarm_size,r_body,pos_set,vel_set,inform_index)
 67 |     global scale;
 68 |     for index=1:1:swarm_size   
 69 |         if ~ismember(index,inform_index)
 70 |             color=[0 0.4470 0.7410];
 71 |         else
 72 |             color=[0.8500 0.3250 0.0980];
 73 |         end
 74 |         rect=[pos_set(1,index)-r_body,pos_set(2,index)-r_body,2*r_body,2*r_body];
 75 |         ha_pos(index)=rectangle('Position',rect,'Curvature',[1,1],'EdgeColor',color); 
 76 |         temp_x=[pos_set(1,index),pos_set(1,index)+scale.*vel_set(1,index)];
 77 |         temp_y=[pos_set(2,index),pos_set(2,index)+scale.*vel_set(2,index)];
 78 |         ha_vel(index)=plot(temp_x,temp_y,'r','linewidth',1.0);
 79 |     end
 80 | end
 81 | 
 82 | function [ha_pos,ha_vel]=DrawRobotMotionB(swarm_size,r_body,pos_set,vel_set)
 83 |     global scale;
 84 |     for index=1:1:swarm_size   
 85 |         color=[0 0.4470 0.7410];
 86 |         rect=[pos_set(1,index)-r_body,pos_set(2,index)-r_body,2*r_body,2*r_body];
 87 |         ha_pos(index)=rectangle('Position',rect,'Curvature',[1,1],'EdgeColor',color); 
 88 |         temp_x=[pos_set(1,index),pos_set(1,index)+scale.*vel_set(1,index)];
 89 |         temp_y=[pos_set(2,index),pos_set(2,index)+scale.*vel_set(2,index)];
 90 |         ha_vel(index)=plot(temp_x,temp_y,'r','linewidth',1.0);
 91 |     end
 92 | end
 93 | 
 94 | % FUNCTION: update robots' movements
 95 | function UpdateRobotMotion(ha_pos,ha_vel,swarm_size,r_body,pos_set,vel_set)
 96 |     global scale;
 97 |     for index=1:1:swarm_size
 98 |         rect=[pos_set(1,index)-r_body,pos_set(2,index)-r_body,2*r_body,2*r_body];
 99 |         set(ha_pos(index),'Position',rect);
100 |         temp_x=[pos_set(1,index),pos_set(1,index)+scale.*vel_set(1,index)];
101 |         temp_y=[pos_set(2,index),pos_set(2,index)+scale.*vel_set(2,index)];
102 |         set(ha_vel(index),'XData',temp_x,'YData',temp_y);
103 |     end
104 | end
105 | 
106 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
107 | % FUNCTION: draw reference movement
108 | function [hr_pos,hr_vel]=DrawReferMotion(r_body,pos_set,vel_set)
109 |     global scale;
110 |     rect=[pos_set(1,:)-r_body,pos_set(2,:)-r_body,2*r_body,2*r_body];
111 |     hr_pos=rectangle('Position',rect,'Curvature',[1,1],'EdgeColor','c','FaceColor','c'); 
112 |     temp_x=[pos_set(1,:),pos_set(1,:)+scale.*vel_set(1,:)];
113 |     temp_y=[pos_set(2,:),pos_set(2,:)+scale.*vel_set(2,:)];
114 |     hr_vel=plot(temp_x,temp_y,'r','linewidth',1.0);
115 | end
116 | 
117 | function UpdateReferMotion(hr_pos,hr_vel,r_body,pos_set,vel_set)
118 |     global scale;
119 |     rect=[pos_set(1,:)-r_body,pos_set(2,:)-r_body,2*r_body,2*r_body];
120 |     set(hr_pos,'Position',rect);
121 |     temp_x=[pos_set(1,:),pos_set(1,:)+scale.*vel_set(1,:)];
122 |     temp_y=[pos_set(2,:),pos_set(2,:)+scale.*vel_set(2,:)];
123 |     set(hr_vel,'XData',temp_x,'YData',temp_y);
124 | end
125 | 
126 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
127 | % FUNCTION: draw formation
128 | function h_shape=DrawTargetShape(swarm_size,rn,cn,grid,shape_state)
129 |     shape_x=shape_state.shape_x; 
130 |     shape_y=shape_state.shape_y;  
131 |     shape_value=shape_state.shape_value; 
132 |     shape_head=shape_state.shape_head;
133 |     width=grid/2-grid/10;
134 |     conut=1;
135 |     for index=1:1:swarm_size
136 |         color=rand(1,3);
137 |         for i=1:1:rn
138 |             for j=1:1:cn
139 |                 if shape_value(i,j)==0
140 |                     temp_x=[shape_x(i,j,index)-width,...
141 |                             shape_x(i,j,index)+width,...
142 |                             shape_x(i,j,index)+width,...
143 |                             shape_x(i,j,index)-width,...
144 |                             shape_x(i,j,index)-width];
145 |                     temp_y=[shape_y(i,j,index)-width,...
146 |                             shape_y(i,j,index)-width,...
147 |                             shape_y(i,j,index)+width,...
148 |                             shape_y(i,j,index)+width,...
149 |                             shape_y(i,j,index)-width];
150 |                     temp_px=(temp_x-shape_x(i,j,index)).*cos(pi*shape_head/180)-(temp_y-shape_y(i,j,index)).*sin(pi*shape_head/180)+shape_x(i,j,index);
151 |                     temp_py=(temp_x-shape_x(i,j,index)).*sin(pi*shape_head/180)+(temp_y-shape_y(i,j,index)).*cos(pi*shape_head/180)+shape_y(i,j,index);  
152 |                     %h_shape(conut)=plot(temp_px,temp_py,'color',color); 
153 |                     h_shape(conut)=plot(temp_px,temp_py,'color',[185 255 185]./255); 
154 |                     conut=conut+1;
155 |                     %plot(shape_mtr(i,j,1),shape_mtr(i,j,2),'*','color',[0 0 255]./255);
156 |                 end
157 |             end
158 |         end        
159 |     end
160 | end


--------------------------------------------------------------------------------
/Shape assembly code/Lib_DrawingFunctions.m:
--------------------------------------------------------------------------------
  1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | % FUNCTION: drawing library
  3 | function lib=Lib_DrawingFunctions
  4 |     lib.DrawNegotError=@DrawNegotError;
  5 |     lib.DrawTrajectory=@DrawTrajectory;
  6 |     lib.DrawPerfMetric=@DrawPerfMetric;
  7 | end
  8 | 
  9 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 10 | % FUNCTION: draw performance metric
 11 | function DrawPerfMetric(sim_param,record_state)
 12 |     % variable substitution
 13 |     t=sim_param.t;
 14 |     max_step=sim_param.max_step;
 15 |     cover_rate=record_state.cover_rate; 
 16 |     ent_rate=record_state.ent_rate;
 17 |     dist_var=record_state.dist_var; 
 18 |     % initialize drawing
 19 |     figure(4); box on; hold on; axis equal;
 20 |     % set drawing parameters
 21 |     line_width=2;
 22 |     aspect_ratio=7;
 23 |     time_set=(0:1:max_step)*t;
 24 |     % draw coverage rate
 25 |     subplot(3,1,1); box on; hold on; axis equal; 
 26 |     plot(time_set,cover_rate','LineWidth',line_width); 
 27 |     min_value=min(cover_rate);
 28 |     max_value=max(cover_rate);
 29 |     SetDrawLabel('Time (s)',['Coverage rate']);
 30 |     SetDrawRange([0,max_step*t,floor(min_value),ceil(max_value)],4,2,aspect_ratio); 
 31 |     % draw entering rate
 32 |     subplot(3,1,2); box on; hold on; axis equal; 
 33 |     plot(time_set,ent_rate','LineWidth',line_width); 
 34 |     min_value=min(ent_rate);
 35 |     max_value=max(ent_rate);
 36 |     SetDrawLabel('Time (s)',['Entering rate']);
 37 |     SetDrawRange([0,max_step*t,floor(min_value),ceil(max_value)],4,2,aspect_ratio); 
 38 |     % draw uniformity
 39 |     subplot(3,1,3); box on; hold on; axis equal; 
 40 |     plot(time_set,dist_var','LineWidth',line_width); 
 41 |     min_value=min(dist_var);
 42 |     max_value=max(dist_var);
 43 |     SetDrawLabel('Time (s)',['Uniformity']);
 44 |     SetDrawRange([0,max_step*t,floor(min_value),ceil(max_value)],4,2,aspect_ratio); 
 45 | end
 46 | 
 47 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 48 | % FUNCTION: draw motion trajectory
 49 | function DrawTrajectory(sim_param,sim_range,form_dyn,shape_info,record_state)
 50 |     % variable substitution
 51 |     swarm_size=sim_param.swarm_size;
 52 |     max_step=sim_param.max_step;
 53 |     ptra_rec=record_state.ptra_rec; 
 54 |     pset_rec=record_state.pset_rec; 
 55 |     vset_rec=record_state.vset_rec;
 56 |     % initialize drawing
 57 |     figure(3); box on; hold on; axis equal;
 58 |     %set(gcf,'units','normalized','position',[0.05,0.05,0.9,0.85]);
 59 |     % set drawing parameters
 60 |     scale=0.2;
 61 |     clarity=0.2;
 62 |     % draw target shape
 63 |     DrawTargetShape(form_dyn.size,shape_info.rn,shape_info.cn,shape_info.grid,form_dyn);
 64 |     % draw trajectory
 65 |     for index=1:1:swarm_size        
 66 |         handle=plot(ptra_rec(1,1:max_step,index),ptra_rec(2,1:max_step,index),'LineWidth',1);
 67 |         handle.Color(4)=clarity;
 68 |     end
 69 |     % draw initial states
 70 |     pos_set=pset_rec(:,:,1); 
 71 |     vel_set=vset_rec(:,:,1); 
 72 |     DrawInitialMotion(scale,sim_param.swarm_size,sim_param.r_body,pos_set,vel_set);
 73 |     % draw final states
 74 |     pos_set=pset_rec(:,:,max_step); 
 75 |     vel_set=vset_rec(:,:,max_step); 
 76 |     DrawFinalMotion(scale,sim_param.swarm_size,sim_param.r_body,pos_set,vel_set);
 77 |     % set prperties
 78 |     SetDrawLabel('x axis (m)','y axis (m)');
 79 |     SetDrawRange(sim_range,2,2,1); 
 80 | end
 81 | 
 82 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 83 | % FUNCTION: draw negotiation errors
 84 | function DrawNegotError(sim_param,record_state)    
 85 |     % variable substitution
 86 |     t=sim_param.t;
 87 |     max_step=sim_param.max_step;
 88 |     swarm_size=sim_param.swarm_size;
 89 |     pos_set=record_state.spos_rec;
 90 |     head_set=record_state.shead_rec;
 91 |     ref_pos=record_state.rpos_rec;
 92 |     ref_head=record_state.rhead_rec;    
 93 |     % initialize drawing
 94 |     figure(2); box on; hold on; axis equal;
 95 |     %set(gcf,'units','normalized','position',[0.05,0.05,0.9,0.85]);
 96 |     % set drawing parameters
 97 |     clarity=1.0;
 98 |     line_width=1.0;
 99 |     aspect_ratio=7;
100 |     time_set=(0:1:max_step)*t;
101 |     min_set=inf*ones(1,swarm_size);
102 |     max_set=-inf*ones(1,swarm_size);
103 |     % draw x-axis postion error
104 |     subplot(3,1,1); box on; hold on; axis equal;    
105 |     for index=1:1:swarm_size
106 |         rel_value=pos_set(1,:,index)-ref_pos(1,:);
107 |         H=plot(time_set,rel_value,'LineWidth',line_width);
108 |         H.Color(4)=clarity;
109 |         min_set(:,index)=min(rel_value);
110 |         max_set(:,index)=max(rel_value);
111 |     end
112 |     SetDrawLabel('Time (s)',['x-axis position' sprintf('\n' ) 'error (m)']);
113 |     SetDrawRange([0,max_step*t,floor(min(min_set)),ceil(max(max_set))],4,2,aspect_ratio);    
114 |     % draw y-axis postion error
115 |     subplot(3,1,2); box on; hold on; axis equal;
116 |     for index=1:1:swarm_size
117 |         rel_value=pos_set(2,:,index)-ref_pos(2,:);
118 |         H=plot(time_set,rel_value,'LineWidth',line_width);
119 |         H.Color(4)=clarity;
120 |         min_set(:,index)=min(rel_value);
121 |         max_set(:,index)=max(rel_value);
122 |     end
123 |     SetDrawLabel('Time (s)',['y-axis position' sprintf('\n' ) 'error (m)']);
124 |     SetDrawRange([0,max_step*t,floor(min(min_set)),ceil(max(max_set))],4,2,aspect_ratio); 
125 |     % draw orientation error
126 |     subplot(3,1,3); box on; hold on; axis equal;    
127 |     for index=1:1:swarm_size
128 |         rel_value=head_set(:,index)-ref_head(:);
129 |         H=plot(time_set,rel_value,'LineWidth',line_width);
130 |         H.Color(4)=clarity;
131 |         min_set(:,index)=min(rel_value);
132 |         max_set(:,index)=max(rel_value);
133 |     end
134 |     SetDrawLabel('Time (s)',['Orientation' sprintf('\n' ) 'error (rad)']);
135 |     SetDrawRange([0,max_step*t,floor(min(min_set)),ceil(max(max_set))],4,2,aspect_ratio); 
136 | end
137 | 
138 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
139 | 
140 | 
141 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
142 | % FUNCTION: set drawing range
143 | function SetDrawLabel(x_label,y_label)
144 |     xlabel(x_label);
145 |     ylabel(y_label);
146 |     set(gca,'FontSize',10,'Fontname','Times New Roman');
147 | end
148 | 
149 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
150 | % FUNCTION: set drawing range
151 | function SetDrawRange(range,x_num,y_num,scale)
152 |     axis(range);    
153 |     set(gca,'XTick',(range(1):(range(2)-range(1))/x_num:range(2)));
154 |     set(gca,'YTick',(range(3):(range(4)-range(3))/y_num:range(4)));
155 |     set(gca,'DataAspectRatio',[1 scale*(range(4)-range(3))/(range(2)-range(1)) 1]);
156 | end
157 | 
158 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
159 | % FUNCTION: draw robots' movements
160 | function DrawFinalMotion(scale,swarm_size,r_body,pos_set,vel_set)
161 |     edge_color=[0 0.4470 0.7410];
162 |     face_color=[0 0.4470 0.7410]; 
163 |     for index=1:1:swarm_size   
164 |         rect=[pos_set(1,index)-r_body,pos_set(2,index)-r_body,2*r_body,2*r_body];
165 |         rectangle('Position',rect,'Curvature',[1,1],'EdgeColor',edge_color,'FaceColor',face_color); 
166 |         temp_x=[pos_set(1,index),pos_set(1,index)+scale.*vel_set(1,index)];
167 |         temp_y=[pos_set(2,index),pos_set(2,index)+scale.*vel_set(2,index)];
168 |         plot(temp_x,temp_y,'r','linewidth',1.0);
169 |     end
170 | end
171 | 
172 | function DrawInitialMotion(scale,swarm_size,r_body,pos_set,vel_set)
173 |     edge_color=[160 160 160]./255;
174 |     face_color=[160 160 160]./255; 
175 |     for index=1:1:swarm_size   
176 |         rect=[pos_set(1,index)-r_body,pos_set(2,index)-r_body,2*r_body,2*r_body];
177 |         rectangle('Position',rect,'Curvature',[1,1],'EdgeColor','none','FaceColor',face_color); 
178 |         temp_x=[pos_set(1,index),pos_set(1,index)+scale.*vel_set(1,index)];
179 |         temp_y=[pos_set(2,index),pos_set(2,index)+scale.*vel_set(2,index)];
180 |         plot(temp_x,temp_y,'r','linewidth',1.0);
181 |     end
182 | end
183 | 
184 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
185 | % FUNCTION: draw formation
186 | function DrawTargetShape(swarm_size,rn,cn,grid,shape_state)
187 |     shape_x=shape_state.shape_x; 
188 |     shape_y=shape_state.shape_y;  
189 |     shape_value=shape_state.shape_value; 
190 |     shape_head=shape_state.shape_head;
191 |     width=grid/2-grid/10;
192 |     color=[0.85 1 0.85];
193 |     for index=1:1:swarm_size
194 |         for i=1:1:rn
195 |             for j=1:1:cn
196 |                 if shape_value(i,j)<=0.0%3
197 |                     temp_x=[shape_x(i,j,index)-width,...
198 |                             shape_x(i,j,index)+width,...
199 |                             shape_x(i,j,index)+width,...
200 |                             shape_x(i,j,index)-width,...
201 |                             shape_x(i,j,index)-width];
202 |                     temp_y=[shape_y(i,j,index)-width,...
203 |                             shape_y(i,j,index)-width,...
204 |                             shape_y(i,j,index)+width,...
205 |                             shape_y(i,j,index)+width,...
206 |                             shape_y(i,j,index)-width];
207 |                     temp_px=(temp_x-shape_x(i,j,index)).*cos(pi*shape_head/180)-(temp_y-shape_y(i,j,index)).*sin(pi*shape_head/180)+shape_x(i,j,index);
208 |                     temp_py=(temp_x-shape_x(i,j,index)).*sin(pi*shape_head/180)+(temp_y-shape_y(i,j,index)).*cos(pi*shape_head/180)+shape_y(i,j,index);  
209 |                     patch(temp_px,temp_py,color,'EdgeColor',color); 
210 |                 end
211 |             end
212 |         end        
213 |     end
214 | end
215 | 


--------------------------------------------------------------------------------
/Shape assembly code/Lib_InitialFunctions.m:
--------------------------------------------------------------------------------
  1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | % FUNCTION: initialization functions
  3 | function lib=Lib_InitialFunctions
  4 |     lib.InitSimuParam=@InitSimuParam;
  5 |     lib.SelectInformer=@SelectInformer;
  6 |     lib.InitRobotState=@InitRobotState;
  7 |     lib.InitReferState=@InitReferState;
  8 | end
  9 | 
 10 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 11 | % FUNCTION: set simulation params
 12 | function param=InitSimuParam(run_time,swarm_size)
 13 |     % set time params
 14 |     param.time=run_time;                    % runtime
 15 |     param.t=0.01;                           % period
 16 |     param.max_step=ceil(param.time/param.t); % cycles   
 17 |     % set robot params
 18 |     param.r_body=0.20;                      % body size
 19 |     param.r_safe=0.35;                      % safe size
 20 |     param.r_avoid=1.5;                      % avoidance range
 21 |     param.r_sense=2.5;                       % sensing range
 22 |     param.vel_max=5;                        % max speed
 23 |     % set swarm params
 24 |     param.swarm_size=swarm_size;            % swarm size
 25 |     param.leader_num=ceil(swarm_size/5);    % number of leaders 
 26 | end
 27 | 
 28 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 29 | % FUNCTION: randomly select a given number of agents as informed robots
 30 | function index=SelectInformer(sim_param)
 31 |     index=sort(randperm(sim_param.swarm_size,sim_param.leader_num));
 32 | end
 33 | 
 34 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 35 | % FUNCTION: initialize robots' states
 36 | function state=InitRobotState(sim_param,pos_cen)
 37 |     state.pos_set=GetRobotPosition(sim_param,pos_cen);
 38 |     state.vel_set=zeros(2,sim_param.swarm_size);
 39 | end
 40 | 
 41 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 42 | % FUNCTION: initialize reference states
 43 | function state=InitReferState(pos_cen)
 44 |     state.pos_set=pos_cen;
 45 |     state.vel_set=[0,0]';
 46 |     state.head_set=0;
 47 |     state.hvel_set=0;
 48 | end
 49 | 
 50 | %% Auxiliary functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 51 | % FUNCTION: get robots' positions
 52 | function pos_set=GetRobotPosition(sim_param,pos_cen)
 53 |     % variable substitution
 54 |     swarm_size=sim_param.swarm_size; 
 55 |     r_sense=sim_param.r_sense;
 56 |     r_avoid=sim_param.r_avoid;
 57 |     % initialize state variables
 58 |     pos_set=zeros(2,swarm_size);  
 59 |     % initialize variables
 60 |     mtr_size=ceil(swarm_size/2+1);
 61 |     gen_mtr=zeros(mtr_size,mtr_size);
 62 |     gen_set=zeros(2,swarm_size);
 63 |     neigh_mtr=ones(3,3).*[-1,0,1];
 64 |     % generate the first state
 65 |     cell_pos=[ceil(mtr_size/2),ceil(mtr_size/2)]';      
 66 |     gen_mtr(cell_pos(1),cell_pos(2))=1;
 67 |     gen_num=1;
 68 |     gen_set(:,gen_num)=cell_pos; 
 69 |     % generate other states
 70 |     while gen_num~=swarm_size        
 71 |         % select one from the generated cells
 72 |         index=randi([1,gen_num],1,1); 
 73 |         cell_pos=gen_set(:,index);
 74 |         % find valid cells around the focus cell
 75 |         neigh_row=ones(3,3).*cell_pos(1)+neigh_mtr';    
 76 |         neigh_col=ones(3,3).*cell_pos(2)+neigh_mtr;  
 77 |         neigh_row=reshape(neigh_row,1,9);
 78 |         neigh_col=reshape(neigh_col,1,9);
 79 |         neigh_set=zeros(2,8);
 80 |         ungen_count=0;      
 81 |         for i=1:1:9
 82 |             if isequal([neigh_row(i),neigh_col(i)]',cell_pos)
 83 |                 continue;
 84 |             end
 85 |             if neigh_row(i)<1||neigh_row(i)>mtr_size||...
 86 |                neigh_col(i)<1||neigh_col(i)>mtr_size
 87 |                 continue;
 88 |             end
 89 |             if gen_mtr(neigh_row(i),neigh_col(i))==1
 90 |                 continue;
 91 |             end
 92 |             ungen_count=ungen_count+1;
 93 |             neigh_set(:,ungen_count)=[neigh_row(i),neigh_col(i)]';
 94 |         end        
 95 |         % abandon the cell with many valid neighbors
 96 |         if ungen_count<3
 97 |             continue;
 98 |         end
 99 |         % choose a suitable cell
100 |         suit_set=zeros(2,ungen_count);
101 |         suit_count=0;
102 |         for i=1:1:ungen_count
103 |             cell_pos=neigh_set(:,i);   
104 |             neigh_row=ones(3,3).*cell_pos(1)+neigh_mtr';    
105 |             neigh_col=ones(3,3).*cell_pos(2)+neigh_mtr;
106 |             neigh_row=reshape(neigh_row,1,9);
107 |             neigh_col=reshape(neigh_col,1,9);
108 |             gened_count=0;
109 |             for j=1:1:9
110 |                 if neigh_row(j)<1||neigh_row(j)>mtr_size||...
111 |                    neigh_col(j)<1||neigh_col(j)>mtr_size
112 |                     continue;
113 |                 end
114 |                 if gen_mtr(neigh_row(j),neigh_col(j))==1
115 |                     gened_count=gened_count+1;
116 |                 end
117 |             end
118 |             if gened_count>=3
119 |                 continue;
120 |             end
121 |             suit_count=suit_count+1;
122 |             suit_set(:,suit_count)=cell_pos;
123 |         end
124 |         if suit_count==0
125 |             continue;
126 |         end        
127 |         select=randi([1,suit_count],1,1);  
128 |         cell_pos=suit_set(:,select);
129 |         gen_mtr(suit_set(1),suit_set(2))=1;
130 |         gen_num=gen_num+1;
131 |         gen_set(:,gen_num)=cell_pos;    
132 |     end    
133 |     % generate initial positions
134 |     grid_lgth=r_sense/1.5/sqrt(2);
135 |     grid_valid=grid_lgth-r_avoid/2;
136 |     count=0;
137 |     for i=1:1:mtr_size    
138 |         for j=1:1:mtr_size
139 |             if gen_mtr(i,j)==0
140 |                 continue;
141 |             end
142 |             count=count+1;
143 |             temp=[i,j]'-gen_set(:,1);
144 |             temp_minx=temp(1)*grid_lgth-grid_valid/2; temp_maxx=temp(1)*grid_lgth+grid_valid/2;
145 |             temp_miny=-temp(2)*grid_lgth-grid_valid/2; temp_maxy=-temp(2)*grid_lgth+grid_valid/2;
146 |             pos_set(1,count)=temp_minx+(temp_maxx-temp_minx)*rand(1,1);
147 |             pos_set(2,count)=temp_miny+(temp_maxy-temp_miny)*rand(1,1);
148 |         end
149 |     end
150 |     pos_cen_temp=sum(pos_set,2)./swarm_size;    
151 |     pos_set=pos_set-pos_cen_temp+pos_cen;
152 | end
153 | 


--------------------------------------------------------------------------------
/Shape assembly code/Lib_RecordFunctions.m:
--------------------------------------------------------------------------------
 1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | % FUNCTION: record library
 3 | function lib=Lib_RecordFunctions
 4 |     lib.InitRecordState=@InitRecordState;
 5 |     lib.UpdateRecordState=@UpdateRecordState;
 6 | end
 7 | 
 8 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 9 | % FUNCTION: initialize record states
10 | function state=InitRecordState(sim_param,robot_state,refer_state,shape_state,rcent_state,metric)
11 |     % variable substitution
12 |     swarm_size=sim_param.swarm_size; 
13 |     max_step=sim_param.max_step;
14 |     % initialize robot-motion variables
15 |     state.pset_rec=zeros(2,swarm_size,max_step+1); 
16 |     state.vset_rec=zeros(2,swarm_size,max_step+1); 
17 |     state.ptra_rec=zeros(2,max_step+1,swarm_size); % use for drawing trajectories
18 |     state.vtra_rec=zeros(2,max_step+1,swarm_size);
19 |     % initialize referecne variables
20 |     state.rpos_rec=zeros(2,max_step+1);           
21 |     state.rvel_rec=zeros(2,max_step+1);
22 |     state.rhead_rec=zeros(1,max_step+1);
23 |     state.rhvel_rec=zeros(1,max_step+1);
24 |     % initialize shape-motion variables
25 |     state.spos_rec=zeros(2,max_step+1,swarm_size);
26 |     state.svel_rec=zeros(2,max_step+1,swarm_size);
27 |     state.shead_rec=zeros(max_step+1,swarm_size); % head angle
28 |     state.shvel_rec=zeros(max_step+1,swarm_size); % head angular velocity
29 |     state.spcen_rec=zeros(2,max_step+1);
30 |     state.shcen_rec=zeros(2,max_step+1);
31 |     % initialize metric variables
32 |     state.cover_rate=zeros(1,max_step+1); 
33 |     state.ent_rate=zeros(1,max_step+1);
34 |     state.dist_var=zeros(1,max_step+1); 
35 |     state.mean_vel=zeros(1,max_step+1); 
36 |     % record robot-motion variables
37 |     state.pset_rec(:,:,1)=robot_state.pos_set; 
38 |     state.vset_rec(:,:,1)=robot_state.vel_set;
39 |     state.ptra_rec(:,1,:)=robot_state.pos_set; 
40 |     state.vtra_rec(:,1,:)=robot_state.vel_set;
41 |     % record referecne variables
42 |     state.rpos_rec(:,1)=refer_state.pos_set;
43 |     state.rvel_rec(:,1)=refer_state.vel_set;  
44 |     state.rhead_rec(:,1)=refer_state.head_set;
45 |     state.rhvel_rec(:,1)=refer_state.hvel_set;
46 |     % record shape-motion variables
47 |     state.spos_rec(:,1,:)=shape_state.pos_set;
48 |     state.svel_rec(:,1,:)=shape_state.vel_set;
49 |     state.shead_rec(1,:)=shape_state.head_set;
50 |     state.shvel_rec(1,:)=shape_state.hvel_set;
51 |     state.spcen_rec(:,1)=rcent_state.pos_set;
52 |     state.shcen_rec(:,1)=rcent_state.head_set;
53 |     % record metric variables
54 |     state.cover_rate(1,1)=metric.cover_rate; 
55 |     state.ent_rate(1,1)=metric.ent_rate;
56 |     state.dist_var(1,1)=metric.dist_var; 
57 |     state.mean_vel(1,1)=metric.mean_vel; 
58 | end
59 | 
60 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
61 | % FUNCTION: update record states
62 | function state=UpdateRecordState(step,record_state,robot_state,refer_state,shape_state,rcent_state,metric)
63 |     % record robot-motion variables
64 |     record_state.pset_rec(:,:,step+1)=robot_state.pos_set; 
65 |     record_state.vset_rec(:,:,step+1)=robot_state.vel_set;
66 |     record_state.ptra_rec(:,step+1,:)=robot_state.pos_set; 
67 |     record_state.vtra_rec(:,step+1,:)=robot_state.vel_set;
68 |     % record referecne variables
69 |     record_state.rpos_rec(:,step+1)=refer_state.pos_set;
70 |     record_state.rvel_rec(:,step+1)=refer_state.vel_set;  
71 |     record_state.rhead_rec(:,step+1)=refer_state.head_set;
72 |     record_state.rhvel_rec(:,step+1)=refer_state.hvel_set;
73 |     % record shape-motion variables
74 |     record_state.spos_rec(:,step+1,:)=shape_state.pos_set;
75 |     record_state.svel_rec(:,step+1,:)=shape_state.vel_set;
76 |     record_state.shead_rec(step+1,:)=shape_state.head_set;
77 |     record_state.shvel_rec(step+1,:)=shape_state.hvel_set;
78 |     record_state.spcen_rec(:,step+1)=rcent_state.pos_set;
79 |     record_state.shcen_rec(:,step+1)=rcent_state.head_set;
80 |     % record metric variables
81 |     record_state.cover_rate(1,step+1)=metric.cover_rate;
82 |     record_state.ent_rate(1,step+1)=metric.ent_rate;
83 |     record_state.dist_var(1,step+1)=metric.dist_var;
84 |     record_state.mean_vel(1,step+1)=metric.mean_vel;
85 |     % output substitution
86 |     state=record_state; 
87 | end
88 | 


--------------------------------------------------------------------------------
/Shape assembly code/Lib_ShapeFunctions.m:
--------------------------------------------------------------------------------
  1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2 | % FUNCTION: shape processing library
  3 | function lib=Lib_ShapeFunctions
  4 |     lib.LoadShapeImage=@LoadShapeImage;
  5 |     lib.InitFormShape=@InitFormShape;
  6 |     lib.InitFormState=@InitFormState;
  7 |     lib.GetShapeCenter=@GetShapeCenter;
  8 |     lib.GetDynFormation=@GetDynFormation;
  9 | end
 10 | 
 11 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 12 | % FUNCTION: get dynamic formation
 13 | function shape_dyn=GetDynFormation(swarm_size,shape_mtr,shape_info,shape_state)
 14 |     % variable substitution
 15 |     rn=shape_info.rn; 
 16 |     cn=shape_info.cn; 
 17 |     head_set=shape_state.head_set; 
 18 |     pos_set=shape_state.pos_set;
 19 |     % initialize output
 20 |     shape_x=zeros(rn,cn,swarm_size); 
 21 |     shape_y=zeros(rn,cn,swarm_size);    
 22 |     shape_value=shape_mtr(:,:,3);
 23 |     % transform coordinate
 24 |     for i=1:1:swarm_size
 25 |         temp_mtr(:,:,1)=shape_mtr(:,:,1).*cos(head_set(i))-shape_mtr(:,:,2).*sin(head_set(i));
 26 |         temp_mtr(:,:,2)=shape_mtr(:,:,1).*sin(head_set(i))+shape_mtr(:,:,2).*cos(head_set(i));    
 27 |         shape_x(:,:,i)=temp_mtr(:,:,1)+pos_set(1,i);
 28 |         shape_y(:,:,i)=temp_mtr(:,:,2)+pos_set(2,i);
 29 |     end
 30 |     % assign output
 31 |     shape_dyn.shape_x=shape_x;
 32 |     shape_dyn.shape_y=shape_y;
 33 |     shape_dyn.shape_value=shape_value;
 34 |     shape_dyn.shape_head=head_set;
 35 |     shape_dyn.size=swarm_size;
 36 | end
 37 | 
 38 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 39 | % FUNCTION: get shape center 
 40 | function state=GetShapeCenter(swarm_size,shape_state)
 41 |     state.pos_set=sum(shape_state.pos_set,2)./swarm_size;
 42 |     state.head_set=sum(shape_state.head_set,2)./swarm_size;
 43 | end
 44 | 
 45 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 46 | % FUNCTION: initialize formation states
 47 | function state=InitFormState(sim_param,robot_state)
 48 |     state.pos_set=robot_state.pos_set;
 49 |     state.vel_set=robot_state.vel_set;
 50 |     temp_head=rand(1,sim_param.swarm_size)*2*pi;
 51 |     aveg_head=sum(temp_head)/sim_param.swarm_size; 
 52 |     aveg_head(aveg_head>2*pi)=aveg_head-2*pi;
 53 |     state.head_set=temp_head-aveg_head;
 54 |     state.hvel_set=zeros(size(state.head_set));
 55 | end
 56 | 
 57 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 58 | % FUNCTION: initialize formation shape
 59 | function [shape_mtr,shape_info]=InitFormShape(sim_param,image_mtr)
 60 |     % variable substitution
 61 |     swarm_size=sim_param.swarm_size; 
 62 |     r_avoid=sim_param.r_avoid;
 63 |     % get parameters
 64 |     [shape_info.rn,shape_info.cn]=size(image_mtr);       
 65 |     shape_info.cen_x=ceil(shape_info.cn/2);
 66 |     shape_info.cen_y=ceil(shape_info.rn/2);
 67 |     shape_info.black_num=length(find(image_mtr==0));
 68 |     shape_info.gray_num=length(find(image_mtr<1))-shape_info.black_num;
 69 |     temp_mtr=image_mtr(shape_info.cen_y,:); 
 70 |     temp_mtr(temp_mtr==0)=2;
 71 |     shape_info.gray_scale=1/min(temp_mtr);
 72 |     % calculate grid size
 73 |     ratio=1.0;  
 74 |     shape_info.grid=sqrt((pi/4)*(swarm_size/shape_info.black_num))*r_avoid;
 75 |     shape_info.grid=shape_info.grid*ratio;
 76 |     % get grid coordinates
 77 |     shape_mtr=ones(shape_info.rn,shape_info.cn,3);
 78 |     temp_x=((1:1:shape_info.cn)-shape_info.cen_x).*shape_info.grid;
 79 |     temp_y=((1:1:shape_info.rn)-shape_info.cen_y).*shape_info.grid;
 80 |     shape_mtr(:,:,1)=shape_mtr(:,:,1).*temp_x;
 81 |     shape_mtr(:,:,2)=shape_mtr(:,:,2).*flipud(temp_y');
 82 |     shape_mtr(:,:,3)=image_mtr;  
 83 | end
 84 | 
 85 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 86 | % FUNCTION: load shape image
 87 | function image=LoadShapeImage(type)
 88 |     root_str='.\ShapeImage\';
 89 |     file_str='Image_';
 90 |     switch type
 91 |         case 1
 92 |             type_str='geom_snow'; 
 93 |         case 2
 94 |             type_str='geom_starfish'; 
 95 |         case 3
 96 |             type_str='letter_R';
 97 |         case 4
 98 |             type_str='letter_O';
 99 |         case 5
100 |             type_str='letter_B';
101 |         otherwise 
102 |             type_str='geom_snow';
103 |     end
104 |     % generate file path    
105 |     path=[root_str,file_str,type_str];
106 |     temp=load(path);
107 |     image=temp.gray_mtr;
108 | end
109 | 


--------------------------------------------------------------------------------
/Shape assembly code/Lib_UpdateFunctions.m:
--------------------------------------------------------------------------------
 1 | %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 2 | % FUNCTION: update functions
 3 | function lib=Lib_UpdateFunctions
 4 |     lib.UpdateRobotMotion=@UpdateRobotMotion;
 5 |     lib.UpdateRecordState=@UpdateRecordState;
 6 | end
 7 | 
 8 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 9 | % FUNCTION: update robot motion
10 | function state=UpdateRobotMotion(sim_param,robot_state,cmd_set)
11 |     % variable substitution
12 |     t=sim_param.t; 
13 |     vel_max=sim_param.vel_max;
14 |     pos_set=robot_state.pos_set; 
15 |     vel_set=robot_state.vel_set;
16 |     % limit control input (max speed)
17 |     vscl_set=sqrt(cmd_set(1,:).^2+cmd_set(2,:).^2);
18 |     head_set=atan2(cmd_set(2,:),cmd_set(1,:));
19 |     index_set=find(vscl_set>vel_max);
20 |     cmd_set(:,index_set)=vel_max.*[cos(head_set(index_set));sin(head_set(index_set))];
21 |     % upate motion states    
22 |     ratio=0.1;
23 |     state.pos_set=pos_set+vel_set.*t;  
24 |     state.vel_set=ratio.*cmd_set+(1-ratio).*vel_set;      
25 | end
26 | 
27 | 


--------------------------------------------------------------------------------
/Shape assembly code/MainLoop.m:
--------------------------------------------------------------------------------
  1 | close all; clear; clc;
  2 | 
  3 | %% Information %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  4 | % AUTHORS: Guibin Sun, Beihang Universuty
  5 | % INSTRUCTOR: Shiyu Zhao, Westlake University
  6 | % TIME: 2020.9-2021.7
  7 | % DECLARATION: Copyright @ Authors. All Rights Reserved.
  8 | 
  9 | %% Initialization %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 10 | % declare function library
 11 | Init_lib=Lib_InitialFunctions; % use for simulation initialization
 12 | Shape_lib=Lib_ShapeFunctions;  % use for shape processing
 13 | Updt_lib=Lib_UpdateFunctions;  % use to update robot motion
 14 | Anim_lib=Lib_AnimatFunctions;  % use for animated simulation
 15 | Draw_lib=Lib_DrawingFunctions; % use to draw curves 
 16 | Recd_lib=Lib_RecordFunctions;  % use to record data
 17 | 
 18 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 19 | % set simulation params
 20 | Run_time=20;                   % run time of simulation
 21 | Swarm_size=50;                 % size of robot swarm
 22 | Sim_range=[0,30,0,30];         % simulation range
 23 | Sim_param=Init_lib.InitSimuParam(Run_time,Swarm_size);
 24 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 25 | % initialize robots' states
 26 | Pos_init=[Sim_range(1)+(Sim_range(2)-Sim_range(1))/2,Sim_range(3)+(Sim_range(4)-Sim_range(3))/2]'; 
 27 | Robot_state=Init_lib.InitRobotState(Sim_param,Pos_init);
 28 | % initialize reference states
 29 | Refer_state=Init_lib.InitReferState(Pos_init);
 30 | % randomly select a given number of agents as informed robots
 31 | Inform_index=Init_lib.SelectInformer(Sim_param);
 32 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 33 | % initialize formation shape
 34 | Type=1;  % shape type: 1-snowflake, 2-starfish, 3-letter R, 4-letter O, 5-letter B
 35 | Gray_image=Shape_lib.LoadShapeImage(Type);
 36 | [Gray_mtr,Gray_info]=Shape_lib.InitFormShape(Sim_param,Gray_image);
 37 | % initialize formation states
 38 | Shape_state=Shape_lib.InitFormState(Sim_param,Robot_state);
 39 | % initialize shape center 
 40 | Rcent_state=Shape_lib.GetShapeCenter(Sim_param.swarm_size,Shape_state);
 41 | % initialize performance metric
 42 | Neigh_mtr=Fcn_GetNeighborSet(Sim_param,Robot_state); 
 43 | Metric=Fcn_GetPerformMetric(Sim_param,Neigh_mtr,Robot_state,Rcent_state,Gray_mtr,Gray_info);
 44 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 45 | % initialize record states
 46 | Record_state=Recd_lib.InitRecordState(Sim_param,Robot_state,Refer_state,Shape_state,Rcent_state,Metric);
 47 | 
 48 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 49 | % initialize animated simulation
 50 | Anim_mode=3; % 0-no drawing, 1-robot swarm, 2-negotiation, 3-static shape
 51 | form_dyn=Shape_lib.GetDynFormation(1,Gray_mtr,Gray_info,Rcent_state);
 52 | Hand_set=Anim_lib.InitAnimation(Anim_mode,Sim_param,Sim_range,Robot_state,Refer_state,Shape_state,Rcent_state,Inform_index,form_dyn,Gray_info);
 53 | % set curves flags 
 54 | Draw_mode=3; % 0-no curve, 1-negotiation error, 2-motion trajectory, 3-performance metric
 55 | 
 56 | %% Control Process %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 57 | for step=1:1:Sim_param.max_step
 58 |     if Anim_mode==0&&rem(step,25)==0
 59 |         disp(['The simulation has been run ',num2str(step),' steps!']);
 60 |     end
 61 |     
 62 |     % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 63 |     % get neighboring set
 64 |     Neigh_mtr=Fcn_GetNeighborSet(Sim_param,Robot_state); 
 65 |     
 66 |     % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 67 |     % negotiate formation states
 68 |     state=Fcn_NegotPositState(Sim_param,Neigh_mtr,Shape_state,Refer_state,Inform_index);
 69 |     Shape_state=Fcn_NegotOrientState(Sim_param,Neigh_mtr,state,Refer_state,Inform_index);
 70 |     % get target formation
 71 |     Form_dyn=Shape_lib.GetDynFormation(Sim_param.swarm_size,Gray_mtr,Gray_info,Shape_state);
 72 |     
 73 |     % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 74 |     % calculate shape-entering command
 75 |     [cmd_enter,grid_set]=Fcn_CalEnteringCmd(Sim_param,Form_dyn,Gray_info,Robot_state,Shape_state);
 76 |     % calculate shape-exploration command 
 77 |     cmd_explore=Fcn_CalExplorationCmd(Sim_param,Form_dyn,Gray_info,grid_set,Neigh_mtr,Robot_state,Shape_state);
 78 |     % calculate interaction velocity command
 79 |     cmd_interact=Fcn_CalInteractionCmd(Sim_param,Neigh_mtr,Robot_state);
 80 |     % calculate resultant command
 81 |     cmd_set=cmd_enter+cmd_explore+cmd_interact;
 82 | 
 83 |     % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 84 |     % update robot motion
 85 |     state=Updt_lib.UpdateRobotMotion(Sim_param,Robot_state,cmd_set);
 86 |     Robot_state=state;
 87 |     % update shape center 
 88 |     Rcent_state=Shape_lib.GetShapeCenter(Sim_param.swarm_size,Shape_state);
 89 |     % update performance metric
 90 |     Metric=Fcn_GetPerformMetric(Sim_param,Neigh_mtr,Robot_state,Rcent_state,Gray_mtr,Gray_info);
 91 |     % update record state
 92 |     state=Recd_lib.UpdateRecordState(step,Record_state,Robot_state,Refer_state,Shape_state,Rcent_state,Metric);
 93 |     Record_state=state;
 94 |     % update animated simulation    
 95 |     Anim_lib.UpadateAnimation(Hand_set,Anim_mode,Sim_param,Robot_state,Refer_state,Shape_state,Rcent_state);    
 96 |     drawnow;
 97 | end
 98 | 
 99 | %% Curves Drawing %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
100 | switch Draw_mode
101 |     case 1
102 |         Draw_lib.DrawNegotError(Sim_param,Record_state);
103 |     case 2
104 |         form_dyn=Shape_lib.GetDynFormation(1,Gray_mtr,Gray_info,Rcent_state);
105 |         Draw_lib.DrawTrajectory(Sim_param,Sim_range,form_dyn,Gray_info,Record_state);
106 |     case 3
107 |         Draw_lib.DrawPerfMetric(Sim_param,Record_state);
108 | end
109 | 


--------------------------------------------------------------------------------
/Shape assembly code/ShapeImage/Image_geom_snow.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WindyLab/Shape-assembly-code/5da27db8687badd025502b8da9ede2f7f30feac4/Shape assembly code/ShapeImage/Image_geom_snow.mat


--------------------------------------------------------------------------------
/Shape assembly code/ShapeImage/Image_geom_starfish.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WindyLab/Shape-assembly-code/5da27db8687badd025502b8da9ede2f7f30feac4/Shape assembly code/ShapeImage/Image_geom_starfish.mat


--------------------------------------------------------------------------------
/Shape assembly code/ShapeImage/Image_letter_B.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WindyLab/Shape-assembly-code/5da27db8687badd025502b8da9ede2f7f30feac4/Shape assembly code/ShapeImage/Image_letter_B.mat


--------------------------------------------------------------------------------
/Shape assembly code/ShapeImage/Image_letter_O.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WindyLab/Shape-assembly-code/5da27db8687badd025502b8da9ede2f7f30feac4/Shape assembly code/ShapeImage/Image_letter_O.mat


--------------------------------------------------------------------------------
/Shape assembly code/ShapeImage/Image_letter_R.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/WindyLab/Shape-assembly-code/5da27db8687badd025502b8da9ede2f7f30feac4/Shape assembly code/ShapeImage/Image_letter_R.mat


--------------------------------------------------------------------------------