successfully computing tv funnels again. yeah!

just have to save them to a reasonable object in the output, and then sampledFiniteTimeVerification should be back in service.

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

systems/@PolynomialSystem/PolynomialSystem.m

@@ -163,9 +163,10 @@
         function g = newdyn(t,sys,frame,ctf,dtf)
           x = sys.getStateFrame.poly; z=frame.poly;
           p_t = msspoly('t',1);
-          c = subs(ctf.getPolyOutput(t),ctf.getInputFrame.poly,x);
-          d = subs(dtf.getPolyOutput(t),dtf.getInputFrame.poly,z);
-          g = subs(diff(c,p_t),p_t,t) + diff(c,x)*sys.getPolyDynamics(t);
+          [c,dc]=ctf.output(t,[],x);
+          d = dtf.output(t,[],z);
+          dcdt = dc(:,1); dcdx = dc(:,2:end);
+          g = dcdt + dcdx*sys.getPolyDynamics(t);
           g = subss(g,x,d);
         end
 

systems/@PolynomialSystem/sampledFiniteTimeVerification.m

@@ -87,7 +87,6 @@
     f = subs(f,sys.getInputFrame.poly,zeros(sys.getNumInputs,1));
   end
 
-  % got here
   Vdot{i}=diff(V{i},x)*f + V0.getPolyTimeDeriv(ts(i));
 
   % balancing 
@@ -149,7 +148,7 @@
   error('infeasible rho.  increase c');
 end
 
-Vtraj = PolynomialTrajectory(@(t) V0.getPoly(t)/ppvalSafe(rhopp,t),unique([V0.getBreaks(),ts']));
+Vtraj = PolynomialTrajectory(@(t) V0.getPoly(t)/ppvalSafe(rhopp,t),ts);
 V = PolynomialLyapunovFunction(V0.getFrame,Vtraj);
 
 end

systems/AffineSystem.m

@@ -154,6 +154,12 @@
         if (obj.num_x) y=y+obj.C.eval(t)*x; end
         if (obj.num_u) y=y+obj.D.eval(t)*u; end
         if ~isempty(obj.y0) y=y+obj.y0.eval(t); end
+        if (nargout>1)
+          dy = zeros(obj.num_y,1+obj.num_x+obj.num_u);
+          if (obj.num_x) dy(:,1:1+obj.num_x)=dy(:,1:1+obj.num_x) + [obj.C.deriv(t)*x, obj.C.eval(t)]; end
+          if (obj.num_u) dy(:,[1,2+obj.num_x:end])=dy(:,[1,2+obj.num_x:end])+[obj.D.deriv(t)*u,obj.D.eval(t)]; end
+          if ~isempty(obj.y0) dy(:,1)=dy(:,1)+obj.y0.deriv(t); end
+        end
       end
     end