Hi Professor Johan,
I am trying to optimize the following problem
max p
subject
V_dot + pdot + L*(V-p) sos
Here
variable of optimization are u and p. V and L are considered constant.
Since my code has sine and cosine, I also have an additional constraint
that s1^2+c^2-1 = 0
My code is as follow
clear
close all;
m = 2;
M = 0.5*2;
l = 0.8;
g =9.8;
Cd = 0.5;
dt = 0.0100;
I = 26;
Iw = 0.005*2;
R = 0.25;
L = [0.0991 0.0000 -0.0000 0.0000 -0.0000 -0.0000 -0.0000 2.0347 2.0347 1.9644 1.9644]';
P =1.0e+05*[0.0039 0.0791 0.0089 0.1046; 0.0791 4.7818 0.5263 6.2801; 0.0089 0.5263 0.0592 0.6920; 0.1046 6.2801 0.6920 8.2608];
P1
= 1.0e+05*[0.0039 0.0790 0.0089 0.1046; 0.0790 4.7663 0.5254 6.2691;
0.0089 0.5254 0.0592 0.6919; 0.1046 6.2691 0.6919 8.2588];
sdpvar x1 x2 x3 x4 c1 s1 u t1 p;
tau_fric = -2*(Cd/R)*(x3/R - x4);
detM = ((M+m)*R + Iw/R)*(m*l^2+I) - R*m^2*l^2*c1^2;
f3 = (1/t1)*( (m*l^2+I)*(u - tau_fric + m*R*l*x4^2*s1 ) + ...
(-m*R*l*c1)*(-u + tau_fric + m*g*l*s1 ));
f4 = (1/t1)*( (-m*l*c1)*(u - tau_fric + m*R*l*x4^2*s1 ) + ...
((M+m)*R+Iw/R)*(-u + tau_fric + m*g*l*s1 ) );
f = [x3; x4; f3; f4];
phot1 = 0.1;
%Generating constraint model
x = [0;0.010;0;0];
x_t1 = [-0.0014; 0.0101; 0.0793; -0.0067];
c1_val = cos(x(2));
s1_val = sin(x(2));
V_x = [x1 x2 x3 x4]*P*[x1;x2;x3;x4];
Vx_value = x'*P*x;
V_x1 = x_t1'*P1*x_t1;
x_var = [x1;x2;x3;x4];
vL_x = [1;x1;x2;x3;x4;c1;s1;c1^2;s1^2;x2^2;x4^2];
q = -jacobian(V_x,x_var)*f*t1;
detM_val = replace(detM,c1,cos(x(2)));
q = replace(q,t1,detM_val);
q = replace(q,[x1 x2 x3 x4 c1 s1],[x(1) x(2) x(3) x(4) c1_val s1_val]);
q2 = L(:,1)'*vL_x;
q2 = replace(q2,[x1 x2 x3 x4 c1 s1],[x(1) x(2) x(3) x(4) c1_val s1_val]);
veqconstraint = monolist([x1 x2 x3 x4 c1 s1],4);
c_eq = sdpvar(length(veqconstraint),1);
p_eq = c_eq'*veqconstraint;
g1 = c1^2 + s1^2 - 1; %Equality constraint for sine and cosine
Model = [sos(q -(V_x1 - Vx_value)/dt + (phot1-p)/dt + q2*(Vx_value - p) + p_eq*g1)];
% options = sdpsettings('solver','mosek');%,'verbose',0,'warning',1,'beeponproblem',-2:11);
options = [];
[sol,v,Q] = solvesos(Model,-p,options,[c_eq;p;u]);
The
above code gives me dual infeasible solution. However, if I replace -p
with [] to make it a feasibility problem, I get primal and dual feasible
and solution is optimal.
I am not sure that if solution exist, what is getting wrong. My apologies for the inconvenience.
Thanks