c4dynamics 2.2.0__tar.gz → 2.3.1__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/PKG-INFO +4 -4
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/README.md +2 -2
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/__init__.py +1 -1
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/detectors/yolo3_opencv.py +1 -1
- c4dynamics-2.3.1/c4dynamics/envs/mountain_car.py +477 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/filters/ekf.py +5 -5
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/filters/kalman.py +9 -9
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/states/lib/datapoint.py +1 -1
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/states/lib/rigidbody.py +2 -2
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/states/state.py +24 -21
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/const.py +4 -1
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/math.py +6 -1
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics.egg-info/PKG-INFO +4 -4
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/pyproject.toml +1 -1
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/setup.py +2 -3
- c4dynamics-2.2.0/c4dynamics/envs/mountain_car.py +0 -750
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/LICENSE +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/datasets/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/datasets/_manager.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/datasets/_registry.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/detectors/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/envs/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/eqm/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/eqm/derivs.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/eqm/integrate.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/filters/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/filters/lowpass.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/rotmat/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/rotmat/animate.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/rotmat/rotmat.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/sensors/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/sensors/lineofsight.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/sensors/radar.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/sensors/seeker.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/states/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/states/lib/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/states/lib/pixelpoint.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/__init__.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/_struct.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/cprint.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/gen_gif.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/idx2keys.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/images_loader.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/plottools.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/plottracks.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/printpts.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/slides_gen.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/tictoc.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/video_gen.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics/utils/vidgen.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics.egg-info/SOURCES.txt +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics.egg-info/dependency_links.txt +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics.egg-info/requires.txt +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/c4dynamics.egg-info/top_level.txt +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/setup.cfg +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_animate.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_const.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_cprint.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_datapoint.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_datasets.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_derivs.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_ekf.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_gen_gif.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_integrate.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_kalman.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_lowpass.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_math.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_pixelpoint.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_plotdefaults.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_radar.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_rigidbody.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_rotmat.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_seeker.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_state.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_state_assignment.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_state_assignment_strict.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_tictoc.py +0 -0
- {c4dynamics-2.2.0 → c4dynamics-2.3.1}/tests/test_yolov3_opencv.py +0 -0
|
@@ -1,11 +1,11 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: c4dynamics
|
|
3
|
-
Version: 2.
|
|
3
|
+
Version: 2.3.1
|
|
4
4
|
Summary: Python framework for state-space modeling and algorithm development
|
|
5
5
|
Author: c4dynamics
|
|
6
6
|
Author-email: Ziv Meri <zivmeri@gmail.com>
|
|
7
7
|
License: MIT
|
|
8
|
-
Requires-Python: >=3.8
|
|
8
|
+
Requires-Python: >=3.8,<3.13
|
|
9
9
|
Description-Content-Type: text/markdown
|
|
10
10
|
License-File: LICENSE
|
|
11
11
|
Requires-Dist: imageio>=2.37.0
|
|
@@ -52,7 +52,7 @@ Tsipor (bird) Dynamics (c4dynamics) is the Python framework for state-space mode
|
|
|
52
52
|
|
|
53
53
|
|
|
54
54
|
|
|
55
|
-
[Documentation](https://c4dynamics.github.io/
|
|
55
|
+
[Documentation](https://c4dynamics.github.io/c4dynamics/)
|
|
56
56
|
|
|
57
57
|
|
|
58
58
|
## Why c4dynamics?
|
|
@@ -92,7 +92,7 @@ while maintaining flexibility and scalability.
|
|
|
92
92
|
|
|
93
93
|
|
|
94
94
|
## Requirements
|
|
95
|
-
- 3.8 <= Python
|
|
95
|
+
- 3.8 <= Python < 3.13
|
|
96
96
|
- Required packages are listed in [requirements.txt](requirements.txt)
|
|
97
97
|
|
|
98
98
|
|
|
@@ -26,7 +26,7 @@ Tsipor (bird) Dynamics (c4dynamics) is the Python framework for state-space mode
|
|
|
26
26
|
|
|
27
27
|
|
|
28
28
|
|
|
29
|
-
[Documentation](https://c4dynamics.github.io/
|
|
29
|
+
[Documentation](https://c4dynamics.github.io/c4dynamics/)
|
|
30
30
|
|
|
31
31
|
|
|
32
32
|
## Why c4dynamics?
|
|
@@ -66,7 +66,7 @@ while maintaining flexibility and scalability.
|
|
|
66
66
|
|
|
67
67
|
|
|
68
68
|
## Requirements
|
|
69
|
-
- 3.8 <= Python
|
|
69
|
+
- 3.8 <= Python < 3.13
|
|
70
70
|
- Required packages are listed in [requirements.txt](requirements.txt)
|
|
71
71
|
|
|
72
72
|
|
|
@@ -168,7 +168,7 @@ class yolov3:
|
|
|
168
168
|
>>> def ptup(n): return '(' + str(n[0]) + ', ' + str(n[1]) + ')'
|
|
169
169
|
>>> print('{:^10} | {:^10} | {:^16} | {:^16} | {:^10} | {:^14}'.format('center x', 'center y', 'box top-left', 'box bottom-right', 'class', 'frame size')) # doctest: +IGNORE_OUTPUT
|
|
170
170
|
>>> for p in pts:
|
|
171
|
-
... print('{:^
|
|
171
|
+
... print('{:^10} | {:^10} | {:^16} | {:^16} | {:^10} | {:^14}'.format(p.x, p.y, ptup(p.box[0]), ptup(p.box[1]), p.class_id, ptup(p.fsize))) # doctest: +IGNORE_OUTPUT
|
|
172
172
|
... cv2.rectangle(img, p.box[0], p.box[1], [0, 0, 0], 2) # +IGNORE_OUTPUT
|
|
173
173
|
... point = (int((p.box[0][0] + p.box[1][0]) / 2 - 75), p.box[1][1] + 22) # doctest: +IGNORE_OUTPUT
|
|
174
174
|
... cv2.putText(img, p.class_id, point, cv2.FONT_HERSHEY_SIMPLEX, 1, [0, 0, 0], 2) # doctest: +IGNORE_OUTPUT
|
|
@@ -0,0 +1,477 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
import sys
|
|
3
|
+
sys.path.append('.')
|
|
4
|
+
import c4dynamics as c4d
|
|
5
|
+
# import warnings
|
|
6
|
+
# from typing import Optional
|
|
7
|
+
|
|
8
|
+
|
|
9
|
+
class mountain_car(c4d.state):
|
|
10
|
+
'''
|
|
11
|
+
A mountain car environment for a reinforcement learning problem.
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
Parameters
|
|
15
|
+
==========
|
|
16
|
+
|
|
17
|
+
mass : float
|
|
18
|
+
...
|
|
19
|
+
gravity : float
|
|
20
|
+
...
|
|
21
|
+
action : float
|
|
22
|
+
...
|
|
23
|
+
dt : float
|
|
24
|
+
...
|
|
25
|
+
velocity_lim : tuple
|
|
26
|
+
...
|
|
27
|
+
\\ position lim cannot be set becuase it determines the shape of the problem in the cos()
|
|
28
|
+
|
|
29
|
+
See Also
|
|
30
|
+
========
|
|
31
|
+
...
|
|
32
|
+
|
|
33
|
+
|
|
34
|
+
|
|
35
|
+
|
|
36
|
+
**Dynamics**
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
The state variables of a mountain car are:
|
|
40
|
+
|
|
41
|
+
.. math::
|
|
42
|
+
|
|
43
|
+
X = [x, v]^T
|
|
44
|
+
|
|
45
|
+
Where:
|
|
46
|
+
- :math:`x` is the position of the car along the x-axis.
|
|
47
|
+
- :math:`v` is the velocity of the car along the x-axis.
|
|
48
|
+
|
|
49
|
+
The car is goverened by the following equations of motion which represent the dynamics
|
|
50
|
+
of climbing a steep hill in the presence of gravity and resistance forces:
|
|
51
|
+
|
|
52
|
+
.. math::
|
|
53
|
+
\\dot{x} = v
|
|
54
|
+
|
|
55
|
+
\\dot{v} = g \\cdot cos(3 \\cdot x) + a_c / m - k \\cdot v / m
|
|
56
|
+
|
|
57
|
+
Where:
|
|
58
|
+
- :math:`x` is the position coordinate in :math:`x` direction. Default units :math:`[m]`
|
|
59
|
+
- :math:`v` is the velocity coordinate in :math:`x` direction. Default units :math:`[m/s]`
|
|
60
|
+
- :math:`g` is the gravity force. Defaults :math:`9.8 [N]`
|
|
61
|
+
- :math:`m` is the car mass. Defaults :math:`1500 [kg]`
|
|
62
|
+
- :math:`a_c` is the action (force) command. Defaults :math:`-20[N]` (left), :math:`0` (do nothing), or :math:`20[N]` (right)
|
|
63
|
+
- :math:`k` is the resistance coefficient. Defaults :math:`0.5 [1/s]`
|
|
64
|
+
|
|
65
|
+
|
|
66
|
+
The position and velocity are subject to the boundaries:
|
|
67
|
+
|
|
68
|
+
.. math::
|
|
69
|
+
|
|
70
|
+
x \\in speed_lim
|
|
71
|
+
|
|
72
|
+
v \\in [vmin, vmax]
|
|
73
|
+
|
|
74
|
+
Where:
|
|
75
|
+
- :math:`xmin` is the minimum position coordinate in :math:`x` direction. Defaults :math:`-120 [m]`
|
|
76
|
+
- :math:`xmax` is the maximum position coordinate in :math:`x` direction. Defaults :math:`50 [m]`
|
|
77
|
+
- :math:`vmin` is the minimum velocity coordinate in :math:`x` direction. Default units :math:`-30 [m/s]`
|
|
78
|
+
- :math:`vmax` is the maximum velocity coordinate in :math:`x` direction. Default units :math:`30 [m/s]`
|
|
79
|
+
|
|
80
|
+
Outside a boundary the vairable is clipped (extraploated with last value).
|
|
81
|
+
|
|
82
|
+
|
|
83
|
+
**Reward**
|
|
84
|
+
|
|
85
|
+
|
|
86
|
+
\\ Goal: When the car reaches :math:`xmax`, the environment provides
|
|
87
|
+
|
|
88
|
+
|
|
89
|
+
**Discretization**
|
|
90
|
+
...
|
|
91
|
+
|
|
92
|
+
|
|
93
|
+
Example
|
|
94
|
+
=======
|
|
95
|
+
...
|
|
96
|
+
|
|
97
|
+
|
|
98
|
+
'''
|
|
99
|
+
|
|
100
|
+
|
|
101
|
+
def __init__(self, mode = 'gymnasium', n_bins = 12):
|
|
102
|
+
"""
|
|
103
|
+
Args
|
|
104
|
+
----
|
|
105
|
+
mass: mass of the car (default 0.2)
|
|
106
|
+
friction: friction in Newton (default 0.3)
|
|
107
|
+
dt: time step in seconds (default 0.1)
|
|
108
|
+
"""
|
|
109
|
+
|
|
110
|
+
self.mode = mode
|
|
111
|
+
self.n_bins = n_bins
|
|
112
|
+
|
|
113
|
+
if self.mode == 'gymnasium':
|
|
114
|
+
self.steep_factor = 1
|
|
115
|
+
|
|
116
|
+
self.dt = 1
|
|
117
|
+
self.gravity = 0.0025
|
|
118
|
+
|
|
119
|
+
self.mass = 1
|
|
120
|
+
self.cos_factor = 1
|
|
121
|
+
|
|
122
|
+
self.friction = 0
|
|
123
|
+
self.position_lim = (-1.2, 0.5)
|
|
124
|
+
|
|
125
|
+
self.speed_lim = (-0.07, 0.07)
|
|
126
|
+
self.action_list = (-0.001, 0, 0.001)
|
|
127
|
+
|
|
128
|
+
self.normalized = True
|
|
129
|
+
|
|
130
|
+
elif self.mode == 'sarsa':
|
|
131
|
+
self.steep_factor = 1
|
|
132
|
+
|
|
133
|
+
self.dt = 0.1
|
|
134
|
+
self.gravity = 9.8
|
|
135
|
+
|
|
136
|
+
self.mass = 0.1
|
|
137
|
+
self.cos_factor = 1
|
|
138
|
+
|
|
139
|
+
self.friction = 0.1
|
|
140
|
+
self.position_lim = (-1.2, 0.5)
|
|
141
|
+
|
|
142
|
+
self.speed_lim = (-.5, .5)
|
|
143
|
+
self.action_list = (-1, 0, 1) #(-1 * k * 0.31, 0, 1 * k * 0.31) # .25, .3 - FAIL, .75, .5, .333, .32 - SHARP PASS, .31 - gradual (but still not seems momentum-based)
|
|
144
|
+
|
|
145
|
+
self.normalized = True
|
|
146
|
+
|
|
147
|
+
elif self.mode == 'orig_sarsa':
|
|
148
|
+
self.steep_factor = 1
|
|
149
|
+
|
|
150
|
+
self.dt = 0.1
|
|
151
|
+
self.gravity = 9.8
|
|
152
|
+
|
|
153
|
+
self.mass = 0.2
|
|
154
|
+
self.cos_factor = 1
|
|
155
|
+
|
|
156
|
+
self.friction = 0.3
|
|
157
|
+
self.position_lim = (-1.2, 0.5)
|
|
158
|
+
|
|
159
|
+
self.speed_lim = (-1.5, 1.5) # (-3, 3)
|
|
160
|
+
self.action_list = (-1, 0, 1) # it seems like +-0.2 but its not because he mistakenly multiplies the gravity andthe friction by the mass. i'm not sure about that anymore.
|
|
161
|
+
|
|
162
|
+
self.normalized = False
|
|
163
|
+
|
|
164
|
+
elif self.mode == 'physical': # physical scene
|
|
165
|
+
|
|
166
|
+
self.dt = 0.1
|
|
167
|
+
self.gravity = 9.8
|
|
168
|
+
|
|
169
|
+
self.mass = 1500 # kg
|
|
170
|
+
self.cos_factor = 100
|
|
171
|
+
self.steep_factor = 100
|
|
172
|
+
|
|
173
|
+
self.friction = 0
|
|
174
|
+
self.position_lim = (-120, 50) # m
|
|
175
|
+
|
|
176
|
+
self.speed_lim = (-30, 30) # m/s
|
|
177
|
+
self.action_list = (-10, 0, 10)
|
|
178
|
+
|
|
179
|
+
self.normalized = True
|
|
180
|
+
|
|
181
|
+
|
|
182
|
+
super().__init__(position = 0, velocity = 0)
|
|
183
|
+
|
|
184
|
+
xmax = self.position_lim[1]
|
|
185
|
+
vmax = self.speed_lim[1]
|
|
186
|
+
self.e_max = self.mass * self.gravity * (np.sin((xmax - (0.5 * np.pi)) + 0.5) + 1.0) + 0.5 * vmax**2
|
|
187
|
+
|
|
188
|
+
self.state_lim = (-1, 1)
|
|
189
|
+
self.state_interval = np.linspace(self.state_lim[0], self.state_lim[1], num = self.n_bins - 1, endpoint = False)
|
|
190
|
+
self.pos_interval = np.linspace(self.position_lim[0], self.position_lim[1], num = self.n_bins - 1, endpoint = False)
|
|
191
|
+
self.vel_interval = np.linspace(self.speed_lim[0], self.speed_lim[1], num = self.n_bins - 1, endpoint = False)
|
|
192
|
+
|
|
193
|
+
|
|
194
|
+
def step(self, action):
|
|
195
|
+
"""
|
|
196
|
+
Performs one step in the environment following the action.
|
|
197
|
+
|
|
198
|
+
Args
|
|
199
|
+
----
|
|
200
|
+
action: an integer representing one of three actions [0, 1, 2]
|
|
201
|
+
where 0=move_left, 1=do_not_move, 2=move_right
|
|
202
|
+
|
|
203
|
+
Returns
|
|
204
|
+
(postion_t1, velocity_t1): state
|
|
205
|
+
reward: always negative but when the goal is reached
|
|
206
|
+
done: True when the goal is reached
|
|
207
|
+
|
|
208
|
+
"""
|
|
209
|
+
|
|
210
|
+
|
|
211
|
+
# Semi-implicit Euler integraton
|
|
212
|
+
acc = self.action_list[action] / self.mass \
|
|
213
|
+
- self.gravity * self.steep_factor * np.cos(3 * self.position / self.cos_factor) \
|
|
214
|
+
- self.friction * self.velocity / self.mass
|
|
215
|
+
|
|
216
|
+
self.velocity += acc * self.dt
|
|
217
|
+
self.velocity = np.clip(self.velocity, self.speed_lim[0], self.speed_lim[1])
|
|
218
|
+
|
|
219
|
+
self.position += self.velocity * self.dt
|
|
220
|
+
self.position = np.clip(self.position, self.position_lim[0], self.position_lim[1])
|
|
221
|
+
|
|
222
|
+
if self.position <= self.position_lim[0] and self.velocity < 0:
|
|
223
|
+
self.velocity = 0
|
|
224
|
+
|
|
225
|
+
self.store()
|
|
226
|
+
reward, done = (-0.01, False) if self.position < self.position_lim[1] else (1, True)
|
|
227
|
+
|
|
228
|
+
return reward, done
|
|
229
|
+
|
|
230
|
+
|
|
231
|
+
def energy_normalized(self, state = None):
|
|
232
|
+
|
|
233
|
+
if state is None:
|
|
234
|
+
state = self.X
|
|
235
|
+
|
|
236
|
+
e_state = self.mass * self.gravity * (np.sin((state[0] - (0.5 * np.pi)) + 0.5) + 1.0) + 0.5 * state[1]**2
|
|
237
|
+
return 2 * e_state / self.e_max
|
|
238
|
+
|
|
239
|
+
|
|
240
|
+
def energy(self, state = None):
|
|
241
|
+
|
|
242
|
+
if state is None:
|
|
243
|
+
state = self.X
|
|
244
|
+
|
|
245
|
+
# potential energy (height modeled by sine curve)
|
|
246
|
+
potential = self.mass * self.gravity * (np.sin((x - (0.5 * np.pi)) + 0.5) + 1.0)
|
|
247
|
+
# kinetic energy
|
|
248
|
+
kinetic = 0.5 * self.mass * v**2
|
|
249
|
+
|
|
250
|
+
e_state = self.mass * self.gravity * (np.sin((state[0] - (0.5 * np.pi)) + 0.5) + 1.0) + 0.5 * state[1]**2
|
|
251
|
+
return 2 * e_state / self.e_max
|
|
252
|
+
|
|
253
|
+
|
|
254
|
+
def reset(self, exploring_starts = True):
|
|
255
|
+
"""
|
|
256
|
+
Resets the car to an initial position [-1.2, 0.5]
|
|
257
|
+
|
|
258
|
+
Args
|
|
259
|
+
----
|
|
260
|
+
exploring_starts: if True a random position is taken
|
|
261
|
+
initial_position: the initial position of the car (requires exploring_starts=False)
|
|
262
|
+
|
|
263
|
+
Returns
|
|
264
|
+
-------
|
|
265
|
+
Initial position of the car and the velocity
|
|
266
|
+
|
|
267
|
+
"""
|
|
268
|
+
|
|
269
|
+
# seed_seq = np.random.SeedSequence(seed)
|
|
270
|
+
# np_seed = seed_seq.entropy
|
|
271
|
+
# rng = RandomNumberGenerator(np.random.PCG64(seed_seq))
|
|
272
|
+
# seed = np.random.randint(0, 2**24)
|
|
273
|
+
# np.random.seed(seed)
|
|
274
|
+
super().reset()
|
|
275
|
+
|
|
276
|
+
if exploring_starts: # gym: always generates. but on small interval (-0.6,-0.4 uniformly)
|
|
277
|
+
|
|
278
|
+
# initial_position = np.random.uniform(-1.2, 0.5)
|
|
279
|
+
# initial_position = np.random.uniform(initial_position - 0.1, initial_position + 0.1)
|
|
280
|
+
pos0 = self.position_lim[0] + 0.3888 * (self.position_lim[1] - self.position_lim[0])
|
|
281
|
+
dpos = 0.0555 * (self.position_lim[1] - self.position_lim[0])
|
|
282
|
+
initial_position = np.random.uniform(pos0 - dpos, pos0 + dpos)
|
|
283
|
+
|
|
284
|
+
else:
|
|
285
|
+
initial_position = -0.5
|
|
286
|
+
|
|
287
|
+
initial_position = np.clip(initial_position, self.position_lim[0], self.position_lim[1])
|
|
288
|
+
self.position = initial_position
|
|
289
|
+
self.velocity = 0
|
|
290
|
+
|
|
291
|
+
self.store()
|
|
292
|
+
|
|
293
|
+
|
|
294
|
+
def discretize(self):
|
|
295
|
+
|
|
296
|
+
if self.normalized:
|
|
297
|
+
# normalized the state to (-1, 1) and discretize for N bins.
|
|
298
|
+
pos = np.digitize(np.interp(self.position, self.position_lim, self.state_lim), self.state_interval)
|
|
299
|
+
vel = np.digitize(np.interp(self.velocity, self.speed_lim, self.state_lim), self.state_interval)
|
|
300
|
+
|
|
301
|
+
else:
|
|
302
|
+
# normalized the state to (-1, 1) and discretize for N bins.
|
|
303
|
+
pos = np.digitize(self.position, self.pos_interval)
|
|
304
|
+
vel = np.digitize(self.velocity, self.vel_interval)
|
|
305
|
+
|
|
306
|
+
return vel, pos
|
|
307
|
+
|
|
308
|
+
|
|
309
|
+
def render(self, file_path = './simulation_render.gif', mode = 'gif'):
|
|
310
|
+
# def render(self, file_path = './mountain_car.mp4', mode = 'mp4'):
|
|
311
|
+
"""
|
|
312
|
+
When the method is called it saves an animation
|
|
313
|
+
of what happened until that point in the episode.
|
|
314
|
+
Ideally it should be called at the end of the episode,
|
|
315
|
+
and every k episodes.
|
|
316
|
+
|
|
317
|
+
ATTENTION: requires avconv and/or imagemagick installed.
|
|
318
|
+
|
|
319
|
+
Args
|
|
320
|
+
----
|
|
321
|
+
file_path: the name and path of the video file
|
|
322
|
+
mode: the file can be saved as 'gif' or 'mp4'
|
|
323
|
+
|
|
324
|
+
"""
|
|
325
|
+
|
|
326
|
+
# Plot init
|
|
327
|
+
fig = plt.figure()
|
|
328
|
+
ax = fig.add_subplot(111, autoscale_on = False, xlim = (-1.2, 0.5), ylim = (-1.1, 1.1))
|
|
329
|
+
ax.grid(False) # disable the grid
|
|
330
|
+
x_sin = np.linspace(start = -1.2, stop = 0.5, num = 100)
|
|
331
|
+
y_sin = np.sin(3 * x_sin)
|
|
332
|
+
|
|
333
|
+
# plt.plot(x, y)
|
|
334
|
+
ax.plot(x_sin, y_sin) # plot the sine wave
|
|
335
|
+
# line, _ = ax.plot(x, y, 'o-', lw=2)
|
|
336
|
+
dot, = ax.plot([], [], 'ro')
|
|
337
|
+
time_text = ax.text(0.05, 0.9, '', transform = ax.transAxes)
|
|
338
|
+
_position_list = self.data('position')[1]
|
|
339
|
+
_dt = self.dt
|
|
340
|
+
|
|
341
|
+
def _init():
|
|
342
|
+
dot.set_data([], [])
|
|
343
|
+
time_text.set_text('')
|
|
344
|
+
return dot, time_text
|
|
345
|
+
|
|
346
|
+
def _animate(i):
|
|
347
|
+
x = _position_list[i]
|
|
348
|
+
y = np.sin(3 * x)
|
|
349
|
+
dot.set_data([x], [y])
|
|
350
|
+
time_text.set_text("Time: " + str(np.round(i * _dt, 1)) + "s" + '\n' + "Frame: " + str(i))
|
|
351
|
+
return dot, time_text
|
|
352
|
+
|
|
353
|
+
|
|
354
|
+
# Argument | Meaning
|
|
355
|
+
# -------- | -------
|
|
356
|
+
# fig | The figure object where the animation will be drawn.
|
|
357
|
+
# _animate | The function that updates the animation frame by frame.
|
|
358
|
+
# np.arange(1, | The sequence of frame numbers (i) that _animate(i) will receive.
|
|
359
|
+
# len(self.data('t'))) | Starts from 1 to len(self.data('t')) - 1.
|
|
360
|
+
# blit = True | Optimizes rendering by only redrawing changed parts of the frame.
|
|
361
|
+
# init_func = _init | Calls _init() once at the beginning to set up the animation.
|
|
362
|
+
# repeat = False | The animation runs only once and does not loop.
|
|
363
|
+
ani = animation.FuncAnimation(fig, _animate, np.arange(1, len(self.data('t'))), blit = True, init_func = _init, repeat = False)
|
|
364
|
+
|
|
365
|
+
if file_path:
|
|
366
|
+
if mode == 'gif':
|
|
367
|
+
ani.save(file_path, writer = 'imagemagick', fps = int(1 / self.dt))
|
|
368
|
+
elif mode == 'mp4':
|
|
369
|
+
ani.save(file_path, fps = int(1/self.dt), writer='avconv', codec='libx264')
|
|
370
|
+
|
|
371
|
+
plt.show()
|
|
372
|
+
# Clear the figure
|
|
373
|
+
fig.clear()
|
|
374
|
+
plt.close(fig)
|
|
375
|
+
|
|
376
|
+
|
|
377
|
+
def render_axis(self, fig, ax):
|
|
378
|
+
|
|
379
|
+
ax.grid(False) # disable the grid
|
|
380
|
+
dot, = ax.plot([], [], 'ro')
|
|
381
|
+
time_text = ax.text(0.05, 0.9, '', transform = ax.transAxes)
|
|
382
|
+
_position_list = self.data('position')[1]
|
|
383
|
+
_dt = self.dt
|
|
384
|
+
|
|
385
|
+
def _init():
|
|
386
|
+
dot.set_data([], [])
|
|
387
|
+
time_text.set_text('')
|
|
388
|
+
return dot, time_text
|
|
389
|
+
|
|
390
|
+
def _animate(i):
|
|
391
|
+
x = _position_list[i]
|
|
392
|
+
y = np.sin(3 * x)
|
|
393
|
+
dot.set_data([x], [y])
|
|
394
|
+
time_text.set_text("Time: " + str(np.round(i * _dt, 1)) + "s" + '\n' + "Frame: " + str(i))
|
|
395
|
+
return dot, time_text
|
|
396
|
+
|
|
397
|
+
ani = animation.FuncAnimation(fig, _animate
|
|
398
|
+
, np.arange(1, len(self.data('t')))
|
|
399
|
+
, blit = True, init_func = _init
|
|
400
|
+
, repeat = False)
|
|
401
|
+
|
|
402
|
+
return ani, dot
|
|
403
|
+
|
|
404
|
+
|
|
405
|
+
def animate(self, file_path = './simulation_animate.gif', debug = False):
|
|
406
|
+
|
|
407
|
+
from animation_tools import animateit
|
|
408
|
+
from IPython.display import Image
|
|
409
|
+
|
|
410
|
+
# debug = False
|
|
411
|
+
# # Get data
|
|
412
|
+
# t_array = self.data('t')
|
|
413
|
+
# x_array = self.data('position')[1]
|
|
414
|
+
# dt = self.dt
|
|
415
|
+
|
|
416
|
+
# inputs = [(i, x_array[i], dt) for i in range(len(t_array))]
|
|
417
|
+
|
|
418
|
+
# if debug:
|
|
419
|
+
# frames = [render_frame(inp) for inp in inputs]
|
|
420
|
+
# else:
|
|
421
|
+
# # Parallel rendering
|
|
422
|
+
# with Pool() as pool:
|
|
423
|
+
# # frames = pool.map(render_frame, inputs)
|
|
424
|
+
# results = pool.map(square, range(10))
|
|
425
|
+
|
|
426
|
+
# frames = animateit(self)
|
|
427
|
+
|
|
428
|
+
# Get data
|
|
429
|
+
t_array = self.data('t')
|
|
430
|
+
x_array = self.data('position')[1]
|
|
431
|
+
dt = self.dt
|
|
432
|
+
|
|
433
|
+
inputs = [(i, x_array[i], dt) for i in range(len(t_array))]
|
|
434
|
+
|
|
435
|
+
if debug:
|
|
436
|
+
frames = [render_frame(inp) for inp in inputs]
|
|
437
|
+
else:
|
|
438
|
+
# Parallel rendering
|
|
439
|
+
with Pool() as pool:
|
|
440
|
+
frames = pool.map(render_frame, inputs)
|
|
441
|
+
# frames = pool.map(square, range(10))
|
|
442
|
+
|
|
443
|
+
# gifname = 'car_rl.gif'
|
|
444
|
+
imageio.mimsave(file_path, frames, duration = 0.1, loop = 0)
|
|
445
|
+
# c4d.gif(outfol, gifname, duration = 1)
|
|
446
|
+
Image(file_path) # doctest: +IGNORE_OUTPUT
|
|
447
|
+
|
|
448
|
+
|
|
449
|
+
def _plot(self, ax, alpha = 0.4):
|
|
450
|
+
|
|
451
|
+
# Plot init
|
|
452
|
+
# fig = plt.figure()
|
|
453
|
+
# ax = fig.add_subplot(111, autoscale_on = False, xlim = (-1.2, 0.5), ylim = (-1.1, 1.1))
|
|
454
|
+
ax.grid(False) # disable the grid
|
|
455
|
+
# ax.set_facecolor((0, 1, 0, alpha))
|
|
456
|
+
|
|
457
|
+
x = np.linspace(start = self.position_lim[0], stop = self.position_lim[1], num = 100)
|
|
458
|
+
z = np.sin(3 * x / self.cos_factor)
|
|
459
|
+
ax.plot(x, z, color = '#003366') # plot the sine wave
|
|
460
|
+
ax.fill_between(x, ax.get_ylim()[0], z, color = '#9370DB', alpha = alpha)
|
|
461
|
+
|
|
462
|
+
# plt.plot(x, y)
|
|
463
|
+
# line, _ = ax.plot(x, y, 'o-', lw=2)
|
|
464
|
+
positions_x = self.data('position')[1]
|
|
465
|
+
positions_z = np.sin(3 * self.data('position')[1] / self.cos_factor)
|
|
466
|
+
|
|
467
|
+
ax.plot(positions_x, positions_z, 'b.', alpha = 0.3)
|
|
468
|
+
indmaxx = np.argmax(positions_x)
|
|
469
|
+
ax.plot(positions_x[indmaxx], positions_z[indmaxx], 'bo')
|
|
470
|
+
|
|
471
|
+
# plt.show()
|
|
472
|
+
# plt.savefig(file_path)
|
|
473
|
+
|
|
474
|
+
|
|
475
|
+
|
|
476
|
+
|
|
477
|
+
|
|
@@ -128,7 +128,7 @@ class ekf(kalman):
|
|
|
128
128
|
|
|
129
129
|
.. code::
|
|
130
130
|
|
|
131
|
-
>>> _ekf = ekf({'x': 0
|
|
131
|
+
>>> _ekf = ekf({'x': 0}, P0 = 0.5**2, F = 1, H = 1, Q = 0.05, R = 200)
|
|
132
132
|
>>> print(_ekf)
|
|
133
133
|
[ x ]
|
|
134
134
|
>>> _ekf.X # doctest: +NUMPY_FORMAT
|
|
@@ -146,7 +146,7 @@ class ekf(kalman):
|
|
|
146
146
|
|
|
147
147
|
.. code::
|
|
148
148
|
|
|
149
|
-
>>> _ekf = ekf({'x': 0
|
|
149
|
+
>>> _ekf = ekf({'x': 0}, P0 = 0.5**2, F = 1, G = 150, H = 1, R = 200, Q = 0.05)
|
|
150
150
|
>>> _ekf.X # doctest: +NUMPY_FORMAT
|
|
151
151
|
[0]
|
|
152
152
|
>>> _ekf.P # doctest: +NUMPY_FORMAT
|
|
@@ -163,7 +163,7 @@ class ekf(kalman):
|
|
|
163
163
|
|
|
164
164
|
.. code::
|
|
165
165
|
|
|
166
|
-
>>> _ekf = ekf({'x': 0
|
|
166
|
+
>>> _ekf = ekf({'x': 0}, P0 = 0.5**2, F = 1, G = 150, H = 1, R = 200, Q = 0.05)
|
|
167
167
|
>>> _ekf.X # doctest: +NUMPY_FORMAT
|
|
168
168
|
[0]
|
|
169
169
|
>>> _ekf.P # doctest: +NUMPY_FORMAT
|
|
@@ -244,7 +244,7 @@ class ekf(kalman):
|
|
|
244
244
|
|
|
245
245
|
.. code::
|
|
246
246
|
|
|
247
|
-
>>> _ekf = ekf({'x': 0
|
|
247
|
+
>>> _ekf = ekf({'x': 0}, P0 = 0.5**2, F = 1, H = 1, Q = 0.05, R = 200)
|
|
248
248
|
>>> print(_ekf)
|
|
249
249
|
[ x ]
|
|
250
250
|
>>> _ekf.X # doctest: +NUMPY_FORMAT
|
|
@@ -264,7 +264,7 @@ class ekf(kalman):
|
|
|
264
264
|
|
|
265
265
|
.. code::
|
|
266
266
|
|
|
267
|
-
>>> _ekf = ekf({'x': 0
|
|
267
|
+
>>> _ekf = ekf({'x': 0}, P0 = 0.5**2, F = 1, G = 150, H = 1, R = 200, Q = 0.05)
|
|
268
268
|
>>> _ekf.X # doctest: +NUMPY_FORMAT
|
|
269
269
|
[0]
|
|
270
270
|
>>> _ekf.P # doctest: +NUMPY_FORMAT
|
|
@@ -139,7 +139,7 @@ class kalman(c4d.state):
|
|
|
139
139
|
|
|
140
140
|
>>> v = 150
|
|
141
141
|
>>> sensor_noise = 200
|
|
142
|
-
>>> kf = kalman({'x': 0
|
|
142
|
+
>>> kf = kalman({'x': 0}, P0 = 0.5**2, F = 1, G = v, H = 1
|
|
143
143
|
... , Q = 0.05, R = sensor_noise**2, steadystate = True)
|
|
144
144
|
|
|
145
145
|
|
|
@@ -193,7 +193,7 @@ class kalman(c4d.state):
|
|
|
193
193
|
.. code::
|
|
194
194
|
|
|
195
195
|
>>> v = 150
|
|
196
|
-
>>> kf = kalman({'x': 0
|
|
196
|
+
>>> kf = kalman({'x': 0}, P0 = 0.5*2, F = 1, G = v, H = 1, Q = 0.05)
|
|
197
197
|
>>> for t in range(1, 26): # seconds.
|
|
198
198
|
... kf.store(t)
|
|
199
199
|
... sensor_noise = 200 + 8 * t
|
|
@@ -352,7 +352,7 @@ class kalman(c4d.state):
|
|
|
352
352
|
|
|
353
353
|
.. code::
|
|
354
354
|
|
|
355
|
-
>>> kf = kalman({'x': 0
|
|
355
|
+
>>> kf = kalman({'x': 0}, P0 = 0.5**2, F = 1, H = 1, Q = 0.05, R = 200, steadystate = True)
|
|
356
356
|
>>> print(kf)
|
|
357
357
|
[ x ]
|
|
358
358
|
>>> kf.X # doctest: +NUMPY_FORMAT
|
|
@@ -370,7 +370,7 @@ class kalman(c4d.state):
|
|
|
370
370
|
|
|
371
371
|
.. code::
|
|
372
372
|
|
|
373
|
-
>>> kf = kalman({'x': 0
|
|
373
|
+
>>> kf = kalman({'x': 0}, P0 = 0.5**2, F = 1, G = 150, H = 1, R = 200, Q = 0.05)
|
|
374
374
|
>>> kf.X # doctest: +NUMPY_FORMAT
|
|
375
375
|
[0]
|
|
376
376
|
>>> kf.P # doctest: +NUMPY_FORMAT
|
|
@@ -387,7 +387,7 @@ class kalman(c4d.state):
|
|
|
387
387
|
|
|
388
388
|
.. code::
|
|
389
389
|
|
|
390
|
-
>>> kf = kalman({'x': 0
|
|
390
|
+
>>> kf = kalman({'x': 0}, P0 = 0.5**2, F = 1, G = 150, H = 1, R = 200, Q = 0.05)
|
|
391
391
|
>>> kf.X # doctest: +NUMPY_FORMAT
|
|
392
392
|
[0]
|
|
393
393
|
>>> kf.P # doctest: +NUMPY_FORMAT
|
|
@@ -477,7 +477,7 @@ class kalman(c4d.state):
|
|
|
477
477
|
|
|
478
478
|
.. code::
|
|
479
479
|
|
|
480
|
-
>>> kf = kalman({'x': 0
|
|
480
|
+
>>> kf = kalman({'x': 0}, P0 = 0.5**2, F = 1, H = 1, Q = 0.05, R = 200, steadystate = True)
|
|
481
481
|
>>> print(kf)
|
|
482
482
|
[ x ]
|
|
483
483
|
>>> kf.X # doctest: +NUMPY_FORMAT
|
|
@@ -497,7 +497,7 @@ class kalman(c4d.state):
|
|
|
497
497
|
|
|
498
498
|
.. code::
|
|
499
499
|
|
|
500
|
-
>>> kf = kalman({'x': 0
|
|
500
|
+
>>> kf = kalman({'x': 0}, P0 = 0.5**2, F = 1, G = 150, H = 1, R = 200, Q = 0.05)
|
|
501
501
|
>>> kf.X # doctest: +NUMPY_FORMAT
|
|
502
502
|
[0]
|
|
503
503
|
>>> kf.P # doctest: +NUMPY_FORMAT
|
|
@@ -591,7 +591,7 @@ class kalman(c4d.state):
|
|
|
591
591
|
|
|
592
592
|
.. code::
|
|
593
593
|
|
|
594
|
-
>>> kf = kalman({'x': 0
|
|
594
|
+
>>> kf = kalman({'x': 0}, P0 = 0.5**2, F = 1, H = 1, Q = 0.05, R = 200)
|
|
595
595
|
>>> # store initial conditions
|
|
596
596
|
>>> kf.store()
|
|
597
597
|
>>> kf.predict()
|
|
@@ -668,7 +668,7 @@ class kalman(c4d.state):
|
|
|
668
668
|
Q = np.eye(6) * process_noise**2
|
|
669
669
|
R = np.eye(4) * measure_noise**2
|
|
670
670
|
|
|
671
|
-
kf = kalman({'x': 0
|
|
671
|
+
kf = kalman({'x': 0, 'y': 0, 'w': 0, 'h': 0, 'vx': 0, 'vy': 0}
|
|
672
672
|
, steadystate = True, F = F, H = H, Q = Q, R = R)
|
|
673
673
|
return kf
|
|
674
674
|
|
|
@@ -200,7 +200,7 @@ class datapoint(state):
|
|
|
200
200
|
_mass = 1
|
|
201
201
|
|
|
202
202
|
|
|
203
|
-
def __init__(self, x = 0
|
|
203
|
+
def __init__(self, x = 0, y = 0, z = 0, vx = 0, vy = 0, vz = 0):
|
|
204
204
|
# TODO: document the state type being float by default. (required after chaning vars to 0.0 and the mechanism of generating the state vector array. )
|
|
205
205
|
|
|
206
206
|
dpargs = {}
|
|
@@ -181,8 +181,8 @@ class rigidbody(datapoint): #
|
|
|
181
181
|
_iyy = 0
|
|
182
182
|
_izz = 0
|
|
183
183
|
|
|
184
|
-
def __init__(self, x = 0
|
|
185
|
-
, phi = 0
|
|
184
|
+
def __init__(self, x = 0, y = 0, z = 0, vx = 0, vy = 0, vz = 0
|
|
185
|
+
, phi = 0, theta = 0, psi = 0, p = 0, q = 0, r = 0
|
|
186
186
|
):
|
|
187
187
|
|
|
188
188
|
rbargs = {}
|