From 0284830b0acdc6f55133fe1c4be3ec284ff9b63f Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Mon, 13 Nov 2023 22:14:55 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E9=83=A8?= =?UTF-8?q?=E5=88=86=E5=8A=A8=E4=BD=9C=E8=8A=82=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Act.py | 4 ++++ robowaiter/behavior_lib/_base/Behavior.py | 1 + robowaiter/behavior_lib/act/Make.py | 17 +++++++++++------ robowaiter/behavior_lib/act/MakeCoffee.py | 23 ----------------------- robowaiter/behavior_lib/act/MoveTo.py | 12 +++++------- robowaiter/behavior_lib/act/PourWater.py | 23 ----------------------- robowaiter/scene/scene.py | 5 +++-- robowaiter/scene/tasks/VLM.py | 10 +++++----- 8 files changed, 29 insertions(+), 66 deletions(-) delete mode 100644 robowaiter/behavior_lib/act/MakeCoffee.py delete mode 100644 robowaiter/behavior_lib/act/PourWater.py diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py index ee948d5..d2e95e5 100644 --- a/robowaiter/behavior_lib/_base/Act.py +++ b/robowaiter/behavior_lib/_base/Act.py @@ -3,9 +3,13 @@ from robowaiter.behavior_lib._base.Behavior import Bahavior class Act(Bahavior): print_name_prefix = "act " type = 'Act' + all_place = {'Bar', 'WaterTable', 'CoffeeTable', 'Bar2', 'Table1', 'Table2', 'Table3'} + all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', + 'VacuumCup'} def __init__(self,*args): super().__init__(*args) + self.info = self.get_info(*args) @classmethod def get_info(self,*arg): diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 84ed74b..b80c634 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -35,6 +35,7 @@ class Bahavior(ptree.behaviour.Behaviour): # self.valid_arg_list.append((x.strip for x in line.split(","))) self.args = args + super().__init__(self.name) def _update(self) -> ptree.common.Status: diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 8388cc2..ecb99f6 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -7,7 +7,7 @@ class Make(Act): can_be_expanded = True num_args = 1 valid_args = ( - "Coffee", + "Coffee","Water","Dessert" ) def __init__(self, *args): @@ -17,15 +17,20 @@ class Make(Act): @classmethod def get_info(cls,arg): - info = None + info = {} + info["pre"]= {f'Holding(Nothing)'} + info['del'] = set() if arg == "Coffee": - info = { - "add": {f'On(Coffee,Table)'}, - } + info["add"]= {f'On(Coffee,CoffeeTable)'} + elif arg == "Water": + info["add"] = {f'On(Water,WaterTable)'} + elif arg == "Dessert": + info["add"] = {f'On(Dessert,Bar)'} return info def _update(self) -> ptree.common.Status: op_type = 1 self.scene.move_task_area(op_type) self.scene.op_task_execute(op_type) - self.scene.state["condition_set"].add(self.add) + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/MakeCoffee.py b/robowaiter/behavior_lib/act/MakeCoffee.py deleted file mode 100644 index b14727f..0000000 --- a/robowaiter/behavior_lib/act/MakeCoffee.py +++ /dev/null @@ -1,23 +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 MakeCoffee(Act): - - def __init__(self, *args): - super().__init__(*args) - - @property - def cond_sets(self): - pre = {"At(Robot,CoffeeMachine)","NotHolding"} - add = {"At(Coffee,Bar)"} - de = {} - return pre,add,de - - def _update(self) -> ptree.common.Status: - op_type = 1 - self.scene.move_task_area(op_type) - self.scene.op_task_execute(op_type) - self.scene.state["condition_set"].add(self.add) - return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index 000869d..2a1a78a 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -6,10 +6,8 @@ from robowaiter.algos.navigate.DstarLite.navigate import Navigator class MoveTo(Act): can_be_expanded = True num_args = 1 - valid_args = ( - "Bar", - "Table", - ) + valid_args = Act.all_object | Act.all_place + valid_args.add('Customer') def __init__(self, target_place): super().__init__(target_place) @@ -18,9 +16,9 @@ class MoveTo(Act): @classmethod def get_info(self,arg): - info = { - "add": {f'At(Robot,{arg})'}, - } + info = {} + info["add"] = {f'At(Robot,{arg})'} + info["del"] = {f'At(Robot,{place})' for place in self.valid_args if place != arg} return info diff --git a/robowaiter/behavior_lib/act/PourWater.py b/robowaiter/behavior_lib/act/PourWater.py deleted file mode 100644 index 23db1fe..0000000 --- a/robowaiter/behavior_lib/act/PourWater.py +++ /dev/null @@ -1,23 +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 PourWater(Act): - - def __init__(self, *args): - super().__init__(*args) - - @property - def cond_sets(self): - self.pre = {"At(Robot,WaterTable)","NotHolding"} - self.add = {"On(Water,WaterTable)"} - self.de = {} - return self.pre,self.add,self.de - - def _update(self) -> ptree.common.Status: - op_type = 2 - self.scene.move_task_area(op_type) - self.scene.op_task_execute(op_type) - self.scene.state["condition_set"].update(self.add) - return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index b3819b1..b0db6e9 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -409,10 +409,11 @@ class Scene: return True def gen_obj(self,h=100): - # 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 + # 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 14:cake 61:甜牛奶 + type= 9 #9 scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z] - obj_list = [GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z = h, roll=0, pitch=0, yaw=0, type=9)] + obj_list = [GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z = h, roll=0, pitch=0, yaw=0, type=type)] scene = stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID)) time.sleep(1.0) diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index 9b4c5cb..d8e646f 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -11,14 +11,14 @@ class SceneVLM(Scene): super().__init__(robot) # 在这里加入场景中发生的事件， (事件发生的时间，事件函数) self.event_list = [ - # (5, self.create_chat_event("测试VLM：做一杯咖啡")), - (5, self.create_chat_event("测试VLM：倒一杯水")), + (5, self.create_chat_event("测试VLM：做一杯咖啡")), + # (5, self.create_chat_event("测试VLM：倒一杯水")), ] def _reset(self): pass - def _run(self, op_type=2): + def _run(self, op_type=7): # 共17个操作 # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 @@ -30,8 +30,8 @@ class SceneVLM(Scene): # self.gen_obj() # self.op_task_execute(op_type, obj_id=0) # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 70.0], 水杯桌:[-70.0, 500.0, 107] - # # 桌子1:[-55.0, 0.0, 107],桌子1:[-55.0, 150.0, 107] - # elif op_type == 17: self.op_task_execute(op_type, release_pos=[-55.0, 150.0, 107]) + # # 桌子1:[-55.0, 0.0, 107],桌子2:[-55.0, 150.0, 107] + # elif op_type == 17: self.op_task_execute(op_type, release_pos=[247.0, 520.0, 100.0])#[-55.0, 150.0, 107] # else: # self.move_task_area(op_type) # self.op_task_execute(op_type) From 075f9ca6fe2b2569a433eb58eef137c70623226b Mon Sep 17 00:00:00 2001 From: wuziji <2193177243@qq.com> Date: Mon, 13 Nov 2023 23:06:22 +0800 Subject: [PATCH 2/3] =?UTF-8?q?New:=20=E6=96=B0=E5=A2=9E=E4=BA=86=E8=87=AA?= =?UTF-8?q?=E5=8A=A8=E7=94=9F=E6=88=90bt-expansion=E7=9B=AE=E6=A0=87?= =?UTF-8?q?=E7=8A=B6=E6=80=81=E7=9A=84=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_tree/dataset/__init__.py | 0 .../dataset/goal_states_generation.py | 66 +++++++++++++++++++ 2 files changed, 66 insertions(+) create mode 100644 robowaiter/behavior_tree/dataset/__init__.py create mode 100644 robowaiter/behavior_tree/dataset/goal_states_generation.py diff --git a/robowaiter/behavior_tree/dataset/__init__.py b/robowaiter/behavior_tree/dataset/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/robowaiter/behavior_tree/dataset/goal_states_generation.py b/robowaiter/behavior_tree/dataset/goal_states_generation.py new file mode 100644 index 0000000..550d86f --- /dev/null +++ b/robowaiter/behavior_tree/dataset/goal_states_generation.py @@ -0,0 +1,66 @@ +# the empty string '' represents robot holds nothing +Object = ['Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', + 'VacuumCup', ''] + +Place = ['Bar', 'WaterTable', 'CoffeeTable', 'Bar2', 'Table1', 'Table2', 'Table3'] + +Entity = ['Robot', 'Customer'] + +Operable = ['AC', 'ACTemperature', 'HallLight', 'TubeLight', 'Curtain'] + +import random + + +def single_predict_generation(oplist_1, oplist_2, predict_pattern) -> str: + index_1 = random.randint(0, len(oplist_1) - 1) + if oplist_2: + index_2 = random.randint(0, len(oplist_2) - 1) + + match predict_pattern: + case 'at': + return f'At({oplist_1[index_1]}, {oplist_2[index_2]})' + case 'is': + return f'Is({oplist_1[index_1]}, {oplist_2[index_2]})' + case 'hold': + return f'Holding({oplist_1[index_1]})' + case 'on': + return f'On({oplist_1[index_1]}, {oplist_2[index_2]})' + case _: + raise RuntimeError('Incorrect predict pattern!') + + +def generate_goal_states(vln_num: int, vlm_num: int, opentask_num: int): + # res stores lists of sets, while each state represent in set. + res = [] + + # goal states for VLN + for i in range(vln_num): + res.append({single_predict_generation(['Robot'], Place, 'at')}) + + # goal states for VLM + for i in range(int(vlm_num)): + for j in range(int(vlm_num)): + res.append( + { + single_predict_generation(['Robot'], Place, 'at'), + single_predict_generation(Operable, ['0', '1'], 'is') + } + ) + + # goal states for Open-task-1 + for i in range(int(opentask_num)): + for j in range(int(opentask_num)): + res.append( + { + single_predict_generation(['Robot'], Place, 'at'), + single_predict_generation(Object, Place, 'on') + } + ) + + # print(res) + # print(len(res)) + + return res + + +generate_goal_states(30, 6, 6) From 31615f32cb272898aecb65ac346ccdf9b357bde1 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Mon, 13 Nov 2023 23:36:12 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E5=8A=A8?= =?UTF-8?q?=E4=BD=9C=E8=8A=82=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Act.py | 12 ++++- robowaiter/behavior_lib/act/Clean.py | 47 +++++++++++++++++++ robowaiter/behavior_lib/act/Make.py | 14 ++++-- robowaiter/behavior_lib/act/PickUp.py | 34 ++++++++++++++ robowaiter/behavior_lib/act/PutDown.py | 35 ++++++++++++++ robowaiter/behavior_lib/act/Turn.py | 64 ++++++++++++++++++++++++++ 6 files changed, 201 insertions(+), 5 deletions(-) create mode 100644 robowaiter/behavior_lib/act/Clean.py create mode 100644 robowaiter/behavior_lib/act/PickUp.py create mode 100644 robowaiter/behavior_lib/act/PutDown.py create mode 100644 robowaiter/behavior_lib/act/Turn.py diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py index d2e95e5..d2b14fb 100644 --- a/robowaiter/behavior_lib/_base/Act.py +++ b/robowaiter/behavior_lib/_base/Act.py @@ -3,10 +3,18 @@ from robowaiter.behavior_lib._base.Behavior import Bahavior class Act(Bahavior): print_name_prefix = "act " type = 'Act' - all_place = {'Bar', 'WaterTable', 'CoffeeTable', 'Bar2', 'Table1', 'Table2', 'Table3'} + all_place = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3'} all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', 'VacuumCup'} - + place_xyz_dic={ + 'Bar': (247.0, 520.0, 100.0), + 'Bar2': (240.0, 40.0, 70.0), + 'WaterTable':(-70.0, 500.0, 107), + 'CoffeeTable':(247.0, 520.0, 100.0), # 位置需要更改！！！ + 'Table1': (247.0, 520.0, 100.0),# 位置需要更改！！！ + 'Table2': (-55.0, 0.0, 107), + 'Table3':(-55.0, 150.0, 107) + } def __init__(self,*args): super().__init__(*args) self.info = self.get_info(*args) diff --git a/robowaiter/behavior_lib/act/Clean.py b/robowaiter/behavior_lib/act/Clean.py new file mode 100644 index 0000000..81c7af1 --- /dev/null +++ b/robowaiter/behavior_lib/act/Clean.py @@ -0,0 +1,47 @@ +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 Clean(Act): + can_be_expanded = True + num_args = 1 + valid_args = ( + 'Table1','Floor','Chairs' + ) + + def __init__(self, *args): + super().__init__(*args) + self.target_obj = self.args[0] + self.op_type = 5 + if self.target_obj=="Table1": + self.op_type = 5 + elif self.target_obj=="Floor": + self.op_type = 4 + elif self.target_obj=="Chairs": + self.op_type = 7 + + + @classmethod + def get_info(cls,arg): + info = {} + info["pre"]= {f'Holding(Nothing)'} + if arg == "Table1": + info["add"]= {f'Is(Table1,Clean)'} + info["del"] = {f'Is(Table1,Dirty)'} + elif arg == "Floor": + info["add"] = {f'Is(Floor,Clean)'} + info["del"] = {f'Is(Floor,Dirty)'} + elif arg == "Chairs": + info["add"] = {f'Is(Chairs,Clean)'} + info["del"] = {f'Is(Chairs,Dirty)'} + return info + + def _update(self) -> ptree.common.Status: + + self.scene.move_task_area(self.op_type) + self.scene.op_task_execute(self.op_type) + + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] + return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index ade04b1..ef97eb9 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -13,6 +13,13 @@ class Make(Act): def __init__(self, *args): super().__init__(*args) self.target_obj = self.args[0] + self.op_type = 1 + if self.target_obj=="Coffee": + self.op_type = 1 + elif self.target_obj=="Water": + self.op_type = 2 + elif self.target_obj=="Dessert": + self.op_type = 3 @classmethod @@ -29,9 +36,10 @@ class Make(Act): return info def _update(self) -> ptree.common.Status: - op_type = 1 - self.scene.move_task_area(op_type) - self.scene.op_task_execute(op_type) + + self.scene.move_task_area(self.op_type) + self.scene.op_task_execute(self.op_type) + self.scene.state["condition_set"].union(self.info["add"]) self.scene.state["condition_set"] -= self.info["del"] return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/PickUp.py b/robowaiter/behavior_lib/act/PickUp.py new file mode 100644 index 0000000..2e3f0f1 --- /dev/null +++ b/robowaiter/behavior_lib/act/PickUp.py @@ -0,0 +1,34 @@ +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 PickUp(Act): + can_be_expanded = True + num_args = 1 + + def __init__(self, *args): + super().__init__(*args) + self.target_obj = self.args[0] + + + @classmethod + def get_info(self,arg): + info = {} + info["pre"] = {f'At(Robot,{arg})','Holding(Nothing)'} + info["add"] = {f'Holding({arg})'} + info["del"] = {f'Holding(Nothing)'} + return info + + + def _update(self) -> ptree.common.Status: + # self.scene.test_move() + op_type=16 + obj_id = 0 + # 遍历场景里的所有物品，根据名字匹配位置最近的 obj-id + + self.scene.op_task_execute(op_type, obj_id=obj_id) + + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] + return Status.RUNNING diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py new file mode 100644 index 0000000..0fe363c --- /dev/null +++ b/robowaiter/behavior_lib/act/PutDown.py @@ -0,0 +1,35 @@ +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 PutDown(Act): + can_be_expanded = True + num_args = 1 + + def __init__(self, *args): + super().__init__(*args) + self.target_obj = self.args[0] + self.target_place = self.args[1] + + + @classmethod + def get_info(self,arg): + info = {} + info["pre"] = {f'Holding({arg[0]})',f'At(Robot,{arg[1]})'} + info["add"] = {f'Holding(Nothing)',f'At({arg[0]},{arg[1]})'} + info["del"] = {f'Holding(Nothing)'} + 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] + self.scene.op_task_execute(op_type, release_pos=release_pos) + + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] + return Status.RUNNING diff --git a/robowaiter/behavior_lib/act/Turn.py b/robowaiter/behavior_lib/act/Turn.py new file mode 100644 index 0000000..ed47bde --- /dev/null +++ b/robowaiter/behavior_lib/act/Turn.py @@ -0,0 +1,64 @@ +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 Clean(Act): + can_be_expanded = True + num_args = 1 + valid_args = [('AC','ACTemperature','TubeLight','HallLight','Curtain'), + ('Off','On','Up','Down','Clean','Dirty')] + + def __init__(self, *args): + super().__init__(*args) + self.target_obj = self.args[0] + self.op = self.args[1] + self.op_type = 13 + + if self.target_obj=="AC": + self.op_type = 13 + elif self.target_obj=="ACTemperature": + if self.op == 'Up': + self.op_type = 14 + elif self.op == 'Down': + self.op_type = 15 + elif self.target_obj=="TubeLight": + if self.op == 'On': + self.op_type = 6 + elif self.op == 'Off': + self.op_type = 8 + elif self.target_obj=="HallLight": + if self.op == 'On': + self.op_type = 9 + elif self.op == 'Off': + self.op_type = 10 + elif self.target_obj=="Curtain": + if self.op == 'On': + self.op_type = 11 + elif self.op == 'Off': + self.op_type = 12 + + @classmethod + def get_info(cls,arg): + info = {} + # 明天写 + # info["pre"]= {f'Holding(Nothing)'} + # if arg == "Table1": + # info["add"]= {f'Is(Table1,Clean)'} + # info["del"] = {f'Is(Table1,Dirty)'} + # elif arg == "Floor": + # info["add"] = {f'Is(Floor,Clean)'} + # info["del"] = {f'Is(Floor,Dirty)'} + # elif arg == "Chairs": + # info["add"] = {f'Is(Chairs,Clean)'} + # info["del"] = {f'Is(Chairs,Dirty)'} + return info + + def _update(self) -> ptree.common.Status: + + self.scene.move_task_area(self.op_type) + self.scene.op_task_execute(self.op_type) + + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] + return Status.RUNNING \ No newline at end of file