diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 78fa48a..27fd401 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -13,18 +13,23 @@ 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', 'WaterTable', 'CoffeeTable'} + 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', 'Water', 'Dessert', 'Softdrink', 'Yogurt'} - all_object = {'Coffee', 'Water'} + # all_object = {'Coffee', 'Water'} + + # all_object = set() + # all_place=set() + place_xyz_dic={ 'Bar': (247.0, 520.0, 100.0), 'Bar2': (240.0, 40.0, 70.0), 'WaterTable':(-70.0, 500.0, 107), 'CoffeeTable':(250.0, 310.0, 100.0), 'Table1': (340.0, 900.0, 98.0), + # 'Table1': (345.0, 895.0, 98.0), 'Table2': (-55.0, 0.0, 107), 'Table3':(-55.0, 150.0, 107) } diff --git a/robowaiter/behavior_lib/act/Clean.py b/robowaiter/behavior_lib/act/Clean.py index 39df987..bb9e9b3 100644 --- a/robowaiter/behavior_lib/act/Clean.py +++ b/robowaiter/behavior_lib/act/Clean.py @@ -25,7 +25,7 @@ class Clean(Act): @classmethod def get_info(cls,arg): info = {} - info["pre"]= {f'Holding(Nothing)'} + info["pre"]= {f'Holding(Nothing)',f'Is(HallLight,On)'} if arg == "Table1": info["add"]= {f'Is(Table1,Clean)'} info["del_set"] = {f'Is(Table1,Dirty)'} @@ -44,4 +44,6 @@ class Clean(Act): self.scene.state["condition_set"] |= (self.info["add"]) self.scene.state["condition_set"] -= self.info["del_set"] - return Status.SUCCESS \ No newline at end of file + + # print("After Clean condition_set:",self.scene.state["condition_set"] ) + return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py index b4e3af0..7187a69 100644 --- a/robowaiter/behavior_lib/act/PutDown.py +++ b/robowaiter/behavior_lib/act/PutDown.py @@ -39,5 +39,5 @@ class PutDown(Act): 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"]) + # print("After PutDown condition_set:",self.scene.state["condition_set"]) return Status.RUNNING diff --git a/robowaiter/behavior_lib/act/Turn.py b/robowaiter/behavior_lib/act/Turn.py index b499e24..cc14882 100644 --- a/robowaiter/behavior_lib/act/Turn.py +++ b/robowaiter/behavior_lib/act/Turn.py @@ -77,6 +77,7 @@ class Turn(Act): 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["add"]) self.scene.state["condition_set"] -= self.info["del_set"] + return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py index cf88a21..11bab3d 100644 --- a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py +++ b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py @@ -105,8 +105,8 @@ class OptBTExpAlgorithm: [copy.deepcopy(pair_node.cond_leaf), copy.deepcopy(pair_node.act_leaf)]) subtree.add_child([copy.deepcopy(sequence_structure)]) # subtree 是回不断变化的,它的父亲是self.bt # 增加实时条件判断,满足条件就不再扩展 - # if c <= self.scene.state["condition_set"]: - # return True + if c <= self.scene.state["condition_set"]: + return True else: subtree.add_child([copy.deepcopy(pair_node.act_leaf)]) diff --git a/robowaiter/llm_client/data/test_questions.txt b/robowaiter/llm_client/data/test_questions.txt index fe0c3bd..6959b5f 100644 --- a/robowaiter/llm_client/data/test_questions.txt +++ b/robowaiter/llm_client/data/test_questions.txt @@ -1 +1 @@ -{"测试VLM:做一杯咖啡": {"Answer": "测试VLM:做一杯咖啡", "Goal": "{\"On(Coffee,CoffeeTable)\"}"}, "测试VLM:做一杯咖啡放到吧台上": {"Answer": "测试VLM:做一杯咖啡放到吧台上", "Goal": "{\"On(Coffee,Bar)\"}"}, "测试VLM:做一杯咖啡放到水杯桌上,再倒一杯水": {"Answer": "测试VLM:做一杯咖啡放到水杯桌上,再倒一杯水", "Goal": "{\"On(Coffee,WaterTable)\",\"On(Water,WaterTable)\"}"}, "测试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)\"}"}, "我有点热,能开个空调吗?": {"Answer": "当然可以,我现在就开!", "Goal": "{\"Is(AC,On)\"}"}, "可以带我去吗": {"Answer": "当然可以,前往一号桌", "Goal": "{\"At(Robot,Table1)\"}"}} +{"做一杯咖啡": {"Answer": "OK,我这就去做一杯咖啡", "Goal": "{\"On(Coffee,CoffeeTable)\"}"}, "做一杯咖啡放到吧台上": {"Answer": "OK,我这就去做一杯咖啡放到吧台上", "Goal": "{\"On(Coffee,Bar)\"}"}, "做一杯咖啡放到水杯桌上,再倒一杯水": {"Answer": "OK,我这就去做一杯咖啡放到水杯桌上,再倒一杯水", "Goal": "{\"On(Coffee,WaterTable)\",\"On(Water,WaterTable)\"}"}, "前往2号桌": {"Answer": "OK,我这前往2号桌", "Goal": "{\"At(Robot,Table2)\"}"}, "测试AEM": {"Answer": "测试AEM", "Goal": "{\"EnvExplored()\"}"}, "倒一杯水": {"Answer": "OK,我这就去倒一杯水", "Goal": "{\"On(Water,WaterTable)\"}"}, "开空调": {"Answer": "OK,我这就去开空调", "Goal": "{\"Is(AC,On)\"}"}, "关空调": {"Answer": "OK,我这就去关空调", "Goal": "{\"Is(AC,Off)\"}"}, "关大厅灯": {"Answer": "OK,我这就去关大厅灯", "Goal": "{\"Is(HallLight,Off)\"}"}, "开大厅灯": {"Answer": "OK,我这就去开大厅灯", "Goal": "{\"Is(HallLight,On)\"}"}, "关筒灯": {"Answer": "OK,我这就去关筒灯", "Goal": "{\"Is(TubeLight,Off)\"}"}, "开筒灯": {"Answer": "OK,我这就去开筒灯", "Goal": "{\"Is(TubeLight,On)\"}"}, "关窗帘": {"Answer": "OK,我这就去关窗帘", "Goal": "{\"Is(Curtain,Off)\"}"}, "开窗帘": {"Answer": "OK,我这就去开窗帘", "Goal": "{\"Is(Curtain,On)\"}"}, "拖地": {"Answer": "OK,我这就去拖地", "Goal": "{\"Is(Floor,Clean)\"}"}, "擦桌子": {"Answer": "OK,我这就去擦桌子", "Goal": "{\"Is(Table1,Clean)\"}"}, "整理椅子": {"Answer": "OK,我这就去整理椅子", "Goal": "{\"Is(Chairs,Clean)\"}"}, "把冰红茶放到Table2": {"Answer": "OK,我这就去把冰红茶放到Table2", "Goal": "{\"On(BottledDrink,Table2)\"}"}, "我有点热,能开个空调吗?": {"Answer": "当然可以,我现在就开!", "Goal": "{\"Is(AC,On)\"}"}, "可以带我去吗": {"Answer": "当然可以,前往一号桌", "Goal": "{\"At(Robot,Table1)\"}"}, "把酸奶放到1号桌,再做一杯咖啡送到水杯桌上,再倒一杯水。": {"Answer": "明白,我这就去办!", "Goal": "{\"On(Coffee,WaterTable)\",\"On(Yogurt,Table1)\",\"On(Water,WaterTable)\"}"}, "下班啦!打扫卫生,关灯关空调关窗帘。": {"Answer": "太棒啦,下班啦!我这就去打扫卫生,关灯关空调关窗帘。", "Goal": "{\"Is(Floor,Clean)\",\"Is(Table1,Clean)\",\"Is(Chairs,Clean)\",\"Is(AC,Off)\",\"Is(HallLight,Off)\",\"Is(TubeLight,Off)\",\"Is(Curtain,Off)\"}"}} diff --git a/robowaiter/llm_client/data_raw/test_questions.csv b/robowaiter/llm_client/data_raw/test_questions.csv index 19b8af4..8a71f42 100644 --- a/robowaiter/llm_client/data_raw/test_questions.csv +++ b/robowaiter/llm_client/data_raw/test_questions.csv @@ -1,21 +1,23 @@ Question,Answer,Goal -VLMһ,VLMһ,"{""On(Coffee,CoffeeTable)""}" -VLMһȷŵ̨,VLMһȷŵ̨,"{""On(Coffee,Bar)""}" -VLMһȷŵˮϣٵһˮ,VLMһȷŵˮϣٵһˮ,"{""On(Coffee,WaterTable)"",""On(Water,WaterTable)""}" -VLNǰ2,VLNǰ2,"{""At(Robot,Table2)""}" +һ,OKȥһ,"{""On(Coffee,CoffeeTable)""}" +һȷŵ̨,OKȥһȷŵ̨,"{""On(Coffee,Bar)""}" +һȷŵˮϣٵһˮ,OKȥһȷŵˮϣٵһˮ,"{""On(Coffee,WaterTable)"",""On(Water,WaterTable)""}" +ǰ2,OKǰ2,"{""At(Robot,Table2)""}" AEM,AEM,"{""EnvExplored()""}" -VLMһˮ,VLMһˮ,"{""On(Water,WaterTable)""}" -VLMյ,VLMյ,"{""Is(AC,On)""}" -VLMؿյ,VLMؿյ,"{""Is(AC,Off)""}" -VLMش,VLMش,"{""Is(HallLight,Off)""}" -VLM,VLM,"{""Is(HallLight,On)""}" -VLMͲ,VLMͲ,"{""Is(TubeLight,Off)""}" -VLMͲ,VLMͲ,"{""Is(TubeLight,On)""}" -VLMش,VLMش,"{""Is(Curtain,Off)""}" -VLM,VLM,"{""Is(Curtain,On)""}" -VLMϵ,VLMϵ,"{""Is(Floor,Clean)""}" -VLM,VLM,"{""Is(Table1,Clean)""}" -VLM,VLM,"{""Is(Chairs,Clean)""}" -VLMѱŵTable2,VLMѱŵTable2,"{""On(BottledDrink,Table2)""}" +һˮ,OKȥһˮ,"{""On(Water,WaterTable)""}" +յ,OKȥյ,"{""Is(AC,On)""}" +ؿյ,OKȥؿյ,"{""Is(AC,Off)""}" +ش,OKȥش,"{""Is(HallLight,Off)""}" +,OKȥ,"{""Is(HallLight,On)""}" +Ͳ,OKȥͲ,"{""Is(TubeLight,Off)""}" +Ͳ,OKȥͲ,"{""Is(TubeLight,On)""}" +ش,OKȥش,"{""Is(Curtain,Off)""}" +,OKȥ,"{""Is(Curtain,On)""}" +ϵ,OKȥϵ,"{""Is(Floor,Clean)""}" +,OKȥ,"{""Is(Table1,Clean)""}" +,OKȥ,"{""Is(Chairs,Clean)""}" +ѱŵTable2,OKȥѱŵTable2,"{""On(BottledDrink,Table2)""}" еȣܿյ,ȻԣھͿ,"{""Is(AC,On)""}" Դȥ,Ȼԣǰһ,"{""At(Robot,Table1)""}" +̷ŵ1һ͵ˮϣٵһˮ,ףȥ죡,"{""On(Coffee,WaterTable)"",""On(Yogurt,Table1)"",""On(Water,WaterTable)""}" +°ɨصƹؿյش,̫°ȥɨصƹؿյش,"{""Is(Floor,Clean)"",""Is(Table1,Clean)"",""Is(Chairs,Clean)"",""Is(AC,Off)"",""Is(HallLight,Off)"",""Is(TubeLight,Off)"",""Is(Curtain,Off)""}" diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 16838d7..9338e41 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -268,6 +268,19 @@ class Scene: GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID) ) + def control_walkers(self,walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True): + """pose:表示行人的终止位置姿态""" + scene = self.status + walker_loc = walker_loc + controls = [] + for i in range(len(scene.walkers)): + loc = walker_loc[i] + is_autowalk = is_autowalk + pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180) + controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=80, pose=pose)) + scene = stub.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=self.sceneID)) + + def control_joints(self, angles): stub.Do( GrabSim_pb2.Action( diff --git a/robowaiter/scene/tasks/VLM/VLM.py b/robowaiter/scene/tasks/VLM/VLM.py index 04e2caf..b0fa32b 100644 --- a/robowaiter/scene/tasks/VLM/VLM.py +++ b/robowaiter/scene/tasks/VLM/VLM.py @@ -11,24 +11,48 @@ class SceneVLM(Scene): super().__init__(robot) # 在这里加入场景中发生的事件, (事件发生的时间,事件函数) self.event_list = [ + (5, self.create_chat_event("把酸奶放到1号桌,再做一杯咖啡送到水杯桌上,再倒一杯水。")), + (60, self.create_chat_event("开空调")), + (100, self.create_chat_event("下班啦!打扫卫生,关灯关空调关窗帘。")), + + + # (9, self.create_chat_event("关窗帘")), + # (9, self.create_chat_event("开大厅灯")), # (5, self.create_chat_event("测试VLM:做一杯咖啡")), # (5, self.create_chat_event("测试VLM:倒一杯水")), - # (5, self.create_chat_event("测试VLM:开空调")), # (5, self.create_chat_event("测试VLM:关空调")), - # (5, self.create_chat_event("测试VLM:开大厅灯")), # (5, self.create_chat_event("测试VLM:拖地")), # (5, self.create_chat_event("测试VLM:擦桌子")), # (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:做一杯咖啡放到吧台上")), - (5, self.create_chat_event("测试VLM:做一杯咖啡放到水杯桌上,再倒一杯水")), - (10, self.create_chat_event("测试VLM:关窗帘")), + # (5, self.create_chat_event("测试VLM:做一杯咖啡放到水杯桌上,再倒一杯水")), + # (10, self.create_chat_event("测试VLM:关窗帘")), # (5, self.create_chat_event("测试VLN:前往2号桌")), - + # (11, self.create_chat_event("测试VLM:拖地")), + # (12, self.create_chat_event("测试VLM:擦桌子")), ] def _reset(self): + self.gen_obj() + self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)', + 'Holding(Nothing)','Exist(Yogurt)','Exist(Softdrink)','On(Yogurt,Bar)','On(Softdrink,Table1)', + 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)', + 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'} + self.add_walkers([[0, 880], [250, 1200], [-55, 750], [70, -200]]) + # self.add_walkers([[-500, 500]]) + self.control_walkers(walker_loc=[[-55, 750], [70, -200], [250, 1200], [0, 880]],is_autowalk = True) + # self.control_walkers(walker_loc=[[-55, 750]],is_autowalk = False) + # 在场景中随机增加一堆行人。 + # walker_loc = [[-55, 750], [70, -200], [250, 1200], [0, 880]] + # controls = [] + # for i in range(len(s.walkers)): + # loc = walker_loc[i] + # is_autowalk = False + # pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180) + # controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=200, pose=pose)) + # scene = sim_client.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=scene_id)) # self.gen_obj(type=5) # self.gen_obj(type=9) @@ -37,8 +61,6 @@ class SceneVLM(Scene): pass def _run(self, op_type=10): - - # 共17个操作 # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 @@ -61,7 +83,7 @@ class SceneVLM(Scene): # 流程测试 # 抓握放置:抓吧台前生成的酸奶,放到抹布桌上 - self.gen_obj() + # self.gen_obj() # self.move_task_area(16, obj_id=0) # self.op_task_execute(16, obj_id=0) # pos = [340.0, 900.0, 99.0] diff --git a/robowaiter/scene/tasks/VLM/VLM2.py b/robowaiter/scene/tasks/VLM/VLM2.py index 6872edd..fa3595f 100644 --- a/robowaiter/scene/tasks/VLM/VLM2.py +++ b/robowaiter/scene/tasks/VLM/VLM2.py @@ -43,13 +43,15 @@ class SceneVLM(Scene): # 行人说 有点热 # 好的,这就去开空调 - scene = self.add_walkers([[0, 0]]) + scene = self.add_walkers([[47, 920],[70,-200]]) self.control_walker( - [self.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)]) + [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=47, Y=520, Yaw=0)]) - cont = scene.walkers[0].name+":我有点热,能开个空调吗?" + cont = scene.walkers[0].name+":请问可以带我去空位上嘛?我想晒太阳" self.control_robot_action(0,3,cont) + # self.clean_walker() + # 共17个操作 # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7