Reinforcement-Learning-Inverted-Pendulum/getBox.m at master · nikhilpodila/Reinforcement-Learning-Inverted-Pendulum · GitHub

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
%   getBox:  Given the current state, returns a number from 1 to 162
%             designating the region of the state space encompassing the current state.
%             Returns a value of -1 if a failure state is encountered.

function box = getBox(theta,thetaDot,x,xDot)
theta = rad2deg(theta);
thetaDot = rad2deg(thetaDot);
if (x < -2.4 || x > 2.4  || theta < -12 || theta > 12)
    box = -1;
else

if (theta<-6&&theta>=-12)
	thetaBucket = 1;
elseif (theta<-1&&theta>=-6)
	thetaBucket = 2;
elseif (theta<0&&theta>=-1)
	thetaBucket = 3;
elseif (theta<1&&theta>=0)	% zero included
	thetaBucket = 4;
elseif (theta<6&&theta>=1)
	thetaBucket = 5;
elseif (theta<=12&&theta>=6)
	thetaBucket = 6;
end

if (x<-0.8&&x>=-2.4)
	xBucket = 1;
elseif (x<=0.8&&x>=-0.8)
	xBucket = 2;
elseif (x<=2.4&&x>0.8)
	xBucket = 3;
end

if (xDot<-0.5)
	xDotBucket = 1;
elseif (xDot>=-0.5&&xDot<=0.5)
	xDotBucket = 2;
else
	xDotBucket = 3;
end

if (thetaDot<-50)
	thetaDotBucket = 1;
elseif (thetaDot>=-50&&thetaDot<=50)
	thetaDotBucket = 2;
else
	thetaDotBucket = 3;
end

box = sub2ind([6,3,3,3],thetaBucket, thetaDotBucket,xBucket,xDotBucket);
end
return;