Merge branch 'master' into atlas-jump

CMakeLists.txt

@@ -37,7 +37,7 @@ elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
   endif()
 elseif (MSVC)
 #  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /arch:SSE2 /openmp")
-#  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:SSE2 /openmp")  
+#  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /arch:SSE2 /openmp")
 endif()
 
 # set up matlab build
@@ -159,5 +159,5 @@ endif()
 add_test(NAME "RigidBodyManipulatorMemoryTest"
 	WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}/examples/Acrobot"
 	COMMAND "matlab" #"${CMAKE_SOURCE_DIR}/cmake/matlab_clean.pl"
-	"-nosplash" "-nodisplay" "-r" "addpath('${CMAKE_INSTALL_PREFIX}/matlab'); addpath_${POD_NAME}; try, r = RigidBodyManipulator('Acrobot.urdf'); megaclear; catch ex, disp(getReport(ex,'extended')); disp(''); knownIssue('$testname',ex); force_close_system; exit(1); end; force_close_system; exit(0)")
+	"-nosplash" "-nodisplay" "-r" "addpath('${CMAKE_INSTALL_PREFIX}/matlab'); addpath_${POD_NAME}; try, r = RigidBodyManipulator('Acrobot.urdf'); megaclear; catch ex, disp(getReport(ex,'extended')); disp(''); force_close_system; exit(1); end; force_close_system; exit(0)")
 set_tests_properties(RigidBodyManipulatorMemoryTest PROPERTIES TIMEOUT 60)

addpath_drake.m

@@ -29,21 +29,9 @@
   % because I rely on the new matlab classes with classdef
 end
 
-% turn off autosave for simulink models (seems evil, but generating
-% boatloads of autosaves is clearly worse)
-if (com.mathworks.services.Prefs.getBooleanPref('SaveOnModelUpdate'))
-  a = input('You currently have autosave enabled for simulink blocks.\nThis is fine, but will generate a lot of *.mdl.autosave files\nin your directory.  If you aren''t a regular Simulink user,\nthen I can disable that feature now.\n  Disable Simulink Autosave (y/n)? ', 's');
-  if (lower(a(1))=='y')
-    com.mathworks.services.Prefs.setBooleanPref('SaveOnModelUpdate',false);
-  end
-end
-% todo: try setting this before simulating, then resetting it after the
-% simulate?
-
 % add package directories to the matlab path
 addpath(fullfile(root,'systems'));
 addpath(fullfile(root,'systems','plants'));
-addpath(fullfile(root,'systems','plants','affordance'));
 addpath(fullfile(root,'systems','plants','collision'));
 addpath(fullfile(root,'systems','plants','constraint'));
 addpath(fullfile(root,'systems','controllers'));
@@ -56,7 +44,6 @@
 addpath(fullfile(root,'solvers'));
 addpath(fullfile(root,'solvers','trajectoryOptimization'));
 addpath(fullfile(root,'util'));
-addpath(fullfile(root,'util','obstacles'));
 addpath(fullfile(root,'thirdParty'));
 addpath(fullfile(root,'thirdParty','path'));
 addpath(fullfile(root,'thirdParty','spatial'));
@@ -79,4 +66,17 @@
 % from here: http://pages.cs.wisc.edu/~ferris/path.html
 setenv('PATH_LICENSE_STRING', '2096056969&Russ_Tedrake&Massachusetts_Institute_of_Technology&&USR&75042&18_4_2014&1000&PATH&GEN&0_0_0&0_0_0&5000&0_0');
 
+% turn off autosave for simulink models (seems evil, but generating
+% boatloads of autosaves is clearly worse)
+if checkDependency('simulink')
+  autosave_options = get_param(0,'AutoSaveOptions');
+  if autosave_options.SaveOnModelUpdate
+    warning('Drake:DisablingSimulinkAutosave','Disabling autosave for simulink blocks (to avoid generating a lot of *.mdl.autosave files in your directory.  If you aren''t a regular Simulink user and don''t want this disabled, comment out this section in addpath_drake.\nTo disable this warning in the future, add\n   warning(''off'',''Drake:DisablingSimulinkAutosave'')\nto your matlab startup.m');
+    autosave_options.SaveOnModelUpdate = false;
+    set_param(0,'AutoSaveOptions',autosave_options);
+  end
+end
+% todo: try setting this before simulating, then resetting it after the
+% simulate?
+
 end

examples/Acrobot/AcrobotPlant.m

@@ -100,22 +100,22 @@
       tf0 = 4;
 
       N = 21;
-      traj_opt = DircolTrajectoryOptimization(obj,N,[2 6]);
-      traj_opt = traj_opt.addStateConstraint(ConstantConstraint(x0),1);
-      traj_opt = traj_opt.addStateConstraint(ConstantConstraint(xf),N);
-      traj_opt = traj_opt.addRunningCost(@cost);
-      traj_opt = traj_opt.addFinalCost(@finalCost);
+      prog = DircolTrajectoryOptimization(obj,N,[2 6]);
+      prog = prog.addStateConstraint(ConstantConstraint(x0),1);
+      prog = prog.addStateConstraint(ConstantConstraint(xf),N);
+      prog = prog.addRunningCost(@cost);
+      prog = prog.addFinalCost(@finalCost);
 
       traj_init.x = PPTrajectory(foh([0,tf0],[double(x0),double(xf)]));
 
-      info=0;
-      while (info~=1)
+      for attempts=1:10
         tic
-        [xtraj,utraj,z,F,info] = traj_opt.solveTraj(tf0,traj_init);
+        [xtraj,utraj,z,F,info] = prog.solveTraj(tf0,traj_init);
         toc
+        if info==1, break; end
       end
 
-      function [g,dg] = cost(t,x,u);
+      function [g,dg] = cost(dt,x,u)
         R = 1;
         g = sum((R*u).*u,1);
         dg = [zeros(1,1+size(x,1)),2*u'*R];
@@ -130,10 +130,10 @@
         g = sum((Q*xerr).*xerr + (R*u).*u,1);
 
         if (nargout>1)
-          dgdt = 0;
+          dgddt = 0;
           dgdx = 2*xerr'*Q;
           dgdu = 2*u'*R;
-          dg = [dgdt,dgdx,dgdu];
+          dg = [dgddt,dgdx,dgdu];
         end
       end
 

examples/Acrobot/runMaxROA.m

@@ -46,7 +46,7 @@
 % Create closed loop system with optimized controller
 sysCl = feedback(p,c);
 V0 = V.getPoly;
-xinit = getLevelSet(decomp(V0),V0,[0;0;0;0]);
+xinit = getLevelSet(decomp(V0),V0,struct('x0',[0;0;0;0]));
 
 v = AcrobotVisualizer(p);
 

examples/Acrobot/runPassiveURDF.m

@@ -1,12 +1,7 @@
-function runPassiveURDF()
-% Simulate the passive acrobot
+function runPassiveWRL
 
-w = warning('off','Drake:RigidBody:SimplifiedCollisionGeometry');
-d = PlanarRigidBodyManipulator('Acrobot.urdf');
-warning(w);
-v = d.constructVisualizer();
+p = PlanarRigidBodyManipulator('Acrobot.urdf');
+v = p.constructVisualizer;
 
-traj = simulate(d,[0 5],.5*randn(4,1));
-playback(v,traj);
-
-end
+x = p.simulate([0 5],randn(4,1));
+v.playback(x);

util/obstacles/@ObstacleField/ObstacleField.m → examples/Airplane2D/@ObstacleField/ObstacleField.m

@@ -166,17 +166,14 @@ function draw(obj)
            end
        end
 
-       function con = AddConstraints(obj, con)
-           % Adds the con.x.c constraint for the ObstacleField to the given
-           % constraint object.
-           %
-           % @param con input constraint object.  Must be a structure.
-           %    Other than that, we'll just overwrite con.x.c, so anything
-           %    else will be returned unchanged.
+       function prog = AddConstraints(obj, prog)
+           % Adds the non-convex constraints for the ObstacleField to the 
+           % given NonlinearProgram
            %
-           % @retval con Constraint structure with modified con.x.c value.
+           % @param prog a DirectTrajectoryOptimization
+           % @retval prog the updated NonlinearProgram
 
-           con.x.c = @(x)obj.obstacleConstraint(x);
+           prog = prog.addStateConstraint(FunctionHandleConstraint(-inf(obj.number_of_obstacles,1),zeros(obj.number_of_obstacles,1),2,@(x)obj.obstacleConstraint(x)),1:prog.N,1:2);
        end
 
 

examples/Airplane2D/runDircol.m

@@ -14,27 +14,26 @@
 x0 = double(x0);
 xf = double(xf);
 
-con.u.lb = p.umin;
-con.u.ub = p.umax;
-con.x0.lb = x0;
-con.x0.ub = x0;
-con.xf.lb = xf;
-con.xf.ub = xf;
-con.T.lb = 0.1;   
-con.T.ub = 1;
+N = 11;
+prog = DircolTrajectoryOptimization(p,N,[0.1 1]);
 
-options.method='dircol';
-options.MajorOptimalityTolerance=1e-2;
-options.trajectory_cost_fun=@(t,x,u)plotDircolTraj(t,x,u,[1 2]);  % for debugging
-%options.grad_test = true;
+prog = addStateConstraint(prog,ConstantConstraint(x0),1);
+prog = addStateConstraint(prog,ConstantConstraint(xf),N);
 
+prog = addRunningCost(prog,@cost);
+prog = addFinalCost(prog,@finalCost);
+
+prog = addTrajectoryDisplayFunction(prog,@(dt,x,u)plotDircolTraj(dt,x,u,[1 2]));
+%options.MajorOptimalityTolerance=1e-2;
+
+t_init = linspace(0,tf0,N);
 info = 0;
 while (info~=1)
   % generate a random trajectory
-  utraj0 = PPTrajectory(foh(linspace(0,tf0,11),randn(1,11)));
-  
+  traj_init.u = setOutputFrame(PPTrajectory(foh(t_init,randn(1,N))),getInputFrame(p));
+
   tic
-  [utraj,xtraj,info] = trajectoryOptimization(p,@cost,@finalcost,x0,utraj0,con,options);
+  [xtraj,utraj,~,~,info]=solveTraj(prog,t_init,traj_init);
   toc
 end
 
@@ -45,22 +44,23 @@
 
 end
 
-      function [g,dg] = cost(t,x,u)
+      function [g,dg] = cost(dt,x,u)
         R = 0;
         g = u'*R*u;
         %g = sum((R*u).*u,1);
         %dg = [zeros(1,1+size(x,1)),2*u'*R];
         dg = zeros(1, 1 + size(x,1) + size(u,1));
       end
 
-      function [h,dh] = finalcost(t,x)
+      function [h,dh] = finalCost(t,x)
         h = t;
         dh = [1,zeros(1,size(x,1))];
       end
 
       function [J,dJ]=plotDircolTraj(t,x,u,plotdims)
         figure(25);
         h=plot(x(plotdims(1),:),x(plotdims(2),:),'r.-');
+        axis([-2 2 -1 10]); axis equal;
         drawnow;
         delete(h);
         J=0;

examples/Airplane2D/runDircolWithObs.m

@@ -4,71 +4,46 @@
   p = PlanePlant();
 end
 
-x0 = [3.9; 0; 0; 0];
-tf0 = .7;
+x0 = [3.9; 0; 0; 0]; 
 xf = [5; 9; 0; 0];
+tf0 = .7;
 
-utraj0 = PPTrajectory(foh(linspace(0,tf0,11),0*randn(1,11)));
-utraj0 = setOutputFrame(utraj0,p.getInputFrame);
-
-%con.u.lb = p.umin;
-%con.u.ub = p.umax;
-con.x0.lb = x0;
-con.x0.ub = x0;
-con.xf.lb = xf; %[xf(1:2);-inf(2,1)];
-con.xf.ub = xf; %[xf(1:2);inf(2,1)];
-con.T.lb = .2;
-con.T.ub = 3;
+N = 21;
+prog = DircolTrajectoryOptimization(p,N,[0.2 3]);
 
+prog = addStateConstraint(prog,ConstantConstraint(x0),1);
+prog = addStateConstraint(prog,ConstantConstraint(xf),N);
 
 % add obstacles
 disp('Adding obstacles...');
 field = ObstacleField();
 field = field.GenerateRandomObstacles();
-con = field.AddConstraints(con);
+prog = field.AddConstraints(prog);
+
+prog = addRunningCost(prog,@(dt,x,u)cost(dt,x,u,field));
+prog = addFinalCost(prog,@finalCost);
+
+prog = addTrajectoryDisplayFunction(prog,@(dt,x,u)plotDircolTraj(dt,x,u,[1 2]));
+
 
 figure(25); clf;  hold on;
 v = PlaneVisualizer(p,field);
 v.draw(0,x0);
 drawnow
 
-disp('Running trajectory optimization...');
-
-options.method='dircol';
-options.MajorOptimalityTolerance=1e-2;
-options.trajectory_cost_fun=@(t,x,u)plotDircolTraj(t,x,u,[1 2]);  % for debugging
-%options.grad_method={'user','taylorvar'};
+prog = setSolverOptions(prog,'snopt','MajorOptimalityTolerance',1e-2);
+initial_guess.x = PPTrajectory(foh([0,tf0],[x0,xf]));
 tic
-%options.grad_test = true;
-[utraj,xtraj,info] = trajectoryOptimization(p,@(t,x,u)cost(t,x,u,field),@finalcost,x0,utraj0,con,options);
-if (info~=1) error(['Airplane2D:SNOPT:INFO',num2str(info)],'failed to find a trajectory'); end
+[xtraj,utraj,~,~,info]=solveTraj(prog,tf0,initial_guess);
 toc
 
-% upsample
-ts = linspace(utraj.tspan(1),utraj.tspan(2),41);
-options.xtape0='simulate';
-utraj0 = PPTrajectory(foh(ts,utraj.eval(ts))); utraj0 = setOutputFrame(utraj0,utraj.getOutputFrame);
-[utraj,xtraj,info] = trajectoryOptimization(p,@(t,x,u)cost(t,x,u,field),@finalcost,x0,utraj0,con,options);
-if (info~=1) error(['Airplane2D:SNOPT:INFO',num2str(info)],'failed to improve the trajectory'); end
-
-hold off;
-
-if (nargout>0) 
-  return;
+if (nargout<1)
+  v.playback(xtraj);
 end
 
-%disp('Stabilizing trajectory...');
-c = tvlqr(p,xtraj,utraj,eye(4),1,eye(4));
-
-disp('Visualizing trajectory...');
-v.playback(xtraj);
-%v.display_dt=0;v.playback_speed=.5; v.playbackAVI(xtraj,'PlaneWithObs');
-
-disp('Done.');
-
 end
 
-      function [g,dg] = cost(t,x,u,field)
+      function [g,dg] = cost(dt,x,u,field)
         R = 0.0001;
 
         % minimize the max obstacle constraint
@@ -83,14 +58,15 @@
         %dg = zeros(1, 1 + size(x,1) + size(u,1));
       end
 
-      function [h,dh] = finalcost(t,x)
+      function [h,dh] = finalCost(t,x)
         h = t;
         dh = [1,zeros(1,size(x,1))];
       end
 
       function [J,dJ]=plotDircolTraj(t,x,u,plotdims)
         figure(25);
         h=plot(x(plotdims(1),:),x(plotdims(2),:),'r.-');
+        axis([-2 2 -1 10]); axis equal;
         drawnow;
         delete(h);
         J=0;

util/obstacles/test/testObstacles.m → examples/Airplane2D/test/testObstacles.m

@@ -1,6 +1,6 @@
 function testObstacles
 
-close all
+tmp = addpathTemporary(fullfile(pwd,'..'));
 
 f = ObstacleField
 f = f.GenerateRandomObstacles();

util/obstacles/test/testVerticalAndHorizontalObstacles.m → examples/Airplane2D/test/testVerticalAndHorizontalObstacles.m

@@ -1,5 +1,7 @@
 function testVerticalAndHorizontalObstacles
 
+tmp = addpathTemporary(fullfile(pwd,'..'));
+
 %% Test edge cases for vertical and horizontal obstacle borders
 obs = PolygonalObstacle2D([0,0,1,1], [0,1,1,0]);
 

examples/Atlas/robotiqHandInspector.m

@@ -0,0 +1,7 @@
+function robotiqHandInspector
+
+r = RigidBodyManipulator('urdf/robotiq.urdf');
+
+actuated_dof = getActuatedJoints(r);
+v = r.constructVisualizer();
+v.inspector([],actuated_dof);