Merge pull request RobotLocomotion#1375 from RussTedrake/initial_cond…

…itions

Initial conditions

CMakeLists.txt

@@ -28,10 +28,13 @@ option(REMOVE_UNUSED_EXTERNALS "enable this to remove those projects from disk"
 
 # ON by default:
 option(WITH_EIGEN		"required c++ matrix library.  only disable if you have it already."    ON)
-option(WITH_LCM 		"interprocess communications protocol for visualizers, etc" ON)
 if (WIN32)
+   if (CMAKE_SIZEOF_VOID_P EQUAL 8)  # gtk only compiles on win64 right now
+	option(WITH_LCM 		"interprocess communications protocol for visualizers, etc" ON)
 	option(WITH_GTK 		"precompiled gtk binaries/headers for Windows"  ON)  # needed for lcm on windows
+   endif()
 else()
+	option(WITH_LCM 		"interprocess communications protocol for visualizers, etc" ON)
 	option(WITH_DIRECTOR	"vtk-based visualization tool and robot user interface"   ON) # not win32 yet.  it builds on windows, but requires manually installation of vtk, etc.  perhaps make a precompiled director pod (a bit like snopt)
 	option(WITH_LIBBOT 		"simple open-gl visualizer + lcmgl for director"	ON) # there is hope, but i spent a long time cleaning up c/c++ language errors trying to make msvc happy.. and still had a long way to go.
 	option(WITH_BULLET 		"used for collision detection" 	ON) # almost works.  just one remaining link error..?

drake/doc/gallery.html

@@ -27,16 +27,6 @@ <h1 class="gh-header-title instapaper_title">Gallery</h1>
 If you have an example from your own work that you would like to showcase, email a movie or image along with a short description (including a link to the source code if relevant) to <a href="mailto:[email protected]">Russ</a>.
 </td></tr>
 
-<tr>
-<td>
-  Simple demonstration of Drake's rigid-body dynamics engine
-
-  <p>Drake's rigid-body manipulator classes support frictional contact, aerodynamics, and a rich set of forces, sensors, and kinematic constraints.  This video just shows a few quick examples; the source code can be found in the examples and test directories of the Drake distribution.</p>
-</td>
-<td>
-<iframe width="560" height="315" src="//www.youtube.com/embed/M3m-rmPzbRk?rel=0" frameborder="0" allowfullscreen></iframe></td>
-</tr>
-
 
 <tr height="50" />
 <tr><td colspan=2>
@@ -165,6 +155,28 @@ <h3>Canonical Underactuated Systems</h3>
 </td></tr>
 
 
+<tr height="50" />
+<tr><td colspan=2>
+<h3>Physics Engine</h3>
+</td></tr>
+<tr>
+<td>
+  Simple demonstration of Drake's rigid-body dynamics engine
+
+  <p>Drake's rigid-body manipulator classes support frictional contact, aerodynamics, and a rich set of forces, sensors, and kinematic constraints.  This video just shows a few quick examples; the source code can be found in the examples and test directories of the Drake distribution.</p>
+</td>
+<td>
+<iframe width="560" height="315" src="//www.youtube.com/embed/M3m-rmPzbRk?rel=0" frameborder="0" allowfullscreen></iframe></td>
+</tr>
+
+<tr>
+<td><iframe width="420" height="315" src="https://www.youtube.com/embed/gsebSpj4KK8" frameborder="0" allowfullscreen></iframe></td>
+<td>
+  20 falling capsules.  It's not fast to simulate, but it works.
+</td>
+</tr>
+
+
 </table>
   </body>
 </html>

drake/examples/PR2/runPassive.m

@@ -1,7 +1,7 @@
 function runPassive
 
 options.ignore_self_collisions = true;
-options.floating = true;
+options.floating = 'quat';
 options.terrain = RigidBodyFlatTerrain();
 
 w = warning('off','Drake:RigidBodyManipulator:UnsupportedContactPoints');
@@ -14,6 +14,7 @@
 
 r = setSimulinkParam(r,'MinStep','0.001');
 x0 = Point(r.getStateFrame);
+x0(4) = 1;  % make it a valid quaternion
 
 v = r.constructVisualizer;
 v.display_dt = .05;

drake/examples/Quadrotor/Quadrotor.m

@@ -2,9 +2,13 @@
 
   methods
 
-    function obj = Quadrotor(sensor)
+    function obj = Quadrotor(sensor,floating_base_type)
       if nargin<1, sensor=''; end
-      options.floating = true;
+      if (nargin<2) 
+        options.floating = true; 
+      else
+        options.floating = floating_base_type;
+      end
       options.terrain = RigidBodyFlatTerrain();
       w = warning('off','Drake:RigidBodyManipulator:ReplacedCylinder');
       warning('off','Drake:RigidBodyManipulator:UnsupportedContactPoints');
@@ -113,13 +117,14 @@ function visualizePlan(x,lcmgl)
   methods (Static)
 
     function runOpenLoop
-      r = Quadrotor('lidar');
+      r = Quadrotor('lidar','quat');
       r = addTrees(r); 
       sys = TimeSteppingRigidBodyManipulator(r,.01);
 
       v = sys.constructVisualizer();
 
-      x0 = [0;0;.5;zeros(9,1)];
+      x0 = [getRandomConfiguration(r);zeros(6,1)];
+      x0(3) = .5;
       u0 = nominalThrust(r);
 
       sys = cascade(ConstantTrajectory(u0),sys);
@@ -128,7 +133,7 @@ function visualizePlan(x,lcmgl)
 %      simulate(sys,[0 2],double(x0)+.1*randn(12,1));
 
       options.capture_lcm_channels = 'LCMGL';
-      [ytraj,xtraj,lcmlog] = simulate(sys,[0 2],double(x0)+.1*randn(12,1),options);
+      [ytraj,xtraj,lcmlog] = simulate(sys,[0 2],double(x0),options);
       lcmlog
       v.playback(xtraj,struct('lcmlog',lcmlog));
 %      figure(1); clf; fnplt(ytraj);

drake/examples/Quadrotor/test/testCollisions.m

@@ -2,7 +2,7 @@
 
 tmp = addpathTemporary(fullfile(pwd,'..'));
 
-r = Quadrotor();
+r = Quadrotor('','quat');
 r = r.setTerrain([]);
 r = addTrees(r, 25);
 % The important trees to create swerving path
@@ -16,7 +16,9 @@
 x0 = Point(getStateFrame(r));  % initial conditions: all-zeros
 x0.base_y = -1.5;
 x0.base_z = .8;
-x0.base_ydot = 5;
+x0.base_qw = 1;
+x0.base_vy = 5;
+x0 = resolveConstraints(r,x0);
 u0 = double(nominalThrust(r));
 
 v = constructVisualizer(r);%,struct('use_collision_geometry',true));

drake/examples/ZMP/CartTable.m

@@ -2,7 +2,7 @@
 
   methods
     function obj = CartTable()
-      obj = obj@TimeSteppingRigidBodyManipulator('CartTable.urdf',0.005,struct('floating',true,'terrain',RigidBodyFlatTerrain()));
+      obj = obj@TimeSteppingRigidBodyManipulator('CartTable.urdf',0.005,struct('floating','quat','terrain',RigidBodyFlatTerrain()));
 
 %      obj = addSensor(obj,FullStateFeedbackSensor());
 %      obj = addSensor(obj,ContactForceTorqueSensor(obj,'base',[0;0],0);
@@ -12,7 +12,7 @@
   methods (Static = true)
     function run
       r = CartTable;
-      xtraj = simulate(r,[0 7],[0;0;.12;0;.06;0;-.03;0;0;0;0;0;0;0;0;0]);
+      xtraj = simulate(r,[0 7],[0;0;.12;1;0;.06;0;-.03;0;0;0;0;0;0;0;0;0]);
       v = r.constructVisualizer;
       v.playback(xtraj,struct('slider',true));
     end

drake/solvers/NonlinearProgram.m

@@ -590,6 +590,13 @@
       if(nargout>1)
         G = [G(1,:);G(1+obj.nlcon_ineq_idx,:);G(1+obj.nlcon_eq_idx,:)];
       end
+
+      % useful debugging info
+%      fprintf(1,'objective = %f\n',f(1));
+%      cnstr_name = [obj.cin_name;obj.ceq_name];
+%      for i=1:length(cnstr_name),
+%        fprintf(1,'%s = %f\n',cnstr_name{i},f(i+1));
+%      end
     end
 
     function obj = addDecisionVariable(obj,num_new_vars,var_name)

drake/solvers/test/testLQRmex.m

@@ -7,8 +7,8 @@ function testLQRmex()
   compareValues(A,B,Q,R);
 
 
-  addpath([getDrakePath, '/examples/Quadrotor'])
-  r = Quadrotor();
+  tmp = addpathTemporary([getDrakePath, '/examples/Quadrotor']);
+  r = QuadPlantPenn();
   [A,B] = linearize(r,0,[0;0;1;zeros(9,1)],double(nominalThrust(r)));
   Q = diag([10*ones(6,1); ones(6,1)]);
   R = .1*eye(4);
@@ -18,10 +18,14 @@ function testLQRmex()
 end
 
 function compareValues(A,B,Q,R)
+  A = full(A);
+  B = full(B);
+  Q = full(Q);
+  R = full(R);
   [K, S] = lqr(A,B,Q,R);
   [K_mex, S_mex] = lqrmex(A,B,Q,R);
 
-  valuecheck(K, K_mex, 1e-6);
-  valuecheck(S, S_mex, 1e-6);
+  valuecheck(K_mex, K, 1e-6);
+  valuecheck(S_mex, S, 1e-6);
 end
 

drake/systems/@DrakeSystem/DrakeSystem.m

@@ -41,6 +41,11 @@
       % Attempts to return the result of resolveConstraints using a
       % small random vector as an initial seed.
 
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
+
       x0 = .01*randn(obj.num_xd+obj.num_xc,1);
       if ~isempty(obj.state_constraints)
         attempts=0;
@@ -59,7 +64,7 @@
           if (~success)
             x0 = randn(obj.num_xd+obj.num_xc,1);
             if (attempts>=10)
-              error('Drake:Manipulator:FailedToResolveConstraints','Failed to resolve state constraints on initial conditions after 10 tries');
+              error('Drake:DrakeSystem:FailedToResolveConstraints','Failed to resolve state constraints on initial conditions after 10 tries');
             end
           end
         end

drake/systems/@DynamicalSystem/DynamicalSystem.m

@@ -315,7 +315,7 @@
       end
 
       [x,~,exitflag,infeasible_constraint_name] = solve(prog,x0);
-      success=(exitflag==1);
+      success=(exitflag<10);
       if ~isempty(infeasible_constraint_name)
         infeasible_constraint_name
       end
@@ -715,6 +715,7 @@
 
   properties (SetAccess=private,GetAccess=protected)
     simulink_params=struct();     % simulink model parameters
+    initial_state = [];  % getInitialState returns this if non-empty instead of a random state
   end
 
   properties (Access=public)

drake/systems/@DynamicalSystem/simulate.m

@@ -25,7 +25,27 @@
 typecheck(tspan,'double');
 if (length(tspan)<2) error('length(tspan) must be > 1'); end
 if (nargin<4) options=struct(); end
-mdl = getModel(obj);
+pstruct = obj.simulink_params;
+
+if (nargin>2 && ~isempty(x0)) % handle initial conditions
+  if (isa(x0,'Point'))
+    x0 = double(x0.inFrame(obj.getStateFrame));
+  else
+    typecheck(x0,'double');
+    sizecheck(x0,[obj.getStateFrame.dim,1]);
+  end
+  obj.initial_state = x0;
+  mdl = getModel(obj);
+  x0 = stateVectorToStructure(obj,x0,mdl);
+  pstruct.InitialState = registerParameter(mdl,x0,'x0');
+  pstruct.LoadInitialState = 'on';
+
+  if (~isempty(find_system(mdl,'ClassName','InitialCondition')))
+    warning('Your model appears to have an initial conditions block in it (e.g., from SimMechanics).  That block will overwrite any initial conditions that you pass in to simulate.');
+  end
+else
+  mdl = getModel(obj);
+end
 
 if (strcmp(get_param(mdl,'SimulationStatus'),'paused'))
   feval(mdl,[],[],[],'term');  % terminate model, in case it was still running before
@@ -49,7 +69,6 @@
     'parameters',registerParameter(mdl,SimulationControlBlock(mdl,options.gui_control_interface,options.lcm_control_interface),'control'));
 end
 
-pstruct = obj.simulink_params;
 if ~isfield(pstruct,'Solver')
   % set the default solver if it's clear what to do (because someone might
   % have manually changed their default simulink solver)
@@ -59,21 +78,6 @@
 end
 pstruct.StartTime = num2str(tspan(1));
 pstruct.StopTime = num2str(tspan(end));
-if (nargin>2 && ~isempty(x0)) % handle initial conditions
-  if (isa(x0,'Point'))
-    x0 = double(x0.inFrame(obj.getStateFrame));
-  else
-    typecheck(x0,'double');
-    sizecheck(x0,[obj.getStateFrame.dim,1]);
-  end
-  x0 = stateVectorToStructure(obj,x0,mdl);
-  pstruct.InitialState = registerParameter(mdl,x0,'x0');
-  pstruct.LoadInitialState = 'on';
-
-  if (~isempty(find_system(mdl,'ClassName','InitialCondition')))
-    warning('Your model appears to have an initial conditions block in it (e.g., from SimMechanics).  That block will overwrite any initial conditions that you pass in to simulate.');
-  end
-end
 
 pstruct.SaveFormat = 'StructureWithTime';
 pstruct.SaveTime = 'on';

drake/systems/@HybridDrakeSystem/HybridDrakeSystem.m

@@ -192,6 +192,11 @@ function drawModeGraph(obj)
     end
 
     function x0 = getInitialState(obj)
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
+
       m = getInitialMode(obj);
       x0 = [m; getInitialState(obj.modes{m})];
       % pad if necessary:

drake/systems/CascadeSystem.m

@@ -81,6 +81,11 @@
     end
 
     function x0=getInitialState(obj)
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
+
       x0=encodeX(obj,getInitialState(obj.sys1),getInitialState(obj.sys2));
     end
 

drake/systems/FeedbackSystem.m

@@ -88,6 +88,11 @@
     end
 
     function x0=getInitialState(obj)
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
+
       x0=encodeX(obj,getInitialState(obj.sys1),getInitialState(obj.sys2));
     end
 

drake/systems/SimulinkModel.m

@@ -69,8 +69,16 @@
     end
 
     function x0 = getInitialState(obj)
-      x0 = Simulink.BlockDiagram.getInitialState(obj.mdl);
-      x0 = stateStructureToVector(obj,x0);
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
+      if (getNumStates(obj)>0)
+        x0 = Simulink.BlockDiagram.getInitialState(obj.mdl);
+        x0 = stateStructureToVector(obj,x0);
+      else
+        x0 = [];
+      end
     end
 
     function [xcdot,df] = dynamics(obj,t,x,u,yes_i_promise_that_i_called_output_first)

drake/systems/dev/PolytopicSystem.m

@@ -61,6 +61,10 @@
     end
 
     function x0 = getInitialState(obj)
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
       x0 = getInitialState(obj.subsys{1});
     end
 

drake/systems/dev/Reduction.m

@@ -52,6 +52,11 @@
       % system
     end
     function x0 = getInitialState(obj)
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
+
       x0 = obj.reduced_sys.getInitialState();
     end