From 8d40d43c5a47896ddf91dcef9dc75a1939d0b4b0 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Thu, 16 Nov 2023 17:57:27 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=20VLM=20=E5=9C=BA=E6=99=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Behavior.py | 2 + robowaiter/behavior_lib/act/Make.py | 2 +- robowaiter/behavior_lib/act/MoveTo.py | 6 +- robowaiter/behavior_lib/act/PutDown.py | 4 + robowaiter/behavior_lib/cond/On.py | 3 + .../obtea/OptimalBTExpansionAlgorithm.py | 30 +++-- .../behavior_tree/obtea/opt_bt_exp_main.py | 2 +- robowaiter/llm_client/data/test_questions.txt | 2 +- .../llm_client/data_raw/test_questions.csv | 2 +- robowaiter/llm_client/data_tools/csv2dict.py | 1 - robowaiter/scene/tasks/{ => VLM}/VLM.py | 5 +- robowaiter/scene/tasks/VLM/VLM2.py | 123 ++++++++++++++++++ robowaiter/scene/tasks/VLM/__init__.py | 0 sub_task.ptml | 7 +- 14 files changed, 168 insertions(+), 21 deletions(-) rename robowaiter/scene/tasks/{ => VLM}/VLM.py (95%) create mode 100644 robowaiter/scene/tasks/VLM/VLM2.py create mode 100644 robowaiter/scene/tasks/VLM/__init__.py diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 1bd0182..78fa48a 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -33,6 +33,8 @@ class Bahavior(ptree.behaviour.Behaviour): 'Water': 'Glass', 'Dessert':'Plate' } + + @classmethod def get_ins_name(cls,*args): diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index a730820..88f828e 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -57,6 +57,6 @@ class Make(Act): self.scene.state["condition_set"] |= (self.info["add"]) self.scene.state["condition_set"] -= self.info["del_set"] - print("condition_set:",self.scene.state["condition_set"]) + # print("condition_set:",self.scene.state["condition_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 43cef8b..6c31ad5 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -21,7 +21,7 @@ class MoveTo(Act): 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} - info['cost']=5 + info['cost']=10 return info @@ -53,8 +53,6 @@ class MoveTo(Act): if obj.name == target_name: obj_info = obj_dict[id] dis = self.scene.cal_distance_to_robot(obj_info.location.X, obj_info.location.Y, obj_info.location.Z) - if id==275: - print("275'dis:",dis) if dis ptree.common.Status: # if self.scene.status? + # print("self.name:",self.name) + # print("On: condition_set:",self.scene.state["condition_set"]) + if self.name in self.scene.state["condition_set"]: return ptree.common.Status.SUCCESS else: diff --git a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py index 6a69783..cf88a21 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)]) @@ -138,14 +138,14 @@ class OptBTExpAlgorithm: break if valid: - # 把符合条件的动作节点都放到列表里 - if self.verbose: - print("———— -- %s 符合条件放入列表" % actions[i].name) c_attr_node = Leaf(type='cond', content=c_attr, mincost=current_mincost + actions[i].cost) a_attr_node = Leaf(type='act', content=actions[i], mincost=current_mincost + actions[i].cost) cond_anc_pair = CondActPair(cond_leaf=c_attr_node, act_leaf=a_attr_node) self.nodes.append(copy.deepcopy(cond_anc_pair)) # condition node list self.traversed.append(c_attr) # 重点 the set of expanded conditions + # 把符合条件的动作节点都放到列表里 + if self.verbose: + print("———— -- %s 符合条件放入列表,对应的c为 %s" % (actions[i].name,c_attr)) if self.verbose: print("算法结束！\n") @@ -188,13 +188,23 @@ class OptBTExpAlgorithm: # 树的dfs - def dfs_ptml(self,parnode): + def dfs_ptml(self,parnode,is_root=False): for child in parnode.children: if isinstance(child, Leaf): if child.type == 'cond': - self.ptml_string += "cond " - c_set_str = '\n cond '.join(map(str, child.content)) + "\n" - self.ptml_string += c_set_str + + if is_root and len(child.content) > 1: + # 把多个 cond 串起来 + self.ptml_string += "sequence{\n" + self.ptml_string += "cond " + c_set_str = '\n cond '.join(map(str, child.content)) + "\n" + self.ptml_string += c_set_str + self.ptml_string += '}\n' + else: + self.ptml_string += "cond " + c_set_str = '\n cond '.join(map(str, child.content)) + "\n" + self.ptml_string += c_set_str + elif child.type == 'act': if '(' not in child.content.name: self.ptml_string += 'act ' + child.content.name + "()\n" @@ -212,7 +222,7 @@ class OptBTExpAlgorithm: def get_ptml(self): self.ptml_string = "selector{\n" - self.dfs_ptml(self.bt.children[0]) + self.dfs_ptml(self.bt.children[0],is_root=True) self.ptml_string += '}\n' return self.ptml_string diff --git a/robowaiter/behavior_tree/obtea/opt_bt_exp_main.py b/robowaiter/behavior_tree/obtea/opt_bt_exp_main.py index aa07b5d..9a7829f 100644 --- a/robowaiter/behavior_tree/obtea/opt_bt_exp_main.py +++ b/robowaiter/behavior_tree/obtea/opt_bt_exp_main.py @@ -31,7 +31,7 @@ class BTOptExpInterface: :return: A PTML string representing the outcome of the behavior tree. """ self.goal = goal - self.algo = OptBTExpAlgorithm(verbose=True) + self.algo = OptBTExpAlgorithm(verbose=False) self.algo.clear() self.algo.run_algorithm(self.goal, self.actions,self.scene) # 调用算法得到行为树保存至 algo.bt self.ptml_string = self.algo.get_ptml() diff --git a/robowaiter/llm_client/data/test_questions.txt b/robowaiter/llm_client/data/test_questions.txt index e0d8ea9..fe0c3bd 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)\"}"}, "测试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)\"}"}} +{"测试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)\"}"}} diff --git a/robowaiter/llm_client/data_raw/test_questions.csv b/robowaiter/llm_client/data_raw/test_questions.csv index 3bb7a86..19b8af4 100644 --- a/robowaiter/llm_client/data_raw/test_questions.csv +++ b/robowaiter/llm_client/data_raw/test_questions.csv @@ -1,7 +1,7 @@ Question,Answer,Goal VLMһ,VLMһ,"{""On(Coffee,CoffeeTable)""}" VLMһȷŵ̨,VLMһȷŵ̨,"{""On(Coffee,Bar)""}" -VLMһȷŵˮϲˮ,VLMһȷŵˮϲˮ,"{""On(Coffee,WaterTable)""}" +VLMһȷŵˮϣٵһˮ,VLMһȷŵˮϣٵһˮ,"{""On(Coffee,WaterTable)"",""On(Water,WaterTable)""}" VLNǰ2,VLNǰ2,"{""At(Robot,Table2)""}" AEM,AEM,"{""EnvExplored()""}" VLMһˮ,VLMһˮ,"{""On(Water,WaterTable)""}" diff --git a/robowaiter/llm_client/data_tools/csv2dict.py b/robowaiter/llm_client/data_tools/csv2dict.py index 31feb97..6660c79 100644 --- a/robowaiter/llm_client/data_tools/csv2dict.py +++ b/robowaiter/llm_client/data_tools/csv2dict.py @@ -24,4 +24,3 @@ with open(csv_file_path, mode='r', encoding='gbk') as csv_file, \ json_str = json.dumps(output_dict, ensure_ascii=False) # 将JSON字符串写入JSONL文件，并添加换行符 jsonl_file.write(json_str + '\n') -s \ No newline at end of file diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM/VLM.py similarity index 95% rename from robowaiter/scene/tasks/VLM.py rename to robowaiter/scene/tasks/VLM/VLM.py index 23cd3af..04e2caf 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM/VLM.py @@ -22,7 +22,10 @@ class SceneVLM(Scene): # (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：做一杯咖啡放到水杯桌上并倒水")), + (5, self.create_chat_event("测试VLM：做一杯咖啡放到水杯桌上，再倒一杯水")), + (10, self.create_chat_event("测试VLM：关窗帘")), + # (5, self.create_chat_event("测试VLN：前往2号桌")), + ] def _reset(self): diff --git a/robowaiter/scene/tasks/VLM/VLM2.py b/robowaiter/scene/tasks/VLM/VLM2.py new file mode 100644 index 0000000..6872edd --- /dev/null +++ b/robowaiter/scene/tasks/VLM/VLM2.py @@ -0,0 +1,123 @@ +""" +视觉语言操作 +机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板 +""" + +import time +from robowaiter.scene.scene import Scene + +class SceneVLM(Scene): + def __init__(self, robot): + 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：关空调")), + # (5, self.create_chat_event("测试VLM：开大厅灯")), + # (5, self.create_chat_event("测试VLM：拖地")), + # (7, 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：做一杯咖啡放到水杯桌上并倒水")), + # (8, self.create_chat_event("测试VLN：前往1号桌")), + + ] + + def _reset(self): + + # self.gen_obj(type=5) + # self.gen_obj(type=9) + # self.op_task_execute(op_type=16, obj_id=0) + # self.move_task_area(op_type=4) + pass + + def _run(self, op_type=10): + # 一个行人从门口走到 吧台 + # 打招呼需要什么 + # 行人说 哪里有位置，想晒个太阳 + # 带领行人去有太阳的地方 + # 行人说 有点热 + # 好的，这就去开空调 + + scene = self.add_walkers([[0, 0]]) + self.control_walker( + [self.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)]) + + cont = scene.walkers[0].name+":我有点热，能开个空调吗？" + self.control_robot_action(0,3,cont) + + + # 共17个操作 + # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 + # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 + # "调整空调开关","调高空调温度","调低空调温度", # 13-15 + # "抓握物体","放置物体" # 16-17 + + # self.gen_obj() + # if op_type <=15: + # self.move_task_area(op_type) + # self.op_task_execute(op_type) + # if op_type == 16: # 16: 抓操作需要传入物品id + # self.move_task_area(op_type, obj_id=0) + # self.op_task_execute(op_type, obj_id=0) + # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 100.0], 水杯桌:[-70.0, 500.0, 107] + # # 桌子1:[-55.0, 0.0, 107],抹布桌:[340.0, 900.0, 99.0] # 桌子2:[-55.0, 150.0, 107], + # if op_type == 17: # 17: 放操作需要传入放置位置周围的可达区域 + # pos = [240.0, 40.0, 100.0] + # self.move_task_area(op_type, release_pos=pos) + # self.op_task_execute(op_type, release_pos=pos) # [325.0, 860.0, 100] + + # 流程测试 + # 抓握放置:抓吧台前生成的酸奶，放到抹布桌上 + 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] + # self.move_task_area(17, release_pos=pos) + # self.op_task_execute(17, release_pos=pos) + # + # # 做咖啡:做完的咖啡放到水杯桌上 + # self.move_task_area(1) + # self.op_task_execute(1) + # + # self.find_obj("CoffeeCup") + # + # self.move_task_area(16, obj_id=275) + # self.op_task_execute(16, obj_id=275) + # pos = [-70.0, 500.0, 107] + # self.move_task_area(17, release_pos=pos) + # self.op_task_execute(17, release_pos=pos) + # + # # 倒水:倒完的水放到旁边桌子上 + # self.move_task_area(2) + # self.op_task_execute(2) + + # + # self.move_task_area(16, obj_id=190) + # self.op_task_execute(16, obj_id=190) + # pos = [-55.0, 0.0, 107] + # self.move_task_area(17, release_pos=pos) + # self.op_task_execute(17, release_pos=pos) + + # self.test_yaw() + + pass + + def _step(self): + pass + + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneVLM(robot) + task.reset() + task.run() diff --git a/robowaiter/scene/tasks/VLM/__init__.py b/robowaiter/scene/tasks/VLM/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/sub_task.ptml b/sub_task.ptml index bc19507..33895cd 100644 --- a/sub_task.ptml +++ b/sub_task.ptml @@ -1,5 +1,10 @@ selector{ -cond On(Coffee,CoffeeTable) + +sequence{ + cond On(Coffee,CoffeeTable) + cond On(Coffee,CoffeeTable) + } + sequence{ cond Holding(Nothing) act Make(Coffee)