📦 NEW: 提交bt_expansion的原始论文代码
This commit is contained in:
2193177243@qq.com
parent
a144cb930d
commit
370455dd76
|
|
@ -0,0 +1,466 @@
|
|||
import random
|
||||
import numpy as np
|
||||
import copy
|
||||
import time
|
||||
#定义行动类，行动包括前提、增加和删除影响
|
||||
class Action:
|
||||
def __init__(self,name='anonymous action'):
|
||||
self.pre=set()
|
||||
self.add=set()
|
||||
self.del_set=set()
|
||||
self.name=name
|
||||
|
||||
def __str__(self):
|
||||
return self.name
|
||||
|
||||
# 从状态随机生成一个行动
|
||||
def generate_from_state(self,state,num):
|
||||
for i in range(0,num):
|
||||
if i in state:
|
||||
if random.random() >0.5:
|
||||
self.pre.add(i)
|
||||
if random.random() >0.5:
|
||||
self.del_set.add(i)
|
||||
continue
|
||||
if random.random() > 0.5:
|
||||
self.add.add(i)
|
||||
continue
|
||||
if random.random() >0.5:
|
||||
self.del_set.add(i)
|
||||
|
||||
def print_action(self):
|
||||
print (self.pre)
|
||||
print(self.add)
|
||||
print(self.del_set)
|
||||
|
||||
#生成随机状态
|
||||
def generate_random_state(num):
|
||||
result = set()
|
||||
for i in range(0,num):
|
||||
if random.random()>0.5:
|
||||
result.add(i)
|
||||
return result
|
||||
#从状态和行动生成后继状态
|
||||
def state_transition(state,action):
|
||||
if not action.pre <= state:
|
||||
print ('error: action not applicable')
|
||||
return state
|
||||
new_state=(state | action.add) - action.del_set
|
||||
return new_state
|
||||
#叶结点
|
||||
class Leaf:
|
||||
def __init__(self,type,content):
|
||||
self.type=type
|
||||
self.content=content #conditionset or action
|
||||
self.parent=None
|
||||
self.parent_index=0
|
||||
|
||||
# tick 叶节点，返回返回值以及对应的条件或行动对象self.content
|
||||
def tick(self,state):
|
||||
if self.type=='cond':
|
||||
if self.content <= state:
|
||||
return 'success',self.content
|
||||
else:
|
||||
return 'failure',self.content
|
||||
if self.type=='act':
|
||||
if self.content.pre<=state:
|
||||
return 'running',self.content #action
|
||||
else:
|
||||
return 'failure',self.content
|
||||
|
||||
def __str__(self):
|
||||
print( self.content)
|
||||
return ''
|
||||
|
||||
def print_nodes(self):
|
||||
print(self.content)
|
||||
|
||||
def count_size(self):
|
||||
return 1
|
||||
#可能包含控制结点的行为树
|
||||
class ControlBT:
|
||||
def __init__(self,type):
|
||||
self.type=type
|
||||
self.children=[]
|
||||
self.parent=None
|
||||
self.parent_index=0
|
||||
|
||||
|
||||
def add_child(self,subtree_list):
|
||||
for subtree in subtree_list:
|
||||
self.children.append(subtree)
|
||||
subtree.parent=self
|
||||
subtree.parent_index=len(self.children)-1
|
||||
|
||||
# tick行为树，根据不同控制结点逻辑tick子结点
|
||||
def tick(self,state):
|
||||
if len(self.children) < 1:
|
||||
print("error,no child")
|
||||
if self.type =='?':#选择结点，即或结点
|
||||
for child in self.children:
|
||||
val,obj=child.tick(state)
|
||||
if val=='success':
|
||||
return val,obj
|
||||
if val=='running':
|
||||
return val,obj
|
||||
return 'failure','?fails'
|
||||
if self.type =='>':#顺序结点，即与结点
|
||||
for child in self.children:
|
||||
val,obj=child.tick(state)
|
||||
if val=='failure':
|
||||
return val,obj
|
||||
if val=='running':
|
||||
return val,obj
|
||||
return 'success', '>success'
|
||||
if self.type =='act':#行动结点
|
||||
return self.children[0].tick(state)
|
||||
if self.type =='cond':#条件结点
|
||||
return self.children[0].tick(state)
|
||||
|
||||
def getFirstChild(self):
|
||||
return self.children[0]
|
||||
|
||||
def __str__(self):
|
||||
print(self.type+'\n')
|
||||
for child in self.children:
|
||||
print (child)
|
||||
return ''
|
||||
|
||||
def print_nodes(self):
|
||||
print(self.type)
|
||||
for child in self.children:
|
||||
child.print_nodes()
|
||||
|
||||
# 递归统计树中结点数
|
||||
def count_size(self):
|
||||
result=1
|
||||
for child in self.children:
|
||||
result+= child.count_size()
|
||||
return result
|
||||
|
||||
|
||||
#本文所提出的完备规划算法
|
||||
class BTalgorithm:
|
||||
def __init__(self):
|
||||
self.bt = None
|
||||
self.nodes=[]
|
||||
self.traversed=[]
|
||||
self.conditions=[]
|
||||
self.conditions_index=[]
|
||||
#print (self.conditions_list[0])
|
||||
|
||||
def clear(self):
|
||||
self.bt = None
|
||||
self.nodes = []
|
||||
self.traversed = []
|
||||
self.conditions = []
|
||||
self.conditions_index = []
|
||||
|
||||
#运行规划算法，从初始状态、目标状态和可用行动，计算行为树self.bt
|
||||
def run_algorithm(self,start,goal,actions):
|
||||
# 初始行为树只包含目标条件
|
||||
self.bt = ControlBT(type='cond')
|
||||
g_node = Leaf(type='cond', content=goal)
|
||||
self.bt.add_child([g_node])
|
||||
|
||||
|
||||
self.conditions.append(goal)
|
||||
self.nodes.append(g_node) #condition node list
|
||||
# 尝试在初始状态执行行为树
|
||||
val, obj = self.bt.tick(start)
|
||||
canrun = False
|
||||
if val == 'success' or val == 'running':
|
||||
canrun = True
|
||||
# 循环扩展，直到行为树能够在初始状态运行
|
||||
while not canrun:
|
||||
index = -1
|
||||
for i in range(0,len(self.nodes)):
|
||||
if self.nodes[i].content in self.traversed:
|
||||
continue
|
||||
else:
|
||||
c_node = self.nodes[i]
|
||||
index = i
|
||||
break
|
||||
if index == -1:#树中结点扩展完毕，仍无法运行行为树，返回失败
|
||||
print('Failure')
|
||||
return False
|
||||
#根据所选择条件结点扩展子树
|
||||
subtree = ControlBT(type='?')
|
||||
subtree.add_child([copy.deepcopy(c_node)])#子树首先保留所扩展结点
|
||||
c = c_node.content#子树所扩展结点对应的条件（一个文字的set）
|
||||
|
||||
for i in range(0,len(actions)):#选择符合条件的行动，
|
||||
#print("have action")
|
||||
if not c & ( (actions[i].pre | actions[i].add)-actions[i].del_set) <=set():
|
||||
#print ("pass add")
|
||||
if (c - actions[i].del_set) == c:
|
||||
#print("pass delete")
|
||||
c_attr = (actions[i].pre | c )-actions[i].add
|
||||
valid = True
|
||||
for j in self.traversed:#剪枝操作
|
||||
if j<=c_attr:
|
||||
valid = False
|
||||
break
|
||||
if valid:
|
||||
#print("pass prune")
|
||||
# 构建行动的顺序结构
|
||||
sequence_structure=ControlBT(type='>')
|
||||
c_attr_node = Leaf(type='cond', content=c_attr)
|
||||
a_node = Leaf(type='act', content=actions[i])
|
||||
sequence_structure.add_child([c_attr_node,a_node])
|
||||
# 将顺序结构添加到子树
|
||||
subtree.add_child([sequence_structure])
|
||||
|
||||
self.nodes.append(c_attr_node)
|
||||
# 将原条件结点c_node替换为扩展后子树subtree
|
||||
parent_of_c = c_node.parent
|
||||
parent_of_c.children[0] = subtree
|
||||
#记录已扩展条件
|
||||
self.traversed.append(c)
|
||||
# 尝试在初始状态运行行为树
|
||||
val, obj = self.bt.tick(start)
|
||||
canrun = False
|
||||
if val == 'success' or val == 'running':
|
||||
canrun = True
|
||||
return True
|
||||
|
||||
def print_solution(self):
|
||||
print(len(self.nodes))
|
||||
# for i in self.nodes:
|
||||
# if isinstance(i,Node):
|
||||
# print (i.content)
|
||||
# else:
|
||||
# print (i)
|
||||
|
||||
#所对比的基准算法，具体扩展细节有差异
|
||||
class Weakalgorithm:
|
||||
def __init__(self):
|
||||
self.bt=None
|
||||
self.nodes=[]
|
||||
self.traversed=[]
|
||||
self.conditions=[]
|
||||
self.conditions_index=[]
|
||||
self.danger=False
|
||||
#print (self.conditions_list[0])
|
||||
|
||||
def clear(self):
|
||||
self.bt=None
|
||||
self.nodes = []
|
||||
self.traversed = []
|
||||
self.conditions = []
|
||||
self.conditions_index = []
|
||||
self.danger = False
|
||||
|
||||
def run_algorithm(self,start,goal,actions):
|
||||
self.bt = ControlBT(type='cond')
|
||||
g_node = Leaf(type='cond', content=goal)
|
||||
self.bt.add_child([g_node])
|
||||
|
||||
# self.nodes.append(goal)
|
||||
self.conditions.append(goal)
|
||||
self.nodes.append(g_node) # condition node list
|
||||
# nodes_start_index=0
|
||||
# self.conditions_index.append(0)
|
||||
val, obj = self.bt.tick(start)
|
||||
canrun = False
|
||||
if val == 'success' or val == 'running':
|
||||
canrun = True
|
||||
while not canrun:
|
||||
#print ("loop begin")
|
||||
index = -1
|
||||
for i in range(0,len(self.nodes)):
|
||||
if self.nodes[i].content in self.traversed:
|
||||
continue
|
||||
else:
|
||||
c_node = self.nodes[i]
|
||||
index = i
|
||||
break
|
||||
#print (index)
|
||||
if index == -1:
|
||||
print('Algorithm Failure, all conditions expanded')
|
||||
return False
|
||||
subtree = ControlBT(type='?')
|
||||
subtree.add_child([copy.deepcopy(c_node)])
|
||||
c = c_node.content
|
||||
|
||||
for i in range(0,len(actions)):
|
||||
if not c & actions[i].add <= set():
|
||||
if not (c - actions[i].del_set) == c:
|
||||
danger = True
|
||||
self.danger = True
|
||||
#continue
|
||||
c_attr = actions[i].pre
|
||||
valid = True
|
||||
if valid:
|
||||
sequence_structure = ControlBT(type='>')
|
||||
|
||||
for j in c_attr:
|
||||
if j in self.traversed:
|
||||
continue
|
||||
c_attr_node = Leaf(type='cond', content={j})
|
||||
|
||||
|
||||
sequence_structure.add_child([c_attr_node])
|
||||
if j in start:
|
||||
continue
|
||||
self.nodes.append(c_attr_node)
|
||||
|
||||
a_node = Leaf(type='act', content=actions[i])
|
||||
sequence_structure.add_child([ a_node])
|
||||
subtree.add_child([sequence_structure])
|
||||
|
||||
|
||||
parent_of_c = c_node.parent
|
||||
p_index = c_node.parent_index
|
||||
parent_of_c.children[p_index] = subtree
|
||||
|
||||
self.traversed.append(c)
|
||||
val, obj = self.bt.tick(start)
|
||||
canrun = False
|
||||
if val == 'success' or val == 'running':
|
||||
canrun = True
|
||||
return True
|
||||
|
||||
def print_solution(self):
|
||||
print(len(self.nodes))
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
random.seed(1)
|
||||
# 设置生成规划问题集的超参数：文字数、解深度、迭代次数
|
||||
literals_num=10
|
||||
depth = 10
|
||||
iters= 10
|
||||
total_tree_size = []
|
||||
total_action_num = []
|
||||
total_state_num = []
|
||||
total_steps_num=[]
|
||||
#fail_count=0
|
||||
#danger_count=0
|
||||
success_count =0
|
||||
failure_count = 0
|
||||
planning_time_total = 0.0
|
||||
# 实验1000次
|
||||
for count in range (0,1000):
|
||||
# 生成一个规划问题，包括随机的状态和行动，以及目标状态
|
||||
states = []
|
||||
actions = []
|
||||
start = generate_random_state(literals_num)
|
||||
state = start
|
||||
states.append(state)
|
||||
#print (state)
|
||||
for i in range (0,depth):
|
||||
a = Action()
|
||||
a.generate_from_state(state,literals_num)
|
||||
if not a in actions:
|
||||
actions.append(a)
|
||||
state = state_transition(state,a)
|
||||
if state in states:
|
||||
pass
|
||||
else:
|
||||
states.append(state)
|
||||
#print(state)
|
||||
|
||||
goal = states[-1]
|
||||
state = start
|
||||
for i in range (0,iters):
|
||||
a = Action()
|
||||
a.generate_from_state(state,literals_num)
|
||||
if not a in actions:
|
||||
actions.append(a)
|
||||
state = state_transition(state,a)
|
||||
if state in states:
|
||||
pass
|
||||
else:
|
||||
states.append(state)
|
||||
state = random.sample(states,1)[0]
|
||||
# 选择测试本文算法btalgorithm，或对比算法weakalgorithm
|
||||
algo = BTalgorithm()
|
||||
#algo = Weakalgorithm()
|
||||
start_time = time.time()
|
||||
if algo.run_algorithm(start, goal, list(actions)):#运行算法，规划后行为树为algo.bt
|
||||
total_tree_size.append( algo.bt.count_size()-1)
|
||||
else:
|
||||
print ("error")
|
||||
end_time = time.time()
|
||||
planning_time_total += (end_time-start_time)
|
||||
|
||||
|
||||
#开始从初始状态运行行为树，测试
|
||||
state=start
|
||||
steps=0
|
||||
val, obj = algo.bt.tick(state)#tick行为树，obj为所运行的行动
|
||||
while val !='success' and val !='failure':#运行直到行为树成功或失败
|
||||
state = state_transition(state,obj)
|
||||
val, obj = algo.bt.tick(state)
|
||||
if(val == 'failure'):
|
||||
print("bt fails at step",steps)
|
||||
steps+=1
|
||||
if(steps>=500):#至多运行500步
|
||||
break
|
||||
if not goal <= state:#错误解，目标条件不在执行后状态满足
|
||||
#print ("wrong solution",steps)
|
||||
failure_count+=1
|
||||
|
||||
else:#正确解，满足目标条件
|
||||
#print ("right solution",steps)
|
||||
success_count+=1
|
||||
total_steps_num.append(steps)
|
||||
algo.clear()
|
||||
total_action_num.append(len(actions))
|
||||
total_state_num.append(len(states))
|
||||
print (success_count,failure_count)#算法成功和失败次数
|
||||
|
||||
print(np.mean(total_tree_size), np.std(total_tree_size, ddof=1))#1000次测试树大小
|
||||
print (np.mean(total_steps_num),np.std(total_steps_num,ddof=1))
|
||||
print (np.mean(total_state_num))#1000次问题的平均状态数
|
||||
print (np.mean(total_action_num))#1000次问题的平均行动数
|
||||
print(planning_time_total,planning_time_total/1000.0)
|
||||
|
||||
#print(total_state_num)
|
||||
|
||||
#casestudy begin 对应论文的case study，包含三个行动的移动机械臂场景
|
||||
|
||||
# actions=[]
|
||||
# a = Action(name='movebtob')
|
||||
# a.pre={1,2}
|
||||
# a.add={3}
|
||||
# a.del_set={1,4}
|
||||
# actions.append(a)
|
||||
# a=Action(name='moveatob')
|
||||
# a.pre={1}
|
||||
# a.add={5,2}
|
||||
# a.del_set={1,6}
|
||||
# actions.append(a)
|
||||
# a=Action(name='moveatoa')
|
||||
# a.pre={7}
|
||||
# a.add={8,2}
|
||||
# a.del_set={7,6}
|
||||
# actions.append(a)
|
||||
#
|
||||
# start = {1,7,4,6}
|
||||
# goal={3}
|
||||
# algo = BTalgorithm()
|
||||
# algo.clear()
|
||||
# algo.run_algorithm(start, goal, list(actions))
|
||||
# state = start
|
||||
# steps = 0
|
||||
# val, obj = algo.bt.tick(state)
|
||||
# while val != 'success' and val != 'failure':
|
||||
# state = state_transition(state, obj)
|
||||
# print (obj.name)
|
||||
# val, obj = algo.bt.tick(state)
|
||||
# if (val == 'failure'):
|
||||
# print("bt fails at step", steps)
|
||||
# steps += 1
|
||||
# if not goal <= state:
|
||||
# print ("wrong solution",steps)
|
||||
# else:
|
||||
# print ("right solution",steps)
|
||||
# #algo.bt.print_nodes()
|
||||
# print (algo.bt.count_size()-1)
|
||||
# algo.clear()
|
||||
|
||||
#case study end
|
||||
Loading…
Reference in New Issue