Coverage for src/gncpy/dynamics/basic/nonlinear_dynamics_base.py: 78%
67 statements
« prev ^ index » next coverage.py v7.2.7, created at 2023-07-19 05:48 +0000
1import numpy as np
2import scipy.integrate as s_integrate
4import gncpy.math as gmath
5from abc import abstractmethod
6from warnings import warn
7from .dynamics_base import DynamicsBase
9class NonlinearDynamicsBase(DynamicsBase):
10 """Base class for non-linear dynamics models.
12 Child classes should define their own cont_fnc_lst property and set the
13 state names class variable. The remainder of the functions autogenerate
14 based on these values.
16 Attributes
17 ----------
18 dt : float
19 time difference for the integration period
20 integrator_type : string, optional
21 integrator type as defined by scipy's integrate.ode function. The
22 default is `dopri5`.
23 integrator_params : dict, optional
24 additional parameters for the integrator. The default is {}.
25 """
27 def __init__(
28 self, integrator_type="dopri5", integrator_params={}, dt=np.nan, **kwargs
29 ):
30 super().__init__(**kwargs)
32 self.dt = dt
33 self.integrator_type = integrator_type
34 self.integrator_params = integrator_params
36 self._integrator = None
38 @property
39 @abstractmethod
40 def cont_fnc_lst(self):
41 r"""Class property for the continuous time dynamics functions.
43 Must be overridden in the child classes.
45 Notes
46 -----
47 This is a list of functions that implement part of the differential
48 equations :math:`\dot{x} = f(t, x) + g(t, u)` for each state, in order.
49 These functions only represent the dynamics :math:`f(t, x)`.
51 Returns
52 -------
53 list
54 Each element is a function that take the timestep, the state, and
55 f_args. They must return the new state for the given index in the
56 list/state vector.
57 """
58 raise NotImplementedError()
60 def _cont_dyn(self, t, x, u, state_args, ctrl_args):
61 r"""Implements the continuous time dynamics.
63 This automatically sets up the combined differential equation based on
64 the supplied continuous function list.
66 This implements the equation :math:`\dot{x} = f(t, x) + g(t, x, u)` and
67 returns the state derivatives as a vector. Note the control input is
68 only used if an appropriate control model is set by the user.
70 Parameters
71 ----------
72 t : float
73 timestep.
74 x : N x 1 numpy array
75 current state.
76 u : Nu x 1 numpy array
77 Control effort, not used if no control model.
78 state_args : tuple
79 Additional arguements for the state functions.
80 ctrl_args : tuple
81 Additional arguments for the control functions. Not used if no
82 control model
84 Returns
85 -------
86 x_dot : N x 1 numpy array
87 state derivative
88 """
89 out = np.zeros((len(self.state_names), 1))
90 for ii, f in enumerate(self.cont_fnc_lst):
91 out[ii] = f(t, x, *state_args)
92 if self._control_model is not None:
93 for ii, g in enumerate(self._control_model):
94 out[ii] += g(t, x, u, *ctrl_args)
95 return out
97 def get_state_mat(
98 self, timestep, state, *f_args, u=None, ctrl_args=None, use_continuous=False
99 ):
100 """Calculates the state matrix from the ode list.
102 Parameters
103 ----------
104 timestep : float
105 timestep.
106 state : N x 1 numpy array
107 current state.
108 *f_args : tuple
109 Additional arguments to pass to the ode functions.
111 Returns
112 -------
113 N x N numpy array
114 state transition matrix.
115 """
116 if ctrl_args is None:
117 ctrl_args = ()
118 if use_continuous:
119 if self._control_model is not None and u is not None:
121 def factory(ii):
122 return lambda _t, _x, _u, *_args: self._cont_dyn(
123 _t, _x, _u, _args, ctrl_args
124 )[ii]
126 return gmath.get_state_jacobian(
127 timestep,
128 state,
129 [factory(ii) for ii in range(state.size)],
130 f_args,
131 u=u,
132 )
133 return gmath.get_state_jacobian(timestep, state, self.cont_fnc_lst, f_args)
134 else:
136 def factory(ii):
137 return lambda _t, _x, *_args: self.propagate_state(
138 _t, _x, u=u, state_args=_args, ctrl_args=ctrl_args
139 )[ii]
141 return gmath.get_state_jacobian(
142 timestep,
143 state,
144 [factory(ii) for ii in range(state.size)],
145 f_args,
146 )
148 def get_input_mat(self, timestep, state, u, state_args=None, ctrl_args=None):
149 """Calculates the input matrix from the control model.
151 This calculates the jacobian of the control model. If no control model
152 is specified than it returns a zero matrix.
154 Parameters
155 ----------
156 timestep : float
157 current timestep.
158 state : N x 1 numpy array
159 current state.
160 u : Nu x 1
161 current control input.
162 *f_args : tuple
163 Additional arguments to pass to the control model.
165 Returns
166 -------
167 N x Nu numpy array
168 Control input matrix.
169 """
170 if self._control_model is None:
171 warn("Control model is None")
172 return np.zeros((state.size, u.size))
173 if state_args is None:
174 state_args = ()
175 if ctrl_args is None:
176 ctrl_args = ()
178 def factory(ii):
179 return lambda _t, _x, _u, *_args: self.propagate_state(
180 _t, _x, u=_u, state_args=state_args, ctrl_args=ctrl_args
181 )[ii]
183 return gmath.get_input_jacobian(
184 timestep,
185 state,
186 u,
187 [factory(ii) for ii in range(state.size)],
188 (),
189 )
191 def propagate_state(self, timestep, state, u=None, state_args=None, ctrl_args=None):
192 """Propagates the continuous time dynamics.
194 Automatically integrates the defined ode list and adds control inputs
195 (held constant over the integration period).
197 Parameters
198 ----------
199 timestep : float
200 current timestep.
201 state : N x 1 numpy array
202 Current state vector.
203 u : Nu x 1 numpy array, optional
204 Current control effort. The default is None. Only used if a control
205 model is set.
206 state_args : tuple, optional
207 Additional arguments to pass to the state odes. The default is ().
208 ctrl_args : tuple, optional
209 Additional arguments to pass to the control functions. The default
210 is ().
212 Raises
213 ------
214 RuntimeError
215 If the integration fails.
217 Returns
218 -------
219 next_state : N x 1 numpy array
220 the state at time :math:`t + dt`.
221 """
222 if state_args is None:
223 state_args = ()
224 if ctrl_args is None:
225 ctrl_args = ()
226 self._integrator = s_integrate.ode(
227 lambda t, y: self._cont_dyn(t, y, u, state_args, ctrl_args).flatten()
228 )
229 self._integrator.set_integrator(self.integrator_type, **self.integrator_params)
230 self._integrator.set_initial_value(state, timestep)
232 if np.isnan(self.dt) or np.isinf(self.dt):
233 raise RuntimeError("Invalid value for dt ({}).".format(self.dt))
234 next_time = timestep + self.dt
235 next_state = self._integrator.integrate(next_time)
236 next_state = next_state.reshape((next_state.size, 1))
237 if not self._integrator.successful():
238 msg = "Integration failed at time {}".format(timestep)
239 raise RuntimeError(msg)
240 if self.state_constraint is not None:
241 next_state = self.state_constraint(timestep, next_state)
242 return next_state