diff --git a/drake/systems/@DrakeSystem/DrakeSystem.m b/drake/systems/@DrakeSystem/DrakeSystem.m index c52b1ba61f95..5e340852c602 100644 --- a/drake/systems/@DrakeSystem/DrakeSystem.m +++ b/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; diff --git a/drake/systems/@DynamicalSystem/DynamicalSystem.m b/drake/systems/@DynamicalSystem/DynamicalSystem.m index 248fb367ff59..5336234c9ac0 100644 --- a/drake/systems/@DynamicalSystem/DynamicalSystem.m +++ b/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) diff --git a/drake/systems/@DynamicalSystem/simulate.m b/drake/systems/@DynamicalSystem/simulate.m index 27af1fe4f0d1..d5836e926a67 100644 --- a/drake/systems/@DynamicalSystem/simulate.m +++ b/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'; diff --git a/drake/systems/@HybridDrakeSystem/HybridDrakeSystem.m b/drake/systems/@HybridDrakeSystem/HybridDrakeSystem.m index 7cd294a5aa13..474edd4e6986 100644 --- a/drake/systems/@HybridDrakeSystem/HybridDrakeSystem.m +++ b/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)]; diff --git a/drake/systems/DCSFunction.cpp b/drake/systems/DCSFunction.cpp index 833dd9ae3d97..ce053d63a0ab 100644 --- a/drake/systems/DCSFunction.cpp +++ b/drake/systems/DCSFunction.cpp @@ -174,15 +174,34 @@ static void mdlInitializeSampleTimes(SimStruct *S) static void mdlInitializeConditions(SimStruct *S) { mxArray *psys = const_cast(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(mxGetScalar(plhs[0])); mxDestroyArray(plhs[0]); diff --git a/drake/systems/SimulinkModel.m b/drake/systems/SimulinkModel.m index aad55f9b45b6..37355a273b63 100644 --- a/drake/systems/SimulinkModel.m +++ b/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); diff --git a/drake/systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m b/drake/systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m index 594c2e0d3285..11f1bf603038 100644 --- a/drake/systems/plants/@RigidBodyManipulator/RigidBodyManipulator.m +++ b/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; diff --git a/drake/systems/plants/TimeSteppingRigidBodyManipulator.m b/drake/systems/plants/TimeSteppingRigidBodyManipulator.m index c41157b92fd2..58ee10ba77bd 100644 --- a/drake/systems/plants/TimeSteppingRigidBodyManipulator.m +++ b/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')