-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathinstantiate.jl
186 lines (143 loc) · 6.46 KB
/
instantiate.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
# helper functions to check that the given system can be applied to the states/inputs
@inline _is_conformable_state(system, x) = statedim(system) == length(x)
@inline _is_conformable_input(system, u) = inputdim(system) == length(u)
@inline _is_conformable_noise(system, w) = noisedim(system) == length(w)
@inline _in_stateset(system, x) = x ∈ stateset(system)
@inline _in_inputset(system, u) = u ∈ inputset(system)
@inline _in_noiseset(system, w) = w ∈ noiseset(system)
@inline function _argument_error(sym, set=:none)
if sym == :x
vector = "state"
elseif sym == :u
vector = "input"
elseif sym == :w
vector = "noise"
end
if set == :none
throw(ArgumentError("the $vector vector has the wrong dimensions"))
else
throw(ArgumentError("the $vector vector is not contained in the $(vector) set"))
end
end
# ====================
# Generic _instantiate method
# ====================
"""
_instantiate(system::AbstractSystem, x::AbstractVector;
[check_constraints]=true)
Return the result of instantiating an `AbstractSystem` at the current state.
### Input
- `system` -- `AbstractSystem`
- `x` -- state (it should be any vector type)
- `check_constraints` -- (optional, default: `true`) check if the state belongs to
the state set
### Output
The result of applying the system to state `x`.
### Notes
The `_instantiate` method generalizes the `successor` of an `AbstractDiscreteSystem`
and the `vector_field` of an `AbstractContinuousSystem` into a single method.
"""
function _instantiate(system::AbstractSystem, x::AbstractVector;
check_constraints::Bool=true)
!_is_conformable_state(system, x) && _argument_error(:x)
if isconstrained(system) && check_constraints
!_in_stateset(system, x) && _argument_error(:x,:X)
end
if islinear(system)
return state_matrix(system) * x
elseif isaffine(system)
return state_matrix(system) * x + affine_term(system)
elseif ispolynomial(system) || isblackbox(system)
return mapping(system)(x)
else
throw(ArgumentError("_instantiate not defined for type `$(typename(sys))`"))
end
end
"""
_instantiate(system::AbstractSystem, x::AbstractVector, u::AbstractVector;
[check_constraints]=true)
Return the result of instantiating an `AbstractSystem` at the current state and
applying one input.
### Input
- `system` -- `AbstractSystem`
- `x` -- state (it should be any vector type)
- `u` -- input (it should be any vector type) or noise, if `system`
is not controlled
- `check_constraints` -- (optional, default: `true`) check if the state belongs to
the state set
### Output
The result of applying the system to state `x` and input `u`.
### Notes
If the system is not controlled but noisy, the input `u` is interpreted as noise.
The `_instantiate` method generalizes the `successor` of an `AbstractDiscreteSystem`
and the `vector_field` of an `AbstractContinuousSystem` into a single method.
"""
function _instantiate(system::AbstractSystem, x::AbstractVector, u::AbstractVector;
check_constraints::Bool=true)
# Figure out if `system` is a controlled or noisy system and set the function
# `matrix` to either `input_matix` or `noise_matrix`
if iscontrolled(system) && !isnoisy(system)
input_var = :u; input_set = :U; matrix = input_matrix
_is_conformable = _is_conformable_input
_in_set = _in_inputset
elseif isnoisy(system) && !iscontrolled(system)
input_var = :w; input_set = :W; matrix = noise_matrix
_is_conformable = _is_conformable_noise
_in_set = _in_noiseset
else
throw(ArgumentError("successor function for $(typeof(system)) does not have 2 arguments"))
end
!_is_conformable_state(system, x) && _argument_error(:x)
!_is_conformable(system, u) && _argument_error(input_var)
if isconstrained(system) && check_constraints
!_in_stateset(system, x) && _argument_error(:x,:X)
!_in_set(system, u) && _argument_error(input_var, input_set)
end
if islinear(system)
return state_matrix(system) * x + matrix(system) * u
elseif isaffine(system)
return state_matrix(system) * x + affine_term(system) + matrix(system) * u
elseif ispolynomial(system) || isblackbox(system)
return mapping(system)(x, u)
else
throw(ArgumentError("_instantiate not defined for type `$(typename(system))`"))
end
end
"""
_instantiate(system::AbstractSystem,
x::AbstractVector, u::AbstractVector, w::AbstractVector; [check_constraints]=true)
Return the result of instantiating an `AbstractSystem` at the current state and
applying two inputs to an `AbstractSystem`.
### Input
- `system` -- `AbstractSystem`
- `x` -- state (it should be any vector type)
- `u` -- input (it should be any vector type)
- `w` -- noise (it should be any vector type)
- `check_constraints` -- (optional, default: `true`) check if the state belongs to
the state set
### Output
The result of applying the system to state `x`, input `u` and noise `w`.
### Notes
The `_instantiate` method generalizes the `successor` of an `AbstractDiscreteSystem`
and the `vector_field` of an `AbstractContinuousSystem` into a single method.
"""
function _instantiate(system::AbstractSystem, x::AbstractVector, u::AbstractVector, w::AbstractVector;
check_constraints::Bool=true)
!_is_conformable_state(system, x) && _argument_error(:x)
!_is_conformable_input(system, u) && _argument_error(:u)
!_is_conformable_noise(system, w) && _argument_error(:w)
if isconstrained(system) && check_constraints
!_in_stateset(system, x) && _argument_error(:x,:X)
!_in_inputset(system, u) && _argument_error(:u,:U)
!_in_noiseset(system, w) && _argument_error(:w,:W)
end
if islinear(system)
return state_matrix(system) * x + input_matrix(system) * u + noise_matrix(system) * w
elseif isaffine(system)
return state_matrix(system) * x + affine_term(system) + input_matrix(system) * u + noise_matrix(system) * w
elseif ispolynomial(system) || isblackbox(system)
return mapping(system)(x, u, w)
else
throw(ArgumentError("_instantiate not defined for type `$(typename(system))`"))
end
end