getting all examples to work again. getting very close…!

git-svn-id: https://svn.csail.mit.edu/locomotion/robotlib/branches/class-hierarchy-cleanup@3339 c9849af7-e679-4ec6-a44e-fc146a885bd3

doc/modeling.texp

@@ -16,11 +16,11 @@ vector $y$.
 
 \begin{center}
 \begin{tikzpicture}[auto, node distance=2cm,>=latex']
-   \node (input) {};
-   \node [block, right of=input] (system) {System};
-   \draw [->] (input) -- node {$u$} (system);
-   \node [right of=system] (output) {};
-   \draw [->] (system) -- node {$y$} (output);
+  \node (input) {};
+  \node [block, right of=input] (system) {System};
+  \draw [->] (input) -- node {$u$} (system);
+  \node [right of=system] (output) {};
+  \draw [->] (system) -- node {$y$} (output);
 \end{tikzpicture}
 \end{center}
 

examples/Acrobot/test/lqrTest.m

@@ -13,7 +13,7 @@ function lqrTest()
 n=10;
 x0=[pi;0;0;0];
 V = V.inFrame(p.getStateFrame);
-y=getLevelSet(V,x0,struct('num_samples',n));
+y=getLevelSet(V,0,struct('num_samples',n));
 for i=1:n
   xtraj=simulate(sys,[0 4],.01*x0 + .99*y(:,i));
   if (V.eval(4,xtraj.eval(4))>V.eval(0,xtraj.eval(0)))

examples/Airplane2D/dev/PlaneLQRTree.m

@@ -34,7 +34,7 @@
 options.Tsub = 3;
 
 px = p.getStateFrame.poly;
-options.Vf = PolynomialLyapunovFunction(p.getStateFrame,1e4*px'*Qf*px);
+options.Vf = QuadraticLyapunovFunction(p.getStateFrame,1e4*Qf);
 c = LQRTree.buildLQRTree(p,xG,uG,@()rand(4,1).*[6;6;pi/2;4]-[-2;0;pi/4;2],Q,R,options);
 
 end

examples/Airplane2D/runFunnel.m

@@ -25,12 +25,11 @@
 
 %options.stability = true;
 
-V=sampledFiniteTimeVerification(poly,Vf,V,xtraj.getBreaks(),xtrajClosedLoop,utraj,options);
+V=sampledFiniteTimeVerification(poly,xtraj.getBreaks(),diag([1 1 10 10]),V,options);
 disp('done');
-V
 
-
-plotFunnel(V,xtraj,[1 2]);
+options.plotdims = [1 2];
+plotFunnel(V.inFrame(p.getStateFrame()),options);
 fnplt(xtraj,[1 2]); 
 
 end

examples/Airplane2D/runFunnelWithObstacles.m

@@ -24,16 +24,19 @@
 [c,V]=tvlqr(p,xtraj,utraj,Q,R,diag([1 1 10 10]));
 sys = feedback(p,c);
 utraj = ConstantTrajectory(zeros(p.getNumInputs,1)); utraj=utraj.setOutputFrame(p.getInputFrame); 
+poly = taylorApprox(sys,xtraj,utraj,3);
 
 options.stability = true;
 options.max_iterations=20;
-options.converged_tol =1e-4;
-
-V=sampledFiniteTimeVerification(poly,Vf,V,xtraj.getBreaks(),xtrajClosedLoop,utraj,options);
+options.converged_tol =1e-5;
+V=sampledFiniteTimeVerification(poly,xtraj.getBreaks(),diag([1 1 10 10]),V,options);
 disp('done');
 
 figure(25);
-plotFunnel(V,xtraj,[1 2]);
+options.plotdims=[1 2];
+options.inclusion='projection';
+%save itall.mat;
+plotFunnel(V.inFrame(p.getStateFrame()),options);
 h=fnplt(xtraj,[1 2]); 
 set(h,'Color',[1 0 0]);
 %keyboard;
@@ -80,12 +83,13 @@
 %  end
 end
 
+% todo: still need to update this
 Vtrim = PolynomialTrajectory(@(t) V.eval(t)/ppvalSafe(foh(ts,rho),t),ts);
 
 clf;
 v = PlaneVisualizer(p,field);
 v.draw(0,x0);
-plotFunnel(Vtrim,xtraj,[1 2]);
+plotFunnel(Vtrim.inFrame(p.getStateFrame()));
 h=fnplt(xtraj,[1 2]); 
 set(h,'Color',[1 0 0]);
 

examples/CartPole/runHinf.m

@@ -15,7 +15,7 @@
 
 V = V.inFrame(d.getStateFrame);
 
-figure(1); plotFunnel(V,[],[2 4],struct('inclusion','projection')); 
+figure(1); plotFunnel(V,struct('inclusion','projection','plotdims',[2 4])); 
 xlabel('$\theta$','interpreter','latex');
 ylabel('$\dot\theta$','interpreter','latex');
 

examples/CartPole/runLQR.m

@@ -15,7 +15,7 @@
 
 V = V.inFrame(d.getStateFrame);
 
-figure(1); plotFunnel(V,[],[2 4],struct('inclusion','projection')); 
+figure(1); plotFunnel(V,struct('plotdims',[2 4],'inclusion','projection')); 
 xlabel('$\theta$','interpreter','latex');
 ylabel('$\dot\theta$','interpreter','latex');
 

examples/CompassGait/runTransverseCycle.m → examples/CompassGait/dev/runTransverseCycle.m

@@ -1,7 +1,5 @@
 function c=runTransverseCycle
 
-% NOTEST
-
 p=CompassGaitPlant();
 [utraj,xtraj] = runDircolCycle;
 

examples/Glider/dev/runFunnel.m

@@ -35,7 +35,7 @@
 V=sampledFiniteTimeVerification(poly,diag([10,1]),V,xtraj.getBreaks());
 
 figure(1); clf; hold on;
-plotFunnel(V,xtraj);
+plotFunnel(V);
 fnplt(xtraj); 
 
 end

examples/Pendulum/PendulumEnergyControl.m

@@ -43,7 +43,7 @@ function run()
       sys = feedback(pd,c);
 
       figure(2);
-      plotFunnel(c.V);
+      plotFunnel(c.V.inFrame(pd.getStateFrame));
 
       for i=1:5
         xtraj = simulate(sys,[0 6]);

examples/Quadrotor2D/runLQR.m

@@ -12,7 +12,7 @@
 %end
 %return;
 
-figure(1); plotFunnel(V,[],[3 6]); 
+figure(1); plotFunnel(V,struct('plotdims',[3 6])); 
 xlabel('$\theta$','interpreter','latex');
 ylabel('$\dot\theta$','interpreter','latex');
 

examples/VanDerPol.m

@@ -35,7 +35,7 @@ function run()
 
       clf; hold on
       fill(xlim(1,:),xlim(2,:),[0.8 0.8 0.2])
-      plotFunnel(V,zeros(2,1));
+      plotFunnel(V);
     end
   end
 end

systems/@DynamicalSystem/tiHinf.m

@@ -77,7 +77,7 @@
 
 if (nargout>1)
   x=ltisys.getInputFrame.poly; %msspoly('x',getNumStates(obj));
-  Vcandidate=PolynomialLyapunovFunction(ltisys.getInputFrame,x'*S*x);
+  Vcandidate=QuadraticLyapunovFunction(ltisys.getInputFrame,S);
 end
 
 end

systems/@DynamicalSystem/tvlyap.m

@@ -8,29 +8,40 @@
 tspan=xtraj.getBreaks(); %TODO: pass in options for tspan
 %tspan = linspace(tspan(1), tspan(end), 100);
 
+% create new coordinate frame
+fr = CoordinateFrame([obj.getStateFrame.name,' - x0(t)'],nX,obj.getStateFrame.prefix);
+obj.getStateFrame.addTransform(AffineTransform(obj.getStateFrame,fr,eye(nX),-xtraj));
+fr.addTransform(AffineTransform(fr,obj.getStateFrame,eye(nX),xtraj));
+
 if (isa(Q,'double'))
   Q = ConstantTrajectory(Q);
 end
 
 typecheck(Q,'Trajectory');  
 sizecheck(Q,[nX,nX]);
 
-switch class(Qf)
-  case 'cell'
-  case 'double'
-    sizecheck(Qf,[nX,nX]);
-    Qf = {Qf,zeros(nX,1),0};
-  case 'PolynomialLyapunovFunction'
-    Vf=Qf.getPoly(tspan(end));
-    x=Qf.getFrame.poly;
-    % todo: check frames and convert frames (if necessary) here
-    Vf=subss(Vf,x,x+xtraj.eval(tspan(end)));
-    Qf=cell(3,1);
-    Qf{1}=double(.5*subs(diff(diff(Vf,x)',x),x,0*x));
-    Qf{2}=double(subs(diff(Vf,x),x,0*x))';
-    Qf{3}=double(subs(Vf,x,0*x));
-  otherwise
-    error('Qf must be a double, a 3x1 cell array, or a PolynomialLyapunovFunction');
+if iscell(Qf)
+  % intentionally left blank
+elseif isa(Qf,'double')
+  sizecheck(Qf,[nX,nX]);
+  Qf = {Qf,zeros(nX,1),0};
+elseif isa(Qf,'PolynomialLyapunovFunction')
+  Vf = extractQuadraticLyapunovFunction(Qf);
+  Vf = Vf.inFrame(obj.getStateFrame);
+  Vf = Vf.inFrame(fr);
+
+  Qf=cell(3,1);
+  if isTI(Vf)
+    Qf{1}=Vf.S;
+    Qf{2}=Vf.s1;
+    Qf{3}=Vf.s2;
+  else
+    Qf{1}=Vf.S.eval(tspan(end));
+    Qf{2}=Vf.s1.eval(tspan(end));
+    Qf{3}=Vf.s2.eval(tspan(end));
+  end
+else
+  error('Qf must be a double, a 3x1 cell array, or a PolynomialLyapunovFunction');
 end
 sizecheck(Qf,3);
 typecheck(Qf{1},'double');
@@ -46,20 +57,11 @@
 
 odeoptions = odeset('RelTol',1e-6,'AbsTol',1e-12);
 Ssqrt = cellODE(@ode45,@(t,S)affineSdynamics(t,S,obj,Q,xtraj,xdottraj),tspan(end:-1:1),{Qf{1}^(1/2),Qf{2},Qf{3}},odeoptions);
-S = FunctionHandleTrajectory(@(t) recompS(Ssqrt.eval(t)),[nX,nX],tspan);
 %S_nonAffine = matrixODE(@ode45,@(t,S)Sdynamics(t,S,obj,Q,xtraj),tspan(end:-1:1),Qf{1},odeoptions);
+Ssqrt = flipToPP(Ssqrt);
+S = recompS(Ssqrt);
 
-fr = CoordinateFrame([sys.getStateFrame.name,' - x0(t)'],nX,obj.getStateFrame.prefix);
-obj.getStateFrame.addTransform(AffineTransform(obj.getStateFrame,fr,eye(nX),-xtraj));
-fr.addTransform(AffineTransform(fr,obj.getStateFrame,eye(nX),xtraj));
-
-p_x=fr.poly;
-p_t=msspoly('t',1);
-%Sdot = @(t)Sdynamics(t,S.eval(t),obj,Q,xtraj);
-%Vtraj = PolynomialTrajectory(@(t) lyapunov(t,S.eval(t),Sdot(t),xtraj.eval(t),p_x,p_t), tspan);
-Sdot = @(t)recompSdot(Ssqrt.eval(t),affineSdynamics(t,Ssqrt.eval(t),obj,Q,xtraj,xdottraj));
-Vtraj = PolynomialTrajectory(@(t) affineLyapunov(t,S.eval(t),Sdot(t),xtraj.eval(t),p_x,p_t), tspan);
-V = PolynomialLyapunovFunction(fr,Vtraj);
+V=QuadraticLyapunovFunction(ltvsys.getInputFrame,S{1},S{2},S{3});
 
 end
 
@@ -72,9 +74,7 @@
 
   % f = xdot
   % xtraj.deriv(t) should equal f
-
-
-
+
   A = df(:,1+(1:nX));
   Sdot = -(Q + S*A + A'*S);
   if (min(eig(S))<0) error('S is not positive definite'); end
@@ -100,16 +100,4 @@
   S{3} = Ssqrt{3};
 end
 
-function Sdot = recompSdot(Ssqrt,Sdotsqrt)
-  Sdot{1} = Sdotsqrt{1}*Ssqrt{1}' + Ssqrt{1}*Sdotsqrt{1}';
-  Sdot{2} = Sdotsqrt{2};
-  Sdot{3} = Sdotsqrt{3};
-end
 
-function V=lyapunov(t,S,Sdot,x0,p_x,p_t)
-  V = (p_x-x0)'*(S+Sdot*(p_t-t))*(p_x-x0);
-end
-
-function V=affineLyapunov(t,S,Sdot,x0,p_x,p_t)
-  V = (p_x-x0)'*(S{1}+Sdot{1}*(p_t-t))*(p_x-x0) + (p_x-x0)'*(S{2}+Sdot{2}*(p_t-t)) + S{3}+Sdot{3}*(p_t-t);
-end

systems/@PolynomialLyapunovFunction/PolynomialLyapunovFunction.m

@@ -27,6 +27,17 @@
       error('not implemented yet');
     end
 
+    function y = getLevelSet(obj,t,options)
+      if (nargin<3) options=struct(); end
+      if isTI(obj) t=0; elseif nargin<2 || isempty(t), error('you must specify a time'); end
+      y = getLevelSet(obj.getFrame.poly,obj.getPoly(t),options);
+    end
+
+    function v = getLevelSetVolume(obj,t)
+      if isTI(obj) t=0; elseif nargin<2 || isempty(t), error('you must specify a time'); end
+      v = getLevelSetVolume(obj.getFrame.poly,obj.getPoly(t));
+    end
+
     function V = inFrame(obj,frame)
       if (frame==obj.getFrame)
         V = obj;
@@ -84,5 +95,4 @@ function ldivide(a,b)
     end
   end
 
-  % todo: move over getLevelSet, getProjection, plotFunnel, ...
 end