-
Notifications
You must be signed in to change notification settings - Fork 1
/
interpolate.py
67 lines (51 loc) · 2.47 KB
/
interpolate.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
from state_entry import StateEntry
import constants as consts
def interpolateValue(next_state, theta_options, theta_dot_options, Q):
theta = next_state[0]
theta_dot = next_state[1]
if(abs(theta) > consts.theta_max):
return -1000000
nearest_theta_idx = findNearest(theta, theta_options)
nearest_theta_dot_idx = findNearest(theta_dot, theta_dot_options)
return Q[nearest_theta_dot_idx][nearest_theta_idx].value
def interpolateAction(next_state, theta_options, theta_dot_options, Q):
theta = next_state[0]
theta_dot = next_state[1]
nearest_theta_idx = findNearest(theta, theta_options)
nearest_theta_dot_idx = findNearest(theta_dot, theta_dot_options)
nearest_theta_idx2 = nearest_theta_idx
nearest_theta_dot_idx2 = nearest_theta_dot_idx
if(theta < theta_options[nearest_theta_idx]):
if(nearest_theta_idx - 1 >= 0):
nearest_theta_idx2 = nearest_theta_idx - 1
else:
if(nearest_theta_idx + 1 < len(theta_options)):
nearest_theta_idx2 = nearest_theta_idx + 1
if(theta_dot < theta_dot_options[nearest_theta_dot_idx]):
if(nearest_theta_dot_idx - 1 >= 0):
nearest_theta_dot_idx2 = nearest_theta_dot_idx - 1
else:
if(nearest_theta_dot_idx + 1 < len(theta_dot_options)):
nearest_theta_dot_idx2 = nearest_theta_dot_idx + 1
action1 = _interpolate(theta,\
theta_options[nearest_theta_idx], Q[nearest_theta_dot_idx][nearest_theta_idx].best_action,\
theta_options[nearest_theta_idx2], Q[nearest_theta_dot_idx][nearest_theta_idx2].best_action)
action2 = _interpolate(theta,\
theta_options[nearest_theta_idx], Q[nearest_theta_dot_idx2][nearest_theta_idx].best_action,\
theta_options[nearest_theta_idx2], Q[nearest_theta_dot_idx2][nearest_theta_idx2].best_action)
return _interpolate(theta_dot,\
theta_dot_options[nearest_theta_dot_idx], action1,\
theta_dot_options[nearest_theta_dot_idx2], action2)
def findNearest(val, list):
res = 0
min_val = abs(list[0] - val)
for i in range(len(list)):
distance = abs(list[i] - val)
if distance < min_val:
min_val = distance
res = i
return res
def _interpolate(val, point1, point1_val, point2, point2_val):
if(point1 == point2):
return point1_val
return (abs(val - point2) * point1_val + abs(val - point1) * point2_val) / abs(point2 - point1)