ok. i concede that the initial_state property is the best way to do i…

…t. (i spent literally hours trying to do it the right way, but simulink cannot reveal the initialstate parameter in the mdlInitializeSizes method (best list i found was here: http://www.mathworks.com/help/simulink/simulation-information.html and in simstruct.h).  i was able to pass things in through getModel during the construction of the model for each simulation, but it couldn't handle more complex situations like when a DrakeSystem is just part of a SimulinkModel and getInitialState only gets called from the mdlInitializeConditions methods.  so adding a method attached to the object is really the only solution.

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;

drake/systems/@DynamicalSystem/DynamicalSystem.m

@@ -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,26 @@
 typecheck(tspan,'double');
 if (length(tspan)<2) error('length(tspan) must be > 1'); end
 if (nargin<4) options=struct(); end
-mdl = getModel(obj);
+
+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
@@ -60,26 +79,6 @@
 pstruct.StartTime = num2str(tspan(1));
 pstruct.StopTime = num2str(tspan(end));
 
-%% handle initial conditions
-if (nargin<3 || isempty(x0))
-  x0 = getInitialState(obj);
-end
-
-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
-%% 
-
 pstruct.SaveFormat = 'StructureWithTime';
 pstruct.SaveTime = 'on';
 pstruct.TimeSaveName = 'tout';

drake/systems/@HybridDrakeSystem/HybridDrakeSystem.m

@@ -199,7 +199,6 @@ function drawModeGraph(obj)
     end
 
     function x0 = getInitialStateWInput(obj,t,x,u)
-      if x(1)<1, x(1) = getInitialMode(obj); end
       m = x(1); nX = getNumStates(obj.modes{m});
       if (nX>0) xm = x(1+(1:nX)); else xm=[]; end
       x0=[x(1); getInitialStateWInput(obj.modes{m},t,xm,u)];

drake/systems/DCSFunction.cpp

@@ -174,15 +174,34 @@ static void mdlInitializeSampleTimes(SimStruct *S)
 static void mdlInitializeConditions(SimStruct *S)
 {
   mxArray *psys = const_cast<mxArray*>(ssGetSFcnParam(S, 0));
+  mxArray* plhs[1];
+  if (mexCallMATLABsafe(S,1,plhs,1,&psys,"getInitialState")) return;
+
+  real_T* xc0 = ssGetContStates(S);
+  real_T* xd0 = ssGetDiscStates(S);
+  real_T* x = mxGetPr(plhs[0]);
+  int_T num_xd = ssGetNumDiscStates(S);
+  int_T num_xc = ssGetNumContStates(S);
+
   if (isa(psys,"HybridDrakeSystem")) {
     ssSetIWorkValue(S, IS_HYBRID_IDX, 1);
   } else {
     ssSetIWorkValue(S,IS_HYBRID_IDX,0);
   }
+
+  if (num_xd) {
+    memcpy(xd0,x,sizeof(real_T)*num_xd);
+    x+=num_xd;
+  }
+  if (num_xc) {
+    memcpy(xc0, x, sizeof(real_T)*num_xc);
+  }    
+
   ssSetIWorkValue(S,0,1);
 
+  mxDestroyArray(plhs[0]);
+
   if (isa(psys,"StochasticDrakeSystem")) {
-    mxArray* plhs[1];
     if (mexCallMATLABsafe(S,1,plhs,1,&psys,"getNumDisturbances")) return;
     int num_w = static_cast<int>(mxGetScalar(plhs[0]));
     mxDestroyArray(plhs[0]);

drake/systems/SimulinkModel.m

@@ -69,6 +69,10 @@
     end
 
     function x0 = getInitialState(obj)
+      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);

drake/systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m

@@ -332,6 +332,11 @@
     end
 
     function x0 = getInitialState(obj)
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
+
       x0 = [ getRandomConfiguration(obj); .01*randn(getNumVelocities(obj),1)];
       if ~isempty(obj.state_constraints)
         attempts=0;

drake/systems/plants/TimeSteppingRigidBodyManipulator.m

@@ -180,6 +180,11 @@ function checkDirty(obj)
     end
 
     function x0 = getInitialState(obj)
+      if ~isempty(obj.initial_state)
+        x0 = obj.initial_state;
+        return;
+      end
+
       x0 = obj.manip.getInitialState();
       for i=1:length(obj.sensor)
         if isa(obj.sensor{i},'TimeSteppingRigidBodySensorWithState')