def optimeizeTest():
global nodes
global data
global model
data = Data('problem_A.json')
#init obsticles and boundary polygon
initObsticles(data)
goal_vel = np.linalg.norm(np.array(data.goal_vel))
goal_heading = atan2(data.goal_vel[1], data.goal_vel[0])
goalPoint = Node(State(data.goal_pos, goal_vel, goal_heading), None)
#settings
model = Model(data, type='kin-point')
p1 = [0, 0]
p2 = State([1, 2], model.v_max, np.pi / 2)
p3 = State([0, 2], model.v_max, np.pi / 2)
p4 = State([0, 1], model.v_max, np.pi / 2)
P1 = Node(State(p1, 0.1, np.pi / 2), None)
nodes.append(P1)
P2 = extend(P1, p2, extend=False)
nodes.append(P2)
drawPath(P2)
P3 = extend(P2, p3, extend=False)
nodes.append(P3)
drawPath(P3)
P4 = extend(P1, p4, extend=False)
nodes.append(P4)
drawPath(P4)
pygame.display.update()
time.sleep(5)
optimize(P4)
screen.fill(black)
draw_polygons()
for i in range(len(nodes)):
draw_point(nodes[i], 1, dark_green)
if (nodes[i].path is None):
continue
for j in range(1, len(nodes[i].path)):
draw_line(nodes[i].path[j - 1].pos, nodes[i].path[j].pos, green, 1)
pygame.display.update()
time.sleep(1)
def integrate(self, currentState, controlInput, dt):
dTheta = dt * currentState.vel / self.L * np.tan(controlInput)
Theta = currentState.heading + dTheta
v = self.v_max
p = currentState.pos
return State(
[p[0] + dt * v * np.cos(Theta), p[1] + dt * v * np.sin(Theta)], v,
Theta)
def nextState(self, currentState, control, dt):
omega = control[1]
v = control[0]
x = currentState.pos[0]
y = currentState.pos[1]
theta = currentState.heading
theta_new = theta + omega * dt
x_new = x + v * np.cos(theta_new) * dt
y_new = y + v * np.sin(theta_new) * dt
return State([x_new, y_new], v, theta_new)
def integrate(self, currentState, controlInput, dt):
vx = np.cos(currentState.heading) * currentState.vel
vy = np.sin(currentState.heading) * currentState.vel
dvx = controlInput[1] * np.cos(controlInput[0]) * dt
dvy = controlInput[1] * np.sin(controlInput[0]) * dt
vx = vx + dvx
vy = vy + dvy
v = np.linalg.norm([vx, vy])
Theta = atan2(vy, vx)
p = currentState.pos
return State([p[0] + vx * dt, p[1] + dt * vy], v, Theta)
def __init__(self, init_model=None):
# 设置棋盘和游戏的参数
'''
self.node1 = node({'cpu':20, 'memory':20, 'gpu':0})
self.node2 = node({'cpu':20, 'memory':20, 'gpu':0})
self.node3 = node({'cpu':50, 'memory':50, 'gpu':50})
self.node_dict = {'node1':self.node1, 'node2':self.node2, 'node3':self.node3}
self.data_name = 'gpu'
self.c_puct_list = [0.03,0.3,3]
self.n_job_thread_list = [0,5]
self.probability_1_list = [0,0.03,0.3]
self.probability_2_list = [0.3,0.6,0.9]
'''
self.node1 = node({'cpu': 30, 'memory': 30, 'gpu': 30, 'fpga': 0})
self.node2 = node({'cpu': 30, 'memory': 30, 'gpu': 0, 'fpga': 30})
self.node3 = node({'cpu': 50, 'memory': 50, 'gpu': 50, 'fpga': 50})
self.node4 = node({'cpu': 30, 'memory': 30, 'gpu': 0, 'fpga': 0})
self.node5 = node({'cpu': 30, 'memory': 30, 'gpu': 0, 'fpga': 0})
#按比例应该是越大越明显
self.node_dict = {
'node1': self.node1,
'node2': self.node2,
'node3': self.node3,
'node4': self.node4,
'node5': self.node5
}
self.data_name = 'fpga_gpu'
self.c_puct_list = [0.03, 0.3, 3]
self.n_job_thread_list = [0, 5]
self.probability_1_list = [0, 0.03, 0.3]
self.probability_2_list = [0.3, 0.6, 0.9]
#self.weight = {'cpu':0.3, 'memory':0.2, 'gpu':0.5}
self.weight = None
self.state = State(self.node_dict)
self.game = Game(self.node_dict, self.weight)
# 设置训练参数
self.n_playout = 1000 # 每下一步棋，模拟的步骤数
self.c_puct = 1 # exploitation和exploration之间的折中系数
self.game_batch_num = 3
self.n_job_thread = 6 #0
self.probability_1 = 0 #0
self.probability_2 = 0.2 #0.2
#self.path = r'D:\科研\论文\High effient resource scheduling for cloud based on modified MCTS\programing\parameter_check_on_have_fpga.pkl'
# AI Player，设置is_selfplay=1 自我对弈，因为是在进行训练
self.mcts_player = MCTSPlayer(c_puct=self.c_puct,
n_playout=self.n_playout,
is_selfplay=1)
def nextState(self, currentState, control, dt):
a = control[0] #F/M #mass of car = 1
phi = control[1]
x = currentState.pos[0]
y = currentState.pos[1]
theta = currentState.heading
v = currentState.vel
v_new = v + a * dt
theta_new = theta + (v_new / self.L) * np.tan(phi) * dt
x_new = x + v * np.cos(theta_new) * dt
y_new = y + v * np.sin(theta_new) * dt
if (abs(v_new) > self.v_max):
return None
return State([x_new, y_new], v_new, theta_new)
def goTowards(self, startState, targetState, dt, time):
#print("Go towards \ Model")
if (self.type == 'kin-point'):
# 1) angle towards the target
alpha = atan2(targetState.pos[1] - startState.pos[1],
targetState.pos[0] - startState.pos[0])
path = list()
for i in np.linspace(0, self.v_max * time, num=int(time / dt)):
pos = [
startState.pos[0] + i * np.cos(alpha),
startState.pos[1] + i * np.sin(alpha)
]
v = self.v_max
h = targetState.heading
path.append(State(pos, v, h))
return path, time
elif (self.type == "kin-car"):
return self.kinCar.goTowards(startState, targetState, dt, time)
elif (self.type == 'dyn-point'):
return self.dynPoint.goTowards(startState, targetState, dt, time)
else:
return None, 0
def nextState(self, currentState, control, dt):
x = currentState.pos[0]
y = currentState.pos[1]
vx = np.cos(currentState.heading) * currentState.vel
vy = np.sin(currentState.heading) * currentState.vel
ax = np.cos(control[1]) * control[0]
ay = np.sin(control[1]) * control[0]
x_new = x + vx * dt + 0.5 * dt * dt * ax
y_new = y + vy * dt + 0.5 * dt * dt * ay
vx_new = vx + ax * dt
vy_new = vy + ay * dt
direction = atan2(vy_new, vx_new)
vel = np.linalg.norm(np.array([vx_new, vy_new]))
if (vel > self.v_max):
#print("Too fast! " + str(vel))
return None
#print("distance: " + str(np.linalg.norm([x_new-x,y_new-y])))
return State([x_new, y_new], vel, direction)
def getPath(self, startState, endState, dt):
#print("Get path \ Model")
if (self.type == 'kin-point'):
d = np.linalg.norm(
np.array(startState.pos) - np.array(endState.pos))
n = 2 + (d / (dt * startState.vel))
#print("steps: " + str(n))
path = list()
for i in np.linspace(0, 1, num=n):
x = startState.pos[0] + i * (endState.pos[0] -
startState.pos[0])
y = startState.pos[1] + i * (endState.pos[1] -
startState.pos[1])
v = self.v_max
h = endState.heading
path.append(State([x, y], v, h))
return path, d / self.v_max
elif (self.type == "kin-car"):
return self.kinCar.goToState(startState, endState, dt)
elif (self.type == 'dyn-point'):
return self.dynPoint.goToState(startState, endState, dt)
else:
return None, 0
def nextState(self,currentState,control,dt):
a = control[0] #F/M #mass of car = 1
phi = control[1]
x = currentState.pos[0]
y = currentState.pos[1]
theta = currentState.heading
v = currentState.vel
v_new = v + a*dt
theta_new = theta + (v_new/self.L)*np.tan(phi)*dt
x_new = x+v*np.cos(theta_new)*dt
y_new = y+v*np.sin(theta_new)*dt
#exceeding maximum speed
if(abs(v_new) > self.v_max):
return None
#frictionCar
if(np.sqrt( (v_new**4 / self.L*self.L)*np.tan(phi)**2 + a*a ) > self.K ):
return None
return State([x_new,y_new],v_new,theta_new)
def main():
global nodes
global data
global model
data = Data('maps/problem_B.json')
#init obsticles and boundary polygon
initObsticles(data)
#settings
#model = Model(data,type = 'kin-point')
#model = Model(data,type = 'dyn-point')
#model = Model(data,type = 'diff-drive')
#model = Model(data,type = 'kin-car')
#model = Model(data,type = 'dyn-car')
model = Model(data, type='friction-car')
#Create start and goal states
initial_vel = np.linalg.norm(np.array(data.start_vel))
initial_heading = atan2(data.start_vel[1], data.start_vel[0])
initialPoint = Node(State(data.start_pos, initial_vel, initial_heading),
None)
goal_vel = np.linalg.norm(np.array(data.goal_vel))
goal_heading = atan2(data.goal_vel[1], data.goal_vel[0])
goalPoint = Node(State(data.goal_pos, goal_vel, goal_heading), None)
#goalPoint = Node(State([5,5],goal_vel,initial_heading), None)
#Draw start and goal states
draw_point(initialPoint, 5, black, showHeading=True)
draw_point(goalPoint, 5, black, showHeading=True)
#list with all nodes is a global varaible
nodes.append(initialPoint)
counter = 0
imagenr = 0
repaint = False
while True:
#time.sleep(0.1)
#counter = counter+1
#if counter%10 == 0:
# pass
# pygame.image.save(screen,'images\RRT-STAR'+str(imagenr)+'.jpg')
# imagenr +=1
#distance_to_goal = np.linalg.norm(np.array(N.state.pos) - np.array(goalPoint.state.pos))
#--------------------------EXPAND TREE---------------------------------
#for i in range(200):
# P = randomsize_point(pathLength=10,start=initialPoint.state.pos, stop=goalPoint.state.pos)
# draw_line(P,P,red,2)
#time.sleep(2)
if (goalPoint.parent != None):
P = randomsize_point(pathLength=pathLength(goalPoint),
start=initialPoint.state.pos,
stop=goalPoint.state.pos)
else:
P = randomsize_point()
draw_line(P, P, red, 2)
randomState = State(P,
random.random() * model.v_max,
random.random() * 2 * np.pi)
N = find_NN(P)
#if(optimize(N)):
# print("improvement")
child = extend(N, randomState, extend=True)
if (child is None): #target could not be reached
print("Child is None")
continue
#optimize the tree like RRT-star
#if(optimize(child)):
# print("WOHO it has improved")
#Optimize a random node
#if(optimize(nodes[np.random.randint(len(nodes))])):
# print("Random improvement")
#Draw all states to get a nice image!
draw_line(child.parent.state.pos, child.path[0].pos, green, 1)
for i in range(1, len(child.path)):
draw_line(child.path[i - 1].pos, child.path[i].pos, green, 1)
draw_point(child, 1, dark_green)
nodes.append(child)
#Redraw everything
if (repaint or len(nodes) % 1000 == 999):
repaint = False
#Redraw tree
#print("Redraw")
print("Redrawing " + str(len(nodes)) + " nodes")
screen.fill(black)
draw_polygons()
draw_point(initialPoint, 5, black, showHeading=True)
draw_point(goalPoint, 5, black, showHeading=True)
for i in range(len(nodes)):
if (nodes[i].path is None):
continue
for j in range(1, len(nodes[i].path)):
draw_line(nodes[i].path[j - 1].pos, nodes[i].path[j].pos,
green, 1)
#Draw all states to get a nice image!
if (child.parent != None):
draw_line(nodes[i].parent.state.pos, nodes[i].path[0].pos,
green, 1)
draw_line(nodes[i].state.pos, nodes[i].path[-1].pos, green, 1)
draw_point(nodes[i], 1, dark_green)
if (not goalPoint.parent is None):
drawPath(goalPoint)
pygame.display.update()
#time.sleep(10)
#Try to extend to the goal if the node is close
if (np.linalg.norm(
np.array(child.state.pos) - np.array(goalPoint.state.pos)) <
GOAL_TRYDISTANCE):
print("Trying to reach goal")
#attempt = extend(child,goalPoint.state,extend = False) #------------------------
attempt = child
if (attempt is None): #target could not be reached
continue
distance_diff = np.linalg.norm(
np.array(attempt.state.pos) - np.array(goalPoint.state.pos))
heading_diff = angle_differance(attempt.state.heading,
goalPoint.state.heading)
print("distance is off by: " + str(distance_diff))
print("heading is off by : " + str(180.0 * heading_diff / np.pi))
if (distance_diff < GOAL_RADIUS
and heading_diff < GOAL_DIRECTIONTOL):
print("Goal reached!")
if (goalPoint.parent is None):
#time for new Node
c_c = 0
current = child
while (not current is None):
c_c += current.time
current = current.parent
goalPoint.parent = attempt
goalPoint.pathLength = c_c
nodes.append(attempt)
nodes.append(goalPoint)
drawPath(goalPoint)
else:
# Current time for goalPoint
c_t = 0
current = goalPoint
while (not current is None):
c_t += current.time
current = current.parent
#time for new Node
c_c = 0
current = attempt
while (not current is None):
c_c += current.time
current = current.parent
if (c_c < c_t):
print("Best path has been improved")
goalPoint.parent = attempt
goalPoint.pathLength = c_c
nodes.append(attempt)
drawPath(goalPoint)
#drawPath(goalPoint)
#pygame.display.update()
#handle events
for e in pygame.event.get():
if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE):
sys.exit("Exiting")
if (e.type == KEYUP and e.key == K_SPACE):
repaint = True
if (e.type == KEYUP and e.key == K_o):
l = len(nodes)
print("Optimizing " + str(l) +
" nodes, this might take some time")
for i in range(l):
if (i % 20 == 0):
print("optimizing node: " + str(i))
optimize(nodes[i])
if (e.type == KEYUP and e.key == K_p):
if (goalPoint.parent != None and random.random() < 0.2):
print("Optimize best path")
optimeizePath(goalPoint)
if (e.type == KEYUP and e.key == K_s):
if (goalPoint.parent != None):
print("Save path!: ")
steps = 0
current = goalPoint
while (current.parent != None):
steps += 1
if current.path != None:
steps += len(current.path)
current = current.parent
print("Steps to goal: " + str(steps))
current = goalPoint
output = np.zeros((steps, 4))
index = 0
while (current.parent != None):
output[index][0] = current.state.pos[0]
output[index][1] = current.state.pos[1]
output[index][2] = current.state.vel
output[index][3] = current.state.heading
index = index + 1
if current.path != None:
for i in range(len(current.path) - 2, 0, -1):
output[index][0] = current.path[i].pos[0]
output[index][1] = current.path[i].pos[1]
output[index][2] = current.path[i].vel
output[index][3] = current.path[i].heading
index += 1
current = current.parent
output[index][0] = current.state.pos[0]
output[index][1] = current.state.pos[1]
output[index][2] = current.state.vel
output[index][3] = current.state.heading
index = index + 1
output = output[:index, :]
np.savetxt("TEST-dynamiccar-friction.csv",
output,
delimiter=",")
print("saving path")
time.sleep(2)
if (len(nodes) % 50 == 0):
print("Nodes: " + str(len(nodes)))
pygame.display.update()
fpsClock.tick(10000)
def xTrackTest():
global nodes
global data
global model
data = Data('maps/problem_A_niklas2.json')
#init obsticles and boundary polygon
initObsticles(data)
#settings
#model = Model(data,type = 'kin-point')
model = Model(data, type='dyn-point')
#model = Model(data,type = 'kin-car')
#Create start and goal states
initial_vel = 2
initial_heading = 1 * np.pi / 2
initialPoint = Node(State([1, -1], initial_vel, initial_heading), None)
goal_vel = 1
goal_heading = np.pi / 2
goalPoint = Node(State([1, 1.5], goal_vel, goal_heading), None)
#Draw start and goal states
draw_point(initialPoint, 5, black, showHeading=True)
draw_point(goalPoint, 5, black, showHeading=True)
Start = initialPoint
Stop = goalPoint.state
child = extend(Start, Stop, extend=False)
if (child is None): #target could not be reached
print("Child is None")
#list with all nodes is a global varaible
nodes.append(initialPoint)
counter = 0
imagenr = 0
repaint = False
while True:
#time.sleep(0.1)
#counter = counter+1
#if counter%10 == 0:
# pass
# pygame.image.save(screen,'images\RRT-STAR'+str(imagenr)+'.jpg')
# imagenr +=1
#distance_to_goal = np.linalg.norm(np.array(N.state.pos) - np.array(goalPoint.state.pos))
#--------------------------EXPAND TREE---------------------------------
#P = randomsize_point()
#draw_line(P,P,red,2)
#Draw all states to get a nice image!
draw_line(child.parent.state.pos, child.path[0].pos, green, 1)
for i in range(1, len(child.path)):
draw_line(child.path[i - 1].pos, child.path[i].pos, green, 1)
draw_point(child, 1, dark_green)
#nodes.append(child)
if (repaint or len(nodes) % 1000 == 999):
repaint = False
#Redraw tree
#print("Redraw")
print("Redrawing " + str(len(nodes)) + " nodes")
screen.fill(black)
draw_polygons()
draw_point(initialPoint, 5, black, showHeading=True)
draw_point(goalPoint, 5, black, showHeading=True)
for i in range(len(nodes)):
if (nodes[i].path is None):
continue
for j in range(1, len(nodes[i].path)):
draw_line(nodes[i].path[j - 1].pos, nodes[i].path[j].pos,
green, 1)
#Draw all states to get a nice image!
if (child.parent != None):
draw_line(nodes[i].parent.state.pos, nodes[i].path[0].pos,
green, 1)
draw_line(nodes[i].state.pos, nodes[i].path[-1].pos, green, 1)
draw_point(nodes[i], 1, dark_green)
if (not goalPoint.parent is None):
drawPath(goalPoint)
pygame.display.update()
#time.sleep(10)
#handle events
for e in pygame.event.get():
if e.type == QUIT or (e.type == KEYUP and e.key == K_ESCAPE):
sys.exit("Exiting")
if (e.type == KEYUP and e.key == K_SPACE):
repaint = True
if (len(nodes) % 50 == 0):
print("Nodes: " + str(len(nodes)))
pygame.display.update()
fpsClock.tick(10000)
time.sleep(1)
def start_self_play(self,
player,
n_job_thread,
probability_1,
probability_2,
is_shown=0):
# 初始化棋盘
self.state1 = State(self.node_dict, self.weight)
self.state2 = State(self.node_dict, self.weight)
self.state3 = State(self.node_dict, self.weight)
# 记录该局对应的数据：states, mcts_probs, current_players
if self.jobs == None:
self.jobs = []
moves1, states1, mcts_probs1 = [], [], []
moves2, states2, mcts_probs2 = [], [], []
moves3, states3, mcts_probs3 = [], [], []
n1 = 1
n2 = 1
n3 = 1
# 一直循环到比赛结束
while True:
# 得到player的下棋位置
# 加入job
self.jobs.append(
random_job(self.state1, n1, n_job_thread, probability_1,
probability_2))
self.state1.job(self.jobs[-1])
self.state2.job(self.jobs[-1])
self.state3.job(self.jobs[-1])
end1 = self.state1.game_end()
end2 = self.state2.game_end()
end3 = self.state3.game_end()
if end1 and end2 and end3:
#resourse_last1 = self.state1.now()
#credit1 = self.get_algorithm_credit(self.state1)#返回该局得分
player.reset_player()
#resourse_last2 = self.state2.now()
#credit2 = self.get_algorithm_credit(self.state2)#返回该局得分
#resourse_last3 = self.state3.now()
#credit3 = self.get_algorithm_credit(self.state3)#返回该局得分
#return self.jobs, moves1, moves2, moves3, credit1, credit2, credit3, resourse_last1, resourse_last2, resourse_last3
return self.jobs, moves1, moves2, moves3
if not end1:
move1, move_probs1 = player.get_action(self.state1, n1,
n_job_thread,
probability_1,
probability_2)
#move1, move_probs1 = node_select_value(self.state1)
# 存储数据 states1.append(self.state1.get_state_resource_now()) #棋盘状态
mcts_probs1.append(move_probs1)
moves1.append(move1)
# 按照move来下棋
self.state1.scheduling(move1)
if is_shown:
pass
n1 += 1
if not end2:
move2, move_probs2 = node_select_value_cos(self.state2)
states2.append(self.state2.get_state_resource_now()) #棋盘状态
mcts_probs2.append(move_probs2)
moves2.append(move2)
# 按照move来下棋
self.state2.scheduling(move2)
if is_shown:
pass
n2 += 1
if not end3:
#move1, move_probs1 = player.get_action(self.state1)
move3, move_probs3 = node_select_value(self.state3)
# 存储数据 states1.append(self.state1.get_state_resource_now()) #棋盘状态
mcts_probs3.append(move_probs3)
moves3.append(move3)
# 按照move来下棋
self.state3.scheduling(move3)
if is_shown:
pass
n3 += 1