-
Notifications
You must be signed in to change notification settings - Fork 0
/
dynamical_systems.py
112 lines (84 loc) · 3.04 KB
/
dynamical_systems.py
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
'''
Author: Edoardo Caldarelli
Affiliation: Institut de Robòtica i Informàtica Industrial, CSIC-UPC
email: [email protected]
July 2024
'''
from abc import ABC, abstractmethod
import numpy as np
class DynamicalSystem(ABC):
@abstractmethod
def update_SOM(self, xbef, control):
pass
class DuffingOscillator(DynamicalSystem):
"""
Duffing oscillator with Runge-Kutta discretization.
"""
def __init__(self, **kwargs):
for key in kwargs:
setattr(self, key, kwargs[key])
def _f_u(self, x, u):
return -np.vstack((-x[1, :],
0.5 * x[1, :]+ x[0, :] * (4 * x[0, :] ** 2 - 1) - 0.5 * u))
def _f_ud(self, x, u):
return (x + (self.Ts / 6) * (self._k1(x, u) + 2 * self._k2(x, u) + 2 * self._k3(x, u) +
self._k4(x, u)))
def _k1(self, x, u):
return self._f_u(x, u)
def _k2(self, x, u):
return self._f_u(x + self._k1(x, u) * self.Ts / 2, u)
def _k3(self, x, u):
return self._f_u(x + self._k2(x, u) * self.Ts / 2, u)
def _k4(self, x, u):
return self._f_u(x + self._k1(x, u) * self.Ts, u)
def update_SOM(self, xbef, u):
if len(xbef.shape) == 1:
xbef = xbef.reshape([-1, 1])
return self._f_ud(xbef, u)
class DoubleIntegrator(DynamicalSystem):
"""
Double integrator with Runge-Kutta discretization.
"""
def __init__(self, **kwargs):
for key in kwargs:
setattr(self, key, kwargs[key])
def _f_u(self, x, u):
return np.vstack((x[1, :],
u))
def _f_ud(self, x, u):
return (x + (self.Ts / 6) * (self._k1(x, u) + 2 * self._k2(x, u) + 2 * self._k3(x, u) +
self._k4(x, u)))
def _k1(self, x, u):
return self._f_u(x, u)
def _k2(self, x, u):
return self._f_u(x + self._k1(x, u) * self.Ts / 2, u)
def _k3(self, x, u):
return self._f_u(x + self._k2(x, u) * self.Ts / 2, u)
def _k4(self, x, u):
return self._f_u(x + self._k1(x, u) * self.Ts, u)
def update_SOM(self, xbef, u):
if len(xbef.shape) == 1:
xbef = xbef.reshape([-1, 1])
return self._f_ud(xbef, u)
class HJB(DynamicalSystem):
"""
Tutorial system from Guo et al. 2022 with Runge-Kutta discretization.
"""
def __init__(self, **kwargs):
for key in kwargs:
setattr(self, key, kwargs[key])
def _f_u(self, x, u):
return -x ** 3 + u
def _f_ud(self, x, u):
return (x + (self.Ts / 6) * (self._k1(x, u) + 2 * self._k2(x, u) + 2 * self._k3(x, u) +
self._k4(x, u)))
def _k1(self, x, u):
return self._f_u(x, u)
def _k2(self, x, u):
return self._f_u(x + self._k1(x, u) * self.Ts / 2, u)
def _k3(self, x, u):
return self._f_u(x + self._k2(x, u) * self.Ts / 2, u)
def _k4(self, x, u):
return self._f_u(x + self._k1(x, u) * self.Ts, u)
def update_SOM(self, xbef, u):
return self._f_ud(xbef, u)