i can get through finite time verification on the pendulum again. yeah.

now have to clean up the remaining examples (just in the way they call sampledFiniteTimeVerification) and I should be ready to move back to trunk.

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

examples/Pendulum/PendulumPlant.m

@@ -106,7 +106,7 @@
 
     function c=trajectorySwingUpAndBalance(obj)
       [ti,Vf] = balanceLQR(obj);
-      Vf = SpotPolynomialLyapunovFunction(Vf.getFrame,Vf.getPoly*5);  % artificially prune, since ROA is solved without input limits
+      Vf = 5*Vf;  % artificially prune, since ROA is solved without input limits
 
 %      c = LQRTree(ti,Vf);
       [utraj,xtraj]=swingUpTrajectory(obj);  
@@ -118,7 +118,7 @@
       options.degL1=2;
       Vswingup=sampledFiniteTimeVerification(psys,xtraj.getBreaks(),Vf,Vswingup,options);
 
-      c = c.addTrajectory(tv,Vtraj);
+%      c = c.addTrajectory(tv,Vswingup);
     end
 
     function c=balanceLQRTree(p)

systems/@PolynomialLyapunovFunction/PolynomialLyapunovFunction.m

@@ -52,6 +52,36 @@
 
       V = QuadraticLyapunovFunction(obj.getFrame,S,s1,s2);
     end
+
+    function V = mtimes(a,b)
+      % support simple scaling of Lyapunov functions via multiplication by
+      % a (scalar) double
+      error('not implemented yet'); % derived classes should implement this method
+    end
+
+
+    % since I only care about scalar multiplication / division, i can
+    % support the related calls, too.
+    function V = times(a,b)
+      sizecheck(a,1); sizecheck(b,1);
+      V = mtimes(a,b); 
+    end
+    function V = mrdivide(a,b)
+      sizecheck(a,1); sizecheck(b,1);
+      V = mtimes(a,inv(b));
+    end
+    function mldivide(a,b)
+      sizecheck(a,1); sizecheck(b,1);
+      V = mtimes(inv(a),b);
+    end
+    function rdivide(a,b)
+      sizecheck(a,1); sizecheck(b,1);
+      V = mtimes(a,inv(b));
+    end
+    function ldivide(a,b)
+      sizecheck(a,1); sizecheck(b,1);
+      V = mtimes(inv(a),b);
+    end
   end
 
   % todo: move over getLevelSet, getProjection, plotFunnel, ...

systems/@PolynomialSystem/sampledFiniteTimeVerification.m

@@ -126,10 +126,10 @@
   L=findMultipliers(x,V,Vdot,rho,rhodot,options);
   [rho,rhointegral]=optimizeRho(x,V,Vdot,L,dts,Vmin,rhof,options);
   rhodot = diff(rho)./dts;
-  rhopp=foh(ts,rho');
 
   % plot current rho
   if (options.plot_rho)
+    rhopp=foh(ts,rho');
     figure(10); fnplt(rhopp); title(['iteration ',num2str(iter)]); drawnow;
   end
 
@@ -148,8 +148,7 @@
   error('infeasible rho.  increase c');
 end
 
-Vtraj = PolynomialTrajectory(@(t) V0.getPoly(t)/ppvalSafe(rhopp,t),ts);
-V = PolynomialLyapunovFunction(V0.getFrame,Vtraj);
+V = PPTrajectory(foh(ts,1./rho'))*V0;  % note: the inverse here is an approximation (since 1/rho is not polynomial).  It would be better to rewrite the verification conditions in terms of inv(rho)
 
 end
 

systems/QuadraticLyapunovFunction.m

@@ -1,4 +1,4 @@
-classdef QuadraticLyapunovFunction < PolynomialLyapunovFunction
+classdef (InferiorClasses = {?ConstantTrajectory,?PPTrajectory,?FunctionHandleTrajectory}) QuadraticLyapunovFunction < PolynomialLyapunovFunction
   % x'*S*x + x'*s1 + s2
   % S,s1,s2 can be doubles or trajectories (yielding a time-varying quadratic)
 
@@ -75,5 +75,18 @@
       end
     end
 
+    function V = mtimes(a,b)
+      % support simple scaling of Lyapunov functions via multiplication by
+      % a (scalar) double
+      if ~isa(b,'PolynomialLyapunovFunction')
+        % then a must be the lyapunov function.  swap them.
+        tmp=a; a=b; b=a;
+      end
+      typecheck(a,{'numeric','Trajectory'});
+      sizecheck(a,1);
+
+      V = QuadraticLyapunovFunction(b.getFrame, a*b.S, a*b.s1, a*b.s2);
+    end
+
   end
 end

systems/SpotPolynomialLyapunovFunction.m

@@ -29,5 +29,18 @@
         Vpoly=subs(obj.Vpoly,obj.p_t,t);
       end
     end
+
+    function b = mtimes(a,b)
+      % support simple scaling of Lyapunov functions via multiplication by
+      % a (scalar) double
+      if ~isa(b,'PolynomialLyapunovFunction')
+        % then a must be the lyapunov function.  swap them.
+        tmp=a; a=b; b=a;
+      end
+      typecheck(a,'numeric');
+      sizecheck(a,1);
+
+      b.Vpoly = a*b.Vpoly;
+    end
   end
 end

systems/trajectories/PPTrajectory.m

@@ -169,7 +169,7 @@
 %       ( sum a(:,:,j)(t-t0)^(k-j) ) ( sum b(:,:,j)(t-t0)^(k-j) )
 
       cbreaks = abreaks; % also bbreaks, by our assumption above
-      cd = [ad(1) bd(2)];
+      if isscalar(a), cd = bd; elseif isscalar(b) cd = ad; else cd = [ad(1) bd(2)]; end
       cl = al;  % also bl, by our assumption that abreaks==bbreaks
       ck = ak+bk-1;
 
@@ -184,7 +184,7 @@
       end
       c = PPTrajectory(mkpp(cbreaks,ccoefs,cd));
     end
-
+        
     function c = vertcat(a,varargin)
       typecheck(a,'PPTrajectory');  % todo: handle vertcat with non-PP trajectories
       [breaks,coefs,l,k,d] = unmkpp(a.pp);