RoboWaiter/BTExpansionCode/EXP/exp_tools.py

978 lines
39 KiB
Python
Raw Normal View History

2024-01-04 22:51:33 +08:00
from utils.bt.load import load_behavior_tree_lib
from OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm
2024-01-05 14:11:54 +08:00
2024-01-04 22:51:33 +08:00
import copy
from tabulate import tabulate
import numpy as np
import os
from sympy import symbols, Not, Or, And, to_dnf
from OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm
from BTExpansionAlgorithm import BTExpAlgorithm # 调用最优行为树扩展算法
import time
from utils.bt.draw import render_dot_tree
from utils.bt.load import load_bt_from_ptml
2024-04-10 19:59:13 +08:00
from EXP.behavior_lib._base.Behavior import Bahavior
from EXP.behavior_lib.cond import Holding
2024-01-04 22:51:33 +08:00
root_path = os.path.abspath(
os.path.join(__file__, "../../..")
)
def goal_transfer_str(goal):
goal_dnf = str(to_dnf(goal, simplify=True))
# print(goal_dnf)
goal_set = []
if ('|' in goal or '&' in goal or 'Not' in goal) or not '(' in goal:
goal_ls = goal_dnf.split("|")
for g in goal_ls:
g_set = set()
g = g.replace(" ", "").replace("(", "").replace(")", "")
g = g.split("&")
for literal in g:
if '_' in literal:
first_part, rest = literal.split('_', 1)
literal = first_part + '(' + rest
# 添加 ')' 到末尾
literal += ')'
# 替换剩余的 '_' 为 ','
literal = literal.replace('_', ',')
2024-04-10 19:59:13 +08:00
literal=literal.replace('~', 'Not ')
2024-01-04 22:51:33 +08:00
g_set.add(literal)
goal_set.append(g_set)
else:
g_set = set()
w = goal.split(")")
g_set.add(w[0] + ")")
if len(w) > 1:
for x in w[1:]:
if x != "":
g_set.add(x[1:] + ")")
goal_set.append(g_set)
return goal_set
2024-04-10 19:59:13 +08:00
def collect_action_nodes_multiple_num(random,multiple_num=1):
2024-01-04 22:51:33 +08:00
behavior_dict = load_behavior_tree_lib()
2024-04-10 19:59:13 +08:00
action_list = []
2024-01-05 14:11:54 +08:00
2024-01-04 22:51:33 +08:00
for cls in behavior_dict["act"].values():
2024-01-05 14:11:54 +08:00
if cls.can_be_expanded:
print(f"可扩展动作:{cls.__name__}, 存在{len(cls.valid_args)}个有效论域组合")
if cls.num_args == 0:
for num in range(multiple_num):
info = cls.get_info()
action_list.append(Action(name=cls.get_ins_name() + str(num), **info))
if cls.num_args == 1:
for num in range(multiple_num):
for arg in cls.valid_args:
info = cls.get_info(arg)
action_list.append(Action(name=cls.get_ins_name(arg) + str(num), **info))
if cls.num_args > 1:
2024-04-10 19:59:13 +08:00
# for num in range(multiple_num):
2024-01-05 14:11:54 +08:00
for num in range(multiple_num):
for args in cls.valid_args:
2024-04-10 19:59:13 +08:00
# xx += 1
2024-01-05 14:11:54 +08:00
info = cls.get_info(*args)
action_list.append(Action(name=cls.get_ins_name(*args) + str(num),**info))
action_list = sorted(action_list, key=lambda x: x.name)
for i in range(len(action_list)):
cost = random.randint(1, 100)
action_list[i].cost=cost
2024-04-10 19:59:13 +08:00
2024-01-05 14:11:54 +08:00
return action_list
2024-04-10 19:59:13 +08:00
def collect_action_nodes(random,multiple_num=1,iters_times=1):
behavior_dict = load_behavior_tree_lib()
iter_action_ls=[]
for iter in range(iters_times):
action_list = []
for cls in behavior_dict["act"].values():
if cls.can_be_expanded:
print(f"可扩展动作:{cls.__name__}, 存在{len(cls.valid_args)}个有效论域组合")
if cls.num_args == 0:
mr = random.randint(1, multiple_num+1)
for num in range(mr):
info = cls.get_info()
action_list.append(Action(name=cls.get_ins_name() + str(num), **info))
if cls.num_args == 1:
mr = random.randint(1, multiple_num+1)
for num in range(mr):
for arg in cls.valid_args:
info = cls.get_info(arg)
action_list.append(Action(name=cls.get_ins_name(arg) + str(num), **info))
if cls.num_args > 1:
# for num in range(multiple_num):
mr = random.randint(1, multiple_num+1)
for num in range(mr):
for args in cls.valid_args:
# xx += 1
info = cls.get_info(*args)
action_list.append(Action(name=cls.get_ins_name(*args) + str(num),**info))
# if xx%2==0 or xx%3==0:
# break
action_list = sorted(action_list, key=lambda x: x.name)
for i in range(len(action_list)):
cost = random.randint(1, 100)
action_list[i].cost=cost
iter_action_ls.append(action_list)
print("len(action_list):",len(action_list))
if iters_times==1:
return action_list
else:
return iter_action_ls
2024-01-05 14:11:54 +08:00
def collect_action_nodes_old(random):
action_list = []
behavior_dict = load_behavior_tree_lib()
2024-04-10 19:59:13 +08:00
behavior_ls = list() # behavior_ls.sort()
2024-01-05 14:11:54 +08:00
behavior_ls = [cls for cls in behavior_ls]
behavior_ls = sorted(behavior_ls, key=lambda x: x.__class__.__name__)
for cls in behavior_ls:
2024-01-04 22:51:33 +08:00
if cls.can_be_expanded:
print(f"可扩展动作:{cls.__name__}, 存在{len(cls.valid_args)}个有效论域组合")
if cls.num_args == 0:
for num in range(2):
cost = random.randint(1, 100)
info = cls.get_info()
info.pop('cost', None)
action_list.append(Action(name=cls.get_ins_name()+str(num),cost=cost, **info))
if cls.num_args == 1:
for num in range(2):
for arg in cls.valid_args:
cost = random.randint(1, 100)
info = cls.get_info(arg)
info.pop('cost', None)
action_list.append(Action(name=cls.get_ins_name(arg)+str(num),cost=cost, **info))
if cls.num_args > 1:
for num in range(2):
for args in cls.valid_args:
cost = random.randint(1, 100)
info = cls.get_info(*args)
info.pop('cost', None)
action_list.append(Action(name=cls.get_ins_name(*args)+str(num),cost=cost, **info))
return action_list
2024-04-10 19:59:13 +08:00
def collect_cond_nodes():
cond_list = []
behavior_dict = load_behavior_tree_lib()
num=0
vaild_num=1
vaild_num = f"{vaild_num:.2e}"
for cls in behavior_dict["cond"].values():
if cls.can_be_expanded:
print(f"可扩展条件:{cls.__name__}, 存在{len(cls.valid_args)}个有效论域组合")
if cls.num_params == 0:
num+=1
# vaild_num*=2
if cls.num_params == 1:
num += len(cls.valid_args)
# if cls.__name__=="Holding" or cls.__name__=='RobotNear':
# vaild_num *= (len(cls.valid_args))
# else:
# vaild_num *= (2**len(cls.valid_args))
if cls.num_params > 1:
cartesian_product_size=1
for s in cls.valid_args:
cartesian_product_size *= len(s)
num += cartesian_product_size
# if cls.__name__ == "On":
# vaild_num *= (len(cls.valid_args[1])**len(cls.valid_args[0]))
return num,vaild_num
2024-01-04 22:51:33 +08:00
def get_start():
2024-04-10 19:59:13 +08:00
# start_robowaiter = {'At(Robot,Bar)', 'Is(AC,Off)',
# 'Exist(Yogurt)', 'Exist(BottledDrink)', 'Exist(Softdrink)', 'Exist(ADMilk)',
# 'On(Yogurt,Bar)','On(BottledDrink,Bar)','On(ADMilk,Bar)','On(Chips,Bar)',
# 'Exist(Milk)', 'On(Softdrink,Table1)', 'On(Softdrink,Table3)',
# 'Exist(Chips)', 'Exist(NFCJuice)', 'Exist(Bernachon)', 'Exist(ADMilk)', 'Exist(SpringWater)', 'Exist(MilkDrink)',
# 'Exist(ADMilk)','On(ADMilk,Bar)','On(Bernachon,Bar)','On(SpringWater,Bar2)','On(MilkDrink,Bar)',
# 'Holding(Nothing)',
# 'Exist(VacuumCup)', 'On(VacuumCup,Table2)',
# 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)',
# 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'}
start_robowaiter = {'RobotNear(Bar)',
'Not Active(AC)','Not Active(HallLight)','Active(TubeLight)','Not Closed(Curtain)',
'Not Exists(Coffee)','Not Exists(Water)','Not Exists(Dessert)',
'Holding(Nothing)',
'Not IsClean(Table1)', 'Not IsClean(Floor)', 'Not IsClean(Chairs)',
'On(Softdrink,Table1)','On(VacuumCup,Table2)',
# 'On(Yogurt,Bar)','On(BottledDrink,Bar)','On(ADMilk,Bar)','On(Chips,Bar)',
# 'On(Softdrink,Table1)', 'On(Softdrink,Table3)',
# 'On(ADMilk,Bar)','On(Bernachon,Bar)','On(SpringWater,Bar2)','On(MilkDrink,Bar)',
# 'On(VacuumCup,Table2)',
}
all_obj_place= Bahavior.all_object | Bahavior.tables_for_placement | Bahavior.tables_for_guiding
start_robowaiter |= {f'Not RobotNear({place})' for place in all_obj_place if place != 'Bar'}
start_robowaiter |= {f'Not Holding({obj})' for obj in Bahavior.all_object}
start_robowaiter |= {f'Exists({obj})' for obj in Bahavior.all_object if obj != 'Coffee' and obj != 'Water' and obj != 'Dessert'}
# 'Softdrink' 在Table1
start_robowaiter |= {f'Not On(Softdrink,{place})' for place in Bahavior.all_place if place!="Table1"}
start_robowaiter |= {f'Not On(VacuumCup,{place})' for place in Bahavior.all_place if place != "Table2"}
# 默认物品都在 Bar 上
start_robowaiter |= {f'On({obj},Bar)' for obj in Bahavior.all_object if obj != 'Coffee' and obj != 'Water' and obj != 'Dessert' \
and obj != 'Softdrink' and obj != 'VacuumCup' }
for place in Bahavior.all_place:
if place!="Bar":
start_robowaiter |= {f'Not On({obj},{place})' for obj in Bahavior.all_object}
# start_robowaiter |= {f'On({obj},{place})' for obj in Bahavior.all_object if
# obj != 'Coffee' and obj != 'Water' and obj != 'Dessert'}
# 这三样哪里都没有
make_obj = {"Coffee",'Water','Dessert'}
for place in Bahavior.all_place:
start_robowaiter |= {f'Not On({obj},{place})' for obj in make_obj }
2024-01-04 22:51:33 +08:00
return start_robowaiter
def print_action_data_table(goal,start,actions):
data = []
for a in actions:
data.append([a.name ,a.pre ,a.add ,a.del_set ,a.cost])
data.append(["Goal" ,goal ," " ,"Start" ,start])
print(tabulate(data, headers=["Name", "Pre", "Add" ,"Del" ,"Cost"], tablefmt="fancy_grid")) # grid plain simple github fancy_grid
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
2024-04-10 19:59:13 +08:00
def BTTest_easy_medium_hard(bt_algo_opt,goal_states,action_list,start_robowaiter):
2024-01-04 22:51:33 +08:00
if bt_algo_opt:
print("============= OptBT Test ==============")
else:
print("============= XiaoCai BT Test ==============")
total_tree_size = []
total_action_num = []
total_state_num = []
total_steps_num = []
total_cost = []
total_tick = []
success_count = 0
failure_count = 0
planning_time_total = 0.0
2024-04-10 19:59:13 +08:00
planning_time_ls=[]
2024-01-05 14:11:54 +08:00
states=[] ####
2024-01-04 22:51:33 +08:00
actions = copy.deepcopy(action_list)
start = copy.deepcopy(start_robowaiter)
2024-01-05 14:11:54 +08:00
2024-01-04 22:51:33 +08:00
error=False
2024-04-10 19:59:13 +08:00
total_count = len(goal_states)
total_cond_tick = []
2024-01-04 22:51:33 +08:00
for count, goal_str in enumerate(goal_states):
goal = copy.deepcopy(goal_transfer_str(goal_str))
2024-04-10 19:59:13 +08:00
# goal = goal_str
# print("count:", count, "goal:", goal)
2024-01-04 22:51:33 +08:00
if bt_algo_opt:
# if count==874:
# algo = OptBTExpAlgorithm(verbose=False)
# else:
algo = OptBTExpAlgorithm(verbose=False)
else:
algo = BTExpAlgorithm(verbose=False)
algo.clear()
# algo = Weakalgorithm()
2024-04-10 19:59:13 +08:00
# if count == 6 : #874:
# print_action_data_table(goal, start, list(actions))
# print_action_data_table(goal, start, list(actions))
2024-01-04 22:51:33 +08:00
start_time = time.time()
2024-04-10 19:59:13 +08:00
algo_right = algo.run_algorithm(start, goal, actions)
end_time = time.time()
planning_time_ls.append(end_time - start_time)
planning_time_total += (end_time - start_time)
# print("xxxx")
if algo_right: # 运行算法规划后行为树为algo.bt
# total_tree_size.append(algo.bt.count_size() - 1)
total_tree_size.append(algo.bfs_cal_tree_size())
# if count==10:
# algo.print_solution()
# algo.print_solution() # 打印行为树
# 画出行为树
# if count == 2:
# ptml_string = algo.get_ptml_many_act()
# print(ptml_string)
# file_name = "sub_task"
# file_path = f'{file_name}.ptml'
# with open(file_path, 'w') as file:
# file.write(ptml_string)
# ptml_path = os.path.join(root_path, 'BTExpansionCode/EXP/sub_task.ptml')
# behavior_lib_path = os.path.join(root_path, 'BTExpansionCode/EXP/behavior_lib')
# bt = load_bt_from_ptml(None, ptml_path, behavior_lib_path)
# if bt_algo_opt:
# render_dot_tree(bt.root, target_directory="", name="expanded_bt_obt", png_only=False)
# else:
# render_dot_tree(bt.root, target_directory="", name="expanded_bt_xiaocai", png_only=False)
else:
print("error")
# 开始从初始状态运行行为树,测试
state = start
steps = 0
current_cost = 0
current_tick_time = 0
current_cond_tick_time=0
# val, obj, cost, tick_time = algo.bt.cost_tick(state, 0, 0) # tick行为树obj为所运行的行动
val, obj, cost, tick_time, cond_times = algo.bt.cost_tick_cond(state, 0, 0, 0)
current_cond_tick_time += cond_times
current_tick_time += tick_time
current_cost += cost
while val != 'success' and val != 'failure': # 运行直到行为树成功或失败
state = state_transition(state, obj)
# val, obj, cost, tick_time = algo.bt.cost_tick(state, 0, 0)
val, obj, cost, tick_time, cond_times = algo.bt.cost_tick_cond(state, 0, 0, 0)
current_cond_tick_time += cond_times
current_cost += cost
current_tick_time += tick_time
if (val == 'failure'):
print("bt fails at step", steps)
error = True
break
steps += 1
if (steps >= 500): # 至多运行500步
break
# 检查执行后状态满不满足,只有 goal 里有一个满足就行
error = True
for gg in goal:
if gg<=state:
error = False
success_count += 1
total_steps_num.append(steps)
break
if error:
failure_count += 1
# if not goal[0] <= state: # 错误解,目标条件不在执行后状态满足
# # print ("wrong solution",steps)
# failure_count += 1
# error = True
# else: # 正确解,满足目标条件
# # print ("right solution",steps)
# success_count += 1
# total_steps_num.append(steps)
if error:
# print_action_data_table(goal, start, list(actions))
# algo.print_solution()
break
algo.clear()
total_action_num.append(len(actions))
total_state_num.append(len(states))
total_cost.append(current_cost)
total_tick.append(current_tick_time)
total_cond_tick.append(current_cond_tick_time)
print("success:", success_count, "failure:", failure_count) # 算法成功和失败次数
print("*** Total Tree Size: mean=", round(np.mean(total_tree_size),2), "std=", round(np.std(total_tree_size, ddof=1),2)) # 1000次测试树大小
print("Total Steps Num: mean=", np.mean(total_steps_num), "std=", np.std(total_steps_num, ddof=1))
print("Average Number of States:", np.mean(total_state_num)) # 1000次问题的平均状态数
print("Average Number of Actions", np.mean(total_action_num)) # 1000次问题的平均行动数
print("Planning Time Total:", planning_time_total)
print("*** Planning Time mean=:", round(np.mean(planning_time_ls),3), "std=", round(np.std(planning_time_ls),3))
print("*** Average Number of Ticks", round(np.mean(total_tick),3), "std=", round(np.std(total_tick, ddof=1),3))
print("*** Average Cost of Execution:", round(np.mean(total_cost),3), "std=", round(np.std(total_cost, ddof=1),3))
print("*** Cond Ticks:", round(np.mean(total_cond_tick), 3), "std=", round(np.std(total_cond_tick, ddof=1), 3))
tree_size = [round(np.mean(total_tree_size), 3), round(np.std(total_tree_size, ddof=1), 3)]
ticks = [round(np.mean(total_tick), 3), round(np.std(total_tick, ddof=1), 3)]
cond_ticks = [round(np.mean(total_cond_tick), 3)]
cost = [round(np.mean(total_cost), 3), round(np.std(total_cost, ddof=1), 3)]
plan_time = [round(np.mean(planning_time_ls), 5), round(np.std(planning_time_ls), 5), round(planning_time_total, 5)]
tmp_ls=[]
tmp_ls.extend(tree_size)
tmp_ls.extend(ticks)
tmp_ls.extend(cond_ticks)
tmp_ls.extend(cost)
tmp_ls.extend(plan_time)
return tmp_ls
def BTTest_Merge_easy_medium_hard(bt_algo_opt,goal_states,action_list,start_robowaiter,merge_time=99999):
merge_time = merge_time
if bt_algo_opt:
print("============= OptBT Test ==============")
else:
print("============= XiaoCai BT Test ==============")
total_tree_size = []
total_action_num = []
total_state_num = []
total_steps_num = []
total_cost = []
total_tick = []
success_count = 0
failure_count = 0
planning_time_total = 0.0
planning_time_ls=[]
states=[] ####
actions = copy.deepcopy(action_list)
start = copy.deepcopy(start_robowaiter)
total_cond_tick = []
error=False
total_count = len(goal_states)
for count, goal_str in enumerate(goal_states):
goal = copy.deepcopy(goal_transfer_str(goal_str))
# goal = goal_str
# print("count:", count, "goal:", goal)
if bt_algo_opt:
# if count==874:
# algo = OptBTExpAlgorithm(verbose=False)
# else:
algo = OptBTExpAlgorithm(verbose=False)
else:
algo = BTExpAlgorithm(verbose=False)
algo.clear()
# algo = Weakalgorithm()
# if count == 6 : #874:
2024-01-04 22:51:33 +08:00
# print_action_data_table(goal, start, list(actions))
# print_action_data_table(goal, start, list(actions))
2024-04-10 19:59:13 +08:00
start_time = time.time()
# algo_right = algo.run_algorithm(start, goal, actions)
algo_right = algo.run_algorithm(start, goal, actions, merge_time)
end_time = time.time()
planning_time_ls.append(end_time - start_time)
planning_time_total += (end_time - start_time)
if algo_right: # 运行算法规划后行为树为algo.bt
# total_tree_size.append(algo.bt.count_size() - 1)
total_tree_size.append(algo.bfs_cal_tree_size())
2024-01-05 14:11:54 +08:00
# if count==10:
2024-01-04 22:51:33 +08:00
# algo.print_solution()
# algo.print_solution() # 打印行为树
2024-04-10 19:59:13 +08:00
# 画出行为树
# if count == 2:
# ptml_string = algo.get_ptml_many_act()
# # print(ptml_string)
# file_name = "sub_task"
# file_path = f'{file_name}.ptml'
# with open(file_path, 'w') as file:
# file.write(ptml_string)
# ptml_path = os.path.join(root_path, 'BTExpansionCode/EXP/sub_task.ptml')
# behavior_lib_path = os.path.join(root_path, 'BTExpansionCode/EXP/behavior_lib')
# bt = load_bt_from_ptml(None, ptml_path, behavior_lib_path)
# if bt_algo_opt:
# render_dot_tree(bt.root, target_directory="", name="expanded_bt_obt", png_only=False)
# else:
# render_dot_tree(bt.root, target_directory="", name="expanded_bt_xiaocai", png_only=False)
else:
print("error")
# 开始从初始状态运行行为树,测试
state = start
steps = 0
current_cost = 0
current_tick_time = 0
current_cond_tick_time = 0
val, obj, cost, tick_time,cond_times = algo.bt.cost_tick_cond(state, 0, 0,0) # tick行为树obj为所运行的行动
# val, obj, cost, tick_time = algo.bt.cost_tick(state, 0, 0)
current_tick_time += tick_time
current_cost += cost
current_cond_tick_time+=cond_times
while val != 'success' and val != 'failure': # 运行直到行为树成功或失败
state = state_transition(state, obj)
val, obj, cost, tick_time,cond_times = algo.bt.cost_tick_cond(state, 0, 0,0)
# val, obj, cost, tick_time = algo.bt.cost_tick(state, 0, 0)
current_cost += cost
current_tick_time += tick_time
current_cond_tick_time += cond_times
if (val == 'failure'):
print("bt fails at step", steps)
error = True
break
steps += 1
if (steps >= 500): # 至多运行500步
break
# 检查执行后状态满不满足,只有 goal 里有一个满足就行
error = True
for gg in goal:
if gg<=state:
error = False
success_count += 1
total_steps_num.append(steps)
break
if error:
failure_count += 1
# if not goal[0] <= state: # 错误解,目标条件不在执行后状态满足
# # print ("wrong solution",steps)
# failure_count += 1
# error = True
# else: # 正确解,满足目标条件
# # print ("right solution",steps)
# success_count += 1
# total_steps_num.append(steps)
if error:
# print_action_data_table(goal, start, list(actions))
# algo.print_solution()
break
algo.clear()
total_action_num.append(len(actions))
total_state_num.append(len(states))
total_cost.append(current_cost)
total_tick.append(current_tick_time)
total_cond_tick.append(current_cond_tick_time)
2024-01-04 22:51:33 +08:00
2024-04-10 19:59:13 +08:00
print("success:", success_count, "failure:", failure_count) # 算法成功和失败次数
print("*** Total Tree Size: mean=", round(np.mean(total_tree_size),2), "std=", round(np.std(total_tree_size, ddof=1),2)) # 1000次测试树大小
print("Total Steps Num: mean=", np.mean(total_steps_num), "std=", np.std(total_steps_num, ddof=1))
print("Average Number of States:", np.mean(total_state_num)) # 1000次问题的平均状态数
print("Average Number of Actions", np.mean(total_action_num)) # 1000次问题的平均行动数
print("Planning Time Total:", planning_time_total)
print("*** Planning Time mean=:", round(np.mean(planning_time_ls),3), "std=", round(np.std(planning_time_ls),3))
print("*** Average Number of Ticks", round(np.mean(total_tick),3), "std=", round(np.std(total_tick, ddof=1),3))
print("*** Average Number of Cond Ticks", round(np.mean(total_cond_tick), 3), "std=", round(np.std(total_cond_tick, ddof=1), 3))
print("*** Average Cost of Execution:", round(np.mean(total_cost),3), "std=", round(np.std(total_cost, ddof=1),3))
tree_size = [round(np.mean(total_tree_size), 3), round(np.std(total_tree_size, ddof=1), 3)]
ticks = [round(np.mean(total_tick), 3), round(np.std(total_tick, ddof=1), 3)]
cond_ticks = [round(np.mean(total_cond_tick), 3), round(np.std(total_cond_tick, ddof=1), 3)]
cost = [round(np.mean(total_cost), 3), round(np.std(total_cost, ddof=1), 3)]
plan_time = [round(np.mean(planning_time_ls), 5), round(np.std(planning_time_ls), 5), round(planning_time_total, 5)]
tmp_ls=[]
tmp_ls.extend(tree_size)
tmp_ls.extend(ticks)
tmp_ls.extend(cond_ticks)
tmp_ls.extend(cost)
tmp_ls.extend(plan_time)
return tmp_ls
def BTTest(bt_algo_opt,goal_states,action_list,start_robowaiter):
if bt_algo_opt:
print("============= OptBT Test ==============")
else:
print("============= XiaoCai BT Test ==============")
total_tree_size = []
total_action_num = []
total_state_num = []
total_steps_num = []
total_cost = []
total_tick = []
success_count = 0
failure_count = 0
planning_time_total = 0.0
planning_time_ls=[]
states=[] ####
actions = copy.deepcopy(action_list)
start = copy.deepcopy(start_robowaiter)
error=False
total_count = len(goal_states)
for count, goal_str in enumerate(goal_states):
goal = copy.deepcopy(goal_transfer_str(goal_str))
# goal = goal_str
print("count:", count, "goal:", goal)
if bt_algo_opt:
# if count==874:
# algo = OptBTExpAlgorithm(verbose=False)
# else:
algo = OptBTExpAlgorithm(verbose=True)
else:
algo = BTExpAlgorithm(verbose=False)
algo.clear()
# algo = Weakalgorithm()
# if count == 6 : #874:
# print_action_data_table(goal, start, list(actions))
# print_action_data_table(goal, start, list(actions))
start_time = time.time()
algo_right = algo.run_algorithm(start, goal, actions)
end_time = time.time()
planning_time_ls.append(end_time - start_time)
planning_time_total += (end_time - start_time)
if algo_right: # 运行算法规划后行为树为algo.bt
# total_tree_size.append(algo.bt.count_size() - 1)
total_tree_size.append(algo.bfs_cal_tree_size())
# if count==10:
# algo.print_solution()
algo.print_solution() # 打印行为树
# 画出行为树
# if count == 2:
ptml_string = algo.get_ptml_many_act()
# # print(ptml_string)
file_name = "sub_task"
file_path = f'./EXP/{file_name}.ptml'
with open(file_path, 'w') as file:
file.write(ptml_string)
ptml_path = os.path.join(root_path, 'BTExpansionCode/EXP/sub_task.ptml')
behavior_lib_path = os.path.join(root_path, 'BTExpansionCode/EXP/behavior_lib')
bt = load_bt_from_ptml(None, ptml_path, behavior_lib_path)
if bt_algo_opt:
render_dot_tree(bt.root, target_directory="", name="expanded_bt_obt", png_only=False)
else:
render_dot_tree(bt.root, target_directory="", name="expanded_bt_xiaocai", png_only=False)
else:
print("error")
# 开始从初始状态运行行为树,测试
state = start
steps = 0
current_cost = 0
current_tick_time = 0
val, obj, cost, tick_time = algo.bt.cost_tick(state, 0, 0) # tick行为树obj为所运行的行动
current_tick_time += tick_time
current_cost += cost
while val != 'success' and val != 'failure': # 运行直到行为树成功或失败
state = state_transition(state, obj)
val, obj, cost, tick_time = algo.bt.cost_tick(state, 0, 0)
current_cost += cost
current_tick_time += tick_time
if (val == 'failure'):
print("bt fails at step", steps)
error = True
break
steps += 1
if (steps >= 500): # 至多运行500步
break
if not goal[0] <= state: # 错误解,目标条件不在执行后状态满足
# print ("wrong solution",steps)
failure_count += 1
error = True
else: # 正确解,满足目标条件
# print ("right solution",steps)
success_count += 1
total_steps_num.append(steps)
if error:
# print_action_data_table(goal, start, list(actions))
# algo.print_solution()
break
algo.clear()
total_action_num.append(len(actions))
total_state_num.append(len(states))
total_cost.append(current_cost)
total_tick.append(current_tick_time)
print("success:", success_count, "failure:", failure_count) # 算法成功和失败次数
print("*** Total Tree Size: mean=", round(np.mean(total_tree_size),2), "std=", round(np.std(total_tree_size, ddof=1),2)) # 1000次测试树大小
print("Total Steps Num: mean=", np.mean(total_steps_num), "std=", np.std(total_steps_num, ddof=1))
print("Average Number of States:", np.mean(total_state_num)) # 1000次问题的平均状态数
print("Average Number of Actions", np.mean(total_action_num)) # 1000次问题的平均行动数
print("Planning Time Total:", planning_time_total)
print("*** Planning Time mean=:", round(np.mean(planning_time_ls),3), "std=", round(np.std(planning_time_ls),3))
print("*** Average Number of Ticks", round(np.mean(total_tick),3), "std=", round(np.std(total_tick, ddof=1),3))
print("*** Average Cost of Execution:", round(np.mean(total_cost),3), "std=", round(np.std(total_cost, ddof=1),3))
def BTTest_Merge(bt_algo_opt,goal_states,action_list,start_robowaiter,merge_time=3):
total_tree_size = []
total_action_num = []
total_state_num = []
total_steps_num = []
total_cost = []
total_tick = []
success_count = 0
failure_count = 0
planning_time_total = 0.0
planning_time_ls=[]
states=[] ####
actions = copy.deepcopy(action_list)
start = copy.deepcopy(start_robowaiter)
error=False
total_count = len(goal_states)
total_time_dic={}
for count, goal_str in enumerate(goal_states):
goal = copy.deepcopy(goal_transfer_str(goal_str))
if bt_algo_opt:
algo = OptBTExpAlgorithm(verbose=False)
else:
algo = BTExpAlgorithm(verbose=False)
algo.clear()
start_time = time.time()
algo_right,time_dic = algo.run_algorithm(start, goal, actions,merge_time)
end_time = time.time()
planning_time_ls.append(end_time - start_time)
planning_time_total += (end_time - start_time)
if algo_right: # 运行算法规划后行为树为algo.bt
# total_tree_size.append(algo.bt.count_size() - 1)
total_tree_size.append(algo.bfs_cal_tree_size())
# if count==10:
# algo.print_solution()
# algo.print_solution() # 打印行为树
2024-01-04 22:51:33 +08:00
# 画出行为树
2024-04-10 19:59:13 +08:00
# if count == 2:
2024-01-05 14:11:54 +08:00
# ptml_string = algo.get_ptml_many_act()
# file_name = "sub_task"
# file_path = f'./{file_name}.ptml'
# with open(file_path, 'w') as file:
# file.write(ptml_string)
# ptml_path = os.path.join(root_path, 'BTExpansionCode/EXP/sub_task.ptml')
# behavior_lib_path = os.path.join(root_path, 'BTExpansionCode/EXP/behavior_lib')
# bt = load_bt_from_ptml(None, ptml_path, behavior_lib_path)
# if bt_algo_opt:
# render_dot_tree(bt.root, target_directory="", name="expanded_bt_obt", png_only=False)
# else:
# render_dot_tree(bt.root, target_directory="", name="expanded_bt_xiaocai", png_only=False)
2024-01-04 22:51:33 +08:00
else:
print("error")
# 开始从初始状态运行行为树,测试
state = start
steps = 0
current_cost = 0
current_tick_time = 0
val, obj, cost, tick_time = algo.bt.cost_tick(state, 0, 0) # tick行为树obj为所运行的行动
current_tick_time += tick_time
current_cost += cost
while val != 'success' and val != 'failure': # 运行直到行为树成功或失败
state = state_transition(state, obj)
val, obj, cost, tick_time = algo.bt.cost_tick(state, 0, 0)
current_cost += cost
current_tick_time += tick_time
if (val == 'failure'):
print("bt fails at step", steps)
error = True
break
steps += 1
if (steps >= 500): # 至多运行500步
break
if not goal[0] <= state: # 错误解,目标条件不在执行后状态满足
# print ("wrong solution",steps)
failure_count += 1
error = True
else: # 正确解,满足目标条件
# print ("right solution",steps)
success_count += 1
total_steps_num.append(steps)
if error:
print_action_data_table(goal, start, list(actions))
algo.print_solution()
break
algo.clear()
total_action_num.append(len(actions))
total_state_num.append(len(states))
total_cost.append(current_cost)
total_tick.append(current_tick_time)
2024-04-10 19:59:13 +08:00
# print("success:", success_count, "failure:", failure_count) # 算法成功和失败次数
# print("*** Total Tree Size: mean=", round(np.mean(total_tree_size),2), "std=", round(np.std(total_tree_size, ddof=1),2)) # 1000次测试树大小
# print("Total Steps Num: mean=", np.mean(total_steps_num), "std=", np.std(total_steps_num, ddof=1))
# print("Average Number of States:", np.mean(total_state_num)) # 1000次问题的平均状态数
# print("Average Number of Actions", np.mean(total_action_num)) # 1000次问题的平均行动数
# print("Planning Time Total:", planning_time_total)
# print("*** Planning Time mean=:", round(np.mean(planning_time_ls),3), "std=", round(np.std(planning_time_ls),3))
# print("*** Average Number of Ticks", round(np.mean(total_tick),3), "std=", round(np.std(total_tick, ddof=1),3))
# print("*** Average Cost of Execution:", round(np.mean(total_cost),3), "std=", round(np.std(total_cost, ddof=1),3))
tree_size=[round(np.mean(total_tree_size),2), round(np.std(total_tree_size, ddof=1),2)]
plan_time=[round(np.mean(planning_time_ls),3), round(np.std(planning_time_ls),3),round(planning_time_total,3)]
ticks=[round(np.mean(total_tick),3),round(np.std(total_tick, ddof=1),3)]
cost=[round(np.mean(total_cost),3),round(np.std(total_cost, ddof=1),3)]
return tree_size,plan_time,ticks,cost
def get_act_start_goal(seed=1, literals_num=10, depth=10, iters=10, total_count=1000):
max_copy_time = 5
literals_num_set = {i for i in range(literals_num)}
act_list = []
start_list = []
goal_list = []
total_action_num=[]
total_state_num=[]
total_time_dic = {"start_to_goal": 0,
"random_act":0}
start_time_0=time.time()
for count in range(total_count):
# 生成一个规划问题,包括随机的状态和行动,以及目标状态
action_num = 1
states = []
actions = []
start = generate_random_state(literals_num)
state_set = {i for i in range(literals_num)}
state = copy.deepcopy(start)
states.append(state)
# for i in range(0, depth):
# a = Action()
# a.generate_from_state_local(state, literals_num_set)
# a.cost = random.randint(1, 100)
# if not a in actions:
# a.name = "a" + str(action_num)
# action_num += 1
# actions.append(a)
# state = state_transition(state, a)
# if state in states:
# pass
# else:
# states.append(state)
# # print(state)
# goal = states[-1]
# k_act_total = int(iters*np.random.uniform()/depth)
# if k_act_total<1:
# k_act_total = random.randint(1, 5)
# for k in range(k_act_total):
for i in range(0, depth):
start_time = time.time()
a = Action()
a.generate_from_state_local(state, literals_num_set)
a.cost = random.randint(1, 100)
if not a in actions:
a.name = "a" + str(action_num)
action_num += 1
actions.append(a)
copy_times = random.randint(1, max_copy_time)
# copy_times = 0
for ct in range(copy_times):
ca = copy.deepcopy(a)
ca.cost = random.randint(1, 100)
if not ca in actions:
ca.name = "a" + str(action_num)
action_num += 1
actions.append(ca)
end_time = time.time()
total_time_dic["start_to_goal"] += end_time - start_time
state = state_transition(state, a)
if state in states:
pass
else:
states.append(state)
if not goal <= states[-1]:
a = Action()
pre_num = random.randint(0, len(states[-1]))
a.pre = set(random.sample(states[-1], pre_num))
a.add = goal - states[-1]
def_set = state_set - goal
def_num = random.randint(0, len(def_set))
a.del_set = set(random.sample(def_set, def_num))
a.cost = random.randint(1, 100)
a.name = "a" + str(action_num)
action_num += 1
actions.append(a)
# copy_times = random.randint(1, 5)
copy_times = 0
for ct in range(copy_times):
ca = copy.deepcopy(a)
ca.cost = random.randint(1, 100)
if not ca in actions:
ca.name = "a" + str(action_num)
action_num += 1
actions.append(ca)
state = copy.deepcopy(start)
last_act_total=iters+2*depth-len(actions)
if last_act_total<0:
last_act_total = 0
for i in range(last_act_total):
a = Action()
start_time = time.time()
a.generate_from_state_local(state, literals_num_set)
end_time = time.time()
total_time_dic["random_act"] += end_time - start_time
if not a in actions:
a.name = "a" + str(action_num)
action_num += 1
actions.append(a)
state = state_transition(state, a)
if state in states:
pass
else:
states.append(state)
state = random.sample(states, 1)[0]
act_list.append(actions)
start_list.append(start)
goal_list.append(goal)
total_action_num.append(len(actions))
total_state_num.append(len(states))
end_time_0=time.time()
print("Total Time:", end_time_0-start_time_0)
print("Total Time (start_to_goal):", total_time_dic["start_to_goal"])
print("Total Time (random_act):",total_time_dic["random_act"])
print("Average Number of States:", round(np.mean(total_state_num),3)) # 1000次问题的平均状态数
print("Average Number of Actions", round(np.mean(total_action_num),3)) # 1000次问题的平均行动数
# print_action_data_table(goal, start, list(actions))
return act_list, start_list, goal_list,round(np.mean(total_state_num),3),round(np.mean(total_action_num),3)
2024-01-04 22:51:33 +08:00