├── .github └── workflows │ └── manual.yml ├── .gitignore ├── CODEOWNERS ├── DSLC_solving_one_robot_control ├── ANN_xProcess.m ├── ANN_xTrain.m ├── A_new.m ├── B_new.m ├── Barriera.m ├── Barriera_new.m ├── Barriera_new1.m ├── Barriera_new_cen.m ├── CNN_xProcess.m ├── CNN_xTrain.m ├── CreateANN1.m ├── CreateANN_x.m ├── CreateCNN1.m ├── CreateCNN_x.m ├── DSLC_solving_centralized_main.m ├── NNProcess.m ├── NNProcess1.m ├── NNTrain.m ├── NNTrain1.m ├── README.md ├── centralized_cost0623v0.mat ├── centralized_cost0623v1.mat ├── centralized_cost0623v2.mat ├── centralized_cost0623v3.mat ├── centralized_cost0623v4.mat ├── centralized_cost0623v5.mat ├── centralized_version_main.m ├── circle_func.m ├── desired_position.m ├── distributedd_cost0623v0.mat ├── distributedd_cost0623v1.mat ├── distributedd_cost0623v2.mat ├── distributedd_cost0623v3.mat ├── distributedd_cost0623v4.mat ├── distributedd_cost0623v5.mat ├── draw_fig1_new_cent.m ├── draw_fig1_new_one.m ├── plot_optimality.m ├── readme ├── robot.m ├── sys_process12.m └── sys_process21.m ├── DSLC_training ├── ANN_xProcess.m ├── ANN_xTrain.m ├── A_new.m ├── B_new.m ├── Barriera.m ├── Barriera_new.m ├── Barrierx.m ├── CNN_xProcess.m ├── CNN_xTrain.m ├── CreateANN1.m ├── CreateANN_x.m ├── CreateCNN1.m ├── CreateCNN_x.m ├── DSLC_two_robots_verification.m ├── DSLC_two_robots_verification_obstacle.m ├── Initial_state_calculation.m ├── Initial_state_calculation_no_obs.m ├── K_two.mat ├── NNProcess.m ├── NNProcess1.m ├── NNTrain.m ├── NNTrain1.m ├── P_VALUE_two.mat ├── README.md ├── T_1.m ├── T_42a.m ├── T_42b.m ├── calc_T.m ├── cost_LQR1_verification.mat ├── createNetworks.m ├── desired_position.m ├── draw_fig1_new.m ├── draw_fig1_new_no_obs.m ├── read me.txt ├── robot.m ├── safe_actor_critic.mat ├── set_constants.m ├── set_constants_no_obs.m ├── sys_process12.m ├── sys_process21.m └── yz.m ├── DSLC_xtdrone18 ├── avoid.py ├── draw_figure.py ├── follower.py ├── follower_consensus_promotion.py ├── formation_dict.py ├── formation_dict.pyc ├── get_local_pose.py ├── leader.py ├── multi_vehicle.launch ├── multi_vehicle_communication.sh ├── multirotor_communication_sigle.py ├── multirotor_keyboard_control_promotion.py ├── run_formation_promotion.sh └── usebook ├── DSLC_xtdrone6 ├── TEST ├── avoid.py ├── draw_figure.py ├── follower_consensus_baseline.py ├── follower_consensus_promotion.py ├── formation_dict.py ├── formation_dict.pyc ├── get_local_pose.py ├── leader.py ├── multi_vehicle.launch ├── multi_vehicle_communication.sh ├── multirotor_communication_sigle.py ├── multirotor_keyboard_control_promotion.py ├── run_formation_baseline.sh ├── run_formation_promotion.sh └── usebook ├── LICENSE ├── README.md ├── dlpc_online_train_scales_to_10000.m └── dlpc_online_train_scales_to_10000.py /.github/workflows/manual.yml: -------------------------------------------------------------------------------- 1 | # Workflow to ensure whenever a Github PR is submitted, 2 | # a JIRA ticket gets created automatically. 3 | name: Manual Workflow 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on pull request events but only for the master branch 8 | pull_request_target: 9 | types: [opened, reopened] 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | test-transition-issue: 16 | name: Convert Github Issue to Jira Issue 17 | runs-on: ubuntu-latest 18 | steps: 19 | - name: Checkout 20 | uses: actions/checkout@master 21 | 22 | - name: Login 23 | uses: atlassian/gajira-login@master 24 | env: 25 | JIRA_BASE_URL: ${{ secrets.JIRA_BASE_URL }} 26 | JIRA_USER_EMAIL: ${{ secrets.JIRA_USER_EMAIL }} 27 | JIRA_API_TOKEN: ${{ secrets.JIRA_API_TOKEN }} 28 | 29 | - name: Create NEW JIRA ticket 30 | id: create 31 | uses: atlassian/gajira-create@master 32 | with: 33 | project: CONUPDATE 34 | issuetype: Task 35 | summary: | 36 | Github PR [Assign the ND component] | Repo: ${{ github.repository }} | PR# ${{github.event.number}} 37 | description: | 38 | Repo link: https://github.com/${{ github.repository }} 39 | PR no. ${{ github.event.pull_request.number }} 40 | PR title: ${{ github.event.pull_request.title }} 41 | PR description: ${{ github.event.pull_request.description }} 42 | In addition, please resolve other issues, if any. 43 | fields: '{"components": [{"name":"Github PR"}], "customfield_16449":"https://classroom.udacity.com/", "customfield_16450":"Resolve the PR", "labels": ["github"], "priority":{"id": "4"}}' 44 | 45 | - name: Log created issue 46 | run: echo "Issue ${{ steps.create.outputs.issue }} was created" 47 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Mac OS 2 | .DS_Store 3 | 4 | # Byte-compiled / optimized / DLL files 5 | __pycache__/ 6 | *.py[cod] 7 | *$py.class 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | .Python 14 | env/ 15 | build/ 16 | develop-eggs/ 17 | dist/ 18 | downloads/ 19 | eggs/ 20 | .eggs/ 21 | lib/ 22 | lib64/ 23 | parts/ 24 | sdist/ 25 | var/ 26 | wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | 31 | # PyInstaller 32 | # Usually these files are written by a python script from a template 33 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 34 | *.manifest 35 | *.spec 36 | 37 | # Installer logs 38 | pip-log.txt 39 | pip-delete-this-directory.txt 40 | 41 | # Unit test / coverage reports 42 | htmlcov/ 43 | .tox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | .hypothesis/ 51 | 52 | # Translations 53 | *.mo 54 | *.pot 55 | 56 | # Django stuff: 57 | *.log 58 | local_settings.py 59 | 60 | # Flask stuff: 61 | instance/ 62 | .webassets-cache 63 | 64 | # Scrapy stuff: 65 | .scrapy 66 | 67 | # Sphinx documentation 68 | docs/_build/ 69 | 70 | # PyBuilder 71 | target/ 72 | 73 | # Jupyter Notebook 74 | .ipynb_checkpoints 75 | 76 | # pyenv 77 | .python-version 78 | 79 | # celery beat schedule file 80 | celerybeat-schedule 81 | 82 | # SageMath parsed files 83 | *.sage.py 84 | 85 | # dotenv 86 | .env 87 | 88 | # virtualenv 89 | .venv 90 | venv/ 91 | ENV/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @xinglongzhangnudt 2 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/ANN_xProcess.m: -------------------------------------------------------------------------------- 1 | function NN=ANN_xProcess(NN,input) 2 | NN.NetworkIn=input; 3 | NN.NetworkOut=NN.W1*NN.NetworkIn; 4 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/ANN_xTrain.m: -------------------------------------------------------------------------------- 1 | function NN=ANN_xTrain(NN,NNError) 2 | Delta2=NNError; 3 | dB1=Delta2; 4 | dW1=Delta2*NN.NetworkIn'; 5 | NN.W1=NN.W1-NN.LR*dW1; 6 | 7 | 8 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/A_new.m: -------------------------------------------------------------------------------- 1 | function [B_u]=A_new(Umaxh,Umaxl,U) 2 | MaxControl_1=Umaxh(1); 3 | MinControl_1=Umaxl(1); 4 | MaxControl_2=Umaxh(2); 5 | MinControl_2=Umaxl(2); 6 | AproU1=U(1); AproU2=U(2); 7 | offset1=0.1;offset2=0.1; 8 | AproU1=0;AproU2=0; 9 | 10 | if (AproU1>MinControl_1+offset1 & AproU1=MaxControl_1-offset1) 14 | up=MaxControl_1-offset1; 15 | k1=(MaxControl_1-up)^(-2)-(up-MinControl_1)^(-2); 16 | k2=1/(MaxControl_1-up)-1/(up-MinControl_1); 17 | b2=-log(MaxControl_1-up)-log(up-MinControl_1); 18 | B_u1=1/2*k1*(AproU1-up)^2+k2*(AproU1-up)+b2; 19 | else AproU1<=MinControl_1+offset1; 20 | down=MinControl_1+offset1; 21 | k1=(MaxControl_1-down)^(-2)-(down-MinControl_1)^(-2); 22 | k2=1/(MaxControl_1-down)-1/(down-MinControl_1); 23 | b2=-log(MaxControl_1-down)-log(down-MinControl_1); 24 | B_u1=-1/2*k1*(down-AproU1)^2-k2*(down-AproU1)+b2; 25 | end 26 | 27 | if (AproU2>MinControl_2+offset2 && AproU2=MaxControl_2-offset2) 31 | up=MaxControl_2-offset2; 32 | k1=(MaxControl_2-up)^(-2)-(up-MinControl_2)^(-2); 33 | k2=1/(MaxControl_2-up)-1/(up-MinControl_2); 34 | b2=-log(MaxControl_2-up)-log(up-MinControl_2); 35 | B_u2=1/2*k1*(AproU2-up)^2+k2*(AproU2-up)+b2; 36 | else(AproU2<=MinControl_2+offset2); 37 | down=MinControl_2+offset2; 38 | k1=(MaxControl_2-down)^(-2)-(down-MinControl_2)^(-2); 39 | k2=1/(MaxControl_2-down)-1/(down-MinControl_2); 40 | b2=-log(MaxControl_2-down)-log(down-MinControl_2); 41 | B_u2=-1/2*k1*(down-AproU2)^2-k2*(down-AproU2)+b2; 42 | end 43 | B_u=[B_u1+B_u2]; 44 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/B_new.m: -------------------------------------------------------------------------------- 1 | function [B_x]=B_new(PresentState,Obstacle) 2 | % global Data 3 | distance = sqrt((PresentState(1)-Obstacle(1))^2+(PresentState(2)-Obstacle(2))^2); 4 | MaxControl_1=0.5; 5 | offset1=0.05; %偏置 6 | %% 7 | B_x=-log(distance-1)+log(10); 8 | if (distance>MaxControl_1+offset1) 9 | B_x1=-log(distance-0.5)+log(10); 10 | elseif (distance<=MaxControl_1+offset1) 11 | up=MaxControl_1+offset1; 12 | k1=(up-0.5)^-2; 13 | k2=-1/(up-0.5); 14 | b2=-log(up-0.5)+log(10); 15 | B_x1=1/2*k1*(distance-up)^2+k2*(distance-up)+b2; 16 | end 17 | B_x=[B_x1]; 18 | if distance>10 19 | B_x=0; 20 | end 21 | end 22 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/Barriera.m: -------------------------------------------------------------------------------- 1 | function [B_u]=Barriera(Umaxh,Umaxl,U) 2 | MaxControl_1=Umaxh(1); 3 | MinControl_1=Umaxl(1); 4 | MaxControl_2=Umaxh(2); 5 | MinControl_2=Umaxl(2); 6 | AproU1=U(1); AproU2=U(2); 7 | 8 | g_1max=1/MaxControl_1; 9 | g_1min=1/MinControl_1; 10 | g_2max=1/MaxControl_2; 11 | g_2min=1/MinControl_2; 12 | 13 | offset1=0.1;offset2=0.1; 14 | if (AproU1>MinControl_1+offset1 & AproU1=MaxControl_1-offset1); 17 | up=MaxControl_1-offset1; 18 | k=(g_1max/(1-g_1max*up))^2+(g_1min/(1-g_1min*up))^2; 19 | b=g_1max/(1-g_1max*up)+g_1min/(1-g_1min*up)-g_1max-g_1min; 20 | B_u1=k*(AproU1-up)+b; 21 | else(AproU1<=MinControl_1+offset1); 22 | down=MinControl_1+offset1; 23 | k=((g_1max/(1-g_1max*down))^2+(g_1min/(1-g_1min*down))^2); 24 | b=(g_1max/(1-g_1max*down)+g_1min/(1-g_1min*down)-g_1max-g_1min); 25 | B_u1=k*(AproU1-down)+b; 26 | end 27 | if (AproU2>MinControl_2+offset2 & AproU2=MaxControl_2-offset2); 30 | up=MaxControl_2-offset2; 31 | k=(g_2max/(1-g_2max*up))^2+(g_2min/(1-g_2min*up))^2; 32 | b=g_2max/(1-g_2max*up)+g_2min/(1-g_2min*up)-g_2max-g_2min; 33 | B_u2=k*(AproU2-up)+b; 34 | else(AproU2<=MinControl_2+offset2); 35 | down=MinControl_2+offset2; 36 | k=((g_2max/(1-g_2max*down))^2+(g_2min/(1-g_2min*down))^2); 37 | b=(g_2max/(1-g_2max*down)+g_2min/(1-g_2min*down)-g_2max-g_2min); 38 | B_u2=k*(AproU2-down)+b; 39 | end 40 | B_u=[B_u1;B_u2]; 41 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/Barriera_new.m: -------------------------------------------------------------------------------- 1 | function [B_u]=Barriera_new(Umaxh,Umaxl,U) 2 | MaxControl_1=Umaxh(1); 3 | MinControl_1=Umaxl(1); 4 | AproU1=U(1); 5 | offset1=0.1; 6 | if (AproU1>MinControl_1+offset1 && AproU1=MaxControl_1-offset1) 9 | up=MaxControl_1-offset1; 10 | k=1/(MaxControl_1-up)^2-1/(MinControl_1-up)^2; 11 | b=1/(MaxControl_1-up)-1/(MinControl_1-up)-1/MaxControl_1+1/MinControl_1; 12 | B_u1=k*(AproU1-up)+b; 13 | else AproU1<=MinControl_1+offset1; 14 | down=MinControl_1+offset1; 15 | k=1/(MaxControl_1-down)^2-1/(MinControl_1-down)^2; 16 | b=1/(MaxControl_1-down)-1/(MinControl_1-down)-1/MaxControl_1+1/MinControl_1; 17 | B_u1=k*(AproU1-down)+b; 18 | end 19 | B_u=B_u1; 20 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/Barriera_new1.m: -------------------------------------------------------------------------------- 1 | function [B_u]=Barriera_new1(Umaxh,Umaxl,U) 2 | MaxControl_1=Umaxh(1); 3 | MinControl_1=Umaxl(1); 4 | MaxControl_2=Umaxh(2); 5 | MinControl_2=Umaxl(2); 6 | AproU1=U(1); AproU2=U(2); 7 | offset1=0.1;offset2=0.1; 8 | 9 | if (AproU1>MinControl_1+offset1 && AproU1=MaxControl_1-offset1) 12 | up=MaxControl_1-offset1; 13 | k=1/(MaxControl_1-up)^2+1/(MinControl_1-up)^2; 14 | b=1/(MaxControl_1-up)-1/(MinControl_1-up)-1/MaxControl_1-1/MinControl_1; 15 | B_u1=k*(AproU1-up)+b; 16 | else(AproU1<=MinControl_1+offset1); 17 | down=MinControl_1+offset1; 18 | k=1/(MaxControl_1-down)^2+1/(MinControl_1-down)^2; 19 | b=1/(MaxControl_1-down)-1/(MinControl_1-down)-1/MaxControl_1-1/MinControl_1; 20 | B_u1=k*(AproU1-down)+b; 21 | end 22 | if (AproU2>MinControl_2+offset2 && AproU2=MaxControl_2-offset2) 25 | up=1/(MaxControl_2-AproU2)-1/(MinControl_2-AproU2)-1/MaxControl_2-1/MinControl_2; 26 | k=1/(MaxControl_2-up)^2+1/(MinControl_2-up)^2; 27 | b=1/(MaxControl_2-up)-1/(MinControl_2-up)-1/MaxControl_2-1/MinControl_2; 28 | B_u2=k*(AproU2-up)+b; 29 | else(AproU2<=MinControl_2+offset2); 30 | down=MinControl_2+offset2; 31 | k=1/(MaxControl_2-down)^2+1/(MinControl_2-down)^2; 32 | b=1/(MaxControl_2-down)-1/(MinControl_2-down)-1/MaxControl_2-1/MinControl_2; 33 | B_u2=k*(AproU2-down)+b; 34 | end 35 | B_u=[B_u1;B_u2]; 36 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/Barriera_new_cen.m: -------------------------------------------------------------------------------- 1 | function [B_u]=Barriera_new_cen(Umaxh,Umaxl,U) 2 | MaxControl_1=Umaxh(1); 3 | MinControl_1=Umaxl(1); 4 | MaxControl_2=Umaxh(2); 5 | MinControl_2=Umaxl(2); 6 | AproU1=U(1); AproU2=U(2); 7 | offset1=0.1;offset2=0.1; 8 | if (AproU1>MinControl_1+offset1 && AproU1=MaxControl_1-offset1) 11 | up=MaxControl_1-offset1; 12 | k=1/(MaxControl_1-up)^2-1/(MinControl_1-up)^2; 13 | b=1/(MaxControl_1-up)-1/(MinControl_1-up)-1/MaxControl_1+1/MinControl_1; 14 | B_u1=k*(AproU1-up)+b; 15 | else AproU1<=MinControl_1+offset1; 16 | down=MinControl_1+offset1; 17 | k=1/(MaxControl_1-down)^2-1/(MinControl_1-down)^2; 18 | b=1/(MaxControl_1-down)-1/(MinControl_1-down)-1/MaxControl_1+1/MinControl_1; 19 | B_u1=k*(AproU1-down)+b; 20 | end 21 | if (AproU2>MinControl_2+offset2 && AproU2=MaxControl_2-offset2) 24 | up=MaxControl_2-offset2; 25 | k=1/(MaxControl_2-up)^2-1/(MinControl_2-up)^2; 26 | b=1/(MaxControl_2-up)-1/(MinControl_2-up)-1/MaxControl_2+1/MinControl_2; 27 | B_u2=k*(AproU2-up)+b; 28 | else(AproU2<=MinControl_2+offset2); 29 | down=MinControl_2+offset2; 30 | k=1/(MaxControl_2-down)^2-1/(MinControl_2-down)^2; 31 | b=1/(MaxControl_2-down)-1/(MinControl_2-down)-1/MaxControl_2+1/MinControl_2; 32 | B_u2=k*(AproU2-down)+b; 33 | end 34 | B_u=[B_u1;B_u2]; 35 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/CNN_xProcess.m: -------------------------------------------------------------------------------- 1 | function NN=CNN_xProcess(NN,input) 2 | NN.NetworkIn=input; 3 | NN.NetworkOut=NN.W1*NN.NetworkIn; 4 | NN.Jacobian=[NN.W1]; 5 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/CNN_xTrain.m: -------------------------------------------------------------------------------- 1 | function NN=CNN_XTrain(NN,NNError) 2 | Delta2=NNError; 3 | dB1=Delta2; 4 | dW1=Delta2*NN.NetworkIn'; 5 | NN.W1=NN.W1-NN.LR*dW1; 6 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/CreateANN1.m: -------------------------------------------------------------------------------- 1 | function ANN=CreateANN1(x_dim,u_dim) 2 | ANN.HiddenUnitNum=5; 3 | ANN.InDim=x_dim; 4 | ANN.OutDim=u_dim; 5 | ANN.LR=0.2; 6 | ANN.W1=1*(rand(ANN.OutDim,ANN.InDim)-0.5); 7 | ANN.B1=1*(1*rand(ANN.OutDim,1)-0.5); -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/CreateANN_x.m: -------------------------------------------------------------------------------- 1 | function ANN_x=CreateANN_x(x_dim,u_dim) 2 | ANN_x.InDim=x_dim; 3 | ANN_x.OutDim=u_dim; 4 | ANN_x.LR=0.000002; 5 | weight=0.1; 6 | ANN_x.W1=0.1*(rand(ANN_x.OutDim,ANN_x.InDim)-0.5); 7 | ANN_x.B1=0.0*(rand(ANN_x.OutDim,1)-0.5); -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/CreateCNN1.m: -------------------------------------------------------------------------------- 1 | function CNN=CreateCNN1(x_dim,y_dim) 2 | CNN.HiddenUnitNum=5; 3 | CNN.InDim=x_dim; 4 | CNN.OutDim=y_dim; 5 | CNN.LR=0.4; 6 | 7 | CNN.W1=1*(1*rand(CNN.OutDim,CNN.InDim)-0.5); 8 | CNN.B1=1*(1*rand(CNN.OutDim,1)-0.5); 9 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/CreateCNN_x.m: -------------------------------------------------------------------------------- 1 | function CNN=CreateCNN_x(x_dim,y_dim) 2 | CNN.InDim=x_dim; 3 | CNN.OutDim=y_dim; 4 | CNN.W1=0.1*(rand(CNN.OutDim,CNN.InDim)-0.5); 5 | CNN.B1=0.1*(rand(CNN.OutDim,1)-0.5); 6 | CNN.LR=0.00001; 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/DSLC_solving_centralized_main.m: -------------------------------------------------------------------------------- 1 | clear all;close all; 2 | global tau iter run; 3 | tau=0.05; 4 | Gamma=0.95; %%discount factor 5 | state_bound1=[2;2;2]; %X,Y,V 6 | state_bound2=[2;2]; %THETA 7 | Iterations_num=1600; 8 | MaxTrials=30; 9 | ConHor_len=1; %% Control Horizon length 10 | PreHor_len=20; %% Predictive Horizon length 11 | center_barrier=2.5; 12 | Obstacle=[3000;1]; 13 | %% Initialization 14 | ANN1=CreateANN1(5,1); 15 | ANN2=CreateANN1(2,1); 16 | ANN_u1=CreateANN_x(1,1); 17 | ANN_u2=CreateANN_x(1,1); 18 | ANN_x2=CreateANN_x(2,1); 19 | CNN1=CreateCNN1(5,5); 20 | CNN2=CreateCNN1(2,2); 21 | %% 22 | State=[0;0.9;1;0;0]; 23 | R_State=[0;1.0;1;0;0.2]; 24 | 25 | Thita=State(3); 26 | T=[cos(Thita),sin(Thita),0,0,0; 27 | -sin(Thita),cos(Thita),0,0,0; 28 | 0,0,1,0,0; 29 | 0,0,0,1,0; 30 | 0, 0,0,0,1]; 31 | NIError=T*(R_State-State); 32 | mu=0.02; 33 | Q1=1*eye(5); 34 | Q2=1*eye(2); 35 | R=0.1*eye(2); 36 | J=[];J_1=[];J_1d=[]; 37 | Umax1=[5];Umin1=[-5]; % enlarge the control constraint for collision avoidance 38 | Umax2=[5];Umin2=[-5]; % enlarge the control constraint for collision avoidance 39 | flag1=0; 40 | tic 41 | %% Main loop 42 | run=200; 43 | NIError0=NIError; 44 | State0=State; 45 | R_State0=R_State; 46 | Jc=zeros(run,Iterations_num); 47 | %% 48 | for iter=1:run 49 | disp(['run=',num2str(iter)]); 50 | NIError=NIError0; 51 | State=State0; 52 | R_State=R_State0; 53 | for k=1:ConHor_len:Iterations_num 54 | disp(['iteration=',num2str(k)]); 55 | RealError=NIError; 56 | RealState=State; 57 | Present_rx=R_State; 58 | f=1; 59 | %% Determine whether the learning is successful in the current prediction time domain 60 | while(f>=1) 61 | PresentError=RealError; 62 | PresentState=RealState; 63 | Present_x=Present_rx; 64 | Err=0; 65 | timesteps=0; j=0;ANN1d=[];ANN_u1d=[];ANN_x1d=[]; 66 | while(timestepsMaxTrials) 138 | f=1; 139 | end 140 | end 141 | end 142 | if flag1==1 143 | disp(['Training complete']); 144 | break 145 | end 146 | end 147 | toc 148 | JL=sum(Jc') -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/NNProcess.m: -------------------------------------------------------------------------------- 1 | function NN=NNProcess(NN,input) 2 | 3 | NN.NetworkIn=input; 4 | NN.HiddenOut=logsig(NN.W1*NN.NetworkIn+NN.B1); 5 | NN.NetworkOut=NN.W2*NN.HiddenOut+NN.B2; 6 | NN.Jacobian=NN.W2*((NN.HiddenOut).*(1-NN.HiddenOut)*ones(1,NN.InDim).*NN.W1); -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/NNProcess1.m: -------------------------------------------------------------------------------- 1 | function NN=NNProcess1(NN,input) 2 | NN.NetworkIn=input; 3 | NN.NetworkOut=NN.W1*tanh(NN.NetworkIn)+NN.B1; 4 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/NNTrain.m: -------------------------------------------------------------------------------- 1 | function NN=NNTrain(NN,NNError) 2 | 3 | Delta2=NNError; 4 | Delta1=NN.W2'*Delta2.*(NN.HiddenOut).*(1-NN.HiddenOut); 5 | 6 | dW2=Delta2*NN.HiddenOut'; 7 | dB2=Delta2; 8 | dW1=Delta1*NN.NetworkIn'; 9 | dB1=Delta1; 10 | 11 | NN.W1=NN.W1-NN.LR*dW1; 12 | NN.B1=NN.B1-NN.LR*dB1; 13 | NN.W2=NN.W2-NN.LR*dW2; 14 | NN.B2=NN.B2-NN.LR*dB2; 15 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/NNTrain1.m: -------------------------------------------------------------------------------- 1 | function NN=NNTrain1(NN,NNError) 2 | Delta2=NNError; 3 | dB1=Delta2; 4 | dW1=Delta2*tanh(NN.NetworkIn)'; 5 | NN.W1=NN.W1-NN.LR*dW1; 6 | NN.B1=NN.B1-NN.LR*dB1; -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/README.md: -------------------------------------------------------------------------------- 1 | # DLPC for distributedly solving centralized control: problem -- Policy Training 2 | 3 | ### Introduction 4 | 5 | For this project, you will run the codes in the Matlab environment. The codes verify the optimality gap to the centralized version by solving one robot's centralized path-following control problem distributedly. 6 | 7 | ### Running the code 8 | 9 | - Run `DSLC_solving_centralized_main` to solve the one robot's path-following problem by partitioning the robot model (input-state: 2-5) into two subsystems (1-2 and 1-3). 10 | - Run `centralized_version_main` to generate the results of the centralized version for comparison. 11 | - Run `plot_optimality` to display the results. 12 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/centralized_cost0623v0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/centralized_cost0623v0.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/centralized_cost0623v1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/centralized_cost0623v1.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/centralized_cost0623v2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/centralized_cost0623v2.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/centralized_cost0623v3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/centralized_cost0623v3.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/centralized_cost0623v4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/centralized_cost0623v4.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/centralized_cost0623v5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/centralized_cost0623v5.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/centralized_version_main.m: -------------------------------------------------------------------------------- 1 | clear all;close all; 2 | global tau Data iter run; 3 | tau=0.05; 4 | Gamma=0.95; %% discount factor 5 | state_bound=[2;2;2;2;2]; %X,Y,V theta,w 6 | Iterations_num=1600; 7 | MaxTrials=30; 8 | ConHor_len=1; %% Control Horizon length 9 | PreHor_len=20; %% Predictive Horizon length 10 | center_barrier=2.5; 11 | Obstacle=[3000;1];% no collision 12 | Data=circle_func(center_barrier,0.1,Obstacle); 13 | %% Initialization 14 | % load safe_actor_critic.mat; 15 | ANN=CreateANN1(5,2); 16 | ANN_u=CreateANN_x(2,2); 17 | CNN=CreateCNN1(5,5); 18 | %% 19 | State=[0;0.9;1;0;0] ; 20 | R_State=[0;1.0;1;0;0.2]; 21 | 22 | Thita=State(3); 23 | T=[cos(Thita),sin(Thita),0,0,0; 24 | -sin(Thita),cos(Thita),0,0,0; 25 | 0,0,1,0,0; 26 | 0,0,0,1,0; 27 | 0, 0,0,0,1]; 28 | 29 | NIError=T*(R_State-State); 30 | mu=0.02; 31 | Q1=1*eye(5); 32 | R=0.1*eye(2); 33 | Q1(4,4)=2; 34 | Q1(5,5)=2; 35 | J=[];J_1=[];J_1d=[]; 36 | Umax1=[5;5];Umin1=[-5;5]; % enlarge the control constraint to for collision avoidance 37 | Umax2=[5];Umin2=[-5]; % enlarge the control constraint to for collision avoidance 38 | OO=zeros(4,4); 39 | flag1=0; 40 | tic 41 | %% Main loop 42 | run=200; 43 | NIError0=NIError; 44 | State0=State; 45 | R_State0=R_State; 46 | J=zeros(run,Iterations_num); 47 | %% runing the iteration 48 | for iter=1:run 49 | disp(['run=',num2str(iter)]); 50 | NIError=NIError0; 51 | State=State0; 52 | R_State=R_State0; 53 | for k=1:ConHor_len:Iterations_num 54 | disp(['iteration=',num2str(k)]); 55 | RealError=NIError; 56 | RealState=State; 57 | Present_rx=R_State; 58 | f=1;Cost1=0;Cost1d=0; 59 | %% Determine whether the learning is successful in the current prediction time domain 60 | while(f>=1) 61 | PresentError=RealError; 62 | PresentState=RealState; 63 | Present_x=Present_rx; 64 | Err=0; 65 | timesteps=0; j=0;ANN1d=[];ANN_u1d=[];ANN_x1d=[]; 66 | while(timestepsMaxTrials) 117 | f=1; 118 | end 119 | end 120 | end 121 | if flag1==1 122 | disp(['Training complete']); 123 | break 124 | end 125 | end -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/circle_func.m: -------------------------------------------------------------------------------- 1 | function Data=circle_func(d,theta_sam,Obstacle) 2 | Data=[]; 3 | for theta=theta_sam:theta_sam:2*pi 4 | x=d*cos(theta)+Obstacle(1); 5 | y=d*sin(theta)+Obstacle(2); 6 | Data=[Data,[x;y]]; 7 | end -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/desired_position.m: -------------------------------------------------------------------------------- 1 | function Future_x=desired_position(x) 2 | tau=0.05; 3 | vr=1; 4 | wr=0.2; 5 | % v=x(3)+tau*u(1); 6 | % w=x(5)+tau*u(2); 7 | FutureState(1,1)=x(1)+tau*vr*cos(x(4));%+0.1*(rand(1)-0.5); 8 | FutureState(2,1)=x(2)+tau*vr*sin(x(4));%+0.1*(rand(1)-0.5); 9 | FutureState(3,1)=vr; 10 | FutureState(4,1)=x(4)+tau*wr; 11 | FutureState(5,1)=wr; 12 | if(FutureState(4,1)>pi) 13 | FutureState(4,1)=FutureState(4,1)-2*pi; 14 | elseif(FutureState(4,1)<-pi) 15 | FutureState(4,1)=FutureState(4,1)+2*pi; 16 | end 17 | 18 | Future_x=[FutureState(1,1);FutureState(2,1);FutureState(3,1);FutureState(4,1);FutureState(5,1)]; -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/distributedd_cost0623v0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/distributedd_cost0623v0.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/distributedd_cost0623v1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/distributedd_cost0623v1.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/distributedd_cost0623v2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/distributedd_cost0623v2.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/distributedd_cost0623v3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/distributedd_cost0623v3.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/distributedd_cost0623v4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/distributedd_cost0623v4.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/distributedd_cost0623v5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_solving_one_robot_control/distributedd_cost0623v5.mat -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/draw_fig1_new_cent.m: -------------------------------------------------------------------------------- 1 | function [NIError,State,R_State]=draw_fig1_new_cent(ANN,ANN_u,ConHor_len,state_bound,NIError,State,R_State,Obstacle,k,Iterations_num,Umax1,Umin1) 2 | global tau iter run; 3 | persistent E U X J j PresentState PE Present_x 4 | if(k==1&iter==1) 5 | J=zeros(run,Iterations_num); 6 | end 7 | if(k==1) 8 | E=zeros(5,Iterations_num); 9 | j=0; 10 | U=zeros(2,Iterations_num); 11 | X=zeros(5,Iterations_num); 12 | PresentState=[0;0.9;1;0;0]; 13 | PE=NIError; 14 | Present_x=[0;1;1;0;0.2]; 15 | end 16 | PresentError=NIError; 17 | timesteps=0; 18 | PresentState=State; 19 | Present_x=R_State; 20 | Umax=[5;5];Umin=[-5;-5]; 21 | while(timesteps=Iterations_num) 59 | % figure; 60 | % plot(1:size(E,2),E); 61 | % subplot(4,1,1); 62 | % plot(E1(1,:)); 63 | % subplot(4,1,2); 64 | % plot(E1(2,:)); 65 | % subplot(4,1,3); 66 | % plot(E1(3,:)); 67 | % subplot(4,1,4); 68 | % plot(E1(4,:)); 69 | % title('State error'); 70 | if iter==run 71 | pause(); 72 | end 73 | %% plot path 74 | % figure; 75 | % plot(X(1,:),X(2,:),'b'); 76 | % axis equal; 77 | %% 78 | % figure; 79 | % subplot(2,1,1); 80 | % plot(U1(1,:)); 81 | % subplot(2,1,2); 82 | % plot(U1(2,:)); 83 | % title('Control input'); 84 | end -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/draw_fig1_new_one.m: -------------------------------------------------------------------------------- 1 | function [NIError,State,R_State]=draw_fig1_new_one(ANN1,ANN2,ANN_u1,ANN_u2,ConHor_len,state_bound1,state_bound2,NIError,State,R_State,Obstacle,k,Iterations_num,Umax1,Umin1,Umax2,Umin2) 2 | global tau iter run; 3 | persistent E U X PresentState PE Present_x j J 4 | if(k==1&iter==1) 5 | J=zeros(run,Iterations_num); 6 | end 7 | if(k==1) 8 | E=zeros(5,Iterations_num); 9 | j=0; 10 | U=zeros(2,Iterations_num); 11 | X=zeros(5,Iterations_num); 12 | Y=zeros(3,Iterations_num); 13 | PresentState=[0;0.9;1;0;0]; 14 | PE=NIError; 15 | Present_x=[0;1.0;1;0;0.2]; 16 | end 17 | PresentError=NIError; 18 | timesteps=0; 19 | PresentState=State; 20 | Present_x=R_State; 21 | Umax=[5;5];Umin=[-5;-5]; 22 | while(timesteps=Iterations_num) 65 | % figure; 66 | % subplot(4,1,1); 67 | % plot(1:size(E,2),E); 68 | % subplot(4,1,2); 69 | % plot(E1(2,:)); 70 | % subplot(4,1,3); 71 | % plot(E1(3,:)); 72 | % subplot(4,1,4); 73 | % plot(E1(4,:)); 74 | % title('State error'); 75 | %% plot path 76 | if iter==run 77 | pause(); 78 | end 79 | % figure; 80 | % plot(X(1,:),X(2,:),'b'); 81 | % axis equal; 82 | %% 83 | % figure; 84 | % subplot(2,1,1); 85 | % plot(U1(1,:)); 86 | % subplot(2,1,2); 87 | % plot(U1(2,:)); 88 | % title('Control input'); 89 | end -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/plot_optimality.m: -------------------------------------------------------------------------------- 1 | clear 2 | load distributedd_cost0623v0.mat %v0,...v5 for different initial conditions 3 | Jd=J; 4 | Ud=U; 5 | Ed=E; 6 | load centralized_cost0623v0.mat %v0,...v5 for different initial conditions 7 | figure 8 | subplot(2,1,1) 9 | plot(tau:tau:tau*size(Ud,2),Ud(1,:),'b-','LineWidth',3) 10 | hold on 11 | plot(tau:tau:tau*size(Ud,2),U(1,:),'-.','Color',[0.85,0.33,0.1],'LineWidth',3) 12 | axis([0,tau*200,-0.2,4]) 13 | title('$a_v$','FontName', 'Times New Roman','Interpreter','latex'); 14 | set(gca,'FontName','Times New Roman','FontSize',16,'LineWidth',1.5); 15 | 16 | subplot(2,1,2) 17 | plot(tau:tau:tau*size(Ud,2),Ud(2,:),'b-','LineWidth',3) 18 | hold on 19 | plot(tau:tau:tau*size(Ud,2),U(2,:),'r-.','LineWidth',3) 20 | axis([0,tau*200,-0.2,1]) 21 | title('$a_w$','FontName', 'Times New Roman','Interpreter','latex'); 22 | set(gca,'FontName','Times New Roman','FontSize',16,'LineWidth',1.5); 23 | xlabel('Time (s)','FontName', 'Times New Roman'); 24 | 25 | figure 26 | subplot(4,2,1) 27 | plot(tau:tau:tau*size(Ed,2),Ed(1,:),'b-',tau:tau:tau*size(Ed,2),E(1,:),'r-.','LineWidth',3) 28 | axis([0,tau*500,0,0.3]) 29 | title('$e_{x}$','FontName', 'Times New Roman','Interpreter','latex'); 30 | set(gca,'FontName','Times New Roman','FontSize',16,'LineWidth',1.5); 31 | subplot(4,2,2) 32 | plot(tau:tau:tau*size(Ed,2),Ed(2,:),'b-',tau:tau:tau*size(Ed,2),E(2,:),'r-.','LineWidth',3) 33 | axis([0,tau*500,-0.05,0.1]) 34 | title('$e_{y}$','FontName', 'Times New Roman','Interpreter','latex'); 35 | set(gca,'FontName','Times New Roman','FontSize',16,'LineWidth',1.5); 36 | legend('Ours', 'Centralized','FontName','Times New Roman') 37 | % subplot(5,2,3) 38 | % plot(tau:tau:tau*size(Ed,2),Ed(3,:),'-',tau:tau:tau*size(Ed,2),E(3,:),'-.','LineWidth',3) 39 | subplot(4,2,3) 40 | plot(tau:tau:tau*size(Ed,2),Ed(4,:),'b-',tau:tau:tau*size(Ed,2),E(4,:),'r-.','LineWidth',3) 41 | axis([0,tau*500,0,0.05]) 42 | xlabel('Time (s)','FontName', 'Times New Roman'); 43 | title('$e_{\theta}$','FontName', 'Times New Roman','Interpreter','latex'); 44 | set(gca,'FontName','Times New Roman','FontSize',16,'LineWidth',1.5); 45 | subplot(4,2,4) 46 | plot(tau:tau:tau*size(Ed,2),Ed(4,:),'b-',tau:tau:tau*size(Ed,2),E(4,:),'r-.','LineWidth',3) 47 | axis([0,tau*500,0,0.05]) 48 | xlabel('Time (s)','FontName', 'Times New Roman'); 49 | title('$e_w$','FontName', 'Times New Roman','Interpreter','latex'); 50 | set(gca,'FontName','Times New Roman','FontSize',16,'LineWidth',1.5); 51 | % figure 52 | % plot(1:150,Jd(:,end),'b',1:150,J(:,end),'r') 53 | figure 54 | plot(1:120,((Jd(1:120,end)-J(1:120,end))/1600),'LineWidth',3, ... 55 | 'LineStyle','-','Color','blue') 56 | hold on 57 | plot(1:6:120,((Jd(1:6:120,end)-J(1:6:120,end))/1600),'.','LineWidth',5, ... 58 | 'MarkerSize',30,... 59 | 'MarkerEdgeColor','red',... 60 | 'MarkerFaceColor',[1 .6 .6]) 61 | grid; 62 | xlabel('Learning Episodes','FontName', 'Times New Roman'); 63 | ylabel('$\Delta J=J_{\rm our}-J_{c}$','FontName', 'Times New Roman','Interpreter','latex'); 64 | title('Cost gap to the centralized solution','fontsize',18,'Interpreter','latex','FontName', 'Times New Roman') 65 | set(gca,'FontName','Times New Roman','FontSize',18,'LineWidth',1.5); 66 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/readme: -------------------------------------------------------------------------------- 1 | xxxx 2 | -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/robot.m: -------------------------------------------------------------------------------- 1 | function FutureState=robot(x,u) 2 | global tau; 3 | % v=x(3)+tau*u(1); 4 | % w=x(5)+tau*u(2); 5 | FutureState(1,1)=x(1)+tau*x(3)*cos(x(4)); 6 | FutureState(2,1)=x(2)+tau*x(3)*sin(x(4)); 7 | FutureState(3,1)=x(3)+tau*u(1); 8 | FutureState(4,1)=x(4)+tau*x(5); 9 | FutureState(5,1)=x(5)+tau*u(2); 10 | if(FutureState(4,1)>pi) 11 | FutureState(4,1)=FutureState(4,1)-2*pi; 12 | elseif(FutureState(4,1)<-pi) 13 | FutureState(4,1)=FutureState(4,1)+2*pi; 14 | end -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/sys_process12.m: -------------------------------------------------------------------------------- 1 | function [MSJ,MCJ]=sys_process12(ep,u) 2 | global tau; 3 | ep1=ep(1); 4 | ep2=ep(2); 5 | ep3=ep(3); 6 | ep4=ep(4); 7 | ep5=ep(5); 8 | u1=u(1); 9 | u2=u(2); 10 | wr=0.08; 11 | vr=0.72; 12 | 13 | % 14 | % FutureState1=[ ep1 + tau*(ep2*(wr-ep5) -vr+ep3+ cos(ep4)*(vr)); 15 | % ep2 + tau*(sin(ep4)*(vr)-ep1*(wr-ep5)); 16 | % ep3 - tau*(u1); 17 | % ep4 + tau*ep5; 18 | % ep5 + tau*(-u2)]; 19 | MSJ = [1, tau*(wr-ep5),tau,-tau*sin(ep4)*vr,-tau*ep2; 20 | -(wr-ep5), 1, 0, tau*cos(ep4)*vr,tau*ep1; 21 | 0, 0, 1,0,0; 22 | 0,0,0,1,tau; 23 | 0 0 0 0 1]; 24 | MCJ =[0, 0; 25 | 0, 0; 26 | -tau, 0; 27 | 0,0; 28 | 0, -tau]; -------------------------------------------------------------------------------- /DSLC_solving_one_robot_control/sys_process21.m: -------------------------------------------------------------------------------- 1 | function [MSJ2,MCJ2,MSJ21]=sys_process21(ep,u,PresentState1,PresentState2) 2 | global tau; 3 | ep1=ep(1); 4 | ep2=ep(2); 5 | ep3=ep(3); 6 | ep4=ep(4); 7 | u1=u(1); 8 | u2=u(2); 9 | wr=0; 10 | vr=1; 11 | v1=PresentState1(4); 12 | thita2=PresentState2(3); 13 | thita1=PresentState1(3); 14 | MSJ2 = [ 1, tau*u2, -tau*sin(ep3)*vr-tau*sin(thita1-thita2)*(v1),2*tau; 15 | -tau*u2, 1, tau*cos(ep3)*vr+tau*cos(thita1-thita2)*(v1),0; 16 | 0, 0, 1,0; 17 | 0,0,0,1]; 18 | MCJ2 =[ 0, ep2*tau; 19 | 0, -ep1*tau; 20 | 0, -tau; 21 | -tau,0]; 22 | MSJ21 = [0, 0, tau*sin(thita1-thita2)*(v1),-tau*cos(thita1-thita2); 23 | 0, 0, -tau*cos(thita1-thita2)*(v1),-tau*sin(thita1-thita2); 24 | 0,0, 0,0; 25 | 0,0,0,0]; -------------------------------------------------------------------------------- /DSLC_training/ANN_xProcess.m: -------------------------------------------------------------------------------- 1 | function NN=ANN_xProcess(NN,input) 2 | NN.NetworkIn=input; 3 | NN.NetworkOut=NN.W1*NN.NetworkIn; 4 | -------------------------------------------------------------------------------- /DSLC_training/ANN_xTrain.m: -------------------------------------------------------------------------------- 1 | function NN=ANN_xTrain(NN,NNError) 2 | Delta2=NNError; 3 | dB1=Delta2; 4 | dW1=Delta2*NN.NetworkIn'; 5 | NN.W1=NN.W1-NN.LR*dW1; 6 | 7 | 8 | -------------------------------------------------------------------------------- /DSLC_training/A_new.m: -------------------------------------------------------------------------------- 1 | function [B_u]=A_new(Umaxh,Umaxl,U) 2 | MaxControl_1=Umaxh(1); 3 | MinControl_1=Umaxl(1); 4 | MaxControl_2=Umaxh(2); 5 | MinControl_2=Umaxl(2); 6 | AproU1=U(1); AproU2=U(2); 7 | offset1=0.1;offset2=0.1; 8 | AproU1=0;AproU2=0; 9 | 10 | if (AproU1>MinControl_1+offset1 & AproU1=MaxControl_1-offset1) 14 | up=MaxControl_1-offset1; 15 | k1=(MaxControl_1-up)^(-2)-(up-MinControl_1)^(-2); 16 | k2=1/(MaxControl_1-up)-1/(up-MinControl_1); 17 | b2=-log(MaxControl_1-up)-log(up-MinControl_1); 18 | B_u1=1/2*k1*(AproU1-up)^2+k2*(AproU1-up)+b2; 19 | else AproU1<=MinControl_1+offset1; 20 | down=MinControl_1+offset1; 21 | k1=(MaxControl_1-down)^(-2)-(down-MinControl_1)^(-2); 22 | k2=1/(MaxControl_1-down)-1/(down-MinControl_1); 23 | b2=-log(MaxControl_1-down)-log(down-MinControl_1); 24 | B_u1=-1/2*k1*(down-AproU1)^2-k2*(down-AproU1)+b2; 25 | end 26 | 27 | if (AproU2>MinControl_2+offset2 && AproU2=MaxControl_2-offset2) 31 | up=MaxControl_2-offset2; 32 | k1=(MaxControl_2-up)^(-2)-(up-MinControl_2)^(-2); 33 | k2=1/(MaxControl_2-up)-1/(up-MinControl_2); 34 | b2=-log(MaxControl_2-up)-log(up-MinControl_2); 35 | B_u2=1/2*k1*(AproU2-up)^2+k2*(AproU2-up)+b2; 36 | else(AproU2<=MinControl_2+offset2); 37 | down=MinControl_2+offset2; 38 | k1=(MaxControl_2-down)^(-2)-(down-MinControl_2)^(-2); 39 | k2=1/(MaxControl_2-down)-1/(down-MinControl_2); 40 | b2=-log(MaxControl_2-down)-log(down-MinControl_2); 41 | B_u2=-1/2*k1*(down-AproU2)^2-k2*(down-AproU2)+b2; 42 | end 43 | B_u=[B_u1+B_u2]; 44 | -------------------------------------------------------------------------------- /DSLC_training/B_new.m: -------------------------------------------------------------------------------- 1 | function [B_x]=B_new(PresentState,Obstacle) 2 | % global Data 3 | distance = sqrt((PresentState(1)-Obstacle(1))^2+(PresentState(2)-Obstacle(2))^2); 4 | MaxControl_1=0.5; 5 | offset1=0.05; %偏置 6 | %% 7 | B_x=-log(distance-1)+log(10); 8 | if (distance>MaxControl_1+offset1) 9 | B_x1=-log(distance-0.5)+log(10); 10 | elseif (distance<=MaxControl_1+offset1) 11 | up=MaxControl_1+offset1; 12 | k1=(up-0.5)^-2; 13 | k2=-1/(up-0.5); 14 | b2=-log(up-0.5)+log(10); 15 | B_x1=1/2*k1*(distance-up)^2+k2*(distance-up)+b2; 16 | B_x=[B_x1]; 17 | end 18 | if distance>10 19 | B_x=0; 20 | end 21 | end 22 | -------------------------------------------------------------------------------- /DSLC_training/Barriera.m: -------------------------------------------------------------------------------- 1 | function [B_u]=Barriera(Umaxh,Umaxl,U) 2 | MaxControl_1=Umaxh(1); 3 | MinControl_1=Umaxl(1); 4 | MaxControl_2=Umaxh(2); 5 | MinControl_2=Umaxl(2); 6 | AproU1=U(1); AproU2=U(2); 7 | 8 | g_1max=1/MaxControl_1; 9 | g_1min=1/MinControl_1; 10 | g_2max=1/MaxControl_2; 11 | g_2min=1/MinControl_2; 12 | 13 | offset1=0.1;offset2=0.1; 14 | if (AproU1>MinControl_1+offset1 & AproU1=MaxControl_1-offset1); 17 | up=MaxControl_1-offset1; 18 | k=(g_1max/(1-g_1max*up))^2+(g_1min/(1-g_1min*up))^2; 19 | b=g_1max/(1-g_1max*up)+g_1min/(1-g_1min*up)-g_1max-g_1min; 20 | B_u1=k*(AproU1-up)+b; 21 | else(AproU1<=MinControl_1+offset1); 22 | down=MinControl_1+offset1; 23 | k=((g_1max/(1-g_1max*down))^2+(g_1min/(1-g_1min*down))^2); 24 | b=(g_1max/(1-g_1max*down)+g_1min/(1-g_1min*down)-g_1max-g_1min); 25 | B_u1=k*(AproU1-down)+b; 26 | end 27 | if (AproU2>MinControl_2+offset2 & AproU2=MaxControl_2-offset2); 30 | up=MaxControl_2-offset2; 31 | k=(g_2max/(1-g_2max*up))^2+(g_2min/(1-g_2min*up))^2; 32 | b=g_2max/(1-g_2max*up)+g_2min/(1-g_2min*up)-g_2max-g_2min; 33 | B_u2=k*(AproU2-up)+b; 34 | else(AproU2<=MinControl_2+offset2); 35 | down=MinControl_2+offset2; 36 | k=((g_2max/(1-g_2max*down))^2+(g_2min/(1-g_2min*down))^2); 37 | b=(g_2max/(1-g_2max*down)+g_2min/(1-g_2min*down)-g_2max-g_2min); 38 | B_u2=k*(AproU2-down)+b; 39 | end 40 | B_u=[B_u1;B_u2]; 41 | -------------------------------------------------------------------------------- /DSLC_training/Barriera_new.m: -------------------------------------------------------------------------------- 1 | function [B_u]=Barriera_new(Umaxh,Umaxl,U) 2 | MaxControl_1=Umaxh(1); 3 | MinControl_1=Umaxl(1); 4 | MaxControl_2=Umaxh(2); 5 | MinControl_2=Umaxl(2); 6 | AproU1=U(1); AproU2=U(2); 7 | offset1=0.1;offset2=0.1; 8 | if (AproU1>MinControl_1+offset1 && AproU1=MaxControl_1-offset1) 11 | up=MaxControl_1-offset1; 12 | k=1/(MaxControl_1-up)^2-1/(MinControl_1-up)^2; 13 | b=1/(MaxControl_1-up)-1/(MinControl_1-up)-1/MaxControl_1+1/MinControl_1; 14 | B_u1=k*(AproU1-up)+b; 15 | else AproU1<=MinControl_1+offset1; 16 | down=MinControl_1+offset1; 17 | k=1/(MaxControl_1-down)^2-1/(MinControl_1-down)^2; 18 | b=1/(MaxControl_1-down)-1/(MinControl_1-down)-1/MaxControl_1+1/MinControl_1; 19 | B_u1=k*(AproU1-down)+b; 20 | end 21 | if (AproU2>MinControl_2+offset2 && AproU2=MaxControl_2-offset2) 24 | up=MaxControl_2-offset2; 25 | k=1/(MaxControl_2-up)^2-1/(MinControl_2-up)^2; 26 | b=1/(MaxControl_2-up)-1/(MinControl_2-up)-1/MaxControl_2+1/MinControl_2; 27 | B_u2=k*(AproU2-up)+b; 28 | else(AproU2<=MinControl_2+offset2); 29 | down=MinControl_2+offset2; 30 | k=1/(MaxControl_2-down)^2-1/(MinControl_2-down)^2; 31 | b=1/(MaxControl_2-down)-1/(MinControl_2-down)-1/MaxControl_2+1/MinControl_2; 32 | B_u2=k*(AproU2-down)+b; 33 | end 34 | B_u=[B_u1;B_u2]; 35 | -------------------------------------------------------------------------------- /DSLC_training/Barrierx.m: -------------------------------------------------------------------------------- 1 | function [B_x]=Barrierx(PresentState,Obstacle) 2 | global Data 3 | persistent pre1; 4 | persistent pre2; 5 | distance = sqrt((PresentState(1)-Obstacle(1))^2+(PresentState(2)-Obstacle(2))^2); 6 | MaxControl_1=1; 7 | MaxControl_2=1; 8 | g_1max=1/MaxControl_1; 9 | g_2max=1/MaxControl_2; 10 | 11 | offset1=0.1;offset2=0.1; 12 | % 13 | %% the recentered barrie function is -log(x^2+y^2-r^2)-log(R^2-r^2)+1/(x_c^2+y_c^2-r^2)*[2xc;2yc]'[x;y]; 14 | % d1=2.5; radius=1.5; 15 | % offset1=0.5;offset2=0.1; 16 | % posX=PresentState(1); 17 | % posY=PresentState(2); 18 | % [minindex,xc,yc]=find_minindex(Data,posX,posY); 19 | % if distance>d1 20 | % B_x=[0;0]; 21 | % elseif (distanceradius^2+(offset1)^2) 22 | % % pause(2); 23 | % B_x=(-1/(PresentState(1)^2+PresentState(2)^2-radius^2)*[2*PresentState(1);2*PresentState(2)]+1/(xc^2+yc^2-radius^2)*[2*xc;2*yc]); 24 | % elseif (distance^2<=(radius^2+(offset1)^2)) 25 | % B_x=-2/(radius^2+offset1^2)*[PresentState(1);PresentState(2)]; 26 | % end 27 | %% 28 | B_x1=(g_1max/(1-g_1max*distance))*abs(PresentState(1)-Obstacle(1))/distance; 29 | B_x2=(g_2max/(1-g_2max*distance))*abs(PresentState(2)-Obstacle(2))/distance; 30 | if(distance>MaxControl_1+offset1 & distanceMaxControl_1+offset1); 35 | B_x1=(g_1max/(1-g_1max*distance))*abs(PresentState(1)-Obstacle(1))/distance; 36 | elseif (distance<=MaxControl_1+offset1); 37 | up=MaxControl_1+offset1; 38 | %k=(g_1max/(1-g_1max*up))^2; 39 | k=g_1max*(-1)*(up-g_1max*up^2)^(-2)*(1-2*g_1max*up)*abs(pre1-Obstacle(1)); 40 | %k=g_1max*(-1)*(up-g_1max*up^2)^(-2)*(1-2*g_1max*up); 41 | b=(g_1max/(1-g_1max*up))*abs(PresentState(1)-Obstacle(1))/up; 42 | B_x1=k*(distance-up)+b; 43 | end 44 | 45 | if (distance>MaxControl_2+offset2); 46 | B_x2=(g_2max/(1-g_2max*distance))*abs(PresentState(2)-Obstacle(2))/distance; 47 | elseif (distance<=MaxControl_2+offset2); 48 | up=MaxControl_2+offset2; 49 | k=g_2max*(-1)*(up-g_2max*up^2)^(-2)*(1-2*g_2max*up)*abs(pre2-Obstacle(2)); 50 | %k=g_2max*(-1)*(up-g_2max*up^2)^(-2)*(1-2*g_2max*up); 51 | b=(g_2max/(1-g_2max*up))*abs(PresentState(2)-Obstacle(2))/up; 52 | B_x2=k*(distance-up)+b; 53 | end 54 | B_x=[B_x1;B_x2]; 55 | 56 | 57 | % distance = sqrt((PresentState(1)-Obstacle(1))^2+(PresentState(2)-Obstacle(2))^2); 58 | % MaxControl_1=1; 59 | % MaxControl_2=1; 60 | % g_1max=1/MaxControl_1; 61 | % g_2max=1/MaxControl_2; 62 | % offset1=0.1; 63 | % 64 | % if (distance>MaxControl_1+offset1); 65 | % B_x1=(g_1max/(1-g_1max*distance)); 66 | % elseif (distance<=MaxControl_1+offset1); 67 | % up=MaxControl_1+offset1; 68 | % %k=(up*(1-up)-abs(PresentState(1)-Obstacle(1))*(abs(PresentState(1)-Obstacle(1))/up-2*abs(PresentState(1)-Obstacle(1))))/(up*(1-up))^2; 69 | % %b=(g_1max/(1-g_1max*up))*abs(PresentState(1)-Obstacle(1))/up; 70 | % k=(g_1max/(1-g_1max*up))^2; 71 | % b=(g_1max/(1-g_1max*up)); 72 | % B_x1=k*(distance-up)+b; 73 | % end 74 | % 75 | % 76 | % B_x=[B_x1;B_x1]; 77 | -------------------------------------------------------------------------------- /DSLC_training/CNN_xProcess.m: -------------------------------------------------------------------------------- 1 | function NN=CNN_xProcess(NN,input) 2 | NN.NetworkIn=input; 3 | NN.NetworkOut=NN.W1*NN.NetworkIn; 4 | NN.Jacobian=[NN.W1]; 5 | -------------------------------------------------------------------------------- /DSLC_training/CNN_xTrain.m: -------------------------------------------------------------------------------- 1 | function NN=CNN_XTrain(NN,NNError) 2 | Delta2=NNError; 3 | dB1=Delta2; 4 | dW1=Delta2*NN.NetworkIn'; 5 | NN.W1=NN.W1-NN.LR*dW1; 6 | -------------------------------------------------------------------------------- /DSLC_training/CreateANN1.m: -------------------------------------------------------------------------------- 1 | function ANN=CreateANN1(x_dim,u_dim) 2 | ANN.HiddenUnitNum=5; 3 | ANN.InDim=x_dim; 4 | ANN.OutDim=u_dim; 5 | ANN.LR=0.2; 6 | ANN.W1=0.1*(rand(ANN.OutDim,ANN.InDim)-0.5); 7 | ANN.B1=0.1*(rand(ANN.OutDim,1)-0.5); -------------------------------------------------------------------------------- /DSLC_training/CreateANN_x.m: -------------------------------------------------------------------------------- 1 | function ANN_x=CreateANN_x(x_dim,u_dim) 2 | ANN_x.InDim=x_dim; 3 | ANN_x.OutDim=u_dim; 4 | ANN_x.LR=0.000002; 5 | weight=0.1; 6 | ANN_x.W1=[0.00,0.00;weight,weight]; 7 | ANN_x.B1=0.0*(rand(ANN_x.OutDim,1)-0.5); -------------------------------------------------------------------------------- /DSLC_training/CreateCNN1.m: -------------------------------------------------------------------------------- 1 | function CNN=CreateCNN1(x_dim,y_dim) 2 | CNN.HiddenUnitNum=5; 3 | CNN.InDim=x_dim; 4 | CNN.OutDim=y_dim; 5 | CNN.LR=0.4; 6 | CNN.W1=0.1*(rand(CNN.OutDim,CNN.InDim)-0.5); 7 | CNN.B1=0.1*(rand(CNN.OutDim,1)-0.5); 8 | -------------------------------------------------------------------------------- /DSLC_training/CreateCNN_x.m: -------------------------------------------------------------------------------- 1 | function CNN=CreateCNN_x(x_dim,y_dim) 2 | CNN.InDim=x_dim; 3 | CNN.OutDim=y_dim; 4 | CNN.W1=0.1*(rand(CNN.OutDim,CNN.InDim)-0.5); 5 | CNN.B1=0.1*(rand(CNN.OutDim,1)-0.5); 6 | CNN.LR=0.00001; 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /DSLC_training/DSLC_two_robots_verification.m: -------------------------------------------------------------------------------- 1 | clear all;close all; 2 | global tau iter; 3 | load P_VALUE_two.mat % Load the given .mat files 4 | load K_two.mat 5 | load cost_LQR1_verification.mat 6 | J_3d=J_1d; 7 | %% Initialization 8 | % Initialize constants, including control horizon, prediction horizon, barrier centers, obstacles, etc. 9 | [tau,Gamma,Iterations_num,MaxTrials,ConHor_len,PreHor_len,center_barrier,Obstacle,mu,Q,R,run,scale,Umax,Umin] = set_constants_no_obs(); 10 | agent=2; % Set agent count to 2 11 | [ANN,ANN_u,ANN_x,CNN,CNN_x,state_bound] = createNetworks(agent); % Create network structures for each agent 12 | [State,R_State,V,NIError] =Initial_state_calculation_no_obs; % Compute the initial state 13 | J=[];J_1=[];J_1d=[]; % Preallocate memory for performance measurement 14 | flag1=0; % Flag used to indicate if an obstacle is too close 15 | run=1; 16 | J=zeros(run,Iterations_num); 17 | const=0.1; 18 | scale=50; 19 | tic % Start timing the execution 20 | %% Main loop 21 | for iter=1:run 22 | disp(['run=',num2str(iter)]); 23 | Theorem4_1_robot{1}=[];Theorem4_2_robot{1}=[]; 24 | Theorem4_1_robot{2}=[];Theorem4_2_robot{2}=[]; 25 | for k=1:ConHor_len:Iterations_num % Loop over control horizon 26 | disp(['iteration=',num2str(k)]); % Display the current run number 27 | RealError=NIError; 28 | RealState=State; 29 | Present_rx=R_State; 30 | f=1;CosT{1}=0;CosT_d{1}=0; 31 | %% Determine whether the learning is successful in the current prediction time domain 32 | while(f>=1) 33 | PresentError=RealError; 34 | PresentState=RealState; 35 | Present_x=Present_rx; 36 | Err1=0;Err2=0; 37 | timesteps=0; j=0;ANN_d{1}=[];ANN_u_d{1}=[];ANN_x_d{1}=[]; 38 | while(timestepsMaxTrials) 227 | [ANN,ANN_u,ANN_x,CNN,CNN_x] = createNetworks(agent); 228 | f=1; 229 | end 230 | end 231 | end 232 | if flag1==1 233 | disp(['Training complete']); 234 | break 235 | end 236 | end 237 | toc % Stop timing the execution 238 | JL=sum(J') % Calculate the sum of the performance metric J 239 | 240 | figure % Plot the learning convergence condition verification 241 | plot(tau:tau:3600*tau,Theorem4_1_robot{1},tau:tau:3600*tau,Theorem4_2_robot{1},'linewidth',3); 242 | hold on 243 | xlabel('Time (s)','FontName', 'Times New Roman'); 244 | yline(1,'-','Threshold','FontName','Times New Roman','fontsize',20,'linewidth',3); 245 | title('Learning convergence condition verification','fontsize',20,'Interpreter','latex','FontName', 'Times New Roman') 246 | axis([0,70,-0.2,1.2]); 247 | set(gca,'FontName','Times New Roman','FontSize',20,'LineWidth',1.5); 248 | 249 | J_2d=[]; 250 | CosT_d{2}=J_1d(1); 251 | for iter=1:180 252 | CosT_d{2}=CosT_d{2}*0.995^(iter^0.6); 253 | J_2d=[J_2d,CosT_d{2}]; 254 | end 255 | 256 | figure % Plot the stability verification 257 | plot(tau:tau:180*tau,J_1d(end-179:end),tau:tau:180*tau,J_2d(end-179:end),tau:tau:180*tau,J_3d(end-179:end),tau:tau:180*tau,J_1(end-179:end),'linewidth',3); 258 | xlabel('Time (s)','FontName', 'Times New Roman'); 259 | ylabel('Cost $\bar V$','FontName', 'Times New Roman','Interpreter','latex'); 260 | title('Stability verification','fontsize',16,'Interpreter','latex','FontName', 'Times New Roman') 261 | set(gca,'FontName','Times New Roman','FontSize',18,'LineWidth',1.5); 262 | axis([0,6,0,100]); 263 | -------------------------------------------------------------------------------- /DSLC_training/DSLC_two_robots_verification_obstacle.m: -------------------------------------------------------------------------------- 1 | clear all;close all; 2 | global tau iter Bx1 Bx2; 3 | load P_VALUE_two.mat % Load the given .mat files 4 | load K_two.mat 5 | %% Initialization 6 | % Initialize constants, including control horizon, prediction horizon, barrier centers, obstacles, etc. 7 | [tau,Gamma,Iterations_num,MaxTrials,ConHor_len,PreHor_len,center_barrier,Obstacle,mu,Q,R,run,scale,Umax,Umin] = set_constants(); 8 | agent=2; % Set agent count to 2 9 | [ANN,ANN_u,ANN_x,CNN,CNN_x,state_bound] = createNetworks(agent); % Create network structures for each agent 10 | [State0,R_State0,V,NIError0] =Initial_state_calculation; % Compute the initial state 11 | J=zeros(run,Iterations_num);J_1=[];% Preallocate memory for performance measurement 12 | flag1=0; % Flag used to indicate if an obstacle is too close 13 | tic % Start timing the execution 14 | %% Main loop 15 | for iter=1:run 16 | NIError=NIError0; 17 | State=State0; 18 | R_State=R_State0; 19 | disp(['run=',num2str(iter)]); % Display the current run number 20 | Theorem4_1_roboT{1}=[];Theorem4_2_roboT{1}=[]; 21 | Theorem4_1_roboT{2}=[];Theorem4_2_roboT{2}=[]; 22 | for k=1:ConHor_len:Iterations_num % Loop over control horizon 23 | disp(['iteration=',num2str(k)]); % Display the current iteration number 24 | RealError=NIError; 25 | RealState=State; 26 | Present_rx=R_State; 27 | f=1;CosT{1}=0; 28 | %% Determine whether the learning is successful in the current prediction time domain 29 | while(f>=1) 30 | PresentError=RealError; 31 | PresentState=RealState; 32 | Present_x=Present_rx; 33 | timesteps=0; 34 | j=0; 35 | while(timestepsMaxTrials) 161 | [ANN,ANN_u,ANN_x,CNN,CNN_x] = createNetworks(agent); 162 | f=1; 163 | end 164 | end 165 | end 166 | % if flag1==1 % If an obstacle is too close, training is complete 167 | % disp(['Training complete']); 168 | % break 169 | % end 170 | end 171 | toc % Stop timing the execution 172 | JL=sum(J') % Calculate the sum of the performance metric J 173 | 174 | 175 | figure % Plot the learning convergence condition verification 176 | plot(tau:tau:3600*tau,Theorem4_1_roboT{1},tau:tau:3600*tau,Theorem4_2_roboT{1},'linewidth',3); 177 | hold on 178 | xlabel('Time (s)','FontName', 'Times New Roman'); 179 | yline(1,'-','Threshold','FontName','Times New Roman','fontsize',20,'linewidth',3); 180 | title('Learning convergence condition verification','fontsize',20,'Interpreter','latex','FontName', 'Times New Roman') 181 | axis([0,70,-0.2,1.2]) 182 | set(gca,'FontName','Times New Roman','FontSize',20,'LineWidth',1.5); 183 | 184 | -------------------------------------------------------------------------------- /DSLC_training/Initial_state_calculation.m: -------------------------------------------------------------------------------- 1 | function [State,R_State,V,NIError] =Initial_state_calculation(); 2 | State{1}=[0;1.0;0;1.0]; 3 | State{2}=[0;0;0;1.0]; 4 | R_State=[0;1.0;0;1.0]; 5 | Thita1=State{1}(3);Thita2=State{2}(3); 6 | T1 = calc_T(Thita1); 7 | T2 = calc_T(Thita2); 8 | V=diag([1,1,0,0]); 9 | NIError{1}=T1*((R_State-State{1})+(V*(State{2}-State{1})+[0;1;0;0])); 10 | NIError{2}=T2*((R_State-State{2}+[0;-1;0;0])+(V*(State{1}-State{2})+[0;-1;0;0])); -------------------------------------------------------------------------------- /DSLC_training/Initial_state_calculation_no_obs.m: -------------------------------------------------------------------------------- 1 | function [State,R_State,V,NIError] =Initial_state_calculation_no_obs(); 2 | State{1}=[0;1.5;0;1.5];% no collision 3 | State{2}=[0;0;0;1.5]; 4 | R_State=[0;1.5;0;1.5]; 5 | Thita1=State{1}(3);Thita2=State{2}(3); 6 | T1 = calc_T(Thita1); 7 | T2 = calc_T(Thita2); 8 | V=diag([1,1,0,0]); 9 | NIError{1}=T1*((R_State-State{1})+(V*(State{2}-State{1})+[0;1;0;0])); 10 | NIError{2}=T2*((R_State-State{2}+[0;-1;0;0])+(V*(State{1}-State{2})+[0;-1;0;0])); -------------------------------------------------------------------------------- /DSLC_training/K_two.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_training/K_two.mat -------------------------------------------------------------------------------- /DSLC_training/NNProcess.m: -------------------------------------------------------------------------------- 1 | function NN=NNProcess(NN,input) 2 | 3 | NN.NetworkIn=input; 4 | NN.HiddenOut=logsig(NN.W1*NN.NetworkIn+NN.B1); 5 | NN.NetworkOut=NN.W2*NN.HiddenOut+NN.B2; 6 | NN.Jacobian=NN.W2*((NN.HiddenOut).*(1-NN.HiddenOut)*ones(1,NN.InDim).*NN.W1); -------------------------------------------------------------------------------- /DSLC_training/NNProcess1.m: -------------------------------------------------------------------------------- 1 | function NN=NNProcess1(NN,input) 2 | NN.NetworkIn=input; 3 | NN.NetworkOut=NN.W1*tanh(NN.NetworkIn)+NN.B1; 4 | -------------------------------------------------------------------------------- /DSLC_training/NNTrain.m: -------------------------------------------------------------------------------- 1 | function NN=NNTrain(NN,NNError) 2 | 3 | Delta2=NNError; 4 | Delta1=NN.W2'*Delta2.*(NN.HiddenOut).*(1-NN.HiddenOut); 5 | 6 | dW2=Delta2*NN.HiddenOut'; 7 | dB2=Delta2; 8 | dW1=Delta1*NN.NetworkIn'; 9 | dB1=Delta1; 10 | 11 | NN.W1=NN.W1-NN.LR*dW1; 12 | NN.B1=NN.B1-NN.LR*dB1; 13 | NN.W2=NN.W2-NN.LR*dW2; 14 | NN.B2=NN.B2-NN.LR*dB2; 15 | -------------------------------------------------------------------------------- /DSLC_training/NNTrain1.m: -------------------------------------------------------------------------------- 1 | function NN=NNTrain1(NN,NNError) 2 | Delta2=NNError; 3 | dB1=Delta2; 4 | dW1=Delta2*tanh(NN.NetworkIn)'; 5 | NN.W1=NN.W1-NN.LR*dW1; 6 | NN.B1=NN.B1-NN.LR*dB1; -------------------------------------------------------------------------------- /DSLC_training/P_VALUE_two.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_training/P_VALUE_two.mat -------------------------------------------------------------------------------- /DSLC_training/README.md: -------------------------------------------------------------------------------- 1 | 2 | # Distributed Safe Learning Control -- Policy Training 3 | 4 | ### Introduction 5 | 6 | For this project, you will run the codes in the Matlab environment. The codes verify the convergence condition of actor-critic learning and the closed-stability condition under actor-critic learning in the receding horizon control framework. 7 | 8 | ### Running the code 9 | 10 | - Run `DSLC_two_robots_verification` to verify the convergence condition and the stability condition in the no-obstacle scenario. 11 | - Run `DSLC_two_robots_verification_obstacle` to verify the convergence condition in the obstacle avoidance scenario. 12 | 13 | -------------------------------------------------------------------------------- /DSLC_training/T_1.m: -------------------------------------------------------------------------------- 1 | function [Tau]=T_1(z,z_next,F,rate) 2 | Tau=rate*(norm(z,2)^2-2*norm(z'*z_next,2)*norm(F,2)+norm(z_next,2)^2+norm(F,2)^2); -------------------------------------------------------------------------------- /DSLC_training/T_42a.m: -------------------------------------------------------------------------------- 1 | function [Tau_42a]=T_42a(CNN1eta,CNN1eta_f,F1,CNN1_LR,dB_x1,dB_x1f,CNN_x1_LR) 2 | Tau_CNN1=CNN1_LR*(norm(CNN1eta,2)^2-2*norm(CNN1eta'*CNN1eta_f,2)*norm(F1,2)+norm(CNN1eta_f,2)^2+norm(F1,2)^2); 3 | Tau_B1=CNN_x1_LR*(norm(dB_x1,2)^2-2*norm(dB_x1'*dB_x1f,2)*norm(F1,2)+norm(dB_x1f,2)^2+norm(F1,2)^2); 4 | Tau_42a=Tau_CNN1+Tau_B1; -------------------------------------------------------------------------------- /DSLC_training/T_42b.m: -------------------------------------------------------------------------------- 1 | function [Tau_42b]=T_42b(R,ANN1_LR,ANN_x1_LR,ANN_u1_LR,ANN1eta,dB_x1,dB_u1) 2 | lamada_max=max(diag(R)); 3 | Tau_42b=lamada_max*(ANN1_LR*norm(ANN1eta,2)^2+ANN_x1_LR*norm(dB_x1,2)^2+ANN_u1_LR*norm(dB_u1,2)^2); -------------------------------------------------------------------------------- /DSLC_training/calc_T.m: -------------------------------------------------------------------------------- 1 | function T = calc_T(Thita) 2 | T=[cos(Thita),sin(Thita),0,0; 3 | -sin(Thita),cos(Thita),0,0; 4 | 0,0,1,0; 5 | 0,0,0,1]; 6 | end -------------------------------------------------------------------------------- /DSLC_training/cost_LQR1_verification.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_training/cost_LQR1_verification.mat -------------------------------------------------------------------------------- /DSLC_training/createNetworks.m: -------------------------------------------------------------------------------- 1 | function [ANN, ANN_u,ANN_x,CNN,CNN_x,state_bound] = createNetworks(agent) 2 | load safe_actor_critic.mat; 3 | for i = 1:agent 4 | ANN{i} = CreateANN1(8,2); 5 | ANN_u{i} = CreateANN_x(2,2); 6 | ANN_x{i} = CreateANN_x(2,2); 7 | CNN{i} = CreateCNN1(8,8); 8 | CNN_x{i} = CreateCNN_x(2,2); 9 | ANN_x{1}=ANN_x1;ANN_x{2}=ANN_x2; 10 | state_bound{1}=[2;2;2;2]; 11 | state_bound{2}=[2;2;2;2]; 12 | end 13 | end -------------------------------------------------------------------------------- /DSLC_training/desired_position.m: -------------------------------------------------------------------------------- 1 | function Future_x=desired_position(x) 2 | global tau; 3 | vr=1; 4 | wr=0; 5 | FutureState(1,1)=x(1)+tau*vr*cos(x(3)); 6 | FutureState(2,1)=x(2)+tau*vr*sin(x(3)); 7 | FutureState(3,1)=x(3)+tau*wr; 8 | FutureState(4,1)=vr; 9 | if(FutureState(3,1)>pi) 10 | FutureState(3,1)=FutureState(3,1)-2*pi; 11 | elseif(FutureState(3,1)<-pi) 12 | FutureState(3,1)=FutureState(3,1)+2*pi; 13 | end 14 | Future_x=[FutureState(1,1);FutureState(2,1);FutureState(3,1);FutureState(4,1)]; -------------------------------------------------------------------------------- /DSLC_training/draw_fig1_new.m: -------------------------------------------------------------------------------- 1 | function [NIError,State,R_State]=draw_fig1_new(ANN,ANN_x,ANN_u,ConHor_len,state_bound,NIError,State,R_State,Obstacle,k,Iterations_num) 2 | % This function performs the iteration and drawing operation for given parameters. 3 | % Inputs: ANN, ANN_x, ANN_u (networks for computation) 4 | % ConHor_len (control horizon length) 5 | % state_bound (state bounds) 6 | % NIError (initial error) 7 | % State (initial states) 8 | % R_State (desired state) 9 | % Obstacle (Obstacle positions) 10 | % k (current iteration number) 11 | % Iterations_num (total number of iterations) 12 | 13 | % Define some global and persistent variables used in the function 14 | global tau iter; 15 | persistent E1 U1 X1 E2 X2 PresentState PE Present_x 16 | 17 | % Initialization for the first iteration 18 | if(k==1) 19 | % Initialize arrays for Errors, Controls and States for the first iteration 20 | E1=zeros(4,Iterations_num); 21 | E2=zeros(4,Iterations_num); 22 | U1=zeros(2,Iterations_num); 23 | U2=zeros(2,Iterations_num); 24 | X1=zeros(4,Iterations_num); 25 | X2=zeros(4,Iterations_num); 26 | Y=zeros(3,Iterations_num); 27 | PresentState{1}=[0;0.9;0;1]; 28 | PresentState{2}=[0;-0.1;0;1]; 29 | PE=NIError; 30 | Present_x=[0;1;0;1]; 31 | end 32 | % Initialize some variables for the while loop 33 | V=diag([1,1,0,0]); 34 | PresentError=NIError; 35 | timesteps=0; 36 | PresentState=State; 37 | Present_x=R_State; 38 | Umax1=[5;5];Umin1=[-5;-5]; 39 | Umax2=[5;5];Umin2=[-5;-5]; 40 | 41 | % Loop until the desired control horizon length is reached 42 | while(timesteps30) 56 | dB_x1=[0;0];dB_x2=[0;0]; 57 | end 58 | ANN_u{1}=ANN_xProcess(ANN_u{1},dB_u1); 59 | ANN_u{2}=ANN_xProcess(ANN_u{2},dB_u2); 60 | ANN_x{1}=ANN_xProcess(ANN_x{1},dB_x1); 61 | ANN_x{2}=ANN_xProcess(ANN_x{2},dB_x2); 62 | u1=ANN{1}.NetworkOut+ANN_u{1}.NetworkOut+1*ANN_x{1}.NetworkOut; 63 | u2=ANN{2}.NetworkOut+ANN_u{2}.NetworkOut+1*ANN_x{2}.NetworkOut; 64 | 65 | % The FutureStates are obtained by applying the calculated controls to the current states. 66 | FutureState{1}=robot(PresentState{1},u1); 67 | FutureState{2}=robot(PresentState{2},u2); 68 | 69 | Future_x=desired_position(Present_x); % Future_x is the desired position for the next time step 70 | Thita1=FutureState{1}(3); 71 | Thita2=FutureState{2}(3); 72 | T1=calc_T(Thita1); 73 | T2 =calc_T(Thita2); 74 | 75 | % Calculate future error based on transformations, current and future states. 76 | FE{1}=T1*((Future_x-FutureState{1})+(V*(FutureState{2}-FutureState{1})+[0;1;0;0])); 77 | FE{2}=T2*((Future_x-FutureState{2}+[0;-1;0;0])+(V*(FutureState{1}-FutureState{2})+[0;-1;0;0])); 78 | 79 | FutureError=FE; 80 | E1(:,k+timesteps)=PE{1}; 81 | E2(:,k+timesteps)=PE{2}; 82 | X1(:,k+timesteps)=PresentState{1}; 83 | X2(:,k+timesteps)=PresentState{2}; 84 | U1(:,k+timesteps)=u1; 85 | U2(:,k+timesteps)=u2; 86 | PresentError=FutureError; 87 | PresentState=FutureState; 88 | Present_x=Future_x; 89 | PE=FE; 90 | timesteps=timesteps+1; 91 | end 92 | % Return the final Error, State, and R_State 93 | NIError=PE; 94 | State=PresentState; 95 | R_State=Present_x; 96 | 97 | % Plotting section 98 | if(k+timesteps-1>=Iterations_num) 99 | % Uncomment the following lines for visualizing the simulation 100 | % The first figure shows state errors 101 | % The second figure shows the trajectory of the robots 102 | % The third figure shows control inputs 103 | 104 | % figure; 105 | % subplot(4,1,1); 106 | % plot(E1(1,:)); 107 | % subplot(4,1,2); 108 | % plot(E1(2,:)); 109 | % subplot(4,1,3); 110 | % plot(E1(3,:)); 111 | % subplot(4,1,4); 112 | % plot(E1(4,:)); 113 | % title('State error'); 114 | %% plot path 115 | % figure; 116 | % plot(X1(1,:),X1(2,:),'b'); 117 | % hold on; 118 | % plot(X2(1,:),X2(2,:),'r'); 119 | % hold on; 120 | % rectangle('position',[Obstacle(1)-0.1,Obstacle(2)-0.1,0.1*2,0.1*2],'curvature',[1,1],'edgecolor','[0,0,0.5]','linestyle','-','facecolor','b'); 121 | % title('Trajectory'); 122 | %% 123 | % figure; 124 | % subplot(2,1,1); 125 | % plot(U1(1,:)); 126 | % subplot(2,1,2); 127 | % plot(U1(2,:)); 128 | %title('Control input'); 129 | end -------------------------------------------------------------------------------- /DSLC_training/draw_fig1_new_no_obs.m: -------------------------------------------------------------------------------- 1 | function [NIError,State,R_State]=draw_fig1_new_no_obs(ANN,ANN_x,ANN_u,ConHor_len,state_bound,NIError,State,R_State,Obstacle,k,Iterations_num) 2 | % This function performs the iteration and drawing operation for given parameters. 3 | % Inputs: ANN, ANN_x, ANN_u (networks for computation) 4 | % ConHor_len (control horizon length) 5 | % state_bound (state bounds) 6 | % NIError (initial error) 7 | % State (initial states) 8 | % R_State (desired state) 9 | % Obstacle (Obstacle positions) 10 | % k (current iteration number) 11 | % Iterations_num (total number of iterations) 12 | 13 | % Define some global and persistent variables used in the function 14 | global tau iter; 15 | persistent E1 U1 X1 E2 X2 PresentState PE Present_x 16 | 17 | % Initialization for the first iteration 18 | if(k==1) 19 | % Initialize arrays for Errors, Controls and States for the first iteration 20 | E1=zeros(4,Iterations_num); 21 | E2=zeros(4,Iterations_num); 22 | U1=zeros(2,Iterations_num); 23 | U2=zeros(2,Iterations_num); 24 | X1=zeros(4,Iterations_num); 25 | X2=zeros(4,Iterations_num); 26 | Y=zeros(3,Iterations_num); 27 | PresentState{1}=[0;0.9;0;1]; 28 | PresentState{2}=[0;-0.1;0;1]; 29 | PE=NIError; 30 | Present_x=[0;1;0;1]; 31 | end 32 | % Initialize some variables for the while loop 33 | V=diag([1,1,0,0]); 34 | PresentError=NIError; 35 | timesteps=0; 36 | PresentState=State; 37 | Present_x=R_State; 38 | Umax1=[5;5];Umin1=[-5;-5]; 39 | Umax2=[5;5];Umin2=[-5;-5]; 40 | 41 | % Loop until the desired control horizon length is reached 42 | while(timesteps30) 56 | dB_x1=[0;0];dB_x2=[0;0]; 57 | end 58 | ANN_u{1}=ANN_xProcess(ANN_u{1},dB_u1); 59 | ANN_u{2}=ANN_xProcess(ANN_u{2},dB_u2); 60 | ANN_x{1}=ANN_xProcess(ANN_x{1},dB_x1); 61 | ANN_x{2}=ANN_xProcess(ANN_x{2},dB_x2); 62 | u1=ANN{1}.NetworkOut+ANN_u{1}.NetworkOut+0*ANN_x{1}.NetworkOut; 63 | u2=ANN{2}.NetworkOut+ANN_u{2}.NetworkOut+0*ANN_x{2}.NetworkOut; 64 | 65 | % The FutureStates are obtained by applying the calculated controls to the current states. 66 | FutureState{1}=robot(PresentState{1},u1); 67 | FutureState{2}=robot(PresentState{2},u2); 68 | 69 | Future_x=desired_position(Present_x); % Future_x is the desired position for the next time step 70 | Thita1=FutureState{1}(3); 71 | Thita2=FutureState{2}(3); 72 | T1=calc_T(Thita1); 73 | T2 =calc_T(Thita2); 74 | 75 | % Calculate future error based on transformations, current and future states. 76 | FE{1}=T1*((Future_x-FutureState{1})+(V*(FutureState{2}-FutureState{1})+[0;1;0;0])); 77 | FE{2}=T2*((Future_x-FutureState{2}+[0;-1;0;0])+(V*(FutureState{1}-FutureState{2})+[0;-1;0;0])); 78 | 79 | FutureError=FE; 80 | E1(:,k+timesteps)=PE{1}; 81 | E2(:,k+timesteps)=PE{2}; 82 | X1(:,k+timesteps)=PresentState{1}; 83 | X2(:,k+timesteps)=PresentState{2}; 84 | U1(:,k+timesteps)=u1; 85 | U2(:,k+timesteps)=u2; 86 | PresentError=FutureError; 87 | PresentState=FutureState; 88 | Present_x=Future_x; 89 | PE=FE; 90 | timesteps=timesteps+1; 91 | end 92 | % Return the final Error, State, and R_State 93 | NIError=PE; 94 | State=PresentState; 95 | R_State=Present_x; 96 | 97 | % Plotting section 98 | if(k+timesteps-1>=Iterations_num) 99 | % Uncomment the following lines for visualizing the simulation 100 | % The first figure shows state errors 101 | % The second figure shows the trajectory of the robots 102 | % The third figure shows control inputs 103 | 104 | % figure; 105 | % subplot(4,1,1); 106 | % plot(E1(1,:)); 107 | % subplot(4,1,2); 108 | % plot(E1(2,:)); 109 | % subplot(4,1,3); 110 | % plot(E1(3,:)); 111 | % subplot(4,1,4); 112 | % plot(E1(4,:)); 113 | % title('State error'); 114 | %% plot path 115 | % figure; 116 | % plot(X1(1,:),X1(2,:),'b'); 117 | % hold on; 118 | % plot(X2(1,:),X2(2,:),'r'); 119 | % hold on; 120 | % %plot(Obstacle(1),Obstacle(2),'ok','MarkerFaceColor','k'); 121 | % rectangle('position',[Obstacle(1)-0.2,Obstacle(2)-0.2,0.2*2,0.2*2],'curvature',[1,1],'edgecolor','[0,0,0.5]','linestyle','-','facecolor','b'); 122 | % title('Trajectory'); 123 | %% 124 | % figure; 125 | % subplot(2,1,1); 126 | % plot(U1(1,:)); 127 | % subplot(2,1,2); 128 | % plot(U1(2,:)); 129 | %title('Control input'); 130 | end -------------------------------------------------------------------------------- /DSLC_training/read me.txt: -------------------------------------------------------------------------------- 1 | r_dhp_train_obstacle_2_new_v2 // main program, Learning with obstacles 2 | r_dhp_train_obstacle_2_new_v2_no_obstacle// main program, Learning without obstacles -------------------------------------------------------------------------------- /DSLC_training/robot.m: -------------------------------------------------------------------------------- 1 | function FutureState=robot(x,u) 2 | global tau; 3 | v=x(4)+tau*u(1); 4 | FutureState(1,1)=x(1)+tau*v*cos(x(3)); 5 | FutureState(2,1)=x(2)+tau*v*sin(x(3)); 6 | FutureState(3,1)=x(3)+tau*u(2); 7 | FutureState(4,1)=v; 8 | if(FutureState(3,1)>pi) 9 | FutureState(3,1)=FutureState(3,1)-2*pi; 10 | elseif(FutureState(3,1)<-pi) 11 | FutureState(3,1)=FutureState(3,1)+2*pi; 12 | end -------------------------------------------------------------------------------- /DSLC_training/safe_actor_critic.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_training/safe_actor_critic.mat -------------------------------------------------------------------------------- /DSLC_training/set_constants.m: -------------------------------------------------------------------------------- 1 | function [tau, Gamma, Iterations_num, MaxTrials, ConHor_len, PreHor_len, center_barrier, Obstacle,mu,Q,R,run,scale,Umax,Umin] = set_constants() 2 | tau = 0.05; Gamma = 0.95; % discount factor 3 | Iterations_num = 180; 4 | MaxTrials = 30; 5 | ConHor_len = 1; % Control Horizon length 6 | PreHor_len = 20; % Predictive Horizon length 7 | center_barrier = 2.5; 8 | Obstacle = [3;1]; 9 | mu=0.02; 10 | Q=1*eye(8); 11 | R=0.5*eye(2); 12 | run=10; 13 | scale=50; 14 | Umax{1}=[5;5];Umin{1}=[-5;-5]; % enlarge the control constraint to for collision avoidance 15 | Umax{2}=[5;5];Umin{2}=[-5;-5]; 16 | end -------------------------------------------------------------------------------- /DSLC_training/set_constants_no_obs.m: -------------------------------------------------------------------------------- 1 | function [tau, Gamma, Iterations_num, MaxTrials, ConHor_len, PreHor_len, center_barrier, Obstacle,mu,Q,R,run,scale,Umax,Umin] = set_constants_no_obs() 2 | tau = 0.05; Gamma = 0.95; % discount factor 3 | Iterations_num = 180; 4 | MaxTrials = 30; 5 | ConHor_len = 1; % Control Horizon length 6 | PreHor_len = 20; % Predictive Horizon length 7 | center_barrier = 2.5; 8 | Obstacle = [30;1]; % no collision 9 | mu=0.02; 10 | Q=1*eye(8); 11 | R=0.5*eye(2); 12 | run=1; 13 | scale=50; 14 | Umax{1}=[5;5];Umin{1}=[-5;-5]; % enlarge the control constraint to for collision avoidance 15 | Umax{2}=[5;5];Umin{2}=[-5;-5]; 16 | end -------------------------------------------------------------------------------- /DSLC_training/sys_process12.m: -------------------------------------------------------------------------------- 1 | function [MSJ1,MCJ1,MSJ12]=sys_process12(ep,u,PresentState1,PresentState2) 2 | global tau; 3 | ep1=ep(1); 4 | ep2=ep(2); 5 | ep3=ep(3); 6 | ep4=ep(4); 7 | u1=u(1); 8 | u2=u(2); 9 | wr=0; 10 | vr=1; 11 | v2=PresentState2(4); 12 | thita2=PresentState2(3); 13 | thita1=PresentState1(3); 14 | % 15 | % FutureState1=[ ep1 + tau*(ep2*u2 - 2*(vr-ep4) + cos(ep3)*(vr)+cos(thita2-thita1)*(v2)); 16 | % ep2 + tau*(sin(ep3)*(vr)-ep1*u2+sin(thita2-thita1)*(v2)); 17 | % ep3 + tau*(wr-u2); 18 | % ep4 - tau*(u1)]; 19 | MSJ1 = [1, tau*u2, -tau*sin(ep3)*vr-tau*sin(thita2-thita1)*(v2),2*tau; 20 | -tau*u2, 1, tau*cos(ep3)*vr+tau*cos(thita2-thita1)*(v2),0; 21 | 0, 0, 1,0; 22 | 0,0,0,1]; 23 | MCJ1 =[ 0, ep2*tau; 24 | 0, -ep1*tau; 25 | 0, -tau; 26 | -tau,0]; 27 | MSJ12 = [0, 0, tau*sin(thita2-thita1)*(v2),-tau*cos(thita2-thita1); 28 | 0, 0, -tau*cos(thita2-thita1)*(v2),-tau*sin(thita2-thita1); 29 | 0,0, 0,0; 30 | 0,0,0,0]; -------------------------------------------------------------------------------- /DSLC_training/sys_process21.m: -------------------------------------------------------------------------------- 1 | function [MSJ2,MCJ2,MSJ21]=sys_process21(ep,u,PresentState1,PresentState2) 2 | global tau; 3 | ep1=ep(1); 4 | ep2=ep(2); 5 | ep3=ep(3); 6 | ep4=ep(4); 7 | u1=u(1); 8 | u2=u(2); 9 | wr=0; 10 | vr=1; 11 | v1=PresentState1(4); 12 | thita2=PresentState2(3); 13 | thita1=PresentState1(3); 14 | MSJ2 = [ 1, tau*u2, -tau*sin(ep3)*vr-tau*sin(thita1-thita2)*(v1),2*tau; 15 | -tau*u2, 1, tau*cos(ep3)*vr+tau*cos(thita1-thita2)*(v1),0; 16 | 0, 0, 1,0; 17 | 0,0,0,1]; 18 | MCJ2 =[ 0, ep2*tau; 19 | 0, -ep1*tau; 20 | 0, -tau; 21 | -tau,0]; 22 | MSJ21 = [0, 0, tau*sin(thita1-thita2)*(v1),-tau*cos(thita1-thita2); 23 | 0, 0, -tau*cos(thita1-thita2)*(v1),-tau*sin(thita1-thita2); 24 | 0,0, 0,0; 25 | 0,0,0,0]; -------------------------------------------------------------------------------- /DSLC_training/yz.m: -------------------------------------------------------------------------------- 1 | clear all;close all; 2 | B=[];A=[]; 3 | for i=-10:0.01:20 4 | [B_x]=B_new([i;0],[10;0]); 5 | B=[B,B_x]; 6 | end 7 | plot(B); 8 | for i=-2:0.01:2 9 | [A_x]=A_new([1;1],[-1;-1],[i;i]); 10 | A=[A,A_x(1)]; 11 | end 12 | plot(A); 13 | 14 | -------------------------------------------------------------------------------- /DSLC_xtdrone18/avoid.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import Vector3, PoseStamped 3 | import time 4 | import numpy 5 | import sys 6 | 7 | vehicle_type = sys.argv[1] 8 | vehicle_num = int(sys.argv[2]) 9 | control_type = sys.argv[3] 10 | pose = [None]*vehicle_num 11 | pose_sub = [None]*vehicle_num 12 | avoid_control_pub = [None]*vehicle_num 13 | avoid_radius = 1.5 14 | aid_vec1 = [1, 0, 0] 15 | aid_vec2 = [0, 1, 0] 16 | vehicles_avoid_control = [Vector3()] * vehicle_num 17 | 18 | def pose_callback(msg, id): 19 | pose[id] = msg 20 | 21 | rospy.init_node('avoid') 22 | for i in range(vehicle_num): 23 | pose_sub[i] = rospy.Subscriber(vehicle_type+'_'+str(i)+'/mavros/local_position/pose',PoseStamped,pose_callback,i,queue_size=1) 24 | if control_type == "vel": 25 | avoid_control_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i)+"/avoid_vel", Vector3,queue_size=1) 26 | elif control_type == "accel": 27 | avoid_control_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i)+"/avoid_accel", Vector3,queue_size=1) 28 | else: 29 | print("Only vel and accel are supported.") 30 | 31 | time.sleep(1) 32 | rate = rospy.Rate(30) 33 | while not rospy.is_shutdown(): 34 | for i in range(vehicle_num): 35 | position1 = numpy.array([pose[i].pose.position.x, pose[i].pose.position.y, pose[i].pose.position.z]) 36 | for j in range(1, vehicle_num-i): 37 | position2 = numpy.array([pose[i+j].pose.position.x, pose[i+j].pose.position.y, pose[i+j].pose.position.z]) 38 | dir_vec = position1-position2 39 | k = 1 - numpy.linalg.norm(dir_vec) / avoid_radius 40 | if k > 0: 41 | cos1 = dir_vec.dot(aid_vec1)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec1)) 42 | cos2 = dir_vec.dot(aid_vec2)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec2)) 43 | if abs(cos1) < abs(cos2): 44 | avoid_control = k * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1)) 45 | else: 46 | avoid_control = k * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2)) 47 | 48 | vehicles_avoid_control[i] = Vector3(vehicles_avoid_control[i].x+avoid_control[0],vehicles_avoid_control[i].y+avoid_control[1],vehicles_avoid_control[i].z+avoid_control[2]) 49 | vehicles_avoid_control[i+j] = Vector3(vehicles_avoid_control[i+j].x-avoid_control[0],vehicles_avoid_control[i+j].y-avoid_control[1],vehicles_avoid_control[i+j].z-avoid_control[2]) 50 | 51 | for i in range(vehicle_num): 52 | avoid_control_pub[i].publish(vehicles_avoid_control[i]) 53 | vehicles_avoid_control = [Vector3()] * vehicle_num 54 | rate.sleep() -------------------------------------------------------------------------------- /DSLC_xtdrone18/follower.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: UTF-8 -*- 3 | 4 | import rospy 5 | from geometry_msgs.msg import Twist, Vector3, PoseStamped 6 | from std_msgs.msg import String, Float32MultiArray 7 | import sys 8 | import numpy 9 | 10 | class Follower: 11 | 12 | def __init__(self, uav_type, uav_id, uav_num): 13 | self.hover = "HOVER" 14 | self.uav_type = uav_type 15 | self.uav_num = uav_num 16 | self.id = uav_id 17 | self.f = 30 18 | self.pose = PoseStamped() 19 | self.cmd_vel_enu = Twist() 20 | self.avoid_vel = Vector3() 21 | self.formation_pattern = None 22 | self.Kp = 1.0 23 | self.Kp_avoid = 2.0 24 | self.vel_max = 1.0 25 | self.leader_pose = PoseStamped() 26 | 27 | self.pose_sub = rospy.Subscriber(self.uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped, self.pose_callback, queue_size=1) 28 | self.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+self.uav_type+'_'+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback, queue_size=1) 29 | self.formation_pattern_sub = rospy.Subscriber("/xtdrone/formation_pattern", Float32MultiArray, self.formation_pattern_callback, queue_size=1) 30 | 31 | self.vel_enu_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd_vel_enu', Twist, queue_size=1) 32 | self.info_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/info', String, queue_size=1) 33 | self.cmd_pub = rospy.Publisher('/xtdrone/'+self.uav_type+'_'+str(self.id)+'/cmd',String,queue_size=1) 34 | self.leader_pose_sub = rospy.Subscriber(self.uav_type+"_0/mavros/local_position/pose", PoseStamped, self.leader_pose_callback, queue_size=1) 35 | 36 | def formation_pattern_callback(self, msg): 37 | self.formation_pattern = numpy.array(msg.data).reshape(3, self.uav_num-1) 38 | 39 | def pose_callback(self, msg): 40 | self.pose = msg 41 | 42 | def leader_pose_callback(self, msg): 43 | self.leader_pose = msg 44 | 45 | def avoid_vel_callback(self, msg): 46 | self.avoid_vel = msg 47 | 48 | def loop(self): 49 | rospy.init_node('follower'+str(self.id-1)) 50 | rate = rospy.Rate(self.f) 51 | while not rospy.is_shutdown(): 52 | if (not self.formation_pattern is None): 53 | self.cmd_vel_enu.linear.x = self.Kp * ((self.leader_pose.pose.position.x + self.formation_pattern[0, self.id - 1]) - self.pose.pose.position.x) 54 | self.cmd_vel_enu.linear.y = self.Kp * ((self.leader_pose.pose.position.y + self.formation_pattern[1, self.id - 1]) - self.pose.pose.position.y) 55 | self.cmd_vel_enu.linear.z = self.Kp * ((self.leader_pose.pose.position.z + self.formation_pattern[2, self.id - 1]) - self.pose.pose.position.z) 56 | self.cmd_vel_enu.linear.x = self.cmd_vel_enu.linear.x + self.Kp_avoid * self.avoid_vel.x 57 | self.cmd_vel_enu.linear.y = self.cmd_vel_enu.linear.y + self.Kp_avoid * self.avoid_vel.y 58 | self.cmd_vel_enu.linear.z = self.cmd_vel_enu.linear.z + self.Kp_avoid * self.avoid_vel.z 59 | cmd_vel_magnitude = (self.cmd_vel_enu.linear.x**2 + self.cmd_vel_enu.linear.y**2 + self.cmd_vel_enu.linear.z**2)**0.5 60 | if cmd_vel_magnitude > 3**0.5 * self.vel_max: 61 | self.cmd_vel_enu.linear.x = self.cmd_vel_enu.linear.x / cmd_vel_magnitude * self.vel_max 62 | self.cmd_vel_enu.linear.y = self.cmd_vel_enu.linear.y / cmd_vel_magnitude * self.vel_max 63 | self.cmd_vel_enu.linear.z = self.cmd_vel_enu.linear.z / cmd_vel_magnitude * self.vel_max 64 | 65 | self.vel_enu_pub.publish(self.cmd_vel_enu) 66 | 67 | try: 68 | rate.sleep() # 69 | except: 70 | continue 71 | 72 | if __name__ == '__main__': 73 | follower = Follower(sys.argv[1],int(sys.argv[2]), int(sys.argv[3])) 74 | follower.loop() 75 | -------------------------------------------------------------------------------- /DSLC_xtdrone18/formation_dict.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | formation_dict_6 = {"origin":np.array([[3,0,0],[0,3,0],[3,3,0],[0,6,0],[3,6,0]]),"doubleorigin":np.array([[6,0,0],[0,6,0],[6,6,0],[0,12,0],[6,12,0]]),"T":np.array([[4,0,0],[2,0,0],[2,0,-2],[2,0,-4],[2,0,-6]]) , "diamond": np.array([[2,2,-2],[2,-2,-2],[-2,-2,-2],[-2,2,-2],[0,0,-4]]), "triangle": np.array([[-3,0,-3],[3,0,-3],[-1.5,0,-1.5],[1.5,0,-1.5],[0,0,-3]])} 4 | formation_dict_6["origin"] = np.transpose(formation_dict_6["origin"]) 5 | formation_dict_6["doubleorigin"]=np.transpose(formation_dict_6["doubleorigin"]) 6 | formation_dict_6["T"] = np.transpose(formation_dict_6["T"]) 7 | formation_dict_6["diamond"] = np.transpose(formation_dict_6["diamond"]) 8 | formation_dict_6["triangle"] = np.transpose(formation_dict_6["triangle"]) 9 | 10 | formation_dict_9 = {"origin":np.array([[3,0,0],[6,0,0],[0,3,0],[3,3,0],[6,3,0],[0,6,0],[3,6,0],[6,6,0]]), "cube": np.array([[2,2,2],[-2,2,2],[-2,-2,2],[2,-2,2],[2,2,-2],[-2,2,-2],[-2,-2,-2],[2,-2,-2]]), "pyramid": np.array([[2,2,-2],[-2,2,-2],[-2,-2,-2],[2,-2,-2],[4,4,-4],[-4,4,-4],[-4,-4,-4],[4,-4,-4]]), "triangle": np.array([[0,4,-4],[0,0,-2],[0,0,-4],[0,-2,-2],[0,2,-4],[0,2,-2],[0,-4,-4],[0,-2,-4]])} 11 | formation_dict_9["origin"] = np.transpose(formation_dict_9["origin"]) 12 | formation_dict_9["cube"] = np.transpose(formation_dict_9["cube"]) 13 | formation_dict_9["pyramid"] = np.transpose(formation_dict_9["pyramid"]) 14 | formation_dict_9["triangle"] = np.transpose(formation_dict_9["triangle"]) 15 | 16 | formation_dict_18 = {"origin":np.array([[3,0,0],[6,0,0],[0,3,0],[3,3,0],[6,3,0],[0,6,0],[3,6,0],[6,6,0],[0,9,0],[3,9,0],[6,9,0],[0,12,0],[3,12,0],[6,12,0],[0,15,0],[3,15,0],[6,15,0]]), "doubleorigin":2*np.array([[3,0,0],[6,0,0],[0,3,0],[3,3,0],[6,3,0],[0,6,0],[3,6,0],[6,6,0],[0,9,0],[3,9,0],[6,9,0],[0,12,0],[3,12,0],[6,12,0],[0,15,0],[3,15,0],[6,15,0]]),"cuboid":2*np.array([[0, 2, 0], [2, 0, 0], [2, 2, 0], [4, 0, 0], [4, 2, 0], [0, 0, 2], [0, 2, 2], [2, 0, 2], [2, 2, 2], [4, 0, 2], [4, 2, 2], [0, 0, 4], [0, 2, 4], [2, 0, 4], [2, 2, 4], [4, 0, 4], [4, 2, 4]] 17 | ) , "sphere": 3*np.array([[0.5857864376269, -1.4142135623731, 0.0], [2.0, -2.0, 0.0], [3.4142135623731003, -1.4142135623731, 0.0], [4.0, 0.0, 0.0], [3.4142135623731003, 1.4142135623731, 0.0], [2.0, 2.0, 0.0], [0.5857864376269, 1.4142135623731, 0.0], [1.0, -1.0, -1.4142135623731], [3.0, -1.0, -1.4142135623731], [3.0, 1.0, -1.4142135623731], [1.0, 1.0, -1.4142135623731], [1.0, -1.0, 1.4142135623731], [3.0, -1.0, 1.4142135623731], [3.0, 1.0, 1.4142135623731], [1.0, 1.0, 1.4142135623731], [2.0, 0.0, 2.0], [2.0, 0.0, -2.0]] 18 | ), "diamond": 2*np.array([[2.0, 0.0, 0.0], [4.0, 0.0, 0.0], [4.0, 2.0, 0.0], [4.0, 4.0, 0.0], [2.0, 4.0, 0.0], [0.0, 4.0, 0.0], [0.0, 2.0, 0.0], [1.0, 1.0, 1.0], [3.0, 1.0, 1.0], [3.0, 3.0, 1.0], [1.0, 3.0, 1.0], [1.0, 1.0, -1.0], [3.0, 1.0, -1.0], [3.0, 3.0, -1.0], [1.0, 3.0, -1.0], [2.0, 2.0, 2.0], [2.0, 2.0, -2.0]] 19 | )} 20 | formation_dict_18["origin"] = np.transpose(formation_dict_18["origin"]) 21 | formation_dict_18["doubleorigin"]=np.transpose(formation_dict_18["doubleorigin"]) 22 | formation_dict_18["cuboid"] = np.transpose(formation_dict_18["cuboid"]) 23 | formation_dict_18["sphere"] = np.transpose(formation_dict_18["sphere"]) 24 | formation_dict_18["diamond"] = np.transpose(formation_dict_18["diamond"]) -------------------------------------------------------------------------------- /DSLC_xtdrone18/formation_dict.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_xtdrone18/formation_dict.pyc -------------------------------------------------------------------------------- /DSLC_xtdrone18/get_local_pose.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import PoseStamped, Vector3Stamped 3 | import sys 4 | from gazebo_msgs.msg import ModelStates 5 | 6 | vehicle_type = sys.argv[1] 7 | vehicle_num = int(sys.argv[2]) 8 | multi_pose_pub = [None]*vehicle_num 9 | multi_speed_pub = [None]*vehicle_num 10 | multi_local_pose = [PoseStamped() for i in range(vehicle_num)] 11 | multi_speed = [Vector3Stamped() for i in range(vehicle_num)] 12 | 13 | def gazebo_model_state_callback(msg): 14 | for vehicle_id in range(vehicle_num): 15 | id = msg.name.index(vehicle_type+'_'+str(vehicle_id)) 16 | multi_local_pose[vehicle_id].header.stamp = rospy.Time().now() 17 | multi_local_pose[vehicle_id].header.frame_id = 'map' 18 | multi_local_pose[vehicle_id].pose = msg.pose[id] 19 | multi_speed[vehicle_id].header.stamp = rospy.Time().now() 20 | multi_speed[vehicle_id].header.frame_id = 'map' 21 | multi_speed[vehicle_id].vector = msg.twist[id] 22 | 23 | if __name__ == '__main__': 24 | rospy.init_node(vehicle_type+'_get_pose_groundtruth') 25 | gazebo_model_state_sub = rospy.Subscriber("/gazebo/model_states", ModelStates, gazebo_model_state_callback,queue_size=1) 26 | for i in range(vehicle_num): 27 | multi_pose_pub[i] = rospy.Publisher(vehicle_type+'_'+str(i)+'/mavros/vision_pose/pose', PoseStamped, queue_size=1) 28 | multi_speed_pub[i] = rospy.Publisher(vehicle_type+'_'+str(i)+'/mavros/vision_speed/speed', Vector3Stamped, queue_size=1) 29 | print("Get " + vehicle_type + "_" + str(i) + " groundtruth pose") 30 | rate = rospy.Rate(30) 31 | while not rospy.is_shutdown(): 32 | for i in range(vehicle_num): 33 | multi_pose_pub[i].publish(multi_local_pose[i]) 34 | multi_speed_pub[i].publish(multi_speed[i]) 35 | try: 36 | rate.sleep() 37 | except: 38 | continue 39 | 40 | -------------------------------------------------------------------------------- /DSLC_xtdrone18/leader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: UTF-8 -*- 3 | ### This code is about the distributed formation control with a consensus protocol and the task allocation by KM algorithm 4 | ### For more details, please see the paper on https://arxiv.org/abs/2005.01125 5 | 6 | import rospy 7 | from geometry_msgs.msg import Twist, Vector3, PoseStamped 8 | from std_msgs.msg import String, Int32MultiArray, Float32MultiArray 9 | import sys 10 | import numpy 11 | if sys.argv[2] == '6': 12 | from formation_dict import formation_dict_6 as formation_dict 13 | elif sys.argv[2] == '9': 14 | from formation_dict import formation_dict_9 as formation_dict 15 | elif sys.argv[2] == '18': 16 | from formation_dict import formation_dict_18 as formation_dict 17 | else: 18 | print("Only 6, 9 and 18 UAVs are supported.") 19 | 20 | class Leader: 21 | 22 | def __init__(self, uav_type, leader_id, uav_num): 23 | self.id = leader_id 24 | self.pose = PoseStamped() 25 | self.cmd_vel_enu = Twist() 26 | self.uav_num = uav_num 27 | self.avoid_vel = Vector3(0,0,0) 28 | self.formation_config = 'waiting' 29 | self.origin_formation = formation_dict["origin"] 30 | self.new_formation = self.origin_formation 31 | self.adj_matrix = None 32 | self.communication_topology = None 33 | self.changed_id = numpy.arange(0, self.uav_num - 1) 34 | self.target_height_recorded = False 35 | self.cmd = String() 36 | self.f = 200 37 | self.Kz = 0.5 38 | self.pose_sub = rospy.Subscriber(uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.pose_callback, queue_size=1) 39 | self.cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel_flu", Twist, self.cmd_vel_callback, queue_size=1) 40 | self.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+uav_type+'_'+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback, queue_size=1) 41 | self.leader_cmd_sub = rospy.Subscriber("/xtdrone/leader/cmd",String, self.cmd_callback, queue_size=1) 42 | 43 | self.pose_pub = rospy.Publisher("/xtdrone/leader/pose", PoseStamped , queue_size=1) 44 | self.formation_pattern_pub = rospy.Publisher('/xtdrone/formation_pattern', Float32MultiArray, queue_size=1) 45 | self.communication_topology_pub = rospy.Publisher('/xtdrone/communication_topology', Int32MultiArray, queue_size=1) 46 | self.vel_enu_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd_vel_flu', Twist, queue_size=1) 47 | self.cmd_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd', String, queue_size=1) 48 | 49 | def pose_callback(self, msg): 50 | self.pose = msg 51 | 52 | def cmd_vel_callback(self, msg): 53 | self.cmd_vel_enu = msg 54 | 55 | def cmd_callback(self, msg): 56 | # print(msg.data) 57 | if (msg.data in formation_dict.keys() and not msg.data == self.formation_config): 58 | self.formation_config = msg.data 59 | print("Formation pattern: ", self.formation_config) 60 | # These variables are determined for KM algorithm 61 | self.adj_matrix = self.build_graph(self.origin_formation, formation_dict[self.formation_config]) 62 | self.label_left = numpy.max(self.adj_matrix, axis=1) # init label for the left set 63 | self.label_right = numpy.array([0] * (self.uav_num - 1)) # init label for the right set 64 | self.match_right = numpy.array([-1] * (self.uav_num - 1)) 65 | self.visit_left = numpy.array([0] * (self.uav_num - 1)) 66 | self.visit_right = numpy.array([0] * (self.uav_num - 1)) 67 | self.slack_right = numpy.array([100] * (self.uav_num - 1)) 68 | self.changed_id = self.KM() 69 | # Get a new formation pattern of UAVs based on KM. 70 | self.new_formation = self.get_new_formation(self.changed_id, formation_dict[self.formation_config]) 71 | print(self.new_formation) 72 | self.communication_topology = self.get_communication_topology(self.new_formation) 73 | self.origin_formation = self.new_formation 74 | else: 75 | self.cmd = msg.data 76 | 77 | def avoid_vel_callback(self, msg): 78 | self.avoid_vel = msg 79 | 80 | # 'build_graph', 'find_path' and 'KM' functions are all determined for KM algorithm. 81 | # A graph of UAVs is established based on distances between them in 'build_graph' function. 82 | def build_graph(self, orig_formation, change_formation): 83 | distance = [[0 for i in range(self.uav_num - 1)] for j in range(self.uav_num - 1)] 84 | for i in range(self.uav_num - 1): 85 | for j in range(self.uav_num - 1): 86 | distance[i][j] = numpy.linalg.norm(orig_formation[:, i] - change_formation[:, j]) 87 | distance[i][j] = int(50 - distance[i][j]) 88 | return distance 89 | 90 | # Determine whether a path has been found. 91 | def find_path(self, i): 92 | self.visit_left[i] = True 93 | for j, match_weight in enumerate(self.adj_matrix[i], start=0): 94 | if self.visit_right[j]: 95 | continue 96 | gap = self.label_left[i] + self.label_right[j] - match_weight 97 | if gap == 0: 98 | self.visit_right[j] = True 99 | if self.match_right[j] == -1 or self.find_path(self.match_right[j]): 100 | self.match_right[j] = i 101 | return True 102 | else: 103 | self.slack_right[j] = min(gap, self.slack_right[j]) 104 | return False 105 | 106 | # Main body of KM algorithm. 107 | def KM(self): 108 | for i in range(self.uav_num - 1): 109 | self.slack_right = numpy.array([100] * (self.uav_num - 1)) 110 | while True: 111 | self.visit_left = numpy.array([0] * (self.uav_num - 1)) 112 | self.visit_right = numpy.array([0] * (self.uav_num - 1)) 113 | if self.find_path(i): 114 | break 115 | d = numpy.inf 116 | for j, slack in enumerate(self.slack_right): 117 | if not self.visit_right[j]: 118 | d = min(d, slack) 119 | for k in range(self.uav_num - 1): 120 | if self.visit_left[k]: 121 | self.label_left[k] -= d 122 | if self.visit_right[k]: 123 | self.label_right[k] += d 124 | else: 125 | self.slack_right[k] -= d 126 | return self.match_right 127 | 128 | def get_new_formation(self, changed_id, change_formation): 129 | new_formation = numpy.zeros((3, self.uav_num - 1)) 130 | position = numpy.zeros((3, self.uav_num - 1)) 131 | changed_id = [i + 1 for i in changed_id] 132 | for i in range(0, self.uav_num - 1): 133 | position[:, i] = change_formation[:, i] 134 | 135 | for i in range(0, self.uav_num - 1): 136 | for j in range(0, self.uav_num - 1): 137 | if (j + 1) == changed_id[i]: 138 | new_formation[:, i] = position[:, j] 139 | return new_formation 140 | 141 | def get_communication_topology(self, rel_posi): 142 | 143 | c_num = int((self.uav_num) / 2) 144 | min_num_index_list = [0] * c_num 145 | 146 | comm = [[] for i in range(self.uav_num)] 147 | communication = numpy.ones((self.uav_num, self.uav_num)) * 0 148 | nodes_next = [] 149 | node_flag = [self.uav_num - 1] 150 | node_mid_flag = [] 151 | 152 | rel_d = [0] * (self.uav_num - 1) 153 | 154 | for i in range(0, self.uav_num - 1): 155 | rel_d[i] = pow(rel_posi[0][i], 2) + pow(rel_posi[1][i], 2) + pow(rel_posi[2][i], 2) 156 | 157 | c = numpy.copy(rel_d) 158 | c.sort() 159 | count = 0 160 | 161 | for j in range(0, c_num): 162 | for i in range(0, self.uav_num - 1): 163 | if rel_d[i] == c[j]: 164 | if not i in node_mid_flag: 165 | min_num_index_list[count] = i 166 | node_mid_flag.append(i) 167 | count = count + 1 168 | if count == c_num: 169 | break 170 | if count == c_num: 171 | break 172 | 173 | for j in range(0, c_num): 174 | nodes_next.append(min_num_index_list[j]) 175 | 176 | comm[self.uav_num - 1].append(min_num_index_list[j]) 177 | 178 | size_ = len(node_flag) 179 | 180 | while (nodes_next != []) and (size_ < (self.uav_num - 1)): 181 | 182 | next_node = nodes_next[0] 183 | nodes_next = nodes_next[1:] 184 | min_num_index_list = [0] * c_num 185 | node_mid_flag = [] 186 | rel_d = [0] * (self.uav_num - 1) 187 | for i in range(0, self.uav_num - 1): 188 | 189 | if i == next_node or i in node_flag: 190 | 191 | rel_d[i] = 2000 192 | else: 193 | 194 | rel_d[i] = pow((rel_posi[0][i] - rel_posi[0][next_node]), 2) + pow( 195 | (rel_posi[1][i] - rel_posi[1][next_node]), 2) + pow((rel_posi[2][i] - rel_posi[2][next_node]), 196 | 2) 197 | c = numpy.copy(rel_d) 198 | c.sort() 199 | count = 0 200 | 201 | for j in range(0, c_num): 202 | for i in range(0, self.uav_num - 1): 203 | if rel_d[i] == c[j]: 204 | if not i in node_mid_flag: 205 | min_num_index_list[count] = i 206 | node_mid_flag.append(i) 207 | count = count + 1 208 | if count == c_num: 209 | break 210 | if count == c_num: 211 | break 212 | node_flag.append(next_node) 213 | 214 | size_ = len(node_flag) 215 | 216 | for j in range(0, c_num): 217 | 218 | if min_num_index_list[j] in node_flag: 219 | 220 | nodes_next = nodes_next 221 | 222 | else: 223 | if min_num_index_list[j] in nodes_next: 224 | nodes_next = nodes_next 225 | else: 226 | nodes_next.append(min_num_index_list[j]) 227 | 228 | comm[next_node].append(min_num_index_list[j]) 229 | 230 | for i in range(0, self.uav_num): 231 | for j in range(0, self.uav_num - 1): 232 | if i == 0: 233 | if j in comm[self.uav_num - 1]: 234 | communication[j + 1][i] = 1 235 | else: 236 | communication[j + 1][i] = 0 237 | else: 238 | if j in comm[i - 1] and i < (j+1): 239 | communication[j + 1][i] = 1 240 | else: 241 | communication[j + 1][i] = 0 242 | 243 | for i in range(1, self.uav_num): # 防止某个无人机掉队 244 | if sum(communication[i]) == 0: 245 | communication[i][0] = 1 246 | return communication 247 | 248 | 249 | def loop(self): 250 | rospy.init_node('leader') 251 | rate = rospy.Rate(self.f) 252 | while True: 253 | self.cmd_vel_enu.linear.x = self.cmd_vel_enu.linear.x 254 | self.cmd_vel_enu.linear.y = self.cmd_vel_enu.linear.y 255 | self.cmd_vel_enu.linear.z = self.cmd_vel_enu.linear.z 256 | formation_pattern = Float32MultiArray() 257 | formation_pattern.data = self.new_formation.flatten().tolist() 258 | self.formation_pattern_pub.publish(formation_pattern) 259 | if(not self.communication_topology is None): 260 | communication_topology = Int32MultiArray() 261 | communication_topology.data = self.communication_topology.flatten().tolist() 262 | self.communication_topology_pub.publish(communication_topology) 263 | self.vel_enu_pub.publish(self.cmd_vel_enu) 264 | self.pose_pub.publish(self.pose) 265 | self.cmd_pub.publish(self.cmd) 266 | try: 267 | rate.sleep() 268 | except: 269 | continue 270 | 271 | if __name__ == '__main__': 272 | leader = Leader(sys.argv[1], 0, int(sys.argv[2])) 273 | leader.loop() 274 | -------------------------------------------------------------------------------- /DSLC_xtdrone18/multi_vehicle_communication.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | iris_num=18 3 | typhoon_h480_num=0 4 | solo_num=0 5 | plane_num=0 6 | rover_num=0 7 | standard_vtol_num=0 8 | tiltrotor_num=0 9 | tailsitter_num=0 10 | 11 | vehicle_num=0 12 | while(( $vehicle_num< iris_num)) 13 | do 14 | python3 multirotor_communication_sigle.py iris $vehicle_num& 15 | let "vehicle_num++" 16 | done 17 | 18 | vehicle_num=0 19 | while(( $vehicle_num< typhoon_h480_num)) 20 | do 21 | python3 multirotor_communication_sigle.py typhoon_h480 $vehicle_num& 22 | let "vehicle_num++" 23 | done 24 | 25 | vehicle_num=0 26 | while(( $vehicle_num< solo_num)) 27 | do 28 | python3 multirotor_communication_sigle.py solo $vehicle_num& 29 | let "vehicle_num++" 30 | done 31 | 32 | vehicle_num=0 33 | while(( $vehicle_num< plane_num)) 34 | do 35 | python3 plane_communication.py $vehicle_num& 36 | let "vehicle_num++" 37 | done 38 | 39 | vehicle_num=0 40 | while(( $vehicle_num< rover_num)) 41 | do 42 | python3 rover_communication.py $vehicle_num& 43 | let "vehicle_num++" 44 | done 45 | 46 | vehicle_num=0 47 | while(( $vehicle_num< standard_vtol_num)) 48 | do 49 | python3 vtol_communication.py standard_vtol $vehicle_num& 50 | let "vehicle_num++" 51 | done 52 | 53 | vehicle_num=0 54 | while(( $vehicle_num< tiltrotor_num)) 55 | do 56 | python3 vtol_communication.py tiltrotor $vehicle_num& 57 | let "vehicle_num++" 58 | done 59 | 60 | vehicle_num=0 61 | while(( $vehicle_num< tailsitter_num)) 62 | do 63 | python3 vtol_communication.py tailsitter $vehicle_num& 64 | let "vehicle_num++" 65 | done 66 | -------------------------------------------------------------------------------- /DSLC_xtdrone18/multirotor_communication_sigle.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from mavros_msgs.msg import PositionTarget 3 | from mavros_msgs.srv import CommandBool, SetMode 4 | from geometry_msgs.msg import PoseStamped, Pose, Twist 5 | from std_msgs.msg import String 6 | from pyquaternion import Quaternion 7 | import sys 8 | 9 | rospy.init_node(sys.argv[1]+'_'+sys.argv[2]+"_communication") 10 | rate = rospy.Rate(30) 11 | 12 | class Sigle_Communication: 13 | 14 | def __init__(self, vehicle_type, vehicle_id): 15 | 16 | self.vehicle_type = vehicle_type 17 | self.vehicle_id = vehicle_id 18 | self.current_position = None 19 | self.current_yaw = 0 20 | self.hover_flag = 0 21 | self.target_motion = PositionTarget() 22 | self.arm_state = False 23 | self.motion_type = 0 24 | self.flight_mode = None 25 | self.mission = None 26 | self.last_cmd = None 27 | 28 | ''' 29 | ros subscribers 30 | ''' 31 | self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback,queue_size=1) 32 | self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback,queue_size=3) 33 | self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback,queue_size=1) 34 | self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback,queue_size=1) 35 | self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback,queue_size=1) 36 | self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback,queue_size=1) 37 | self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback,queue_size=1) 38 | self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback,queue_size=1) 39 | 40 | ''' 41 | ros publishers 42 | ''' 43 | self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=1) 44 | 45 | ''' 46 | ros services 47 | ''' 48 | self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool) 49 | self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode) 50 | 51 | print(self.vehicle_type+'_'+self.vehicle_id+": "+"communication initialized") 52 | 53 | def start(self): 54 | ''' 55 | main ROS thread 56 | ''' 57 | while not rospy.is_shutdown(): 58 | self.target_motion_pub.publish(self.target_motion) 59 | rate.sleep() 60 | 61 | def local_pose_callback(self, msg): 62 | self.current_position = msg.pose.position 63 | self.current_yaw = self.q2yaw(msg.pose.orientation) 64 | 65 | def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0): 66 | target_raw_pose = PositionTarget() 67 | target_raw_pose.coordinate_frame = self.coordinate_frame 68 | 69 | target_raw_pose.position.x = x 70 | target_raw_pose.position.y = y 71 | target_raw_pose.position.z = z 72 | 73 | target_raw_pose.velocity.x = vx 74 | target_raw_pose.velocity.y = vy 75 | target_raw_pose.velocity.z = vz 76 | 77 | target_raw_pose.acceleration_or_force.x = afx 78 | target_raw_pose.acceleration_or_force.y = afy 79 | target_raw_pose.acceleration_or_force.z = afz 80 | 81 | target_raw_pose.yaw = yaw 82 | target_raw_pose.yaw_rate = yaw_rate 83 | 84 | if(self.motion_type == 0): #0:ignore v,a and yaw rate 85 | target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ 86 | + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ 87 | + PositionTarget.IGNORE_YAW_RATE 88 | if(self.motion_type == 1): 89 | target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ 90 | + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ 91 | + PositionTarget.IGNORE_YAW 92 | if(self.motion_type == 2): 93 | target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ 94 | + PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ 95 | + PositionTarget.IGNORE_YAW 96 | 97 | return target_raw_pose 98 | 99 | def cmd_pose_flu_callback(self, msg): 100 | self.coordinate_frame = 9 101 | self.motion_type = 0 102 | yaw = self.q2yaw(msg.orientation) 103 | self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw) 104 | 105 | def cmd_pose_enu_callback(self, msg): 106 | self.coordinate_frame = 1 107 | self.motion_type = 0 108 | yaw = self.q2yaw(msg.orientation) 109 | self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw) 110 | 111 | def cmd_vel_flu_callback(self, msg): 112 | self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) 113 | if self.hover_flag == 0: 114 | self.coordinate_frame = 8 115 | self.motion_type = 1 116 | self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z) 117 | 118 | def cmd_vel_enu_callback(self, msg): 119 | self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) 120 | if self.hover_flag == 0: 121 | self.coordinate_frame = 1 122 | self.motion_type = 1 123 | self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z) 124 | 125 | def cmd_accel_flu_callback(self, msg): 126 | self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) 127 | if self.hover_flag == 0: 128 | self.coordinate_frame = 8 129 | self.motion_type = 2 130 | self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z) 131 | 132 | def cmd_accel_enu_callback(self, msg): 133 | self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) 134 | if self.hover_flag == 0: 135 | self.coordinate_frame = 1 136 | self.motion_type = 2 137 | self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z) 138 | 139 | def hover_state_transition(self,x,y,z,w): 140 | if abs(x) > 0.02 or abs(y) > 0.02 or abs(z) > 0.02 or abs(w) > 0.005: 141 | self.hover_flag = 0 142 | self.flight_mode = 'OFFBOARD' 143 | elif not self.flight_mode == "HOVER": 144 | self.hover_flag = 1 145 | self.flight_mode = 'HOVER' 146 | self.hover() 147 | def cmd_callback(self, msg): 148 | if msg.data == self.last_cmd or msg.data == '' or msg.data == 'stop controlling': 149 | return 150 | 151 | elif msg.data == 'ARM': 152 | self.arm_state =self.arm() 153 | print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state)) 154 | 155 | elif msg.data == 'DISARM': 156 | self.arm_state = not self.disarm() 157 | print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state)) 158 | 159 | elif msg.data[:-1] == "mission" and not msg.data == self.mission: 160 | self.mission = msg.data 161 | print(self.vehicle_type+'_'+self.vehicle_id+": "+msg.data) 162 | 163 | else: 164 | self.flight_mode = msg.data 165 | self.flight_mode_switch() 166 | 167 | self.last_cmd = msg.data 168 | 169 | def q2yaw(self, q): 170 | if isinstance(q, Quaternion): 171 | rotate_z_rad = q.yaw_pitch_roll[0] 172 | else: 173 | q_ = Quaternion(q.w, q.x, q.y, q.z) 174 | rotate_z_rad = q_.yaw_pitch_roll[0] 175 | 176 | return rotate_z_rad 177 | 178 | def arm(self): 179 | if self.armService(True): 180 | return True 181 | else: 182 | print(self.vehicle_type+'_'+self.vehicle_id+": arming failed!") 183 | return False 184 | 185 | def disarm(self): 186 | if self.armService(False): 187 | return True 188 | else: 189 | print(self.vehicle_type+'_'+self.vehicle_id+": disarming failed!") 190 | return False 191 | 192 | def hover(self): 193 | self.coordinate_frame = 1 194 | self.motion_type = 0 195 | self.target_motion = self.construct_target(x=self.current_position.x,y=self.current_position.y,z=self.current_position.z,yaw=self.current_yaw) 196 | print(self.vehicle_type+'_'+self.vehicle_id+":"+self.flight_mode) 197 | 198 | def flight_mode_switch(self): 199 | if self.flight_mode == 'HOVER': 200 | self.hover_flag = 1 201 | self.hover() 202 | elif self.flightModeService(custom_mode=self.flight_mode): 203 | print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode) 204 | return True 205 | else: 206 | print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode+"failed") 207 | return False 208 | 209 | if __name__ == '__main__': 210 | sigle_communication = Sigle_Communication(sys.argv[1],sys.argv[2]) 211 | sigle_communication.start() 212 | -------------------------------------------------------------------------------- /DSLC_xtdrone18/multirotor_keyboard_control_promotion.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import Twist 3 | import sys, select, os 4 | import tty, termios 5 | from std_msgs.msg import String 6 | 7 | 8 | MAX_LINEAR = 20 9 | MAX_ANG_VEL = 3 10 | LINEAR_STEP_SIZE = 0.1 11 | ANG_VEL_STEP_SIZE = 0.1 12 | 13 | cmd_vel_mask = False 14 | ctrl_leader = False 15 | 16 | msg2all = """ 17 | Control Your XTDrone! 18 | To all drones (press g to control the leader) 19 | --------------------------- 20 | 1 2 3 4 5 6 7 8 9 0 21 | w r t y i 22 | a s d g j k l 23 | x v b n , 24 | 25 | w/x : increase/decrease forward 26 | a/d : increase/decrease leftward 27 | i/, : increase/decrease yaw 28 | j/l : increase/decrease yaw 29 | r : return home 30 | t/y : arm/disarm 31 | v/n : takeoff/land 32 | b : offboard 33 | s/k : hover and remove the mask of keyboard control 34 | f/h : turn right/left 35 | 0 : mask the keyboard control (for single UAV) 36 | 0~9 : extendable mission(eg.different formation configuration) 37 | this will mask the keyboard control 38 | g : control the leader 39 | CTRL-C to quit 40 | """ 41 | 42 | msg2leader = """ 43 | Control Your XTDrone! 44 | To the leader (press g to control all drones) 45 | --------------------------- 46 | 1 2 3 4 5 6 7 8 9 0 47 | w r t y i 48 | a s d g j k l 49 | x v b n , 50 | 51 | w/x : increase/decrease forward 52 | a/d : increase/decrease leftward 53 | i/, : increase/decrease upward 54 | j/l : increase/decrease yaw 55 | r : return home 56 | t/y : arm/disarm 57 | v/n : takeoff/land 58 | b : offboard 59 | s/k : hover and remove the mask of keyboard control 60 | f/h : turn right/left 61 | g : control all drones 62 | CTRL-C to quit 63 | """ 64 | 65 | e = """ 66 | Communications Failed 67 | """ 68 | 69 | def getKey(): 70 | tty.setraw(sys.stdin.fileno()) 71 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 72 | if rlist: 73 | key = sys.stdin.read(1) 74 | else: 75 | key = '' 76 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 77 | return key 78 | 79 | def print_msg(): 80 | if ctrl_leader: 81 | print(msg2leader) 82 | else: 83 | print(msg2all) 84 | 85 | 86 | 87 | if __name__=="__main__": 88 | 89 | settings = termios.tcgetattr(sys.stdin) 90 | multirotor_type = 'iris' 91 | multirotor_num = 18 92 | control_type = 'vel' 93 | 94 | if multirotor_num == 18: 95 | formation_configs = ['origin', 'doubleorigin','cuboid', 'sphere', 'diamond'] 96 | elif multirotor_num == 9: 97 | formation_configs = ['waiting', 'cube', 'pyramid', 'triangle','origin'] 98 | elif multirotor_num == 6: 99 | formation_configs = ['origin', 'doubleorigin','T', 'diamond', 'triangle'] 100 | elif multirotor_num == 1: 101 | formation_configs = ['stop controlling'] 102 | 103 | cmd= String() 104 | twist = Twist() 105 | 106 | rospy.init_node(multirotor_type + '_multirotor_keyboard_control') 107 | 108 | if control_type == 'vel': 109 | multi_cmd_vel_flu_pub = [None]*multirotor_num 110 | multi_cmd_pub = [None]*multirotor_num 111 | for i in range(multirotor_num): 112 | multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=1) 113 | multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=3) 114 | leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=1) 115 | leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=1) 116 | 117 | else: 118 | multi_cmd_accel_flu_pub = [None]*multirotor_num 119 | multi_cmd_pub = [None]*multirotor_num 120 | for i in range(multirotor_num): 121 | multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=1) 122 | multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=1) 123 | leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=1) 124 | leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=3) 125 | 126 | forward = 0.0 127 | leftward = 0.0 128 | upward = 0.0 129 | angular = 0.0 130 | 131 | print_msg() 132 | while(1): 133 | key = getKey() 134 | if key == 'w' : 135 | forward = forward + LINEAR_STEP_SIZE 136 | print_msg() 137 | if control_type == 'vel': 138 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 139 | else: 140 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 141 | 142 | elif key == 'x' : 143 | forward = forward - LINEAR_STEP_SIZE 144 | print_msg() 145 | if control_type == 'vel': 146 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 147 | else: 148 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 149 | 150 | elif key == 'a' : 151 | 152 | leftward = leftward + LINEAR_STEP_SIZE 153 | print_msg() 154 | if control_type == 'vel': 155 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 156 | else: 157 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 158 | 159 | elif key == 'd' : 160 | leftward = leftward - LINEAR_STEP_SIZE 161 | print_msg() 162 | if control_type == 'vel': 163 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 164 | else: 165 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 166 | 167 | elif key == 'i' : 168 | upward = upward + LINEAR_STEP_SIZE 169 | print_msg() 170 | if control_type == 'vel': 171 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 172 | else: 173 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 174 | 175 | elif key == ',' : 176 | upward = upward - LINEAR_STEP_SIZE 177 | print_msg() 178 | if control_type == 'vel': 179 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 180 | else: 181 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 182 | 183 | elif key == 'j': 184 | angular = angular + ANG_VEL_STEP_SIZE 185 | print_msg() 186 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 187 | 188 | elif key == 'l': 189 | angular = angular - ANG_VEL_STEP_SIZE 190 | print_msg() 191 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 192 | 193 | elif key == 'r': 194 | cmd = 'AUTO.RTL' 195 | print_msg() 196 | print('Returning home') 197 | elif key == 't': 198 | cmd = 'ARM' 199 | print_msg() 200 | print('Arming') 201 | elif key == 'y': 202 | cmd = 'DISARM' 203 | print_msg() 204 | print('Disarming') 205 | elif key == 'v': 206 | cmd = 'AUTO.TAKEOFF' 207 | cmd = '' 208 | print_msg() 209 | elif key == 'b': 210 | cmd = 'OFFBOARD' 211 | print_msg() 212 | print('Offboard') 213 | elif key == 'n': 214 | cmd = 'AUTO.LAND' 215 | print_msg() 216 | print('Landing') 217 | elif key == 'g': 218 | ctrl_leader = not ctrl_leader 219 | print_msg() 220 | elif key=='f': 221 | forward = 2.0 222 | angular = -0.2 223 | print_msg() 224 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 225 | elif key=='h': 226 | forward=2.0 227 | angular = 0.2 228 | print_msg() 229 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 230 | elif key in ['k', 's']: 231 | cmd_vel_mask = False 232 | forward = 0.0 233 | leftward = 0.0 234 | upward = 0.0 235 | angular = 0.0 236 | cmd = 'HOVER' 237 | print_msg() 238 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 239 | print('Hover') 240 | else: 241 | for i in range(10): 242 | if key == str(i): 243 | cmd = formation_configs[i] 244 | print_msg() 245 | print(cmd) 246 | cmd_vel_mask = True 247 | if (key == '\x03'): 248 | break 249 | 250 | if forward > MAX_LINEAR: 251 | forward = MAX_LINEAR 252 | elif forward < -MAX_LINEAR: 253 | forward = -MAX_LINEAR 254 | if leftward > MAX_LINEAR: 255 | leftward = MAX_LINEAR 256 | elif leftward < -MAX_LINEAR: 257 | leftward = -MAX_LINEAR 258 | if upward > MAX_LINEAR: 259 | upward = MAX_LINEAR 260 | elif upward < -MAX_LINEAR: 261 | upward = -MAX_LINEAR 262 | if angular > MAX_ANG_VEL: 263 | angular = MAX_ANG_VEL 264 | elif angular < -MAX_ANG_VEL: 265 | angular = - MAX_ANG_VEL 266 | 267 | twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upward 268 | twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = angular 269 | 270 | for i in range(multirotor_num): 271 | if ctrl_leader: 272 | if control_type == 'vel': 273 | leader_cmd_vel_flu_pub.publish(twist) 274 | else: 275 | leader_cmd_accel_flu_pub.publish(twist) 276 | leader_cmd_pub.publish(cmd) 277 | else: 278 | if not cmd_vel_mask: 279 | if control_type == 'vel': 280 | multi_cmd_vel_flu_pub[i].publish(twist) 281 | else: 282 | multi_cmd_accel_flu_pub[i].publish(twist) 283 | multi_cmd_pub[i].publish(cmd) 284 | 285 | cmd = '' 286 | 287 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 288 | 289 | -------------------------------------------------------------------------------- /DSLC_xtdrone18/run_formation_promotion.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | python3 leader.py 'iris' 18 & 3 | python3 avoid.py 'iris' 18 'vel' & 4 | python3 e1_pub.py 'iris' 18 'vel' & 5 | uav_id=1 6 | while(( $uav_id< 18 )) 7 | do 8 | python3 follower_consensus_promotion.py 'iris' $uav_id 18 'vel' & 9 | let "uav_id++" 10 | done -------------------------------------------------------------------------------- /DSLC_xtdrone18/usebook: -------------------------------------------------------------------------------- 1 | %% enter the catalog of the file that will be utilized 2 | %% run the command sequentially 3 | 4 | % load worlds and the drones 5 | 1.roslaunch multi_vehicle.launch 6 | 7 | % obtain the position information of drones 8 | 2.python3 get_local_pose.py iris 18 9 | 10 | % build the communication network among drones 11 | 3.bash multi_vehicle_communication.sh 12 | 13 | % keyboard control code 14 | 4.python3 multirotor_keyboard_control_promotion.py 15 | % use keyboard to control all drones to take off and press s to hover after a desired height.Then press g to enter leader control mode. 16 | 17 | 5.bash run_formation_promotion.sh 18 | %When the script has been launched, then switch to the keyboard control code, pressing f or h can achieve turning in 6 drones formation. Moreover, pressing number 0-9 can change the formation. 19 | 20 | 5.bash run_formation_baseline.sh 21 | 22 | %Note: 23 | % When the run_formation_promotion script is running and the drones are stationary, switch to the keyboard control terminal to press w to give leader a specified velocity for formation flight. Turning and formation transformation can also be achieved when the drones are running. 24 | % The run_formation_baseline script is for comparison with run_formation_promotion script in performance of running straight line at variable velocity. 25 | 26 | % The following script is for plot the information of drones such as error,position and etc. 27 | 6.python3 draw_figure.py 28 | 29 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/TEST: -------------------------------------------------------------------------------- 1 | ZZZ 2 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/avoid.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import Vector3, PoseStamped 3 | import time 4 | import numpy 5 | import sys 6 | 7 | vehicle_type = sys.argv[1] 8 | vehicle_num = int(sys.argv[2]) 9 | control_type = sys.argv[3] 10 | pose = [None]*vehicle_num 11 | pose_sub = [None]*vehicle_num 12 | avoid_control_pub = [None]*vehicle_num 13 | avoid_radius = 1.5 14 | aid_vec1 = [1, 0, 0] 15 | aid_vec2 = [0, 1, 0] 16 | vehicles_avoid_control = [Vector3()] * vehicle_num 17 | 18 | def pose_callback(msg, id): 19 | pose[id] = msg 20 | 21 | rospy.init_node('avoid') 22 | for i in range(vehicle_num): 23 | pose_sub[i] = rospy.Subscriber(vehicle_type+'_'+str(i)+'/mavros/local_position/pose',PoseStamped,pose_callback,i,queue_size=1) 24 | if control_type == "vel": 25 | avoid_control_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i)+"/avoid_vel", Vector3,queue_size=1) 26 | elif control_type == "accel": 27 | avoid_control_pub[i] = rospy.Publisher("/xtdrone/"+vehicle_type+'_'+str(i)+"/avoid_accel", Vector3,queue_size=1) 28 | else: 29 | print("Only vel and accel are supported.") 30 | 31 | time.sleep(1) 32 | rate = rospy.Rate(30) 33 | while not rospy.is_shutdown(): 34 | for i in range(vehicle_num): 35 | position1 = numpy.array([pose[i].pose.position.x, pose[i].pose.position.y, pose[i].pose.position.z]) 36 | for j in range(1, vehicle_num-i): 37 | position2 = numpy.array([pose[i+j].pose.position.x, pose[i+j].pose.position.y, pose[i+j].pose.position.z]) 38 | dir_vec = position1-position2 39 | k = 1 - numpy.linalg.norm(dir_vec) / avoid_radius 40 | if k > 0: 41 | cos1 = dir_vec.dot(aid_vec1)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec1)) 42 | cos2 = dir_vec.dot(aid_vec2)/(numpy.linalg.norm(dir_vec) * numpy.linalg.norm(aid_vec2)) 43 | if abs(cos1) < abs(cos2): 44 | avoid_control = k * numpy.cross(dir_vec, aid_vec1)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec1)) 45 | else: 46 | avoid_control = k * numpy.cross(dir_vec, aid_vec2)/numpy.linalg.norm(numpy.cross(dir_vec, aid_vec2)) 47 | 48 | vehicles_avoid_control[i] = Vector3(vehicles_avoid_control[i].x+avoid_control[0],vehicles_avoid_control[i].y+avoid_control[1],vehicles_avoid_control[i].z+avoid_control[2]) 49 | vehicles_avoid_control[i+j] = Vector3(vehicles_avoid_control[i+j].x-avoid_control[0],vehicles_avoid_control[i+j].y-avoid_control[1],vehicles_avoid_control[i+j].z-avoid_control[2]) 50 | 51 | for i in range(vehicle_num): 52 | avoid_control_pub[i].publish(vehicles_avoid_control[i]) 53 | vehicles_avoid_control = [Vector3()] * vehicle_num 54 | rate.sleep() 55 | 56 | 57 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/draw_figure.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | e2_1=np.genfromtxt("test_iris_1.txt",dtype=float) 5 | e2_2=np.genfromtxt("test_iris_2.txt",dtype=float) 6 | e2_3=np.genfromtxt("test_iris_3.txt",dtype=float) 7 | e2_4=np.genfromtxt("test_iris_4.txt",dtype=float) 8 | e2_5=np.genfromtxt("test_iris_5.txt",dtype=float) 9 | 10 | p_1=np.genfromtxt("test_position_1.txt",dtype=float) 11 | p_2=np.genfromtxt("test_position_2.txt",dtype=float) 12 | p_3=np.genfromtxt("test_position_3.txt",dtype=float) 13 | p_4=np.genfromtxt("test_position_4.txt",dtype=float) 14 | p_5=np.genfromtxt("test_position_5.txt",dtype=float) 15 | 16 | x_1 = np.arange(len(e2_1)) 17 | x_2 = np.arange(len(e2_2)) 18 | x_3 = np.arange(len(e2_3)) 19 | x_4 = np.arange(len(e2_4)) 20 | x_5 = np.arange(len(e2_5)) 21 | 22 | y_1 = np.arange(len(p_1)) 23 | y_2 = np.arange(len(p_2)) 24 | y_3 = np.arange(len(p_3)) 25 | y_4 = np.arange(len(p_4)) 26 | y_5 = np.arange(len(p_5)) 27 | 28 | plt.figure(figsize=(10, 6)) 29 | plt.subplot(6,1,1) 30 | plt.plot(x_1, e2_1[:,0], color='#FF0000', label='ex1', linewidth=3.0) 31 | plt.plot(x_2, e2_2[:,0], color='#FFA500', label='ex2', linewidth=3.0) 32 | plt.plot(x_3, e2_3[:,0], color='#7CFC00', label='ex3', linewidth=3.0) 33 | plt.plot(x_4, e2_4[:,0], color='#1E90FF', label='ex4', linewidth=3.0) 34 | plt.plot(x_5, e2_5[:,0], color='#FF00FF', label='ex5', linewidth=3.0) 35 | 36 | plt.xticks(fontsize=18) 37 | plt.yticks(fontsize=18) 38 | 39 | plt.xlabel(u't', fontsize=18) 40 | plt.ylabel(u'ex', fontsize=18) 41 | plt.legend(fontsize=18,loc=1) 42 | 43 | plt.subplot(6,1,2) 44 | plt.plot(x_1, e2_1[:,1], color='#FF0000', label='ey1', linewidth=3.0) 45 | plt.plot(x_2, e2_2[:,1], color='#FFA500', label='ey2', linewidth=3.0) 46 | plt.plot(x_3, e2_3[:,1], color='#7CFC00', label='ey3', linewidth=3.0) 47 | plt.plot(x_4, e2_4[:,1], color='#1E90FF', label='ey4', linewidth=3.0) 48 | plt.plot(x_5, e2_5[:,1], color='#FF00FF', label='ey5', linewidth=3.0) 49 | 50 | plt.xticks(fontsize=18) 51 | plt.yticks(fontsize=18) 52 | 53 | plt.xlabel(u't', fontsize=18) 54 | plt.ylabel(u'ey', fontsize=18) 55 | plt.legend(fontsize=18,loc=1) 56 | 57 | plt.subplot(6,1,3) 58 | plt.plot(x_1, e2_1[:,2], color='#FF0000', label='etheta1', linewidth=3.0) 59 | plt.plot(x_2, e2_2[:,2], color='#FFA500', label='etheta2', linewidth=3.0) 60 | plt.plot(x_3, e2_3[:,2], color='#7CFC00', label='etheta3', linewidth=3.0) 61 | plt.plot(x_4, e2_4[:,2], color='#1E90FF', label='etheta4', linewidth=3.0) 62 | plt.plot(x_5, e2_5[:,2], color='#FF00FF', label='etheta5', linewidth=3.0) 63 | 64 | plt.xticks(fontsize=18) 65 | plt.yticks(fontsize=18) 66 | 67 | plt.xlabel(u't', fontsize=18) 68 | plt.ylabel(u'etheta', fontsize=18) 69 | plt.legend(fontsize=18,loc=1) 70 | 71 | plt.subplot(6,1,4) 72 | plt.plot(x_1, e2_1[:,3], color='#FF0000', label='ev1', linewidth=3.0) 73 | plt.plot(x_2, e2_2[:,3], color='#FFA500', label='ev2', linewidth=3.0) 74 | plt.plot(x_3, e2_3[:,3], color='#7CFC00', label='ev3', linewidth=3.0) 75 | plt.plot(x_4, e2_4[:,3], color='#1E90FF', label='ev4', linewidth=3.0) 76 | plt.plot(x_5, e2_5[:,3], color='#FF00FF', label='ev5', linewidth=3.0) 77 | 78 | plt.xticks(fontsize=18) 79 | plt.yticks(fontsize=18) 80 | 81 | plt.xlabel(u't', fontsize=18) 82 | plt.ylabel(u'ev', fontsize=18) 83 | plt.legend(fontsize=18,loc=1) 84 | 85 | plt.subplot(6,1,5) 86 | plt.plot(x_1, e2_1[:,4], color='#FF0000', label='ew1', linewidth=3.0) 87 | plt.plot(x_2, e2_2[:,4], color='#FFA500', label='ew2', linewidth=3.0) 88 | plt.plot(x_3, e2_3[:,4], color='#7CFC00', label='ew3', linewidth=3.0) 89 | plt.plot(x_4, e2_4[:,4], color='#1E90FF', label='ew4', linewidth=3.0) 90 | plt.plot(x_5, e2_5[:,4], color='#FF00FF', label='ew5', linewidth=3.0) 91 | 92 | plt.xticks(fontsize=18) 93 | plt.yticks(fontsize=18) 94 | 95 | plt.xlabel(u't', fontsize=18) 96 | plt.ylabel(u'ew', fontsize=18) 97 | plt.legend(fontsize=18,loc=1) 98 | 99 | plt.subplot(6,1,6) 100 | plt.plot(x_1, e2_1[:,5], color='#FF0000', label='v1', linewidth=3.0) 101 | plt.plot(x_2, e2_2[:,5], color='#FFA500', label='v2', linewidth=3.0) 102 | plt.plot(x_3, e2_3[:,5], color='#7CFC00', label='v3', linewidth=3.0) 103 | plt.plot(x_4, e2_4[:,5], color='#1E90FF', label='v4', linewidth=3.0) 104 | plt.plot(x_5, e2_5[:,5], color='#FF00FF', label='v5', linewidth=3.0) 105 | 106 | plt.xticks(fontsize=18) 107 | plt.yticks(fontsize=18) 108 | 109 | plt.xlabel(u't', fontsize=18) 110 | plt.ylabel(u'velocity', fontsize=18) 111 | plt.legend(fontsize=18,loc=1) 112 | 113 | plt.figure(figsize=(10, 6)) 114 | plt.subplot(7,1,1) 115 | plt.plot(y_1, p_1[:,0], color='#FF0000', label='x1', linewidth=3.0) 116 | plt.plot(y_2, p_2[:,0], color='#FFA500', label='x2', linewidth=3.0) 117 | plt.plot(y_3, p_3[:,0], color='#7CFC00', label='x3', linewidth=3.0) 118 | plt.plot(y_4, p_4[:,0], color='#1E90FF', label='x4', linewidth=3.0) 119 | plt.plot(y_5, p_5[:,0], color='#FF00FF', label='x5', linewidth=3.0) 120 | 121 | plt.xticks(fontsize=18) 122 | plt.yticks(fontsize=18) 123 | 124 | plt.xlabel(u't', fontsize=18) 125 | plt.ylabel(u'x', fontsize=18) 126 | plt.legend(fontsize=18,loc=1) 127 | 128 | plt.subplot(7,1,2) 129 | plt.plot(y_1, p_1[:,1], color='#FF0000', label='y1', linewidth=3.0) 130 | plt.plot(y_2, p_2[:,1], color='#FFA500', label='y2', linewidth=3.0) 131 | plt.plot(y_3, p_3[:,1], color='#7CFC00', label='y3', linewidth=3.0) 132 | plt.plot(y_4, p_4[:,1], color='#1E90FF', label='y4', linewidth=3.0) 133 | plt.plot(y_5, p_5[:,1], color='#FF00FF', label='y5', linewidth=3.0) 134 | 135 | plt.xticks(fontsize=18) 136 | plt.yticks(fontsize=18) 137 | 138 | plt.xlabel(u't', fontsize=18) 139 | plt.ylabel(u'y', fontsize=18) 140 | plt.legend(fontsize=18,loc=1) 141 | 142 | plt.subplot(7,1,3) 143 | plt.plot(y_1, p_1[:,2], color='#FF0000', label='z1', linewidth=3.0) 144 | plt.plot(y_2, p_2[:,2], color='#FFA500', label='z2', linewidth=3.0) 145 | plt.plot(y_3, p_3[:,2], color='#7CFC00', label='z3', linewidth=3.0) 146 | plt.plot(y_4, p_4[:,2], color='#1E90FF', label='z4', linewidth=3.0) 147 | plt.plot(y_5, p_5[:,2], color='#FF00FF', label='z5', linewidth=3.0) 148 | 149 | plt.xticks(fontsize=18) 150 | plt.yticks(fontsize=18) 151 | 152 | plt.xlabel(u't', fontsize=18) 153 | plt.ylabel(u'z', fontsize=18) 154 | plt.legend(fontsize=18,loc=1) 155 | 156 | plt.subplot(7,1,4) 157 | plt.plot(y_1, p_1[:,3], color='#FF0000', label='theta1', linewidth=3.0) 158 | plt.plot(y_2, p_2[:,3], color='#FFA500', label='theta2', linewidth=3.0) 159 | plt.plot(y_3, p_3[:,3], color='#7CFC00', label='theta3', linewidth=3.0) 160 | plt.plot(y_4, p_4[:,3], color='#1E90FF', label='theta4', linewidth=3.0) 161 | plt.plot(y_5, p_5[:,3], color='#FF00FF', label='theta5', linewidth=3.0) 162 | 163 | plt.xticks(fontsize=18) 164 | plt.yticks(fontsize=18) 165 | 166 | plt.xlabel(u't', fontsize=18) 167 | plt.ylabel(u'theta', fontsize=18) 168 | plt.legend(fontsize=18,loc=1) 169 | 170 | plt.subplot(7,1,5) 171 | plt.plot(y_1, p_1[:,4], color='#FF0000', label='omega1', linewidth=3.0) 172 | plt.plot(y_2, p_2[:,4], color='#FFA500', label='omega2', linewidth=3.0) 173 | plt.plot(y_3, p_3[:,4], color='#7CFC00', label='omega3', linewidth=3.0) 174 | plt.plot(y_4, p_4[:,4], color='#1E90FF', label='omega4', linewidth=3.0) 175 | plt.plot(y_5, p_5[:,4], color='#FF00FF', label='omega5', linewidth=3.0) 176 | 177 | plt.xticks(fontsize=18) 178 | plt.yticks(fontsize=18) 179 | 180 | plt.xlabel(u't', fontsize=18) 181 | plt.ylabel(u'omega', fontsize=18) 182 | plt.legend(fontsize=18,loc=1) 183 | 184 | plt.subplot(7,1,6) 185 | plt.plot(y_1, p_1[:,5], color='#FF0000', label='theta_l1', linewidth=3.0) 186 | plt.plot(y_2, p_2[:,5], color='#FFA500', label='theta_l2', linewidth=3.0) 187 | plt.plot(y_3, p_3[:,5], color='#7CFC00', label='theta_l3', linewidth=3.0) 188 | plt.plot(y_4, p_4[:,5], color='#1E90FF', label='theta_l4', linewidth=3.0) 189 | plt.plot(y_5, p_5[:,5], color='#FF00FF', label='theta_l5', linewidth=3.0) 190 | 191 | plt.xticks(fontsize=18) 192 | plt.yticks(fontsize=18) 193 | 194 | plt.xlabel(u't', fontsize=18) 195 | plt.ylabel(u'theta_l', fontsize=18) 196 | plt.legend(fontsize=18,loc=1) 197 | 198 | plt.subplot(7,1,7) 199 | plt.plot(p_1[:,0], p_1[:,1], color='#FF0000', label='iris1', linewidth=3.0) 200 | plt.plot(p_2[:,0], p_2[:,1], color='#FFA500', label='iris2', linewidth=3.0) 201 | plt.plot(p_3[:,0], p_3[:,1], color='#7CFC00', label='iris3', linewidth=3.0) 202 | plt.plot(p_4[:,0], p_4[:,1], color='#1E90FF', label='iris4', linewidth=3.0) 203 | plt.plot(p_5[:,0], p_5[:,1], color='#FF00FF', label='iris5', linewidth=3.0) 204 | 205 | plt.xticks(fontsize=18) 206 | plt.yticks(fontsize=18) 207 | 208 | plt.xlabel(u'x', fontsize=18) 209 | plt.ylabel(u'y', fontsize=18) 210 | plt.legend(fontsize=18,loc=1) 211 | plt.show() -------------------------------------------------------------------------------- /DSLC_xtdrone6/formation_dict.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | formation_dict_6 = {"origin":np.array([[3,0,0],[0,3,0],[3,3,0],[0,6,0],[3,6,0]]),"doubleorigin":np.array([[6,0,0],[0,6,0],[6,6,0],[0,12,0],[6,12,0]]),"T":np.array([[4,0,0],[2,0,0],[2,0,-2],[2,0,-4],[2,0,-6]]) , "diamond": np.array([[2,2,-2],[2,-2,-2],[-2,-2,-2],[-2,2,-2],[0,0,-4]]), "triangle": np.array([[-3,0,-3],[3,0,-3],[-1.5,0,-1.5],[1.5,0,-1.5],[0,0,-3]])} 4 | formation_dict_6["origin"] = np.transpose(formation_dict_6["origin"]) 5 | formation_dict_6["doubleorigin"]=np.transpose(formation_dict_6["doubleorigin"]) 6 | formation_dict_6["T"] = np.transpose(formation_dict_6["T"]) 7 | formation_dict_6["diamond"] = np.transpose(formation_dict_6["diamond"]) 8 | formation_dict_6["triangle"] = np.transpose(formation_dict_6["triangle"]) 9 | 10 | formation_dict_9 = {"origin":np.array([[3,0,0],[6,0,0],[0,3,0],[3,3,0],[6,3,0],[0,6,0],[3,6,0],[6,6,0]]), "cube": np.array([[2,2,2],[-2,2,2],[-2,-2,2],[2,-2,2],[2,2,-2],[-2,2,-2],[-2,-2,-2],[2,-2,-2]]), "pyramid": np.array([[2,2,-2],[-2,2,-2],[-2,-2,-2],[2,-2,-2],[4,4,-4],[-4,4,-4],[-4,-4,-4],[4,-4,-4]]), "triangle": np.array([[0,4,-4],[0,0,-2],[0,0,-4],[0,-2,-2],[0,2,-4],[0,2,-2],[0,-4,-4],[0,-2,-4]])} 11 | formation_dict_9["origin"] = np.transpose(formation_dict_9["origin"]) 12 | formation_dict_9["cube"] = np.transpose(formation_dict_9["cube"]) 13 | formation_dict_9["pyramid"] = np.transpose(formation_dict_9["pyramid"]) 14 | formation_dict_9["triangle"] = np.transpose(formation_dict_9["triangle"]) 15 | 16 | formation_dict_18 = {"origin":np.array([[3,0,0],[6,0,0],[0,3,0],[3,3,0],[6,3,0],[0,6,0],[3,6,0],[6,6,0],[0,9,0],[3,9,0],[6,9,0],[0,12,0],[3,12,0],[6,12,0],[0,15,0],[3,15,0],[6,15,0]]), "cuboid":2*np.array([[0, 2, 0], [2, 0, 0], [2, 2, 0], [4, 0, 0], [4, 2, 0], [0, 0, 2], [0, 2, 2], [2, 0, 2], [2, 2, 2], [4, 0, 2], [4, 2, 2], [0, 0, 4], [0, 2, 4], [2, 0, 4], [2, 2, 4], [4, 0, 4], [4, 2, 4]] 17 | ) , "sphere": 3*np.array([[0.5857864376269, -1.4142135623731, 0.0], [2.0, -2.0, 0.0], [3.4142135623731003, -1.4142135623731, 0.0], [4.0, 0.0, 0.0], [3.4142135623731003, 1.4142135623731, 0.0], [2.0, 2.0, 0.0], [0.5857864376269, 1.4142135623731, 0.0], [1.0, -1.0, -1.4142135623731], [3.0, -1.0, -1.4142135623731], [3.0, 1.0, -1.4142135623731], [1.0, 1.0, -1.4142135623731], [1.0, -1.0, 1.4142135623731], [3.0, -1.0, 1.4142135623731], [3.0, 1.0, 1.4142135623731], [1.0, 1.0, 1.4142135623731], [2.0, 0.0, 2.0], [2.0, 0.0, -2.0]] 18 | ), "diamond": 2*np.array([[2.0, 0.0, 0.0], [4.0, 0.0, 0.0], [4.0, 2.0, 0.0], [4.0, 4.0, 0.0], [2.0, 4.0, 0.0], [0.0, 4.0, 0.0], [0.0, 2.0, 0.0], [1.0, 1.0, 1.0], [3.0, 1.0, 1.0], [3.0, 3.0, 1.0], [1.0, 3.0, 1.0], [1.0, 1.0, -1.0], [3.0, 1.0, -1.0], [3.0, 3.0, -1.0], [1.0, 3.0, -1.0], [2.0, 2.0, 2.0], [2.0, 2.0, -2.0]] 19 | )} 20 | formation_dict_18["origin"] = np.transpose(formation_dict_18["origin"]) 21 | formation_dict_18["cuboid"] = np.transpose(formation_dict_18["cuboid"]) 22 | formation_dict_18["sphere"] = np.transpose(formation_dict_18["sphere"]) 23 | formation_dict_18["diamond"] = np.transpose(formation_dict_18["diamond"]) -------------------------------------------------------------------------------- /DSLC_xtdrone6/formation_dict.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/DSLC_xtdrone6/formation_dict.pyc -------------------------------------------------------------------------------- /DSLC_xtdrone6/get_local_pose.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import PoseStamped, Vector3Stamped 3 | import sys 4 | from gazebo_msgs.msg import ModelStates 5 | 6 | vehicle_type = sys.argv[1] 7 | vehicle_num = int(sys.argv[2]) 8 | multi_pose_pub = [None]*vehicle_num 9 | multi_speed_pub = [None]*vehicle_num 10 | multi_local_pose = [PoseStamped() for i in range(vehicle_num)] 11 | multi_speed = [Vector3Stamped() for i in range(vehicle_num)] 12 | 13 | def gazebo_model_state_callback(msg): 14 | for vehicle_id in range(vehicle_num): 15 | id = msg.name.index(vehicle_type+'_'+str(vehicle_id)) 16 | multi_local_pose[vehicle_id].header.stamp = rospy.Time().now() 17 | multi_local_pose[vehicle_id].header.frame_id = 'map' 18 | multi_local_pose[vehicle_id].pose = msg.pose[id] 19 | multi_speed[vehicle_id].header.stamp = rospy.Time().now() 20 | multi_speed[vehicle_id].header.frame_id = 'map' 21 | multi_speed[vehicle_id].vector = msg.twist[id] 22 | 23 | if __name__ == '__main__': 24 | rospy.init_node(vehicle_type+'_get_pose_groundtruth') 25 | gazebo_model_state_sub = rospy.Subscriber("/gazebo/model_states", ModelStates, gazebo_model_state_callback,queue_size=1) 26 | for i in range(vehicle_num): 27 | multi_pose_pub[i] = rospy.Publisher(vehicle_type+'_'+str(i)+'/mavros/vision_pose/pose', PoseStamped, queue_size=1) 28 | multi_speed_pub[i] = rospy.Publisher(vehicle_type+'_'+str(i)+'/mavros/vision_speed/speed', Vector3Stamped, queue_size=1) 29 | print("Get " + vehicle_type + "_" + str(i) + " groundtruth pose") 30 | rate = rospy.Rate(30) 31 | while not rospy.is_shutdown(): 32 | for i in range(vehicle_num): 33 | multi_pose_pub[i].publish(multi_local_pose[i]) 34 | multi_speed_pub[i].publish(multi_speed[i]) 35 | try: 36 | rate.sleep() 37 | except: 38 | continue 39 | 40 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/leader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: UTF-8 -*- 3 | ### This code is about the distributed formation control with a consensus protocol and the task allocation by KM algorithm 4 | ### For more details, please see the paper on https://arxiv.org/abs/2005.01125 5 | 6 | import rospy 7 | from geometry_msgs.msg import Twist, Vector3, PoseStamped 8 | from std_msgs.msg import String, Int32MultiArray, Float32MultiArray 9 | import sys 10 | import numpy 11 | if sys.argv[2] == '6': 12 | from formation_dict import formation_dict_6 as formation_dict 13 | elif sys.argv[2] == '9': 14 | from formation_dict import formation_dict_9 as formation_dict 15 | elif sys.argv[2] == '18': 16 | from formation_dict import formation_dict_18 as formation_dict 17 | else: 18 | print("Only 6, 9 and 18 UAVs are supported.") 19 | 20 | class Leader: 21 | 22 | def __init__(self, uav_type, leader_id, uav_num): 23 | self.id = leader_id 24 | self.pose = PoseStamped() 25 | self.cmd_vel_enu = Twist() 26 | self.uav_num = uav_num 27 | self.avoid_vel = Vector3(0,0,0) 28 | self.formation_config = 'waiting' 29 | self.origin_formation = formation_dict["origin"] 30 | self.new_formation = self.origin_formation 31 | self.adj_matrix = None 32 | self.communication_topology = None 33 | self.changed_id = numpy.arange(0, self.uav_num - 1) 34 | self.target_height_recorded = False 35 | self.cmd = String() 36 | self.f = 200 37 | self.Kz = 0.5 38 | self.pose_sub = rospy.Subscriber(uav_type+'_'+str(self.id)+"/mavros/local_position/pose", PoseStamped , self.pose_callback, queue_size=1) 39 | self.cmd_vel_sub = rospy.Subscriber("/xtdrone/leader/cmd_vel_flu", Twist, self.cmd_vel_callback, queue_size=1) 40 | self.avoid_vel_sub = rospy.Subscriber("/xtdrone/"+uav_type+'_'+str(self.id)+"/avoid_vel", Vector3, self.avoid_vel_callback, queue_size=1) 41 | self.leader_cmd_sub = rospy.Subscriber("/xtdrone/leader/cmd",String, self.cmd_callback, queue_size=1) 42 | 43 | self.pose_pub = rospy.Publisher("/xtdrone/leader/pose", PoseStamped , queue_size=1) 44 | self.formation_pattern_pub = rospy.Publisher('/xtdrone/formation_pattern', Float32MultiArray, queue_size=1) 45 | self.communication_topology_pub = rospy.Publisher('/xtdrone/communication_topology', Int32MultiArray, queue_size=1) 46 | self.vel_enu_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd_vel_flu', Twist, queue_size=1) 47 | self.cmd_pub = rospy.Publisher('/xtdrone/'+uav_type+'_'+str(self.id)+'/cmd', String, queue_size=1) 48 | 49 | def pose_callback(self, msg): 50 | self.pose = msg 51 | 52 | def cmd_vel_callback(self, msg): 53 | self.cmd_vel_enu = msg 54 | 55 | def cmd_callback(self, msg): 56 | # print(msg.data) 57 | if (msg.data in formation_dict.keys() and not msg.data == self.formation_config): 58 | self.formation_config = msg.data 59 | print("Formation pattern: ", self.formation_config) 60 | # These variables are determined for KM algorithm 61 | self.adj_matrix = self.build_graph(self.origin_formation, formation_dict[self.formation_config]) 62 | self.label_left = numpy.max(self.adj_matrix, axis=1) # init label for the left set 63 | self.label_right = numpy.array([0] * (self.uav_num - 1)) # init label for the right set 64 | self.match_right = numpy.array([-1] * (self.uav_num - 1)) 65 | self.visit_left = numpy.array([0] * (self.uav_num - 1)) 66 | self.visit_right = numpy.array([0] * (self.uav_num - 1)) 67 | self.slack_right = numpy.array([100] * (self.uav_num - 1)) 68 | self.changed_id = self.KM() 69 | # Get a new formation pattern of UAVs based on KM. 70 | self.new_formation = self.get_new_formation(self.changed_id, formation_dict[self.formation_config]) 71 | print(self.new_formation) 72 | self.communication_topology = self.get_communication_topology(self.new_formation) 73 | self.origin_formation = self.new_formation 74 | else: 75 | self.cmd = msg.data 76 | 77 | def avoid_vel_callback(self, msg): 78 | self.avoid_vel = msg 79 | 80 | # 'build_graph', 'find_path' and 'KM' functions are all determined for KM algorithm. 81 | # A graph of UAVs is established based on distances between them in 'build_graph' function. 82 | def build_graph(self, orig_formation, change_formation): 83 | distance = [[0 for i in range(self.uav_num - 1)] for j in range(self.uav_num - 1)] 84 | for i in range(self.uav_num - 1): 85 | for j in range(self.uav_num - 1): 86 | distance[i][j] = numpy.linalg.norm(orig_formation[:, i] - change_formation[:, j]) 87 | distance[i][j] = int(50 - distance[i][j]) 88 | return distance 89 | 90 | # Determine whether a path has been found. 91 | def find_path(self, i): 92 | self.visit_left[i] = True 93 | for j, match_weight in enumerate(self.adj_matrix[i], start=0): 94 | if self.visit_right[j]: 95 | continue 96 | gap = self.label_left[i] + self.label_right[j] - match_weight 97 | if gap == 0: 98 | self.visit_right[j] = True 99 | if self.match_right[j] == -1 or self.find_path(self.match_right[j]): 100 | self.match_right[j] = i 101 | return True 102 | else: 103 | self.slack_right[j] = min(gap, self.slack_right[j]) 104 | return False 105 | 106 | # Main body of KM algorithm. 107 | def KM(self): 108 | for i in range(self.uav_num - 1): 109 | self.slack_right = numpy.array([100] * (self.uav_num - 1)) 110 | while True: 111 | self.visit_left = numpy.array([0] * (self.uav_num - 1)) 112 | self.visit_right = numpy.array([0] * (self.uav_num - 1)) 113 | if self.find_path(i): 114 | break 115 | d = numpy.inf 116 | for j, slack in enumerate(self.slack_right): 117 | if not self.visit_right[j]: 118 | d = min(d, slack) 119 | for k in range(self.uav_num - 1): 120 | if self.visit_left[k]: 121 | self.label_left[k] -= d 122 | if self.visit_right[k]: 123 | self.label_right[k] += d 124 | else: 125 | self.slack_right[k] -= d 126 | return self.match_right 127 | 128 | def get_new_formation(self, changed_id, change_formation): 129 | new_formation = numpy.zeros((3, self.uav_num - 1)) 130 | position = numpy.zeros((3, self.uav_num - 1)) 131 | changed_id = [i + 1 for i in changed_id] 132 | for i in range(0, self.uav_num - 1): 133 | position[:, i] = change_formation[:, i] 134 | 135 | for i in range(0, self.uav_num - 1): 136 | for j in range(0, self.uav_num - 1): 137 | if (j + 1) == changed_id[i]: 138 | new_formation[:, i] = position[:, j] 139 | return new_formation 140 | 141 | def get_communication_topology(self, rel_posi): 142 | 143 | c_num = int((self.uav_num) / 2) 144 | min_num_index_list = [0] * c_num 145 | 146 | comm = [[] for i in range(self.uav_num)] 147 | communication = numpy.ones((self.uav_num, self.uav_num)) * 0 148 | nodes_next = [] 149 | node_flag = [self.uav_num - 1] 150 | node_mid_flag = [] 151 | 152 | rel_d = [0] * (self.uav_num - 1) 153 | 154 | for i in range(0, self.uav_num - 1): 155 | rel_d[i] = pow(rel_posi[0][i], 2) + pow(rel_posi[1][i], 2) + pow(rel_posi[2][i], 2) 156 | 157 | c = numpy.copy(rel_d) 158 | c.sort() 159 | count = 0 160 | 161 | for j in range(0, c_num): 162 | for i in range(0, self.uav_num - 1): 163 | if rel_d[i] == c[j]: 164 | if not i in node_mid_flag: 165 | min_num_index_list[count] = i 166 | node_mid_flag.append(i) 167 | count = count + 1 168 | if count == c_num: 169 | break 170 | if count == c_num: 171 | break 172 | 173 | for j in range(0, c_num): 174 | nodes_next.append(min_num_index_list[j]) 175 | 176 | comm[self.uav_num - 1].append(min_num_index_list[j]) 177 | 178 | size_ = len(node_flag) 179 | 180 | while (nodes_next != []) and (size_ < (self.uav_num - 1)): 181 | 182 | next_node = nodes_next[0] 183 | nodes_next = nodes_next[1:] 184 | min_num_index_list = [0] * c_num 185 | node_mid_flag = [] 186 | rel_d = [0] * (self.uav_num - 1) 187 | for i in range(0, self.uav_num - 1): 188 | 189 | if i == next_node or i in node_flag: 190 | 191 | rel_d[i] = 2000 192 | else: 193 | 194 | rel_d[i] = pow((rel_posi[0][i] - rel_posi[0][next_node]), 2) + pow( 195 | (rel_posi[1][i] - rel_posi[1][next_node]), 2) + pow((rel_posi[2][i] - rel_posi[2][next_node]), 196 | 2) 197 | c = numpy.copy(rel_d) 198 | c.sort() 199 | count = 0 200 | 201 | for j in range(0, c_num): 202 | for i in range(0, self.uav_num - 1): 203 | if rel_d[i] == c[j]: 204 | if not i in node_mid_flag: 205 | min_num_index_list[count] = i 206 | node_mid_flag.append(i) 207 | count = count + 1 208 | if count == c_num: 209 | break 210 | if count == c_num: 211 | break 212 | node_flag.append(next_node) 213 | 214 | size_ = len(node_flag) 215 | 216 | for j in range(0, c_num): 217 | 218 | if min_num_index_list[j] in node_flag: 219 | 220 | nodes_next = nodes_next 221 | 222 | else: 223 | if min_num_index_list[j] in nodes_next: 224 | nodes_next = nodes_next 225 | else: 226 | nodes_next.append(min_num_index_list[j]) 227 | 228 | comm[next_node].append(min_num_index_list[j]) 229 | 230 | for i in range(0, self.uav_num): 231 | for j in range(0, self.uav_num - 1): 232 | if i == 0: 233 | if j in comm[self.uav_num - 1]: 234 | communication[j + 1][i] = 1 235 | else: 236 | communication[j + 1][i] = 0 237 | else: 238 | if j in comm[i - 1] and i < (j+1): 239 | communication[j + 1][i] = 1 240 | else: 241 | communication[j + 1][i] = 0 242 | 243 | for i in range(1, self.uav_num): # 防止某个无人机掉队 244 | if sum(communication[i]) == 0: 245 | communication[i][0] = 1 246 | return communication 247 | 248 | 249 | def loop(self): 250 | rospy.init_node('leader') 251 | rate = rospy.Rate(self.f) 252 | while True: 253 | self.cmd_vel_enu.linear.x = self.cmd_vel_enu.linear.x 254 | self.cmd_vel_enu.linear.y = self.cmd_vel_enu.linear.y 255 | self.cmd_vel_enu.linear.z = self.cmd_vel_enu.linear.z 256 | formation_pattern = Float32MultiArray() 257 | formation_pattern.data = self.new_formation.flatten().tolist() 258 | self.formation_pattern_pub.publish(formation_pattern) 259 | if(not self.communication_topology is None): 260 | communication_topology = Int32MultiArray() 261 | communication_topology.data = self.communication_topology.flatten().tolist() 262 | self.communication_topology_pub.publish(communication_topology) 263 | self.vel_enu_pub.publish(self.cmd_vel_enu) 264 | self.pose_pub.publish(self.pose) 265 | self.cmd_pub.publish(self.cmd) 266 | try: 267 | rate.sleep() 268 | except: 269 | continue 270 | 271 | if __name__ == '__main__': 272 | leader = Leader(sys.argv[1], 0, int(sys.argv[2])) 273 | leader.loop() 274 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/multi_vehicle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/multi_vehicle_communication.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | iris_num=6 3 | typhoon_h480_num=0 4 | solo_num=0 5 | plane_num=0 6 | rover_num=0 7 | standard_vtol_num=0 8 | tiltrotor_num=0 9 | tailsitter_num=0 10 | 11 | vehicle_num=0 12 | while(( $vehicle_num< iris_num)) 13 | do 14 | python3 multirotor_communication_sigle.py iris $vehicle_num& 15 | let "vehicle_num++" 16 | done 17 | 18 | vehicle_num=0 19 | while(( $vehicle_num< typhoon_h480_num)) 20 | do 21 | python3 multirotor_communication_sigle.py typhoon_h480 $vehicle_num& 22 | let "vehicle_num++" 23 | done 24 | 25 | vehicle_num=0 26 | while(( $vehicle_num< solo_num)) 27 | do 28 | python3 multirotor_communication_sigle.py solo $vehicle_num& 29 | let "vehicle_num++" 30 | done 31 | 32 | vehicle_num=0 33 | while(( $vehicle_num< plane_num)) 34 | do 35 | python3 plane_communication.py $vehicle_num& 36 | let "vehicle_num++" 37 | done 38 | 39 | vehicle_num=0 40 | while(( $vehicle_num< rover_num)) 41 | do 42 | python3 rover_communication.py $vehicle_num& 43 | let "vehicle_num++" 44 | done 45 | 46 | vehicle_num=0 47 | while(( $vehicle_num< standard_vtol_num)) 48 | do 49 | python3 vtol_communication.py standard_vtol $vehicle_num& 50 | let "vehicle_num++" 51 | done 52 | 53 | vehicle_num=0 54 | while(( $vehicle_num< tiltrotor_num)) 55 | do 56 | python3 vtol_communication.py tiltrotor $vehicle_num& 57 | let "vehicle_num++" 58 | done 59 | 60 | vehicle_num=0 61 | while(( $vehicle_num< tailsitter_num)) 62 | do 63 | python3 vtol_communication.py tailsitter $vehicle_num& 64 | let "vehicle_num++" 65 | done 66 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/multirotor_communication_sigle.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from mavros_msgs.msg import PositionTarget 3 | from mavros_msgs.srv import CommandBool, SetMode 4 | from geometry_msgs.msg import PoseStamped, Pose, Twist 5 | from std_msgs.msg import String 6 | from pyquaternion import Quaternion 7 | import sys 8 | 9 | rospy.init_node(sys.argv[1]+'_'+sys.argv[2]+"_communication") 10 | rate = rospy.Rate(30) 11 | 12 | class Sigle_Communication: 13 | 14 | def __init__(self, vehicle_type, vehicle_id): 15 | 16 | self.vehicle_type = vehicle_type 17 | self.vehicle_id = vehicle_id 18 | self.current_position = None 19 | self.current_yaw = 0 20 | self.hover_flag = 0 21 | self.target_motion = PositionTarget() 22 | self.arm_state = False 23 | self.motion_type = 0 24 | self.flight_mode = None 25 | self.mission = None 26 | self.last_cmd = None 27 | 28 | ''' 29 | ros subscribers 30 | ''' 31 | self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback,queue_size=1) 32 | self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback,queue_size=3) 33 | self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback,queue_size=1) 34 | self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback,queue_size=1) 35 | self.cmd_vel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_flu", Twist, self.cmd_vel_flu_callback,queue_size=1) 36 | self.cmd_vel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_vel_enu", Twist, self.cmd_vel_enu_callback,queue_size=1) 37 | self.cmd_accel_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_flu", Twist, self.cmd_accel_flu_callback,queue_size=1) 38 | self.cmd_accel_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_accel_enu", Twist, self.cmd_accel_enu_callback,queue_size=1) 39 | 40 | ''' 41 | ros publishers 42 | ''' 43 | self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=1) 44 | 45 | ''' 46 | ros services 47 | ''' 48 | self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool) 49 | self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode) 50 | 51 | print(self.vehicle_type+'_'+self.vehicle_id+": "+"communication initialized") 52 | 53 | def start(self): 54 | ''' 55 | main ROS thread 56 | ''' 57 | while not rospy.is_shutdown(): 58 | self.target_motion_pub.publish(self.target_motion) 59 | rate.sleep() 60 | 61 | def local_pose_callback(self, msg): 62 | self.current_position = msg.pose.position 63 | self.current_yaw = self.q2yaw(msg.pose.orientation) 64 | 65 | def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0): 66 | target_raw_pose = PositionTarget() 67 | target_raw_pose.coordinate_frame = self.coordinate_frame 68 | 69 | target_raw_pose.position.x = x 70 | target_raw_pose.position.y = y 71 | target_raw_pose.position.z = z 72 | 73 | target_raw_pose.velocity.x = vx 74 | target_raw_pose.velocity.y = vy 75 | target_raw_pose.velocity.z = vz 76 | 77 | target_raw_pose.acceleration_or_force.x = afx 78 | target_raw_pose.acceleration_or_force.y = afy 79 | target_raw_pose.acceleration_or_force.z = afz 80 | 81 | target_raw_pose.yaw = yaw 82 | target_raw_pose.yaw_rate = yaw_rate 83 | 84 | if(self.motion_type == 0): #0:ignore v,a and yaw rate 85 | target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ 86 | + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ 87 | + PositionTarget.IGNORE_YAW_RATE 88 | if(self.motion_type == 1): 89 | target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ 90 | + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ 91 | + PositionTarget.IGNORE_YAW 92 | if(self.motion_type == 2): 93 | target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ 94 | + PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ 95 | + PositionTarget.IGNORE_YAW 96 | 97 | return target_raw_pose 98 | 99 | def cmd_pose_flu_callback(self, msg): 100 | self.coordinate_frame = 9 101 | self.motion_type = 0 102 | yaw = self.q2yaw(msg.orientation) 103 | self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw) 104 | 105 | def cmd_pose_enu_callback(self, msg): 106 | self.coordinate_frame = 1 107 | self.motion_type = 0 108 | yaw = self.q2yaw(msg.orientation) 109 | self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z,yaw=yaw) 110 | 111 | def cmd_vel_flu_callback(self, msg): 112 | self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) 113 | if self.hover_flag == 0: 114 | self.coordinate_frame = 8 115 | self.motion_type = 1 116 | self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z) 117 | 118 | def cmd_vel_enu_callback(self, msg): 119 | self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) 120 | if self.hover_flag == 0: 121 | self.coordinate_frame = 1 122 | self.motion_type = 1 123 | self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z) 124 | 125 | def cmd_accel_flu_callback(self, msg): 126 | self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) 127 | if self.hover_flag == 0: 128 | self.coordinate_frame = 8 129 | self.motion_type = 2 130 | self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z) 131 | 132 | def cmd_accel_enu_callback(self, msg): 133 | self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z) 134 | if self.hover_flag == 0: 135 | self.coordinate_frame = 1 136 | self.motion_type = 2 137 | self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z) 138 | 139 | def hover_state_transition(self,x,y,z,w): 140 | if abs(x) > 0.02 or abs(y) > 0.02 or abs(z) > 0.02 or abs(w) > 0.005: 141 | self.hover_flag = 0 142 | self.flight_mode = 'OFFBOARD' 143 | elif not self.flight_mode == "HOVER": 144 | self.hover_flag = 1 145 | self.flight_mode = 'HOVER' 146 | self.hover() 147 | def cmd_callback(self, msg): 148 | if msg.data == self.last_cmd or msg.data == '' or msg.data == 'stop controlling': 149 | return 150 | 151 | elif msg.data == 'ARM': 152 | self.arm_state =self.arm() 153 | print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state)) 154 | 155 | elif msg.data == 'DISARM': 156 | self.arm_state = not self.disarm() 157 | print(self.vehicle_type+'_'+self.vehicle_id+": Armed "+str(self.arm_state)) 158 | 159 | elif msg.data[:-1] == "mission" and not msg.data == self.mission: 160 | self.mission = msg.data 161 | print(self.vehicle_type+'_'+self.vehicle_id+": "+msg.data) 162 | 163 | else: 164 | self.flight_mode = msg.data 165 | self.flight_mode_switch() 166 | 167 | self.last_cmd = msg.data 168 | 169 | def q2yaw(self, q): 170 | if isinstance(q, Quaternion): 171 | rotate_z_rad = q.yaw_pitch_roll[0] 172 | else: 173 | q_ = Quaternion(q.w, q.x, q.y, q.z) 174 | rotate_z_rad = q_.yaw_pitch_roll[0] 175 | 176 | return rotate_z_rad 177 | 178 | def arm(self): 179 | if self.armService(True): 180 | return True 181 | else: 182 | print(self.vehicle_type+'_'+self.vehicle_id+": arming failed!") 183 | return False 184 | 185 | def disarm(self): 186 | if self.armService(False): 187 | return True 188 | else: 189 | print(self.vehicle_type+'_'+self.vehicle_id+": disarming failed!") 190 | return False 191 | 192 | def hover(self): 193 | self.coordinate_frame = 1 194 | self.motion_type = 0 195 | self.target_motion = self.construct_target(x=self.current_position.x,y=self.current_position.y,z=self.current_position.z,yaw=self.current_yaw) 196 | print(self.vehicle_type+'_'+self.vehicle_id+":"+self.flight_mode) 197 | 198 | def flight_mode_switch(self): 199 | if self.flight_mode == 'HOVER': 200 | self.hover_flag = 1 201 | self.hover() 202 | elif self.flightModeService(custom_mode=self.flight_mode): 203 | print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode) 204 | return True 205 | else: 206 | print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode+"failed") 207 | return False 208 | 209 | if __name__ == '__main__': 210 | sigle_communication = Sigle_Communication(sys.argv[1],sys.argv[2]) 211 | sigle_communication.start() 212 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/multirotor_keyboard_control_promotion.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import Twist 3 | import sys, select, os 4 | import tty, termios 5 | from std_msgs.msg import String 6 | 7 | 8 | MAX_LINEAR = 20 9 | MAX_ANG_VEL = 3 10 | LINEAR_STEP_SIZE = 0.1 11 | ANG_VEL_STEP_SIZE = 0.1 12 | 13 | cmd_vel_mask = False 14 | ctrl_leader = False 15 | 16 | msg2all = """ 17 | Control Your XTDrone! 18 | To all drones (press g to control the leader) 19 | --------------------------- 20 | 1 2 3 4 5 6 7 8 9 0 21 | w r t y i 22 | a s d g j k l 23 | x v b n , 24 | 25 | w/x : increase/decrease forward 26 | a/d : increase/decrease leftward 27 | i/, : increase/decrease yaw 28 | j/l : increase/decrease yaw 29 | r : return home 30 | t/y : arm/disarm 31 | v/n : takeoff/land 32 | b : offboard 33 | s/k : hover and remove the mask of keyboard control 34 | f/h : turn right/left 35 | 0 : mask the keyboard control (for single UAV) 36 | 0~9 : extendable mission(eg.different formation configuration) 37 | this will mask the keyboard control 38 | g : control the leader 39 | CTRL-C to quit 40 | """ 41 | 42 | msg2leader = """ 43 | Control Your XTDrone! 44 | To the leader (press g to control all drones) 45 | --------------------------- 46 | 1 2 3 4 5 6 7 8 9 0 47 | w r t y i 48 | a s d g j k l 49 | x v b n , 50 | 51 | w/x : increase/decrease forward 52 | a/d : increase/decrease leftward 53 | i/, : increase/decrease upward 54 | j/l : increase/decrease yaw 55 | r : return home 56 | t/y : arm/disarm 57 | v/n : takeoff/land 58 | b : offboard 59 | s/k : hover and remove the mask of keyboard control 60 | g : control all drones 61 | f/h : turn right/left 62 | CTRL-C to quit 63 | """ 64 | 65 | e = """ 66 | Communications Failed 67 | """ 68 | 69 | def getKey(): 70 | tty.setraw(sys.stdin.fileno()) 71 | rlist, _, _ = select.select([sys.stdin], [], [], 0.1) 72 | if rlist: 73 | key = sys.stdin.read(1) 74 | else: 75 | key = '' 76 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 77 | return key 78 | 79 | def print_msg(): 80 | if ctrl_leader: 81 | print(msg2leader) 82 | else: 83 | print(msg2all) 84 | 85 | 86 | 87 | if __name__=="__main__": 88 | 89 | settings = termios.tcgetattr(sys.stdin) 90 | 91 | multirotor_type = 'iris' 92 | multirotor_num = 6 93 | control_type = 'vel' 94 | 95 | formation_configs = ['origin', 'doubleorigin','T', 'diamond', 'triangle'] 96 | 97 | 98 | cmd= String() 99 | twist = Twist() 100 | 101 | rospy.init_node(multirotor_type + '_multirotor_keyboard_control') 102 | 103 | if control_type == 'vel': 104 | multi_cmd_vel_flu_pub = [None]*multirotor_num 105 | multi_cmd_pub = [None]*multirotor_num 106 | for i in range(multirotor_num): 107 | multi_cmd_vel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_vel_flu', Twist, queue_size=1) 108 | multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=3) 109 | leader_cmd_vel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_vel_flu", Twist, queue_size=1) 110 | leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=1) 111 | 112 | else: 113 | multi_cmd_accel_flu_pub = [None]*multirotor_num 114 | multi_cmd_pub = [None]*multirotor_num 115 | for i in range(multirotor_num): 116 | multi_cmd_accel_flu_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd_accel_flu', Twist, queue_size=1) 117 | multi_cmd_pub[i] = rospy.Publisher('/xtdrone/'+multirotor_type+'_'+str(i)+'/cmd',String,queue_size=1) 118 | leader_cmd_accel_flu_pub = rospy.Publisher("/xtdrone/leader/cmd_accel_flu", Twist, queue_size=1) 119 | leader_cmd_pub = rospy.Publisher("/xtdrone/leader/cmd", String, queue_size=3) 120 | 121 | forward = 0.0 122 | leftward = 0.0 123 | upward = 0.0 124 | angular = 0.0 125 | 126 | print_msg() 127 | while(1): 128 | key = getKey() 129 | if key == 'w' : 130 | forward = forward + LINEAR_STEP_SIZE 131 | print_msg() 132 | if control_type == 'vel': 133 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 134 | else: 135 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 136 | 137 | elif key == 'x' : 138 | forward = forward - LINEAR_STEP_SIZE 139 | print_msg() 140 | if control_type == 'vel': 141 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 142 | else: 143 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 144 | 145 | elif key == 'a' : 146 | 147 | leftward = leftward + LINEAR_STEP_SIZE 148 | print_msg() 149 | if control_type == 'vel': 150 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 151 | else: 152 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 153 | 154 | elif key == 'd' : 155 | leftward = leftward - LINEAR_STEP_SIZE 156 | print_msg() 157 | if control_type == 'vel': 158 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 159 | else: 160 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 161 | 162 | elif key == 'i' : 163 | upward = upward + LINEAR_STEP_SIZE 164 | print_msg() 165 | if control_type == 'vel': 166 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 167 | else: 168 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 169 | 170 | elif key == ',' : 171 | upward = upward - LINEAR_STEP_SIZE 172 | print_msg() 173 | if control_type == 'vel': 174 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 175 | else: 176 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 177 | 178 | elif key == 'j': 179 | angular = angular + ANG_VEL_STEP_SIZE 180 | print_msg() 181 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 182 | 183 | elif key == 'l': 184 | angular = angular - ANG_VEL_STEP_SIZE 185 | print_msg() 186 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 187 | 188 | elif key == 'r': 189 | cmd = 'AUTO.RTL' 190 | print_msg() 191 | print('Returning home') 192 | elif key == 't': 193 | cmd = 'ARM' 194 | print_msg() 195 | print('Arming') 196 | elif key == 'y': 197 | cmd = 'DISARM' 198 | print_msg() 199 | print('Disarming') 200 | elif key == 'v': 201 | cmd = 'AUTO.TAKEOFF' 202 | cmd = '' 203 | print_msg() 204 | elif key == 'b': 205 | cmd = 'OFFBOARD' 206 | print_msg() 207 | print('Offboard') 208 | elif key == 'n': 209 | cmd = 'AUTO.LAND' 210 | print_msg() 211 | print('Landing') 212 | elif key == 'g': 213 | ctrl_leader = not ctrl_leader 214 | print_msg() 215 | elif key=='f': 216 | forward = 2 217 | angular = -0.2 218 | print_msg() 219 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 220 | elif key=='h': 221 | forward=2 222 | angular = 0.2 223 | print_msg() 224 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 225 | elif key in ['k', 's']: 226 | cmd_vel_mask = False 227 | forward = 0.0 228 | leftward = 0.0 229 | upward = 0.0 230 | angular = 0.0 231 | cmd = 'HOVER' 232 | print_msg() 233 | print("currently:\t forward vel %.2f\t leftward vel %.2f\t upward vel %.2f\t angular %.2f " % (forward, leftward, upward, angular)) 234 | print('Hover') 235 | else: 236 | for i in range(10): 237 | if key == str(i): 238 | cmd = formation_configs[i] 239 | print_msg() 240 | print(cmd) 241 | cmd_vel_mask = True 242 | if (key == '\x03'): 243 | break 244 | 245 | if forward > MAX_LINEAR: 246 | forward = MAX_LINEAR 247 | elif forward < -MAX_LINEAR: 248 | forward = -MAX_LINEAR 249 | if leftward > MAX_LINEAR: 250 | leftward = MAX_LINEAR 251 | elif leftward < -MAX_LINEAR: 252 | leftward = -MAX_LINEAR 253 | if upward > MAX_LINEAR: 254 | upward = MAX_LINEAR 255 | elif upward < -MAX_LINEAR: 256 | upward = -MAX_LINEAR 257 | if angular > MAX_ANG_VEL: 258 | angular = MAX_ANG_VEL 259 | elif angular < -MAX_ANG_VEL: 260 | angular = - MAX_ANG_VEL 261 | 262 | twist.linear.x = forward; twist.linear.y = leftward ; twist.linear.z = upward 263 | twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = angular 264 | 265 | for i in range(multirotor_num): 266 | if ctrl_leader: 267 | if control_type == 'vel': 268 | leader_cmd_vel_flu_pub.publish(twist) 269 | else: 270 | leader_cmd_accel_flu_pub.publish(twist) 271 | leader_cmd_pub.publish(cmd) 272 | else: 273 | if not cmd_vel_mask: 274 | if control_type == 'vel': 275 | multi_cmd_vel_flu_pub[i].publish(twist) 276 | else: 277 | multi_cmd_accel_flu_pub[i].publish(twist) 278 | multi_cmd_pub[i].publish(cmd) 279 | 280 | cmd = '' 281 | 282 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 283 | 284 | -------------------------------------------------------------------------------- /DSLC_xtdrone6/run_formation_baseline.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | python3 leader.py 'iris' 6 & 3 | python3 avoid.py 'iris' 6 'vel' & 4 | python3 e1_pub.py 'iris' 6 'vel' & 5 | uav_id=1 6 | while(( $uav_id< 6 )) 7 | do 8 | python3 follower_consensus_baseline.py 'iris' $uav_id 6 'vel' & 9 | let "uav_id++" 10 | done -------------------------------------------------------------------------------- /DSLC_xtdrone6/run_formation_promotion.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | python3 leader.py 'iris' 6 & 3 | python3 avoid.py 'iris' 6 'vel' & 4 | python3 e1_pub.py 'iris' 6 'vel' & 5 | uav_id=1 6 | while(( $uav_id< 6 )) 7 | do 8 | python3 follower_consensus_promotion.py 'iris' $uav_id 6 'vel' & 9 | let "uav_id++" 10 | done -------------------------------------------------------------------------------- /DSLC_xtdrone6/usebook: -------------------------------------------------------------------------------- 1 | %% enter the catalog of the file that will be utilized 2 | %% run the command sequentially 3 | 4 | % load worlds and the drones 5 | 1.roslaunch multi_vehicle.launch 6 | 7 | % obtain the position information of drones 8 | 2.python3 get_local_pose.py iris 6 9 | 10 | % build the communication network among drones 11 | 3.bash multi_vehicle_communication.sh 12 | 13 | % keyboard control code 14 | 4.python3 multirotor_keyboard_control_promotion.py 15 | % use keyboard to control all drones to take off and press s to hover after a desired height.Then press g to enter leader control mode. 16 | 17 | 5.bash run_formation_promotion.sh 18 | %When the script has been launched, then switch to the keyboard control code, pressing f or h can achieve turning in 6 drones formation. Moreover, pressing number 0-9 can change the formation. 19 | 20 | 5.bash run_formation_baseline.sh 21 | 22 | %Note: 23 | % When the run_formation_promotion script is running and the drones are stationary, switch to the keyboard control terminal to press w to give leader a specified velocity for formation flight. Turning and formation transformation can also be achieved when the drones are running. 24 | % The run_formation_baseline script is for comparison with run_formation_promotion script in performance of running straight line at variable velocity. 25 | 26 | % The following script is for plot the information of drones such as error,position and etc. 27 | 6.python3 draw_figure.py 28 | 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | GPLv4 License 3 | 4 | Copyright (C) <2023> 5 | 6 | This program is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program. If not, see . 18 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | # Distrbuted policy learning for DMPC Project 4 | 5 | 6 | This repository contains material related to the project on `Distributed policy learning for DMPC`. 7 | 8 | ## Table of Contents 9 | 10 | ### Tutorials 11 | 12 | The tutorials lead you through implementing the code files uploaded. 13 | 14 | * DLPC_training: Implement the code to verify the convergence condition of actor-critic learning and the closed-stability condition under actor-critic learning in the receding horizon control framework. The code is implemented in Matlab. 15 | * DLPC_xtdrone: Deploy the control policy to control a number of multirotor drones in the Gazebo. This part is based on XTDrone, PX4, and MAVROS, containing materials related to [XTDrone project](https://github.com/robin-shaun/XTDrone/blob/master). The code is implemented in Python. 16 | * DLPC_xtdrone6: Control 6 multirotor drones to realize formation control and transformation. 17 | * DLPC_xtdrone18: Control 18 multirotor drones to realize formation control and transformation. 18 | * DLPC_solving_one_robot_control: Implement the code to solve the centralized control problem of one robot distributedly and compare it with the centralized version. The code is implemented in Matlab. 19 | * dlpc_online_train_scales_to_10000.py: The Python code for online training of DLPC. 20 | * dlpc_online_train_scales_to_10000.m: The matlab code for online training of DLPC. 21 | 22 | ## Dependencies 23 | 24 | There is no dependency for `DLPC_training` within Matlab. As for `DLPC_xtdrone`, please follow the instructions in [XTDrone project](https://github.com/robin-shaun/XTDrone/blob/master) to complete the environment installation and basic configuration. 25 | 26 | ## Run DLPC_xtdrone 27 | 28 | To run the code in this repository, follow the instructions below. 29 | 30 | 1. Load worlds and drones. 31 | ```bash 32 | roslaunch multi_vehicle.launch 33 | ``` 34 | 35 | 3. Obtain the position information of drones. Replace 6 with the number in the name of the selected file folder. 36 | ```bash 37 | python3 get_local_pose.py iris 6 38 | ``` 39 | 40 | 5. Build the communication network among drones. 41 | ```bash 42 | multi_vehicle_communication.sh 43 | ``` 44 | 45 | 6. Keyboard control code. 46 | ```bash 47 | python3 multirotor_keyboard_control_promotion.py 48 | ``` 49 | *Use the keyboard to control all drones to take off and press ‘s’ to hover after a desired height. Then press ‘g’ to enter leader control mode. 50 | 51 | 7. Run the DLPC code for formation control. 52 | ```bash 53 | run_formation_promotion.sh 54 | ``` 55 | *Note: When the script is running, and the drones are stationary, switch to the keyboard control terminal to press ‘w’ to give the leader a specified velocity. After the drones achieve the specified formation, press ‘f’ or ‘h’ to turn or press numbers 0-9 to change the formation. 56 | 57 | 8. run the baseline controller for comparison in a straight-line formation scenario. 58 | ```bash 59 | run_formation_baseline.sh 60 | ``` 61 | 62 | 9. Run the following script for the figure plot. 63 | ```bash 64 | python3 draw_figure.py 65 | ``` 66 | -------------------------------------------------------------------------------- /dlpc_online_train_scales_to_10000.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinglongzhangnudt/policy-learning-for-distributed-mpc/16274d5063d2264eb5d780e17eada1d1aeba4e1d/dlpc_online_train_scales_to_10000.m --------------------------------------------------------------------------------