EmbodiedAI
/
RoboWaiter
mirror of https://github.com/HPCL-EI/RoboWaiter.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

更新了行为树扩展算法的合并节点算法

This commit is contained in:
Caiyishuai 2023-12-14 15:28:31 +08:00
parent a5978ff37a
commit 9322bd1c7c
10 changed files with 9826 additions and 244 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
robowaiter/behavior_lib/act/FreeHands.py
View File

@ -1,35 +0,0 @@
import py_trees as ptree
from typing import Any
from robowaiter.behavior_lib._base.Act import Act
from robowaiter.behavior_lib._base.Behavior import Status
class FreeHands(Act):
can_be_expanded = True
num_args = 0
valid_args = set()
def __init__(self, *args):
super().__init__(*args)
@classmethod
def get_info(cls):
info = {}
# info["pre"]= set()
info["pre"] ={f'Holding(Nothing)'}
info['add'] = {f'Holding(Nothing)'}
info['del_set'] = {f'Holding({obj})' for obj in cls.all_object}
info['cost'] = 0
return info
def _update(self) -> ptree.common.Status:
if self.scene.show_ui:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio,update_info_count=1)
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]
return Status.RUNNING

2
robowaiter/behavior_lib/act/PickUp.py
View File

@ -21,7 +21,7 @@ class PickUp(Act):
info["del_set"] = {f'Holding(Nothing)'}
for place in cls.valid_args:
info["del_set"] |= {f'On({arg},{place})'}
info['cost'] = 1
info['cost'] = 2
# if arg != 'Anything':
# info['cost'] = 1

54
robowaiter/behavior_lib/act/PutDown.py
View File

@ -8,7 +8,9 @@ class PutDown(Act):
can_be_expanded = True
num_args = 2
valid_args = tuple(itertools.product(Act.all_object, Act.tables_for_placement))
valid_args = list(itertools.product(Act.all_object, Act.tables_for_placement))
valid_args.append(('Anything','Anywhere'))
valid_args = tuple(valid_args)
def __init__(self, *args):
super().__init__(*args)
@ -18,45 +20,45 @@ class PutDown(Act):
@classmethod
def get_info(cls,*arg):
info = {}
info["pre"] = {f'Holding({arg[0]})',f'At(Robot,{arg[1]})'}
info["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'}
info["del_set"] = {f'Holding({arg[0]})'}
if arg[0] != 'Anything':
info = {}
info["pre"] = {f'Holding({arg[0]})',f'At(Robot,{arg[1]})'}
info["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'}
info["del_set"] = {f'Holding({arg[0]})'}
info['cost'] = 3
else:
info = {}
info["pre"] = set()
info['add'] = {f'Holding(Nothing)'}
info['del_set'] = {f'Holding({obj})' for obj in cls.all_object}
info['cost'] = 0
info['cost'] = 1
# if arg[0]!='Anything':
# info['cost'] = 1
# else:
# info['cost'] = 0
# info["pre"] = {}
# info["add"] = {f'Holding(Nothing)'}
# info["del_set"] = {f'Holding({obj})' for obj in cls.valid_args if obj[0] != arg}
return info
def _update(self) -> ptree.common.Status:
# self.scene.test_move()
op_type=17
release_pos = list(Act.place_xyz_dic[self.target_place])
# # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 70.0], 水杯桌:[-70.0, 500.0, 107]
# # 桌子2:[-55.0, 0.0, 107],桌子3:[-55.0, 150.0, 107]
if Act.num_of_obj_on_place[self.target_place]>=1:
release_pos[1] += 25
if self.target_obj != 'Anything':
op_type=17
release_pos = list(Act.place_xyz_dic[self.target_place])
# # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 70.0], 水杯桌:[-70.0, 500.0, 107]
# # 桌子2:[-55.0, 0.0, 107],桌子3:[-55.0, 150.0, 107]
if Act.num_of_obj_on_place[self.target_place]>=1:
release_pos[1] += 25
Act.num_of_obj_on_place[self.target_place]+=1
Act.num_of_obj_on_place[self.target_place]+=1
self.scene.move_task_area(op_type, release_pos=release_pos)
self.scene.move_task_area(op_type, release_pos=release_pos)
if self.target_obj == "Chips":
release_pos[2] +=3
self.scene.op_task_execute(op_type, release_pos=release_pos)
if self.target_obj == "Chips":
release_pos[2] +=3
self.scene.op_task_execute(op_type, release_pos=release_pos)
if self.scene.show_ui:
self.scene.get_obstacle_point(self.scene.db, self.status, map_ratio=self.scene.map_ratio,update_info_count=1)
self.scene.state["condition_set"] |= (self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]
# print("After PutDown condition_set:",self.scene.state["condition_set"])
return Status.RUNNING

2
robowaiter/behavior_tree/obtea/BehaviorTree.py
View File

@ -1,7 +1,7 @@
#叶结点
class Leaf:
def __init__(self,type,content,mincost):
def __init__(self,type,content,mincost=0):
self.type=type
self.content=content #conditionset or action
self.parent=None

123
robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py
View File

@ -57,12 +57,19 @@ def state_transition(state,action):
def conflict(c):
have_at = False
have_holding = False
for str in c:
if 'At' in str:
if 'At' in str:
if not have_at:
have_at = True
else:
return True
if 'Holding' in str:
if not have_holding:
have_holding = True
else:
return True
return False
@ -76,9 +83,11 @@ class OptBTExpAlgorithm:
self.conditions=[]
self.conditions_index=[]
self.verbose=verbose
self.goal=None
def clear(self):
self.bt = None
self.goal = None
self.nodes = []
self.traversed = [] #存cond
self.expanded = [] #存整个
@ -89,7 +98,7 @@ class OptBTExpAlgorithm:
# def run_algorithm(self,goal,actions,scene):
def run_algorithm(self, start, goal, actions):
# self.scene = scene
self.goal = goal
if self.verbose:
print("\n算法开始！")
@ -160,6 +169,7 @@ class OptBTExpAlgorithm:
# 增加实时条件判断，满足条件就不再扩展
# if c <= self.scene.state["condition_set"]:
if c <= start:
self.merge_adjacent_conditions_stack()
return True
else:
subtree.add_child([copy.deepcopy(pair_node.act_leaf)])
@ -177,10 +187,6 @@ class OptBTExpAlgorithm:
for i in range(0, len(actions)):
if actions[i].name == 'FreeHands()':
kk=1
if not c & ((actions[i].pre | actions[i].add) - actions[i].del_set) <= set():
if (c - actions[i].del_set) == c:
if self.verbose:
@ -209,11 +215,114 @@ class OptBTExpAlgorithm:
# 把符合条件的动作节点都放到列表里
if self.verbose:
print("———— -- %s 符合条件放入列表,对应的c为 %s" % (actions[i].name,c_attr))
self.merge_adjacent_conditions_stack()
if self.verbose:
print("算法结束！\n")
return True
def merge_adjacent_conditions_stack(self):
# 递归合并
bt = ControlBT(type='cond')
sbtree = ControlBT(type='?')
gc_node = Leaf(type='cond', content=self.goal, mincost=0) # 为了统一，都成对出现
sbtree.add_child([copy.deepcopy(gc_node)]) # 子树首先保留所扩展结
bt.add_child([sbtree])
parnode = copy.deepcopy(self.bt.children[0])
stack=[]
for child in parnode.children:
if isinstance(child, ControlBT) and child.type == '>':
if stack==[]:
stack.append(child)
continue
# 检查合并的条件，前面一个的条件包含了后面的条件，把包含部分提取出来
last_child = stack[-1]
set1 = last_child.children[0].content
set2 = child.children[0].content
if set1>=set2:
inter = set1 & set2
dif = set1 - set2
tmp_sub_seq = ControlBT(type='>')
c2 = Leaf(type='cond', content=dif)
a1 = copy.deepcopy(last_child.children[1])
tmp_sub_seq.add_child(
[copy.deepcopy(c2), copy.deepcopy(a1)])
tmp_sub_tree_sel = ControlBT(type='?')
a2 = copy.deepcopy(child.children[1])
tmp_sub_tree_sel.add_child(
[copy.deepcopy(tmp_sub_seq), copy.deepcopy(a2)])
tmp_tree = ControlBT(type='>')
c1 = Leaf(type='cond', content=inter)
tmp_tree.add_child(
[copy.deepcopy(c1), copy.deepcopy(tmp_sub_tree_sel)])
stack.pop()
stack.append(tmp_tree)
else:
stack.append(child)
for tree in stack:
sbtree.add_child([tree])
self.bt = copy.deepcopy(bt)
def merge_cond_node(self):
# bt合并====================================================
bt = ControlBT(type='cond')
sbtree = ControlBT(type='?')
gc_node = Leaf(type='cond', content=self.goal, mincost=0) # 为了统一，都成对出现
sbtree.add_child([copy.deepcopy(gc_node)]) # 子树首先保留所扩展结
bt.add_child([sbtree])
parnode = copy.deepcopy(self.bt.children[0])
skip_next = False
for i in range(len(parnode.children) - 1):
current_child = parnode.children[i]
next_child = parnode.children[i + 1]
if isinstance(current_child, ControlBT) and current_child.type == '>':
if not skip_next:
# 检查合并的条件，前面一个的条件包含了后面的条件，把包含部分提取出来
set1 = current_child.children[0].content
set2 = next_child.children[0].content
if set1>=set2:
inter = set1 & set2
dif = set1 - set2
tmp_sub_seq = ControlBT(type='>')
c2 = Leaf(type='cond', content=dif)
a1 = Leaf(type='act', content=current_child.children[1].content)
tmp_sub_seq.add_child(
[copy.deepcopy(c2), copy.deepcopy(a1)])
tmp_sub_tree_sel = ControlBT(type='?')
a2 = Leaf(type='act', content=next_child.children[1].content)
tmp_sub_tree_sel.add_child(
[copy.deepcopy(tmp_sub_seq), copy.deepcopy(a2)])
tmp_tree = ControlBT(type='>')
c1 = Leaf(type='cond', content=inter)
tmp_tree.add_child(
[copy.deepcopy(c1), copy.deepcopy(tmp_sub_tree_sel)])
sbtree.add_child([tmp_tree])
skip_next = True
elif skip_next:
skip_next = False
self.bt = copy.deepcopy(bt)
# bt合并====================================================
def print_solution(self):
print("========= BT ==========") # 树的bfs遍历
nodes_ls = []

1420
robowaiter/scene/outputs/expanded_bt.dot
View File

File diff suppressed because it is too large Load Diff

BIN
robowaiter/scene/outputs/expanded_bt.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 84 KiB

After

Width:  |  Height:  |  Size: 484 KiB

8428
robowaiter/scene/outputs/expanded_bt.svg
View File

File diff suppressed because it is too large Load Diff
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 14 KiB

After

Width:  |  Height:  |  Size: 638 KiB

2
robowaiter/scene/scene.py
View File

@ -274,7 +274,7 @@ class Scene:
self.is_nav_walk = False
self.bt_algo_opt = True
self.bt_algo_opt = False
file_name = os.path.join(root_path,'robowaiter/algos/navigator/map_5.pkl')
if os.path.exists(file_name):

4
tasks_no_ui/BtTest/Open_tasks.py
View File

@ -24,8 +24,8 @@ class SceneOT(Scene):
# ]
self.signal_event_list =[
# (3, self.set_goal,("On(Yogurt,Bar)",)),
# (3, self.customer_say, ("System", "把盒装冰红茶放到水桌")),
(3, self.customer_say, ("System", "打开空调并降低空调温度")),
(3, self.customer_say, ("System", "把盒装冰红茶放到水桌")),
# (3, self.customer_say, ("System", "打开空调并降低空调温度")),
]
def _reset(self):