I am working on a drone obstacle avoidance project. And I am using the SNOPT tool to solve the non-convex optimization problem. Unfortunately, as a new SNOPT beginner, some problems occured and I am not able to fix them.
It always shows "SNOPTA EXIT 140 -- system error SNOPTA INFO 142 -- error in basis package". I have checked the output file and it said that the constraint and objective gradients seemed to be ok. Here are my matlab code and the output file
function [x,F,xmul,Fmul,INFO] = opt_formation(lb,ub)
options.screen = 'on';
options.printfile = 'opt_formation.out';
options.specsfile = which('opt_formation.spc');
options.name = 'opt_formation';
[x,xlow,xupp,xmul,xstate, ...
Flow,Fupp,Fmul,Fstate, ...
ObjRow, ObjAdd,A.val,A.row,A.col,G.row,G.col] = FormationData(lb,ub);
% size(A.val)
% size(A.row)
% size(A.col)
% [F]=FormationFun(x)
[x,F,INFO,xmul,Fmul]= snopt(x, xlow, xupp, xmul, xstate, ...
Flow, Fupp, Fmul, Fstate, ...
@FormationFun, ObjRow, ObjAdd, ...
A, G, options);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x,xlow,xupp,xmul,xstate, ...
Flow,Fupp,Fmul,Fstate, ...
ObjAdd,ObjRow,A, iAfun,jAvar,iGfun,jGvar] = FormationData(lb,ub)
global Funic
% F0.polytopes{1,F0.robot_number}
[A0, b0] = iris.thirdParty.polytopes.vert2lcon(Funic.polytopes{1,Funic.robot_number}.Vertices);
% A1=[A0,zeros(size(A0,1),1)];
ObjRow=size(A0,1)*size(Funic.Formations{1,2},2)+3;
ObjAdd=Funic.Formations{1,3};
iAfun=[];
jAvar=[];
A=[];
iGfun=[];
jGvar=[];
n=8;
nF=size(A0,1)*size(Funic.Formations{1,2},2)+3;
x=zeros(n,1);
xstate=zeros(n,1);
xmul=zeros(n,1);
xlow=-inf*ones(n,1);
xupp=inf*ones(n,1);
Fstate=zeros(nF,1);
Fmul=zeros(nF,1);
Flow=-inf*ones(nF,1);
Fupp=inf*ones(nF,1);
xlow(1)=lb(1);
xlow(2)=lb(2);
xlow(3)=0;
xlow(4)=0;
xlow(6)=0;
xlow(7)=0;
% xlow(8)=0;
xupp(1)=ub(1);
xupp(2)=ub(2);
xupp(3)=0;
xupp(6)=0;
xupp(7)=0;
% xupp(8)=0;
xstate(1)=Funic.x0(1,1);
xstate(2)=Funic.x0(2,1);
xstate(3)=0;
xstate(4)=1;
xstate(5)=1;
xstate(6)=0;
xstate(7)=0;
xstate(8)=0;
% n_convhull=size(Funic.convex_hull{1,Funic.robot_number},1);
for i=1:size(Funic.Formations{1,2},2)
for j=1:size(A0,1)
Fupp((i-1)*size(A0,1)+j)=b0(j);
end
end
Flow(size(A0,1)*size(Funic.Formations{1,2},2)+2)=1;
Fupp(size(A0,1)*size(Funic.Formations{1,2},2)+2)=1;
Flow(size(A0,1)*size(Funic.Formations{1,2},2)+1)=2*Funic.drone_radius;
% disp('1111')
% Funic.Formations
% size(A0,1)
for k=1:size(Funic.Formations{1,2},2)
for i=1:size(A0,1)
iAfun=[iAfun;(k-1)*size(A0,1)+i];
iAfun=[iAfun;(k-1)*size(A0,1)+i];
for j=1:2
% disp('vvvvvvv')
jAvar=[jAvar;j];
% A0(i,j)
A=[A;A0(i,j)];
end
end
end
% disp('vqqqqqqqvvvvvv')
iAfun=[iAfun;size(A0,1)*size(Funic.Formations{1,2},2)+1];
jAvar=[jAvar;4];
A=[A;Funic.Formations{1,4}];%%%规定无人机之间间距不能小于0.07m
% size(iAfun)
% size(jAvar)
% size(A)
% disp('Fupp Flow')
% size(Fupp)
% size(Flow)
for k=1:size(Funic.Formations{1,2},2)
for i=1:size(A0,1)
for j=[1,2,4,5,8]
iGfun=[iGfun;i+(k-1)*size(A0,1)];
jGvar=[jGvar;j];
end
end
end
iGfun=[iGfun; size(Funic.Formations{1,2},2)*size(A0,1)+1];
jGvar=[jGvar; 4];
iGfun=[iGfun;size(Funic.Formations{1,2},2)*size(A0,1)+2;size(Funic.Formations{1,2},2)*size(A0,1)+2];
jGvar=[jGvar;[5;8]];
for i=1:5
iGfun=[iGfun;size(Funic.Formations{1,2},2)*size(A0,1)+3];
end
jGvar=[jGvar;[1;2;4;5;8]];`
function [F,G]=FormationFun(x)
global Funic
nConv=4;
x0=Funic.x0;
R=0.3;
f=[x0-0.5*[R;0], x0+0.5*[R;0], x0+[0;-0.5*R], x0+[0;-R]];
t=[x(1);x(2);0];%%%%%%%
ws=0.5;
wq=0.3;
F=[];
G=[];
% Funic.robot_number
[A1, b1]=iris.thirdParty.polytopes.vert2lcon(Funic.polytopes{1,Funic.robot_number}.Vertices);
nA=size(A1,1);
A1=[A1,zeros(nA,1)];
for i=1:nConv
f1=f(1,i);
f2=f(2,i);
Rotq=[f1*(1-2*x(8)^2)+2*x(5)*x(8)*f2;-2*x(5)*x(8)*f1+f2*(1-2*x(8)^2);0];
F=[F;A1*(t+x(4)*Rotq)];
% F=[F;A1*(t+x(4)*quatrotate([x(5),0,0,x(8)],fl)')];
DRotq=[2*f2*x(8), 0, 0, 2*f2*x(5)-4*f1*x(8); -2*f1*x(8), 0, 0, -2*f1*x(5)-4*f2*x(8); 0, 0, 0, 0];
T=[A1, A1*Rotq, x(4)*A1*DRotq];
for j=1:size(T,1)
G=[G;T(j,[1,2,4,5,8])'];
end
end
F=[F;Funic.Formations{1,4}*x(4)];
F=[F;x(5)^2+x(8)^2];
F=[F;(x(1)-Funic.g_dir(1,1))^2+(x(2)-Funic.g_dir(2,1))^2 + ws*(x(4)-1)^2 + wq*((x(5)-1)^2+x(8)^2)];
G=[G;Funic.Formations{1,4}];
G=[G;2*[x(5);x(8)]];
G=[G;2*([x(1);x(2)]-Funic.g_dir)];
G=[G;2*ws*(x(4)-1)];
G=[G;2*wq*([x(5);x(8)]-[1;0])];
% disp('F size')
% size(F)
end
And the output file:
==============================
S N O P T 7.7.7 (Feb 2021)
==============================
Trial version of SNOPT -- for evaluation or academic purposes only
1
SNMEMA EXIT 100 -- finished successfully
SNMEMA INFO 104 -- memory requirements estimated
Trial version of SNOPT -- for evaluation or academic purposes only
===> WARNING - Column 3 of the Jacobian is empty.
===> WARNING - Column 6 of the Jacobian is empty.
===> WARNING - Column 7 of the Jacobian is empty.
1
Parameters
==========
Files
-----
Solution file.......... 0 Old basis file ........ 0 Standard input......... 5
Insert file............ 0 New basis file ........ 0 (Printer).............. 10
Punch file............. 0 Backup basis file...... 0 (Specs file)........... 0
Load file.............. 0 Dump file.............. 0 Standard output........ 6
Frequencies
-----------
Print frequency........ 100 Check frequency........ 60 Save new basis map..... 100
Summary frequency...... 100 Factorization frequency 50 Expand frequency....... 10000
QP subproblems
--------------
QPsolver Cholesky......
Scale tolerance........ 0.900 Minor feasibility tol.. 1.00E-06 Iteration limit........ 10000
Scale option........... 0 Minor optimality tol.. 1.00E-06 Minor print level...... 1
Crash tolerance........ 0.100 Pivot tolerance........ 3.25E-11 New superbasics........ 99
Crash option........... 3 Elastic weight......... 1.00E+05
Partial pricing
---------------
LP Partial price....... 1 Prtl price section ( A) 8 Prtl price section (-I) 23
QP Partial price....... 1 Prtl price section ( A) 8 Prtl price section (-I) 23
The SQP Method
--------------
Minimize............... Cold start............. Proximal Point method.. 1
Nonlinear objectiv vars 5 Objective Row.......... 23 Function precision..... 3.00E-13
Unbounded step size.... 1.00E+20 Superbasics limit...... 6 Difference interval.... 5.48E-07
Unbounded objective.... 1.00E+10 Reduced Hessian dim.... 6 Central difference int. 6.70E-05
Major step limit....... 2.00E+00 Derivative linesearch.. Derivative option...... 1
Major iterations limit. 1000 Linesearch tolerance... 0.90000 Verify level........... 0
Minor iterations limit. 500 Penalty parameter...... 0.00E+00 Major Print Level...... 1
Time limit (secs)...... 9999999.0 Major optimality tol... 2.00E-06
Hessian Approximation
---------------------
Hessian full-memory.... Hessian updates........ 99999999 Hessian frequency...... 99999999
Hessian flush.......... 99999999
Nonlinear constraints
---------------------
Nonlinear constraints.. 22 Major feasibility tol.. 1.00E-06 Violation limit........ 1.00E+06
Nonlinear Jacobian vars 5
Miscellaneous
-------------
LU factor tolerance.... 3.99 LU singularity tol..... 3.25E-11 Timing level........... 3
LU update tolerance.... 3.99 LU swap tolerance...... 1.22E-04 Debug level............ 0
LU partial pivoting... eps (machine precision) 2.22E-16 System information..... No
Sticky parameters...... No
Total char*8 workspace 500 Total integer workspace 23373 Total real workspace 13485
Total char*8 (minimum) 500 Total integer (minimum) 1614 Total real (minimum) 2491
1
Matrix statistics
-----------------
Total Normal Free Fixed Bounded
Rows 23 21 1 1 0
Columns 8 1 2 3 2
No. of matrix elements 144 Density 78.261
Biggest constant element 0.0000E+00 (excluding fixed columns,
Smallest constant element 0.0000E+00 free rows, and RHS)
No. of objective coefficients 0
Nonlinear constraints 22 Linear constraints 1
Nonlinear variables 5 Linear variables 3
Jacobian variables 5 Objective variables 5
Total constraints 23 Total variables 8
1
Initial basis
-------------
No basis file supplied
Crash option 3
Crash on linear E rows:
Crash on linear LG rows:
Slacks 1 Free cols 0 Preferred 0
Unit 0 Double 0 Triangle 0 Pad 0
Itn 0: Feasible linear rows
Itn 0: PP1. Minimizing Norm(x-x0)
Itn 0: PP1. Norm(x-x0) approximately minimized (0.00E+00)
The user has defined 108 out of 108 first derivatives
Cheap test of user-supplied problem derivatives...
The constraint gradients seem to be OK.
--> The largest discrepancy was 4.38E-08 in constraint 30
The objective gradients seem to be OK.
Gradient projected in one direction -2.42541257879E-01
Difference approximation -2.42541190244E-01
Crash on nonlinear rows:
Slacks 20 Free cols 0 Preferred 0
Unit 0 Double 1 Triangle 0 Pad 1
1
Itn 0: Hessian set to a scaled identity matrix
lu1fac error... entry a( 32) has the same indices as an earlier entry
indc, indr = 21 0
1
SNOPTA EXIT 140 -- system error
SNOPTA INFO 142 -- error in basis package
Problem name opt_form
No. of iterations 0 Objective 2.4501424505E+00
No. of major iterations 0 Linear obj. term 3.5000000000E-01
Penalty parameter 0.000E+00 Nonlinear obj. term 2.1001424505E+00
User function calls (total) 2
No. of degenerate steps 0 Percentage 0.00
Max x 1 0.0E+00 Max pi 1 0.0E+00
Max Primal infeas 30 1.0E+00 Max Dual infeas 1 2.0E+00
Nonlinear constraint violn 1.0E+00
1
Name opt_form Objective Value 2.4501424505E+00
Status Optimal Soln Iteration 0 Superbasics 0
Objective (Min)
RHS
Ranges
Bounds
Section 1 - Constraints
Number ...Row.. State ...Value...... Slack Value... ..Lower Limit. ..Upper Limit. .Dual Variable ..i
9 r 1 BS . -0.96637 None 0.96637 . 1
10 r 2 BS . -1.20486 None 1.20486 . 2
11 r 3 BS . -0.07027 None 0.07027 . 3
12 r 4 BS . -0.13360 None 0.13360 . 4
13 r 5 BS . -1.21279 None 1.21279 . 5
14 r 6 BS . -0.96637 None 0.96637 . 6
15 r 7 BS . -1.20486 None 1.20486 . 7
16 r 8 BS . -0.07027 None 0.07027 . 8
17 r 9 BS . -0.13360 None 0.13360 . 9
18 r 10 BS . -1.21279 None 1.21279 . 10
19 r 11 BS . -0.96637 None 0.96637 . 11
20 r 12 BS . -1.20486 None 1.20486 . 12
21 r 13 BS . -0.07027 None 0.07027 . 13
22 r 14 BS . -0.13360 None 0.13360 . 14
23 r 15 BS . -1.21279 None 1.21279 . 15
24 r 16 BS . -0.96637 None 0.96637 . 16
25 r 17 BS . -1.20486 None 1.20486 . 17
26 r 18 BS . -0.07027 None 0.07027 . 18
27 r 19 BS . -0.13360 None 0.13360 . 19
28 r 20 BS . -1.21279 None 1.21279 . 20
29 r 21 I LL . -0.20000 0.20000 None . 21
30 r 22 I BS . -1.0 1.0 1.0 . 22
31 r 23 BS . . None None 1.0 23
1
Section 2 - Variables
Number .Column. State ...Value...... .Obj Gradient. ..Lower Limit. ..Upper Limit. .Dual Variable m+j
1 x 1 N FR . -1.96685 -20.00000 20.00000 -1.96685 24
2 x 2 N FR . -1.15415 -20.00000 20.00000 -1.15415 25
3 x 3 A EQ . . . . . 26
4 x 4 D BS . -1.0 . None -1.0 27
5 x 5 N FR . -0.60000 None None -0.60000 28
6 x 6 A EQ . . . . . 29
7 x 7 A EQ . . . . . 30
8 x 8 A FR . . None None . 31
Time for MPS input 0.00 seconds
Time for solving problem 0.00 seconds
Time for solution output 0.00 seconds
Time for constraint functions 0.00 seconds
Time for objective function 0.00 seconds
I am working on a drone obstacle avoidance project. And I am using the SNOPT tool to solve the non-convex optimization problem. Unfortunately, as a new SNOPT beginner, some problems occured and I am not able to fix them.
It always shows "SNOPTA EXIT 140 -- system error SNOPTA INFO 142 -- error in basis package". I have checked the output file and it said that the constraint and objective gradients seemed to be ok. Here are my matlab code and the output file
And the output file: