Added some patches from Robin, which add a nice control UI for Visual…

…izers and let you tweak playback on the fly. Also, fixes a missing path in configure.m and a handful of other bugfixes. git-svn-id: https://svn.csail.mit.edu/locomotion/robotlib/trunk@4463 c9849af7-e679-4ec6-a44e-fc146a885bd3
vcmman · Nov 9, 2012 · 37b3ea9 · 37b3ea9
1 parent 570b56b
commit 37b3ea9
Show file tree

Hide file tree

Showing 10 changed files with 104 additions and 45 deletions.
diff --git a/.gitignore b/.gitignore
@@ -69,3 +69,5 @@ doc/modeling.aux
 doc/modeling.tex
 doc/planning.aux
 doc/planning.tex
+examples/CompassGait/runLQR_URDF.m
+.gitignore
diff --git a/configure.m b/configure.m
@@ -68,6 +68,7 @@
 addpath([conf.root,'/thirdParty/path']);
 addpath([conf.root,'/thirdParty/spatial']);
 addpath([conf.root,'/thirdParty/graphviz2mat']);
+addpath([conf.root,'/thirdParty/cprintf']);
 
 % todo: setup java classpath (not hard to do it once... but how can I set
 % it up for future sessions as well?  maybe write to startup.m, or prompt

diff --git a/examples/Acrobot/AcrobotVisualizer.m b/examples/Acrobot/AcrobotVisualizer.m
@@ -14,8 +14,10 @@
     end
 
     function draw(obj,t,x)
+
       % draw the acrobot
       persistent hFig L1r L1a L2r L2a;
+
       l1=obj.l1; l2=obj.l2;
 
       if (isempty(hFig))
@@ -32,7 +34,7 @@ function draw(obj,t,x)
         L2r = (L2x.^2+L2y.^2).^.5;
         L2a = atan2(L2y,L2x);
       end
-
+      
       sfigure(hFig);
       clf;
 

diff --git a/examples/Airplane2D/PlaneVisualizer.m b/examples/Airplane2D/PlaneVisualizer.m
@@ -25,6 +25,7 @@ function draw(obj,t,x)
             hFig = sfigure(25);
             set(hFig,'DoubleBuffer', 'on');
         end
+        sfigure(hFig);
 
         %Airplane definition
         hull_scale=0.04;

diff --git a/examples/CompassGait/CompassGaitVisualizer.m b/examples/CompassGait/CompassGaitVisualizer.m
@@ -23,6 +23,7 @@ function draw(obj,t,x)
       a = obj.a;  b = obj.b; l = obj.l; 
 
       h = sfigure(25);
+
       set(h,'DoubleBuffer','on');
       clf;
       hold on;

diff --git a/examples/CompassGait/dev/runPassiveURDF.m b/examples/CompassGait/dev/runPassiveURDF.m
@@ -1,11 +1,15 @@
-function runPassiveURDF
+% function runPassiveURDF
 % runs the passive dynamics with the rigid body backend (from URDF)
 
-p = PlanarRigidBodyManipulator('CompassGait.urdf');
-xtraj = p.simulate([0 5]);
+m = PlanarRigidBodyModel('CompassGait.urdf');
+p = TimeSteppingRigidBodyManipulator(m, 0.01);
+% p = CompassGaitPlant()
+% p.getInitialState()
+% xtraj = p.simulate([0 2], p.getInitialState);
+xtraj = p.simulate([0 2], p.getInitialState + [0 .1 0 pi/2 0 0 0 0]');
 
 v=p.constructVisualizer();
 v.axis = 2*[-1 1 -1 1];
-v.playback_speed = .2;
+v.playback_speed = 1;
 v.playback(xtraj);
 
diff --git a/examples/CompassGait/runDircolCycle.m b/examples/CompassGait/runDircolCycle.m
@@ -48,15 +48,15 @@
 
 if (nargout<1)
   v = CompassGaitVisualizer(p);
-  v.playback_speed = .4;
-  playback(v,xtraj);
-
   figure(1); clf;
   fnplt(utraj);
 
   figure(2); clf; hold on;
   fnplt(xtraj,[2 4]);
   fnplt(xtraj,[3 5]);
+
+  playback(v,xtraj);
+
 end
 
 end

diff --git a/examples/Glider/GliderVisualizer.m b/examples/Glider/GliderVisualizer.m
@@ -35,7 +35,7 @@ function draw(obj,t,x)
       ct = cos(x(3)); st = sin(x(3));
       ctp = cos(x(3)+x(4)); stp = sin(x(3)+x(4));
 
-      figure(25); clf; hold on;
+      sfigure(25); clf; hold on;
 
       %cg
       plot(x(1),x(2),'ro','MarkerSize',4,'MarkerFaceColor',[1 0 0]);

diff --git a/systems/Visualizer.m b/systems/Visualizer.m
@@ -48,7 +48,7 @@
       end
       draw(obj,t,x,[]);
     end
-    
+
     function playback(obj,xtraj)
       %   Animates the trajectory in quasi- correct time using a matlab timer
       %     optional controlobj will playback the corresponding control scopes
@@ -60,49 +60,97 @@ function playback(obj,xtraj)
         xtraj = xtraj.inFrame(obj.getInputFrame);  % try to convert it
       end
 
+      f = sfigure(89);
+      set(f, 'Position', [560 528 560 70]);
+
       tspan = xtraj.getBreaks();
       t0 = tspan(1);
-
-      tic;
-
       ts = getSampleTime(xtraj); 
+      time_steps = (tspan(end)-tspan(1))/max(obj.display_dt,eps);
+      speed_format = 'Speed = %.3g';
+      time_format = 'Time = %.3g';
 
-      if (obj.playback_speed<=0)  % then playback as quickly as possible
-        t=tspan(1);
-        while (t<tspan(end))
-          t = tspan(1)+obj.playback_speed*toc;
-          if (ts(1)>0) t = round((t-ts(2))/ts(1))*ts(1) + ts(2); end  % align with sample times if necessary
-          x = xtraj.eval(t);
-          obj.draw(t,x);
-          if (obj.display_time)
-            title(['t = ', num2str(t,'%.2f') ' sec']);
-          end
-          drawnow;
-        end
-      else
-        ti = timer('TimerFcn',{@timer_draw},'ExecutionMode','fixedRate','Period',max(obj.display_dt/obj.playback_speed,.01),'TasksToExecute',(tspan(end)-tspan(1))/max(obj.display_dt,eps),'BusyMode','drop');
-        start(ti);
-        wait(ti);  % unfortunately, you can't try/catch a ctrl-c in matlab
-        delete(ti);
+      time_slider = uicontrol('Style', 'slider', 'Min', tspan(1), 'Max', tspan(end),...
+        'Value', tspan(1), 'Position', [110, 10, 440, 20],...
+        'Callback',{@update_time_display});
+      speed_slider = uicontrol('Style', 'slider', 'Min', -3, 'Max', 1, ...
+        'Value', log10(obj.playback_speed), 'Position', [185, 35, 365, 20], ...
+        'Callback', {@update_speed});
+      speed_display = uicontrol('Style', 'text', 'Position', [90, 35, 90, 20],...
+        'String', sprintf(speed_format, obj.playback_speed));
+      rewind_button = uicontrol('Style', 'pushbutton', 'String', 'Reset', ...
+        'Position', [10, 35, 35, 20], 'Callback', {@rewind_vis});
+      play_button = uicontrol('Style', 'pushbutton', 'String', 'Play', ...
+        'Position', [50, 35, 35, 20], 'Callback', {@start_playback},...
+        'Interruptible', 'on');
+      time_display = uicontrol('Style', 'text', 'Position', [10, 10, 90, 20],...
+        'String', sprintf(time_format, tspan(1)));
+      function update_speed(source, eventdata)
+        obj.playback_speed = 10 ^ (get(speed_slider, 'Value'));
+        set(speed_display, 'String', sprintf(speed_format, obj.playback_speed));
       end
-
-      obj.draw(tspan(end),xtraj.eval(tspan(end)));
-
-      function timer_draw(timerobj,event)
-        t=tspan(1)+obj.playback_speed*toc;
-        if (t>tspan(end))
-          stop(timerobj);
+      function rewind_vis(source, eventdata)
+        set(time_slider, 'Value', get(time_slider, 'Min'));
+        update_time_display(time_slider, []);
+      end
+      function update_time_display(source, eventdata)
+        t = get(time_slider, 'Value');
+        set(time_display, 'String', sprintf(time_format, t));
+        obj.draw(t, xtraj.eval(t));
+      end
+      function start_playback(source, eventdata)
+        if get(play_button, 'UserData')
+          set(play_button, 'UserData', 0, 'String', 'Play');
           return;
+        else
+          set(play_button, 'UserData', 1, 'String', 'Pause');
         end
-        if (ts(1)>0) t = round((t-ts(2))/ts(1))*ts(1) + ts(2); end  % align with sample times if necessary
-        x = xtraj.eval(t);
-        obj.draw(t,x);
-        if (obj.display_time)
-          title(['t = ', num2str(t,'%.2f') ' sec']);
+%         set(stop_button, 'UserData', 0);
+        if get(time_slider, 'Value') >= (tspan(end) - 0.02)
+          set(time_slider, 'Value', get(time_slider, 'Min'));
+        end
+        tic;
+        t0 = get(time_slider, 'Value');
+        if (obj.playback_speed<=0)  % then playback as quickly as possible
+          t = t0;
+          while (t<tspan(end))
+            t = t0 + obj.playback_speed*toc;
+            if (ts(1)>0) t = round((t-ts(2))/ts(1))*ts(1) + ts(2); end  % align with sample times if necessary
+            x = xtraj.eval(t);
+            set(time_slider, 'Value', t)
+            update_time_display(time_slider, [])
+            drawnow;
+            if ~get(play_button, 'UserData')
+              break;
+            end
+          end
+        else
+          ti = timer('TimerFcn',{@timer_draw},'ExecutionMode','fixedRate',...
+            'Period',max(obj.display_dt/obj.playback_speed,.01),...
+            'TasksToExecute',time_steps,'BusyMode','queue');
+          start(ti);
+          wait(ti);  % unfortunately, you can't try/catch a ctrl-c in matlab
+          delete(ti);
+        end
+        set(play_button, 'UserData', 0, 'String', 'Play');
+        function timer_draw(timerobj,event)
+          t=t0+obj.playback_speed*toc;
+          if (t>tspan(end))
+            stop(timerobj);
+            return;
+          end
+          if (ts(1)>0) t = round((t-ts(2))/ts(1))*ts(1) + ts(2); end  % align with sample times if necessary
+          x = xtraj.eval(t);
+          set(time_slider, 'Value', t)
+          update_time_display(time_slider, [])
+          drawnow;
+          if ~get(play_button, 'UserData')
+            stop(timerobj);
+            return;
+          end
         end
-        drawnow;
       end
-
+      update_time_display(time_slider, [])
     end
 
     function playbackAVI(obj,xtraj,filename)

diff --git a/systems/plants/RigidBody.m b/systems/plants/RigidBody.m
@@ -254,7 +254,7 @@ function writeWRLJoint(body,fp)
               wrlfile = fullfile(tempdir,[name,'.wrl']);
               stl2vrml(fullfile(path,[name,ext]),tempdir);
               txt=fileread(wrlfile);
-              [~,txt]=strtok(txt,'DEF');
+              [~,txt]=strtok(txt,'DEFI');
               wrlstr=[wrlstr,sprintf('Shape {\n\tgeometry %s\n\t%s}\n',txt,wrl_appearance_str)];
 %              wrlstr=[wrlstr,sprintf('Inline { url "%s" }\n',wrlfile)];
             elseif strcmpi(ext,'.wrl')