feat (sim): parameterized on the notion of state
This commit is contained in:
parent
391e350315
commit
48d6cc4ca8
GPG key ID:
7065F57ED8856128
7 changed files with 481 additions and 102 deletions
|
|
@ -1,4 +1,6 @@
|
|||
|
||||
exception TODO
|
||||
|
||||
let pair = fun a b -> a, b
|
||||
|
||||
let uncurry = fun f (a, b) -> f a b
|
||||
|
|
|
|||
|
|
@ -1,98 +1,113 @@
|
|||
|
||||
open Types
|
||||
|
||||
(** Step mode: discrete or continuous *)
|
||||
type mode =
|
||||
| Discrete
|
||||
| Continuous
|
||||
|
||||
(** Simulation status:
|
||||
- [Idle]: Waiting for input, no activity;
|
||||
- [Running]: Currently integrating: step [mode], current [input], [now]
|
||||
timestamp, and [stop] time. *)
|
||||
type 'a status =
|
||||
| Idle : 'a status
|
||||
| Running :
|
||||
{ mode : mode; (** Step mode. *)
|
||||
input : 'a value; (** Function to integrate. *)
|
||||
now : time; (** Current time of integration. *)
|
||||
stop : time; (** How long until we stop. *)
|
||||
} -> 'a status
|
||||
|
||||
(** Internal state of the simulation node: model state, solver state and current
|
||||
simulation status. *)
|
||||
type ('a, 'ms, 'ss) state =
|
||||
{ status : 'a status; (** Current simulation status. *)
|
||||
mstate : 'ms; (** Model state. *)
|
||||
sstate : 'ss } (** Solver state. *)
|
||||
open State
|
||||
|
||||
(** Offset the [input] function by [now]. *)
|
||||
let offset (input: 'a value) (now: time) : time -> 'a =
|
||||
let offset (input : 'a value) (now : time) : time -> 'a =
|
||||
fun t -> input.u ((now -. input.start) +. t)
|
||||
|
||||
let simulate (HNode model: ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode)
|
||||
(DNode solver : ('y, 'yder, 'zin, 'zout) solver)
|
||||
: ('p, 'a signal, 'b signal) dnode =
|
||||
let s = { status = Idle; mstate = model.s; sstate = solver.s } in
|
||||
let step (state: ('a, _, _) state) (input: 'a signal)
|
||||
: 'b signal * ('a, _, _) state =
|
||||
match input, state.status with
|
||||
| Some i, _ ->
|
||||
(* New input. *)
|
||||
let status = Running
|
||||
{ mode = Continuous; input = i; now = 0.0; stop = i.length } in
|
||||
None, { state with status }
|
||||
| None, Idle ->
|
||||
(* Waiting for input. *)
|
||||
None, state
|
||||
| None, Running ({ mode = Discrete; _ } as r) ->
|
||||
(* Discrete step. *)
|
||||
let o, mstate = model.step state.mstate (r.input.u r.now) in
|
||||
(* Four possible outcomes: *)
|
||||
let state =
|
||||
let h = model.horizon mstate in
|
||||
if h <= 0.0 then (* - Cascade (new discrete step) *)
|
||||
{ state with mstate }
|
||||
else if r.now >= r.stop then (* - Reached horizon *)
|
||||
s
|
||||
else if model.jump mstate then (* - Discontinuity: reset solver *)
|
||||
let y = model.cget mstate in
|
||||
let fder t = model.fder mstate @@ offset r.input r.now t in
|
||||
let fzer t = model.fzer mstate @@ offset r.input r.now t in
|
||||
let ivp = { fder; stop = r.stop -. r.now; init = y } in
|
||||
let zc = { yc = y; fzer } in
|
||||
let sstate = solver.reset (ivp, zc) state.sstate in
|
||||
let status = Running { r with mode = Continuous } in
|
||||
{ status; mstate; sstate }
|
||||
else (* - Continue *)
|
||||
{ state with status = Running { r with mode = Continuous } }
|
||||
in Some { start = r.now; length = 0.0; u = fun _ -> o }, state
|
||||
| None, Running ({ mode = Continuous; _ } as r) ->
|
||||
(* Continuous step *)
|
||||
let (h, f, z), sstate = solver.step state.sstate r.stop in
|
||||
let mstate = model.cset state.mstate (f h) in
|
||||
(* Three possible outcomes: *)
|
||||
let now = r.input.start +. h in
|
||||
let state = match z with
|
||||
| None ->
|
||||
let status =
|
||||
if h >= r.stop then (* Reached the end. *)
|
||||
Running { r with mode = Discrete; now }
|
||||
else (* Not finished integrating *)
|
||||
Running { r with now } in
|
||||
{ status; mstate; sstate }
|
||||
| Some z ->
|
||||
let status = Running { r with mode = Discrete; now } in
|
||||
let mstate = model.zset mstate z in
|
||||
{ status; mstate; sstate }
|
||||
in
|
||||
let fout t =
|
||||
let t = r.now +. t in
|
||||
model.fout mstate (r.input.u t) (f t) in
|
||||
let out =
|
||||
{ start = r.input.start +. r.now; length = h -. r.now; u = fout } in
|
||||
Some out, state
|
||||
in
|
||||
let reset _p _ = s in
|
||||
DNode { s; step; reset }
|
||||
module LazySim (S : SimState) =
|
||||
struct
|
||||
|
||||
(* TODO: figure out where we initialize the solvers; the initialization
|
||||
function already supposes an initialized solver state, but could we
|
||||
parameterize [LazySim] with a solver state module that provides its
|
||||
own initialization function ? *)
|
||||
|
||||
let sim
|
||||
(HNode model : ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode)
|
||||
(DNode solver : ('y, 'yder, 'zin, 'zout) solver)
|
||||
: ('p, 'a, 'b) sim
|
||||
= let s = S.init ~mstate:model.state ~sstate:solver.state in
|
||||
let step state input =
|
||||
let mstate = S.mstate state and sstate = S.sstate state
|
||||
and status = S.status state in
|
||||
match input, S.is_running state with
|
||||
| Some input, _ ->
|
||||
let mode = Discrete and now = 0.0 and stop = input.length in
|
||||
let status = S.running ~mode ~input ~now ~stop status in
|
||||
let state = S.update ~status state in
|
||||
None, state
|
||||
| None, false -> None, state
|
||||
| None, true ->
|
||||
let input = S.input state and now = S.now state
|
||||
and stop = S.stop state in
|
||||
match S.mode state with
|
||||
| Discrete ->
|
||||
let o, mstate = model.step mstate (input.u now) in
|
||||
let state =
|
||||
let h = model.horizon mstate in
|
||||
if h <= 0.0 then S.update ~mstate state
|
||||
else if now >= stop then
|
||||
(* TODO: equivalent of [s] (initial state of model and
|
||||
solvers) ? *)
|
||||
raise Common.Utils.TODO
|
||||
else if model.jump mstate then
|
||||
let y = model.cget mstate in
|
||||
let fder t = model.fder mstate (offset input now t) in
|
||||
let fzer t = model.fzer mstate (offset input now t) in
|
||||
let ivp = { fder; stop = stop -. now; init = y } in
|
||||
let zc = { yc = y; fzer } in
|
||||
let sstate = solver.reset (ivp, zc) sstate in
|
||||
let status = S.running ~mode:Continuous status in
|
||||
S.update ~status ~mstate ~sstate state
|
||||
else
|
||||
let status = S.running ~mode:Continuous status in
|
||||
S.update ~status state in
|
||||
let start = input.start +. now in
|
||||
Some { start; length = 0.0; u = fun _ -> o }, state
|
||||
| Continuous ->
|
||||
let (h, f, z), sstate = solver.step sstate stop in
|
||||
let mstate = model.cset mstate (f h) in
|
||||
let h' = input.start +. h in
|
||||
let state = match z with
|
||||
| None ->
|
||||
let status =
|
||||
if h >= stop then S.running ~mode:Discrete ~now:h' status
|
||||
else S.running ~now:h' status in
|
||||
S.update ~status ~mstate ~sstate state
|
||||
| Some z ->
|
||||
let status = S.running ~mode:Discrete ~now:h' status in
|
||||
let mstate = model.zset mstate z in
|
||||
S.update ~status ~mstate ~sstate state in
|
||||
let fout t =
|
||||
model.fout mstate (input.u (now +. t)) (f (now +. t)) in
|
||||
let out =
|
||||
{ start = input.start +. now; length = h -. now; u = fout }
|
||||
in
|
||||
Some out, state in
|
||||
let reset m s =
|
||||
let mstate = model.reset m (S.mstate s) in
|
||||
let y = model.cget mstate in
|
||||
let stop = raise Common.Utils.TODO in
|
||||
let ivp = { fder = model.fder mstate; stop; init = y } in
|
||||
let zc = { fzer = model.fzer mstate; yc = y } in
|
||||
let sstate = solver.reset (ivp, zc) (S.sstate s) in
|
||||
let status = S.idle (S.status s) in
|
||||
S.update ~status ~mstate ~sstate s in
|
||||
DNode { state = s; step; reset }
|
||||
|
||||
end
|
||||
|
||||
module GreedySim (S : SimState) =
|
||||
struct
|
||||
|
||||
let sim
|
||||
(HNode model : ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode)
|
||||
(DNode solver : ('y, 'yder, 'zin, 'zout) solver)
|
||||
: ('p, 'a, 'b) sim
|
||||
= let state = S.init ~mstate:model.state ~sstate:solver.state in
|
||||
let rec step state input =
|
||||
match input, S.is_running state with
|
||||
| Some input, _ ->
|
||||
let mode = Discrete and now = 0.0 and stop = input.length in
|
||||
let status = S.running ~mode ~input ~now ~stop (S.status state) in
|
||||
let state = S.update ~status state in
|
||||
None, state
|
||||
| None, false -> None, state
|
||||
| None, true -> assert false
|
||||
in
|
||||
let reset = assert false in
|
||||
DNode { state; step; reset }
|
||||
|
||||
end
|
||||
|
|
|
|||
|
|
@ -5,11 +5,12 @@ open Types
|
|||
let solver (DNode csolver : ('y, 'yder) csolver)
|
||||
(DNode zsolver : ('y, 'zin, 'zout) zsolver)
|
||||
: ('y, 'yder, 'zin, 'zout) solver =
|
||||
let s = csolver.s, zsolver.s in
|
||||
let step (cs, zs) h =
|
||||
let (h', f), cs' = csolver.step cs h in
|
||||
let (h', z), zs' = zsolver.step zs (h', f) in
|
||||
(h', f, z), (cs', zs') in
|
||||
let reset (ivp, zc) (cs, zs) = csolver.reset ivp cs, zsolver.reset zc zs in
|
||||
DNode { s; step; reset }
|
||||
let state = csolver.state, zsolver.state in
|
||||
let step (cstate, zstate) h =
|
||||
let (h, f), cstate = csolver.step cstate h in
|
||||
let (h, z), zstate = zsolver.step zstate (h, f) in
|
||||
(h, f, z), (cstate, zstate) in
|
||||
let reset (ivp, zc) (cstate, zstate) =
|
||||
csolver.reset ivp cstate, zsolver.reset zc zstate in
|
||||
DNode { state; step; reset }
|
||||
|
||||
|
|
|
|||
205
src/lib/hsim/state.ml
Normal file
205
src/lib/hsim/state.ml
Normal file
|
|
@ -0,0 +1,205 @@
|
|||
|
||||
open Types
|
||||
|
||||
(* Model *)
|
||||
|
||||
module type ModelState =
|
||||
sig
|
||||
type state
|
||||
end
|
||||
|
||||
(* Solvers *)
|
||||
|
||||
module type SolverState =
|
||||
sig
|
||||
type state
|
||||
end
|
||||
|
||||
(* Simulation *)
|
||||
|
||||
(** Step mode: discrete or continuous *)
|
||||
type mode = Discrete | Continuous
|
||||
|
||||
module type SimState =
|
||||
sig
|
||||
(** Simulation status:
|
||||
- Idle: waiting for input
|
||||
- Running: currently integrating; in this case, we have access to the
|
||||
step mode, current input, timestamp and stop time. *)
|
||||
type 'a status
|
||||
|
||||
(** Internal state of the simulation. This contains the state of the model
|
||||
and solver, as well as the current simulation status. *)
|
||||
type ('a, 'ms, 'ss) state
|
||||
|
||||
(** Get the current simulation status. *)
|
||||
val status : ('a, 'ms, 'ss) state -> 'a status
|
||||
|
||||
(** Get the model state. *)
|
||||
val mstate : ('a, 'ms, 'ss) state -> 'ms
|
||||
|
||||
(** Get the solver state. *)
|
||||
val sstate : ('a, 'ms, 'ss) state -> 'ss
|
||||
|
||||
(** Is the simulation running or idle ? *)
|
||||
val is_running : ('a, 'ms, 'ss) state -> bool
|
||||
|
||||
(** Update the state. *)
|
||||
val update : ?status:'a status -> ?mstate:'ms -> ?sstate:'ss ->
|
||||
('a, 'ms, 'ss) state -> ('a, 'ms, 'ss) state
|
||||
|
||||
(** Update the status to idle. *)
|
||||
val idle : 'a status -> 'a status
|
||||
|
||||
(** Update the status to running. *)
|
||||
val running :
|
||||
?mode:mode -> ?input:'a value ->
|
||||
?now:time -> ?stop:time -> 'a status -> 'a status
|
||||
|
||||
(** Get the current step mode.
|
||||
⚠ Should only be called when running (see [is_running]). *)
|
||||
val mode : ('a, 'ms, 'ss) state -> mode
|
||||
|
||||
(** Get the current input.
|
||||
⚠ Should only be called when running (see [is_running]). *)
|
||||
val input : ('a, 'ms, 'ss) state -> 'a value
|
||||
|
||||
(** Get the current timestamp.
|
||||
⚠ Should only be called when running (see [is_running]). *)
|
||||
val now : ('a, 'ms, 'ss) state -> time
|
||||
|
||||
(** Get the current stop time.
|
||||
⚠ Should only be called when running (see [is_running]). *)
|
||||
val stop : ('a, 'ms, 'ss) state -> time
|
||||
|
||||
(** Build an initial state. *)
|
||||
val init : mstate:'ms -> sstate:'ss -> ('a, 'ms, 'ss) state
|
||||
end
|
||||
|
||||
module FunctionalSimState : SimState =
|
||||
struct
|
||||
|
||||
(** Simulation status:
|
||||
- [Idle]: Waiting for input, no activity;
|
||||
- [Running]: Currently integrating: step [mode], current [input], [now]
|
||||
timestamp, and [stop] time. *)
|
||||
type 'a status =
|
||||
| Idle : 'a status
|
||||
| Running :
|
||||
{ mode : mode; (** Step mode. *)
|
||||
input : 'a value; (** Function to integrate. *)
|
||||
now : time; (** Current time of integration. *)
|
||||
stop : time; (** How long until we stop. *)
|
||||
} -> 'a status
|
||||
|
||||
(** Internal state of the simulation node: model state, solver state and
|
||||
current simulation status. *)
|
||||
type ('a, 'ms, 'ss) state =
|
||||
{ status : 'a status; (** Current simulation status. *)
|
||||
mstate : 'ms; (** Model state. *)
|
||||
sstate : 'ss } (** Solver state. *)
|
||||
|
||||
exception Not_running
|
||||
|
||||
let status s = s.status
|
||||
let mstate s = s.mstate
|
||||
let sstate s = s.sstate
|
||||
|
||||
let is_running s =
|
||||
match s.status with Running _ -> true | Idle -> false
|
||||
|
||||
let idle _ = Idle
|
||||
|
||||
let running ?mode ?input ?now ?stop s =
|
||||
match s with
|
||||
| Idle ->
|
||||
begin match mode, input, now, stop with
|
||||
| Some mode, Some input, Some now, Some stop ->
|
||||
Running { mode; input; now; stop }
|
||||
| _ -> raise (Invalid_argument "")
|
||||
end
|
||||
| Running { mode=m; input=i; now=n; stop=s } ->
|
||||
let mode = Option.value mode ~default:m in
|
||||
let input = Option.value input ~default:i in
|
||||
let now = Option.value now ~default:n in
|
||||
let stop = Option.value stop ~default:s in
|
||||
Running { mode; input; now; stop }
|
||||
|
||||
let update ?status ?mstate ?sstate { status=st; mstate=ms; sstate=ss } =
|
||||
let status = Option.value status ~default:st in
|
||||
let mstate = Option.value mstate ~default:ms in
|
||||
let sstate = Option.value sstate ~default:ss in
|
||||
{ status; mstate; sstate }
|
||||
|
||||
let mode s =
|
||||
match s.status with Running r -> r.mode | Idle -> raise Not_running
|
||||
let input s =
|
||||
match s.status with Running r -> r.input | Idle -> raise Not_running
|
||||
let now s =
|
||||
match s.status with Running r -> r.now | Idle -> raise Not_running
|
||||
let stop s =
|
||||
match s.status with Running r -> r.stop | Idle -> raise Not_running
|
||||
|
||||
let init ~mstate ~sstate = { status = Idle; mstate; sstate }
|
||||
end
|
||||
|
||||
module InPlaceSimState : SimState =
|
||||
struct
|
||||
type 'a status =
|
||||
| Idle : 'a status
|
||||
| Running :
|
||||
{ mutable mode : mode;
|
||||
mutable input : 'a value;
|
||||
mutable now : time;
|
||||
mutable stop : time;
|
||||
} -> 'a status
|
||||
|
||||
type ('a, 'ms, 'ss) state =
|
||||
{ mutable status : 'a status;
|
||||
mutable mstate : 'ms;
|
||||
mutable sstate : 'ss }
|
||||
|
||||
exception Not_running
|
||||
|
||||
let status s = s.status
|
||||
let mstate s = s.mstate
|
||||
let sstate s = s.sstate
|
||||
|
||||
let is_running s =
|
||||
match s.status with Running _ -> true | Idle -> false
|
||||
|
||||
let idle _ = Idle
|
||||
|
||||
let running ?mode ?input ?now ?stop status =
|
||||
match status with
|
||||
| Idle ->
|
||||
begin match mode, input, now, stop with
|
||||
| Some mode, Some input, Some now, Some stop ->
|
||||
Running { mode; input; now; stop }
|
||||
| _ -> raise (Invalid_argument "")
|
||||
end
|
||||
| Running ({ mode=m; input=i; now=n; stop=s } as r) ->
|
||||
let mode = Option.value mode ~default:m in r.mode <- mode;
|
||||
let input = Option.value input ~default:i in r.input <- input;
|
||||
let now = Option.value now ~default:n in r.now <- now;
|
||||
let stop = Option.value stop ~default:s in r.stop <- stop;
|
||||
status
|
||||
|
||||
let update ?status ?mstate ?sstate
|
||||
({ status=st; mstate=ms; sstate=ss } as s) =
|
||||
let status = Option.value status ~default:st in
|
||||
let mstate = Option.value mstate ~default:ms in
|
||||
let sstate = Option.value sstate ~default:ss in
|
||||
s.status <- status; s.mstate <- mstate; s.sstate <- sstate; s
|
||||
|
||||
let mode s =
|
||||
match s.status with Running r -> r.mode | Idle -> raise Not_running
|
||||
let input s =
|
||||
match s.status with Running r -> r.input | Idle -> raise Not_running
|
||||
let now s =
|
||||
match s.status with Running r -> r.now | Idle -> raise Not_running
|
||||
let stop s =
|
||||
match s.status with Running r -> r.stop | Idle -> raise Not_running
|
||||
|
||||
let init ~mstate ~sstate = { status = Idle; mstate; sstate }
|
||||
end
|
||||
|
|
@ -9,15 +9,15 @@ type 'a value =
|
|||
|
||||
(** A time signal is a sequence of possibly absent α-values
|
||||
[{ start; length; u }] where:
|
||||
- [horizon] is a positive (possibly null) floating-point number;
|
||||
- [start] and [length] are positive (possibly null) floating-point numbers;
|
||||
- [u: [0, length] -> α] *)
|
||||
type 'a signal = 'a value option
|
||||
|
||||
(** A discrete node. *)
|
||||
type ('p, 'a, 'b) dnode =
|
||||
DNode :
|
||||
{ s : 'ds;
|
||||
step : 'ds -> 'a -> 'b * 'ds;
|
||||
{ state : 'ds;
|
||||
step : 'ds -> 'a -> 'b * 'ds;
|
||||
reset : 'p -> 'ds -> 'ds;
|
||||
} -> ('p, 'a, 'b) dnode
|
||||
|
||||
|
|
@ -32,7 +32,7 @@ type ('a, 'b, 'y, 'yder) cnode =
|
|||
(** A hybrid node. *)
|
||||
type ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode =
|
||||
HNode :
|
||||
{ s : 'hs;
|
||||
{ state : 'hs;
|
||||
step : 'hs -> 'a -> 'b * 'hs; (** Discrete step function. *)
|
||||
fder : 'hs -> 'a -> 'y -> 'yder; (** Continuous derivative function. *)
|
||||
fout : 'hs -> 'a -> 'y -> 'b; (** Continuous output function. *)
|
||||
|
|
@ -78,3 +78,8 @@ type ('y, 'yder, 'zin, 'zout) solver =
|
|||
(('y, 'yder) ivp * ('y, 'zout) zc,
|
||||
time,
|
||||
time * (time -> 'y) * 'zin option) dnode
|
||||
|
||||
(** The simulation of a hybrid system is a synchronous function on streams of
|
||||
functions. *)
|
||||
type ('p, 'a, 'b) sim =
|
||||
('p, 'a signal, 'b signal) dnode
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue