diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index e9e84af..5966286 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -13,15 +13,17 @@ class Bahavior(ptree.behaviour.Behaviour): ''' scene = None print_name_prefix = "" - all_place = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3'} - all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', - 'VacuumCup'} + # all_place = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3'} + # all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', + # 'VacuumCup'} + all_place = {'Bar', 'WaterTable', 'CoffeeTable'} + all_object = {'Coffee'} 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),# 位置需要更改!!! + 'CoffeeTable':(250.0, 310.0, 100.0), + 'Table1': (340.0, 900.0, 98.0), 'Table2': (-55.0, 0.0, 107), 'Table3':(-55.0, 150.0, 107) } diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 86dea7f..ebe3b7b 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -27,12 +27,13 @@ class Make(Act): info = {} info["pre"]= {f'Holding(Nothing)'} info['del_set'] = set() + info['add'] = {f'Exist({arg})'} if arg == "Coffee": - info["add"]= {f'On(Coffee,CoffeeTable)'} + info["add"] |= {f'On(Coffee,CoffeeTable)'} elif arg == "Water": - info["add"] = {f'On(Water,WaterTable)'} + info["add"] |= {f'On(Water,WaterTable)'} elif arg == "Dessert": - info["add"] = {f'On(Dessert,Bar)'} + info["add"] |= {f'On(Dessert,Bar)'} return info def _update(self) -> ptree.common.Status: @@ -40,6 +41,8 @@ class Make(Act): self.scene.move_task_area(self.op_type) self.scene.op_task_execute(self.op_type) + # self.scene.gen_obj(type=40) + self.scene.state["condition_set"].union(self.info["add"]) self.scene.state["condition_set"] -= self.info["del_set"] 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 8b24e72..995a074 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -17,6 +17,8 @@ class MoveTo(Act): def get_info(cls,arg): info = {} info['pre'] = set() + if arg in Act.all_object: + info['pre'] |= {f'Exist({arg})'} info["add"] = {f'At(Robot,{arg})'} info["del_set"] = {f'At(Robot,{place})' for place in cls.valid_args if place != arg} return info @@ -29,20 +31,27 @@ class MoveTo(Act): # goal = self.scene.state['map']['obj_pos'][self.args[0]] # navigator.navigate(goal, animation=False) + # 走到固定的地点 if self.target_place in Act.place_xyz_dic: goal = Act.place_xyz_dic[self.target_place] self.scene.walk_to(goal[0],goal[1]) - else: - # 获取obj_id - for id,obj in enumerate(self.scene.objects): - if obj.name == self.target_place: - obj_id = id - break + else: # 走到物品边上 + obj_id = -1 + obj_dict = self.scene.status.objects + if len(obj_dict)!=0: + # 获取obj_id + for id,obj in enumerate(obj_dict): + if obj.name == self.target_place: + obj_id = id + break + if obj_id == -1: + return ptree.common.Status.FAILURE obj_info = self.scene.objects[obj_id] obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z self.scene.walk_to(obj_x,obj_y) + # goal = self.scene.state['map']['obj_pos'][self.args[0]] # self.scene.walk_to(goal[0],goal[1]) # X, Y, Yaw=None, velocity=200, dis_limit=0 diff --git a/robowaiter/behavior_lib/act/PickUp.py b/robowaiter/behavior_lib/act/PickUp.py index b2a2363..4dae241 100644 --- a/robowaiter/behavior_lib/act/PickUp.py +++ b/robowaiter/behavior_lib/act/PickUp.py @@ -18,6 +18,8 @@ class PickUp(Act): info["pre"] = {f'At(Robot,{arg})','Holding(Nothing)'} info["add"] = {f'Holding({arg})'} info["del_set"] = {f'Holding(Nothing)'} + for place in Act.all_place: + info["del_set"] |= {f'On({arg},{place})'} return info diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py index 532fd31..009a8f8 100644 --- a/robowaiter/behavior_lib/act/PutDown.py +++ b/robowaiter/behavior_lib/act/PutDown.py @@ -20,7 +20,7 @@ class PutDown(Act): def get_info(cls,*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["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'} info["del_set"] = {f'Holding({arg[0]})'} return info diff --git a/robowaiter/behavior_lib/cond/Exist.py b/robowaiter/behavior_lib/cond/Exist.py new file mode 100644 index 0000000..459c556 --- /dev/null +++ b/robowaiter/behavior_lib/cond/Exist.py @@ -0,0 +1,26 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Cond import Cond +import itertools + +class Exist(Cond): + can_be_expanded = True + num_params = 2 + valid_args = tuple(Cond.all_object) + + def __init__(self,*args): + super().__init__(*args) + + + def _update(self) -> ptree.common.Status: + # if self.scene.status? + + if self.name in self.scene.state["condition_set"]: + return ptree.common.Status.SUCCESS + else: + return ptree.common.Status.FAILURE + + # if self.scene.state['chat_list'] == []: + # return ptree.common.Status.FAILURE + # else: + # return ptree.common.Status.SUCCESS diff --git a/robowaiter/behavior_tree/dataset/goal_states.txt b/robowaiter/behavior_tree/dataset/goal_states.txt index 03903d0..c72c9cc 100644 --- a/robowaiter/behavior_tree/dataset/goal_states.txt +++ b/robowaiter/behavior_tree/dataset/goal_states.txt @@ -747,7 +747,7 @@ {At(Robot,Table1)} {At(Robot,Table2)} {At(Robot,Table3)} -{At(Robot, Table1)} +{At(Robot, CoffeeTable)} {On(Softdrink,Bar)} {On(Softdrink,WaterTable)} {On(Softdrink,CoffeeTable)} @@ -1483,21 +1483,21 @@ {On(VacuumCup,Table1)} {On(VacuumCup,Table2)} {On(VacuumCup,Table3)} -{On(Milk, Table1)} -{On(ADMilk, CoffeeTable)} -{On(VacuumCup, Bar2)} +{On(MilkDrink, Table2)} +{On(MilkDrink, Bar2)} {On(Yogurt, WaterTable)} +{On(Softdrink, Bar2)} {On(Softdrink, WaterTable)} -{On(Softdrink, Bar)} -{On(Softdrink, Table2)} -{On(Softdrink, Table1)} -{On(ADMilk, Bar2)} -{On(Milk, Table1)} -{On(Milk, Table1)} -{On(Milk, Bar2)} -{On(VacuumCup, Bar)} +{On(MilkDrink, WaterTable)} +{On(ADMilk, Bar)} +{On(MilkDrink, Table3)} +{On(MilkDrink, WaterTable)} +{On(BottledDrink, CoffeeTable)} +{On(VacuumCup, Table2)} +{On(MilkDrink, Bar)} +{On(ADMilk, WaterTable)} +{On(BottledDrink, Bar2)} {On(ADMilk, Table3)} -{On(VacuumCup, Bar2)} {Is(AC,Off)} {Is(AC,On)} {Is(ACTemperature,Up)} @@ -2234,20 +2234,20 @@ {Is(Floor,Clean)} {Is(Table1,Dirty)} {Is(Table1,Clean)} -{Is(ACTemperature, 0)} -{Is(Chairs, 1)} -{Is(TubeLight, 0)} -{Is(HallLight, 0)} -{Is(Chairs, 0)} -{Is(Floor, 0)} {Is(Floor, 0)} +{Is(AC, 0)} {Is(HallLight, 1)} -{Is(Chairs, 0)} -{Is(TubeLight, 1)} +{Is(ACTemperature, 0)} +{Is(Floor, 1)} +{Is(AC, 1)} +{Is(Floor, 1)} +{Is(Floor, 1)} {Is(TubeLight, 0)} {Is(Table1, 0)} -{Is(AC, 0)} -{Is(ACTemperature, 1)} +{Is(Table1, 0)} +{Is(HallLight, 0)} +{Is(HallLight, 1)} +{Is(Chairs, 1)} {Holding(Softdrink)} {Holding(BottledDrink)} {Holding(Yogurt)} @@ -2744,10 +2744,10 @@ {Holding(Milk)} {Holding(VacuumCup)} {Holding(Nothing)} -{Holding(Softdrink)} -{Holding(VacuumCup)} {Holding(Yogurt)} -{Holding(VacuumCup)} +{Holding(MilkDrink)} +{Holding(ADMilk)} +{Holding(ADMilk)} {On(Coffee,Bar)} {On(Coffee,WaterTable)} {On(Coffee,CoffeeTable)} @@ -3483,21 +3483,21 @@ {On(Dessert,Table1)} {On(Dessert,Table2)} {On(Dessert,Table3)} -{On(Dessert, Table2)} -{On(Dessert, Bar2)} -{On(Water, Table2)} -{On(Dessert, Table3)} -{On(Coffee, WaterTable)} -{On(Coffee, CoffeeTable)} -{On(Coffee, WaterTable)} -{On(Dessert, Bar2)} -{On(Water, CoffeeTable)} -{On(Water, Bar2)} -{On(Dessert, Table2)} +{On(Coffee, Table3)} +{On(Water, WaterTable)} +{On(Coffee, Table2)} +{On(Coffee, Table2)} +{On(Water, Table3)} {On(Coffee, Bar2)} -{On(Coffee, Bar)} -{On(Dessert, Bar)} -{On(Water, Table2)} +{On(Coffee, WaterTable)} +{On(Water, CoffeeTable)} +{On(Dessert, CoffeeTable)} +{On(Water, Table3)} +{On(Water, Table3)} +{On(Coffee, Table1)} +{On(Dessert, WaterTable)} +{On(Water, Bar2)} +{On(Water, Bar2)} {At(Robot,Bar),On(Softdrink,Bar)} {At(Robot,Bar),On(Softdrink,WaterTable)} {At(Robot,Bar),On(Softdrink,CoffeeTable)} diff --git a/robowaiter/llm_client/data/test_questions.txt b/robowaiter/llm_client/data/test_questions.txt index 1e62a74..cc0bb66 100644 --- a/robowaiter/llm_client/data/test_questions.txt +++ b/robowaiter/llm_client/data/test_questions.txt @@ -1 +1 @@ -{"测试VLM:做一杯咖啡": {"Answer": "测试VLM:做一杯咖啡", "Goal": "{\"At(Coffee,Bar)\"}"}, "测试VLN:前往2号桌": {"Answer": " 测试VLN:前往2号桌", "Goal": "{\"At(Robot,Table2)\"}"}, "测试AEM": {"Answer": "测试AEM", "Goal": "{\"EnvExplored()\"}"}, "测试VLM:倒一杯水": {"Answer": "测试VLM:倒一杯水", "Goal": "{\"On(Water,WaterTable)\"}"}, "测试VLM:开空调": {"Answer": "测试VLM:开空调", "Goal": "{\"Is(AC,On)\"}"}, "测试VLM:关空调": {"Answer": "测试VLM:关空调", "Goal": "{\"Is(AC,Off)\"}"}, "测试VLM:关大厅灯": {"Answer": "测试VLM:关大厅灯", "Goal": "{\"Is(HallLight,Off)\"}"}, "测试VLM:开大厅灯": {"Answer": "测试VLM:开大厅灯", "Goal": "{\"Is(HallLight,On)\"}"}, "测试VLM:关筒灯": {"Answer": "测试VLM:关筒灯", "Goal": "{\"Is(TubeLight,Off)\"}"}, "测试VLM:开筒灯": {"Answer": "测试VLM:开筒灯", "Goal": "{\"Is(TubeLight,On)\"}"}, "测试VLM:关窗帘": {"Answer": "测试VLM:关窗帘", "Goal": "{\"Is(Curtain,Off)\"}"}, "测试VLM:开窗帘": {"Answer": "测试VLM:开窗帘", "Goal": "{\"Is(Curtain,On)\"}"}, "测试VLM:拖地": {"Answer": "测试VLM:拖地", "Goal": "{\"Is(Floor,Clean)\"}"}, "测试VLM:擦桌子": {"Answer": "测试VLM:擦桌子", "Goal": "{\"Is(Table1,Clean)\"}"}, "测试VLM:整理椅子": {"Answer": "测试VLM:整理椅子", "Goal": "{\"Is(Chairs,Clean)\"}"}, "测试VLM:把冰红茶放到Table2": {"Answer": "测试VLM:把冰红茶放到Table2", "Goal": "{\"On(BottledDrink,Table2)\"}"}} +{"测试VLM:做一杯咖啡": {"Answer": "测试VLM:做一杯咖啡", "Goal": "{\"On(Coffee,CoffeeTable)\"}"}, "测试VLM:做一杯咖啡放到吧台上": {"Answer": "测试VLM:做一杯咖啡放到吧台上", "Goal": "{\"On(Coffee,Bar)\"}"}, "测试VLN:前往2号桌": {"Answer": "测试VLN:前往2号桌", "Goal": "{\"At(Robot,Table2)\"}"}, "测试AEM": {"Answer": "测试AEM", "Goal": "{\"EnvExplored()\"}"}, "测试VLM:倒一杯水": {"Answer": "测试VLM:倒一杯水", "Goal": "{\"On(Water,WaterTable)\"}"}, "测试VLM:开空调": {"Answer": "测试VLM:开空调", "Goal": "{\"Is(AC,On)\"}"}, "测试VLM:关空调": {"Answer": "测试VLM:关空调", "Goal": "{\"Is(AC,Off)\"}"}, "测试VLM:关大厅灯": {"Answer": "测试VLM:关大厅灯", "Goal": "{\"Is(HallLight,Off)\"}"}, "测试VLM:开大厅灯": {"Answer": "测试VLM:开大厅灯", "Goal": "{\"Is(HallLight,On)\"}"}, "测试VLM:关筒灯": {"Answer": "测试VLM:关筒灯", "Goal": "{\"Is(TubeLight,Off)\"}"}, "测试VLM:开筒灯": {"Answer": "测试VLM:开筒灯", "Goal": "{\"Is(TubeLight,On)\"}"}, "测试VLM:关窗帘": {"Answer": "测试VLM:关窗帘", "Goal": "{\"Is(Curtain,Off)\"}"}, "测试VLM:开窗帘": {"Answer": "测试VLM:开窗帘", "Goal": "{\"Is(Curtain,On)\"}"}, "测试VLM:拖地": {"Answer": "测试VLM:拖地", "Goal": "{\"Is(Floor,Clean)\"}"}, "测试VLM:擦桌子": {"Answer": "测试VLM:擦桌子", "Goal": "{\"Is(Table1,Clean)\"}"}, "测试VLM:整理椅子": {"Answer": "测试VLM:整理椅子", "Goal": "{\"Is(Chairs,Clean)\"}"}, "测试VLM:把冰红茶放到Table2": {"Answer": "测试VLM:把冰红茶放到Table2", "Goal": "{\"On(BottledDrink,Table2)\"}"}} diff --git a/robowaiter/llm_client/data_raw/test_questions.csv b/robowaiter/llm_client/data_raw/test_questions.csv index ab3243e..1a7573a 100644 --- a/robowaiter/llm_client/data_raw/test_questions.csv +++ b/robowaiter/llm_client/data_raw/test_questions.csv @@ -1,5 +1,6 @@ Question,Answer,Goal -VLMһ,VLMһ,"{""At(Coffee,Bar)""}" +VLMһ,VLMһ,"{""On(Coffee,CoffeeTable)""}" +VLMһȷŵ̨,VLMһȷŵ̨,"{""On(Coffee,Bar)""}" VLNǰ2,VLNǰ2,"{""At(Robot,Table2)""}" AEM,AEM,"{""EnvExplored()""}" VLMһˮ,VLMһˮ,"{""On(Water,WaterTable)""}" diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 3c350b8..cea7822 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -180,21 +180,13 @@ class Scene: ) def walk_to(self, X, Y, Yaw=None, velocity=200, dis_limit=0): - if self.use_offset: - X, Y = X + loc_offset[0], Y + loc_offset[1] - - if Yaw is None: - Yaw = self.status.rotation.Yaw - - v = [X, Y, Yaw - 90, velocity, dis_limit] - print(v) + walk_v = [X,Y,Yaw,velocity,dis_limit] action = GrabSim_pb2.Action( - scene=self.sceneID, - action=GrabSim_pb2.Action.ActionType.WalkTo, - values=v + scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v ) - scene_info = stub.Do(action) - return scene_info + scene = stub.Do(action) + + return scene def reachable_check(self, X, Y, Yaw): @@ -418,13 +410,14 @@ class Scene: def gen_obj(self,type=5,h=100): # 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 14:cake 61:甜牛奶 - type= 5 #9 + # type= 5 #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] - 55, y=ginger_loc[1] - 40, z = 95, roll=0, pitch=0, yaw=0, type=5), + obj_list = [GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 55, y=ginger_loc[1] - 40, z = 95, roll=0, pitch=0, yaw=0, type=type), # GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z=h, roll=0, pitch=0, yaw=0, type=9), - GrabSim_pb2.ObjectList.Object(x=340, y=960, z = 88, roll=0, pitch=0, yaw=0, type=9), + # GrabSim_pb2.ObjectList.Object(x=340, y=960, z = 88, roll=0, pitch=0, yaw=0, type=9), ] + 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 2bf37f2..5bfe7f6 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -21,6 +21,8 @@ class SceneVLM(Scene): # (5, self.create_chat_event("测试VLM:整理椅子")), # (5, self.create_chat_event("测试VLM:把冰红茶放到Table2")), # (5, self.create_chat_event("测试VLM:关大厅灯")) + + # (5, self.create_chat_event("测试VLM:做一杯咖啡放到吧台上")), ] def _reset(self): @@ -28,7 +30,7 @@ class SceneVLM(Scene): 'Is(HallLight,Off)','Is(TubeLight,On)','Is(Curtain,On)', 'Is(Table1,Dirty)','Is(Floor,Dirty)','Is(Chairs,Dirty)'} # self.gen_obj(type=5) - # # self.gen_obj(type=9) + # self.gen_obj(type=9) # self.op_task_execute(op_type=16, obj_id=0) # self.move_task_area(op_type=4) pass diff --git a/run_robowaiter.py b/run_robowaiter.py index 54f37c9..dda629e 100644 --- a/run_robowaiter.py +++ b/run_robowaiter.py @@ -1,7 +1,7 @@ import os from robowaiter import Robot, task_map -TASK_NAME = 'OT' +TASK_NAME = 'VLM' # create robot project_path = "./robowaiter" diff --git a/sub_task.ptml b/sub_task.ptml index 7b7a602..bc19507 100644 --- a/sub_task.ptml +++ b/sub_task.ptml @@ -1,4 +1,26 @@ selector{ -cond At(Robot,Table2) -act MoveTo(Table2) +cond On(Coffee,CoffeeTable) +sequence{ +cond Holding(Nothing) +act Make(Coffee) +} +sequence{ +cond At(Robot,CoffeeTable) + cond Holding(Coffee) +act PutDown(Coffee,CoffeeTable) +} +sequence{ +cond At(Robot,Bar) + cond Holding(Coffee) +act PutDown(Coffee,Bar) +} +sequence{ +cond At(Robot,WaterTable) + cond Holding(Coffee) +act PutDown(Coffee,WaterTable) +} +sequence{ +cond Holding(Coffee) +act MoveTo(CoffeeTable) +} }