feat: start of assertions
This commit is contained in:
parent
65918ab59b
commit
883e5fff01
GPG key ID:
7065F57ED8856128
6 changed files with 341 additions and 93 deletions
|
|
@ -26,14 +26,16 @@ module Sim (S : SimState) =
|
|||
let mode, stop, now = Continuous, i.h, 0.0 in
|
||||
update ms ss (set_running ~mode ~input:i ~stop ~now s)
|
||||
end else set_running ~mode:Continuous s in
|
||||
Some { h=0.0; c=Discontinuous; u=fun _ -> o }, s
|
||||
Utils.dot o, s
|
||||
|
||||
let step_continuous s step cset fout zset =
|
||||
let ms, ss = get_mstate s, get_sstate s in
|
||||
let i, now, stop = get_input s, get_now s, get_stop s in
|
||||
let (h, f, z), ss = step ss stop in
|
||||
let ms = cset ms (f h) in
|
||||
let fout t = fout ms (i.u (now +. t)) (f (now +. t)) in
|
||||
let fy t = f (now +. t) in
|
||||
let fms t = cset ms (fy t) in
|
||||
let fout t = fout ms (i.u (now +. t)) (fy t) in
|
||||
let s, c = match z with
|
||||
| None ->
|
||||
let s, c = if h >= stop
|
||||
|
|
@ -43,48 +45,102 @@ module Sim (S : SimState) =
|
|||
| Some z ->
|
||||
let s = set_running ~mode:Discrete ~now:h s in
|
||||
update (zset ms z) ss s, Discontinuous in
|
||||
Some { h=h -. now; u=fout; c }, s
|
||||
let h = h -. now in
|
||||
{ h; u=fout; c }, s, { h; c; u=fms }
|
||||
|
||||
(** Simulation of a model with any solver. *)
|
||||
let run
|
||||
(HNode model : ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode)
|
||||
(DNode solver : ('y, 'yder, 'zin, 'zout) solver)
|
||||
: ('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) lazy_sim
|
||||
= let state = get_init model.state solver.state in
|
||||
let step_discrete s =
|
||||
step_discrete s model.step model.horizon model.fder model.fzer
|
||||
model.cget model.csize model.zsize model.jump solver.reset in
|
||||
(HNode m : ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode)
|
||||
(DNode s : ('y, 'yder, 'zin, 'zout) solver)
|
||||
: ('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) sim
|
||||
= let state = get_init m.state s.state in
|
||||
let step_discrete st =
|
||||
let o, s = step_discrete st m.step m.horizon m.fder m.fzer m.cget
|
||||
m.csize m.zsize m.jump s.reset in
|
||||
Some o, s in
|
||||
let step_continuous st =
|
||||
let o, s, _ = step_continuous st s.step m.cset m.fout m.zset in
|
||||
Some o, s in
|
||||
|
||||
let step_continuous s =
|
||||
step_continuous s solver.step model.cset model.fout model.zset in
|
||||
|
||||
let step s = function
|
||||
let step st = function
|
||||
| Some i ->
|
||||
let mode, now, stop = Discrete, 0.0, i.h in
|
||||
step_discrete (set_running ~mode ~input:i ~now ~stop s)
|
||||
step_discrete (set_running ~mode ~input:i ~now ~stop st)
|
||||
| None ->
|
||||
if is_running s then match get_mode s with
|
||||
| Discrete -> step_discrete s
|
||||
| Continuous -> step_continuous s
|
||||
else None, s in
|
||||
if is_running st then match get_mode st with
|
||||
| Discrete -> step_discrete st
|
||||
| Continuous -> step_continuous st
|
||||
else None, st in
|
||||
|
||||
let reset (pm, ps) s =
|
||||
let ms = model.reset pm (get_mstate s) in
|
||||
let ss = solver.reset ps (get_sstate s) in
|
||||
update ms ss (set_idle s) in
|
||||
let reset (pm, ps) st =
|
||||
let ms = m.reset pm (get_mstate st) in
|
||||
let ss = s.reset ps (get_sstate st) in
|
||||
update ms ss (set_idle st) in
|
||||
|
||||
DNode { state; step; reset }
|
||||
|
||||
let rec run_assert :
|
||||
'a 'b. ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode_a ->
|
||||
(unit -> ('y, 'yder, 'zin, 'zout) solver) ->
|
||||
('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) sim =
|
||||
fun (HNodeA m) get_s ->
|
||||
let DNode s = get_s () in
|
||||
let al = List.map (fun a -> run_assert a get_s) m.assertions in
|
||||
let state = get_init m.body.state s.state, al in
|
||||
|
||||
let step_discrete (st, al) =
|
||||
let m=m.body in
|
||||
let o, st =
|
||||
step_discrete st m.step m.horizon m.fder m.fzer m.cget m.csize m.zsize
|
||||
m.jump s.reset in
|
||||
let al = List.map (fun (DNode a) ->
|
||||
let _, state = a.step a.state @@ Some (Utils.dot @@ get_mstate st) in
|
||||
DNode { a with state }) al in
|
||||
Some o, (st, al) in
|
||||
|
||||
let step_continuous (st, al) =
|
||||
let ({ h; _ } as o), st, u =
|
||||
step_continuous st s.step m.body.cset m.body.fout m.body.zset in
|
||||
let al = List.map (fun (DNode a) ->
|
||||
(* Step assertions repeatedly until they reach the horizon. *)
|
||||
let rec step s =
|
||||
let o, s = a.step s None in
|
||||
match o with None -> s | Some _ -> step s in
|
||||
let state = step (snd @@ a.step a.state (Some u)) in
|
||||
DNode { a with state }) al in
|
||||
(* Reset the model's state to the reached horizon. *)
|
||||
let st = set_mstate (u.u h) st in
|
||||
Some o, (st, al) in
|
||||
|
||||
let step (st, al) = function
|
||||
| Some i ->
|
||||
let mode, now, stop = Discrete, 0.0, i.h in
|
||||
step_discrete (set_running ~mode ~input:i ~now ~stop st, al)
|
||||
| None ->
|
||||
if is_running st then match get_mode st with
|
||||
| Discrete -> step_discrete (st, al)
|
||||
| Continuous -> step_continuous (st, al)
|
||||
else None, (st, al) in
|
||||
|
||||
let reset (pm, ps) (st, al) =
|
||||
let ms = m.body.reset pm (get_mstate st) in
|
||||
let ss = s.reset ps (get_sstate st) in
|
||||
update ms ss (set_idle st), al in
|
||||
|
||||
DNode { state; step; reset }
|
||||
|
||||
let accelerate
|
||||
(HNode m : ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode)
|
||||
(DNodeC s : ('y, 'yder, 'zin, 'zout) solver_c)
|
||||
: ('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) lazy_sim
|
||||
: ('p * (('y, 'yder) ivp * ('y, 'zout) zc), 'a, 'b) sim
|
||||
= let state = get_init m.state s.state in
|
||||
let step_discrete st =
|
||||
step_discrete st m.step m.horizon m.fder m.fzer m.cget m.csize m.zsize
|
||||
m.jump s.reset in
|
||||
let o, st = step_discrete st m.step m.horizon m.fder m.fzer m.cget
|
||||
m.csize m.zsize m.jump s.reset in
|
||||
Some o, st in
|
||||
let step_continuous st =
|
||||
step_continuous st s.step m.cset m.fout m.zset in
|
||||
let o, st, _ = step_continuous st s.step m.cset m.fout m.zset in
|
||||
o, st in
|
||||
|
||||
let rec step st = function
|
||||
| Some i ->
|
||||
|
|
@ -95,14 +151,11 @@ module Sim (S : SimState) =
|
|||
| Discrete -> step_discrete st
|
||||
| Continuous ->
|
||||
let o, st = step_continuous st in
|
||||
match o with
|
||||
| None -> None, st
|
||||
| Some { c=Discontinuous; _ } -> o, st
|
||||
| Some ({ c=Continuous; _ } as o) ->
|
||||
match o.c with
|
||||
| Discontinuous -> Some o, st
|
||||
| Continuous ->
|
||||
let o', st = step st None in
|
||||
match o' with
|
||||
| None -> assert false
|
||||
| Some o' -> Some (Utils.concat [o;o']), st
|
||||
Some (Utils.concat [o; Option.get o']), st
|
||||
else None, st in
|
||||
|
||||
let reset (pm, ps) st =
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@ type continuity = Continuous | Discontinuous
|
|||
type 'a value =
|
||||
{ h : time;
|
||||
u : time -> 'a; (* Defined on [[0, h]]. *)
|
||||
c : continuity }
|
||||
c : continuity } (* Continuity w.r.t. the next value. *)
|
||||
|
||||
(** A time signal is a sequence of possibly absent α-values
|
||||
[{ h; u }] where:
|
||||
|
|
@ -14,57 +14,64 @@ type 'a value =
|
|||
- [u: [0, h] -> α] *)
|
||||
type 'a signal = 'a value option
|
||||
|
||||
type ('s, 'p, 'a, 'b) drec =
|
||||
{ state : 's;
|
||||
step : 's -> 'a -> 'b * 's;
|
||||
reset : 'p -> 's -> 's }
|
||||
|
||||
(** A discrete node. *)
|
||||
type ('p, 'a, 'b) dnode =
|
||||
DNode :
|
||||
{ state : 's;
|
||||
step : 's -> 'a -> 'b * 's;
|
||||
reset : 'p -> 's -> 's;
|
||||
} -> ('p, 'a, 'b) dnode
|
||||
DNode : ('s, 'p, 'a, 'b) drec -> ('p, 'a, 'b) dnode
|
||||
|
||||
type ('s, 'p, 'a, 'b) drec_c =
|
||||
{ state : 's;
|
||||
step : 's -> 'a -> 'b * 's;
|
||||
reset : 'p -> 's -> 's;
|
||||
copy : 's -> 's }
|
||||
|
||||
(** A discrete node which supports a state copy. *)
|
||||
type ('p, 'a, 'b) dnode_c =
|
||||
DNodeC :
|
||||
{ state : 's;
|
||||
step : 's -> 'a -> 'b * 's;
|
||||
reset : 'p -> 's -> 's;
|
||||
copy : 's -> 's;
|
||||
} -> ('p, 'a, 'b) dnode_c
|
||||
DNodeC : ('s, 'p, 'a, 'b) drec_c -> ('p, 'a, 'b) dnode_c
|
||||
|
||||
(** A continuous node. *)
|
||||
type ('a, 'b, 'y, 'yder) cnode =
|
||||
CNode :
|
||||
{ lsty : 'y;
|
||||
fder : 'a -> 'y -> 'yder;
|
||||
fout : 'a -> 'y -> 'b;
|
||||
} -> ('a, 'b, 'y, 'yder) cnode
|
||||
type ('s, 'p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hrec =
|
||||
{ state: 's;
|
||||
step : 's -> 'a -> 'b * 's; (** Discrete step function. *)
|
||||
fder : 's -> 'a -> 'y -> 'yder; (** Continuous derivative function. *)
|
||||
fout : 's -> 'a -> 'y -> 'b; (** Continuous output function. *)
|
||||
fzer : 's -> 'a -> 'y -> 'zout; (** Continuous zero-crossing function. *)
|
||||
reset : 'p -> 's -> 's; (** Reset function. *)
|
||||
horizon : 's -> time; (** Next integration horizon. *)
|
||||
jump : 's -> bool; (** Discontinuity flag. *)
|
||||
cget : 's -> 'y; (** Get continuous state. *)
|
||||
cset : 's -> 'y -> 's; (** Set continuous state. *)
|
||||
zset : 's -> 'zin -> 's; (** Set zero-crossing state. *)
|
||||
csize : int;
|
||||
zsize : int }
|
||||
|
||||
(** A hybrid node. *)
|
||||
type ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode =
|
||||
HNode :
|
||||
{ state: 's;
|
||||
step : 's -> 'a -> 'b * 's; (** Discrete step function. *)
|
||||
fder : 's -> 'a -> 'y -> 'yder; (** Continuous derivative function. *)
|
||||
fout : 's -> 'a -> 'y -> 'b; (** Continuous output function. *)
|
||||
fzer : 's -> 'a -> 'y -> 'zout; (** Continuous zero-crossing function. *)
|
||||
reset : 'p -> 's -> 's; (** Reset function. *)
|
||||
horizon : 's -> time; (** Next integration horizon. *)
|
||||
jump : 's -> bool; (** Discontinuity flag. *)
|
||||
cget : 's -> 'y; (** Get continuous state. *)
|
||||
cset : 's -> 'y -> 's; (** Set continuous state. *)
|
||||
zset : 's -> 'zin -> 's; (** Set zero-crossing state. *)
|
||||
csize : int;
|
||||
zsize : int;
|
||||
} -> ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode
|
||||
HNode : ('s, 'p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hrec ->
|
||||
('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode
|
||||
|
||||
(** A hybrid node with assertions. *)
|
||||
type ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode_a =
|
||||
HNodeA : {
|
||||
body : ('s, 'p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hrec;
|
||||
assertions : ('p, 's, unit, 'y, 'yder, 'zin, 'zout) hnode_a list
|
||||
} -> ('p, 'a, 'b, 'y, 'yder, 'zin, 'zout) hnode_a
|
||||
|
||||
(** The simulation of a hybrid system is a synchronous function on streams of
|
||||
functions. *)
|
||||
type ('p, 'a, 'b) lazy_sim = ('p, 'a signal, 'b signal) dnode
|
||||
type ('p, 'a, 'b) sim = ('p, 'a signal, 'b signal) dnode
|
||||
|
||||
(** Greedy simulation takes in an input and computes as many solver and
|
||||
subsystem steps as needed to reach the input's horizon. *)
|
||||
type ('p, 'a, 'b) greedy_sim = ('p, 'a value, 'b value list) dnode
|
||||
|
||||
(** Utils *)
|
||||
(* Utils *)
|
||||
|
||||
(** Consider a node with state copying as a node without state copying. *)
|
||||
let d_of_dc (DNodeC { state; step; reset; _ }) = DNode { state; step; reset }
|
||||
|
||||
(** Consider a model without assertions as a model with an empty list of
|
||||
assertions. *)
|
||||
let a_of_h (HNode body) = HNodeA { body; assertions=[] }
|
||||
|
||||
(** Consider a model without its assertions. *)
|
||||
let h_of_a (HNodeA { body; _ }) = HNode body
|
||||
|
|
|
|||
|
|
@ -1,18 +1,13 @@
|
|||
|
||||
open Types
|
||||
|
||||
let dot v = { h=0.0; c=Discontinuous; u=fun _ -> v }
|
||||
|
||||
(** Offset the [u] function by [now]. *)
|
||||
let offset (u : time -> 'a) (now : time) : time -> 'a =
|
||||
fun t -> u (t +. now)
|
||||
|
||||
(**
|
||||
Concatenate functions. [
|
||||
^ ^
|
||||
| ---, | ---,
|
||||
| ___ `--- = | _ `---
|
||||
| --' | --'
|
||||
+--------------> +-------------->]
|
||||
*)
|
||||
(** Concatenate functions. *)
|
||||
let rec concat = function
|
||||
| [] -> raise (Invalid_argument "Cannot concatenate an empty value list")
|
||||
| [f] -> f
|
||||
|
|
@ -22,6 +17,7 @@ let rec concat = function
|
|||
let { h=hr; u=ur; c } = concat l in
|
||||
{ c; h=h+.hr; u=fun t -> if t <= h then u t else ur (t -. h) }
|
||||
|
||||
(** Sample a function at [n] equidistant points. *)
|
||||
let sample { h; u; _ } n =
|
||||
let hs = h /. float_of_int n in
|
||||
let rec step i =
|
||||
|
|
@ -44,9 +40,10 @@ let compose (DNode m) (DNode n) =
|
|||
(ms, ns) in
|
||||
DNode { state; step; reset }
|
||||
|
||||
let compose_lazy
|
||||
(DNode m : ('p, 'a, 'b) lazy_sim)
|
||||
(DNode n : ('q, 'b, 'c) lazy_sim)
|
||||
(** Compose two simulations. *)
|
||||
let compose_sim
|
||||
(DNode m : ('p, 'a, 'b) sim)
|
||||
(DNode n : ('q, 'b, 'c) sim)
|
||||
= let state = m.state, n.state in
|
||||
let step (ms, ns) = function
|
||||
| Some i ->
|
||||
|
|
|
|||
|
|
@ -58,7 +58,9 @@ module InPlace =
|
|||
(h, Some vec), s
|
||||
else (h, None), s in
|
||||
|
||||
let copy _ = raise Common.Errors.TODO in
|
||||
let copy s =
|
||||
let vec = zmake (length s.vec) in
|
||||
blit s.vec vec; s.vec <- vec; s in
|
||||
|
||||
DNodeC { state; step; reset; copy }
|
||||
end
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue