RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.35k stars 1.27k forks source link

ROA of Three link under-actuated system #3843

Closed dhruba-chowdhury closed 8 years ago

dhruba-chowdhury commented 8 years ago

Hi I am tying to maximize the ROA of a three link under-actuated system and i am also using the controller as a decision variable
I wrote the following code and the problem i am facing is that some eigen values are positive of the linearized system matrix using the controller found using SOS Could anyone help me with where i am wrong

degV = 2; % Set degree of Lyapunov function degL1 = 2; % Set degrees of multiplier polynomials degu1 = 2; degu2 = 2; L1monom = monomials(x,0:degL1); Vmonom = monomials(x,0:degV); umonom1 = monomials(x,0:degu1); umonom2 = monomials(x,0:degu2);

solver = @spot_mosek; options = spot_sdp_default_options();

rho=0.0020;

for iter=1:10

  x=msspoly('x',6);
  x1=x(1);
  x2=x(2);
  x3=x(3);
  x4=x(4);
  x5=x(5);
  x6=x(6);

  flag1=1;
  flag2=1;
  count=0;

%L1
prog = spotsosprog; prog = prog.withIndeterminate(x);

[prog,L1] = prog.newFreePoly(L1monom); [prog,ux1] = prog.newFreePoly(umonom1); [prog,ux2] = prog.newFreePoly(umonom2);

% construct Vdot Vdot = (diff(V,x)_(f+(g1_ux1)+(g2*ux2)));

Vdot=clean(Vdot,1e-6); [prog,sigma1] = prog.newPos(1);

% setup SOS constraints prog = prog.withSOS(-Vdot + L1_(V - rho)-sigma1_V); prog = prog.withSOS(L1);

% run SeDuMi/MOSEK and check output

sol = prog.minimize(-sigma1,solver,options);

if ~sol.isPrimalFeasible || ~sol.isPrimalFeasible break; end

L1 = sol.eval(L1) ux1 = sol.eval(ux1) ux2 = sol.eval(ux2)

%V prog = spotsosprog; prog = prog.withIndeterminate(x);

% construct V [prog,V] = prog.newFreePoly(Vmonom); Vdot = (diff(V,x)_(f+(g1_ux1)+(g2*ux2))); Vdot=clean(Vdot,1e-6); % construct rho [prog,rho] = prog.newPos(1);

% setup SOS constraints
prog = prog.withSOS(-Vdot + L1_(V-rho)-0.1_sigma1*V); prog = prog.withSOS(V); prog = prog.withEqs(subs(V,x,[0;0;0;0;0;0])); % run SeDuMi/MOSEK and check output

sol = prog.minimize(-rho,solver,options); if ~sol.isPrimalFeasible || ~sol.isPrimalFeasible break; end

V = sol.eval(V) rho = double(sol.eval(rho))

%Checking the eigen values of the linearised system using the controller %found using SOS

fclp=(f+(g1_ux1)+(g2_ux2)); fclp=clean(fclp,1e-6); Ap=double(subs(diff(fclp,x),x,[0;0;0;0;0;0])); eig(Ap) end

RussTedrake commented 8 years ago

sorry... but we don't have the cycles to debug these for you.