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/proto/camera.py b/robowaiter/proto/camera.py index 503e735..8780bbb 100644 --- a/robowaiter/proto/camera.py +++ b/robowaiter/proto/camera.py @@ -240,7 +240,7 @@ def get_semantic_map(camera, cur_objs, objs_name): scene = Observe(0) objs = scene.objects img_data = get_camera([camera]) - show_image(img_data, scene) + # show_image(img_data, scene) objs_name = save_obj_info(img_data, objs_name) for obj_name in list(objs_name): for obj in objs: diff --git a/robowaiter/proto/map_1.pkl b/robowaiter/proto/map_1.pkl new file mode 100644 index 0000000..323c387 Binary files /dev/null and b/robowaiter/proto/map_1.pkl differ diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 0333473..ee888fe 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -1,3 +1,4 @@ +import sys import time import grpc import numpy as np @@ -108,6 +109,7 @@ class Scene: self.visited = set() self.all_frontier_list = set() self.semantic_map = semantic_map + self.auto_map = np.ones((800, 1550)) def reset(self): @@ -578,7 +580,7 @@ class Scene: # v_list = [[0.0, 0.0]] for walk_v in v_list: - walk_v = walk_v + [scene.rotation.Yaw - 90, 250, 60] + walk_v = walk_v + [scene.rotation.Yaw - 90, 250, 10] print("walk_v", walk_v) action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) @@ -587,7 +589,7 @@ class Scene: print(scene.info) return cur_objs, objs_name_set - def isOutMap(self, pos, min_x=-350, max_x=600, min_y=-400, max_y=1450): + def isOutMap(self, pos, min_x=-200, max_x=600, min_y=-250, max_y=1300): if pos[0] <= min_x or pos[0] >= max_x or pos[1] <= min_y or pos[1] >= max_y: return True return False @@ -598,11 +600,13 @@ class Scene: ''' # x = round((x - self.min_x) / self.scale_ratio) # y = round((y - self.min_y) / self.scale_ratio) - x = math.floor((x + 350) / 5) - y = math.floor((y + 400) / 5) + x = math.floor((x + 200)) + y = math.floor((y + 250)) return x, y - def explore(self, map, cur_pos, explore_range): + def explore(self, map, explore_range): + scene = stub.Observe(GrabSim_pb2.SceneID(value=0)) + cur_pos = [int(scene.location.X), int(scene.location.Y)] for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1): for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1): if self.isOutMap((i, j)): @@ -610,6 +614,7 @@ class Scene: x, y = self.real2map(i, j) if map[x, y] == 0: self.visited.add((i, j)) + self.auto_map[x][y] = 0 for i in range(cur_pos[0] - explore_range, cur_pos[0] + explore_range + 1): for j in range(cur_pos[1] - explore_range, cur_pos[1] + explore_range + 1): if self.isOutMap((i, j)): @@ -618,11 +623,26 @@ class Scene: if map[x, y] == 0: if self.isNewFrontier((i, j), map): self.all_frontier_list.add((i, j)) - if len(self.all_frontier_list) <= 400: + if len(self.all_frontier_list) == 0: free_list = list(self.visited) free_array = np.array(free_list) - print(f"探索完成！以下是场景中可以到达的点：{free_array}；其余点均是障碍物不可达") + print(f"主动探索完成！以下是场景中可以到达的点：{free_array}；其余点均是障碍物不可达") + + # 画地图: X行Y列，第一行在下面 + plt.clf() + plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', + extent=(-250, 1300, + -200, 600)) + plt.show() + print("已绘制完成地图！！！") + return None + # 画地图: X行Y列，第一行在下面 + plt.imshow(self.auto_map, cmap='binary', alpha=0.5, origin='lower', + extent=(-250, 1300, + -200, 600)) + plt.show() + print("已绘制部分地图！") return self.getNearestFrontier(cur_pos, self.all_frontier_list) def isNewFrontier(self, pos, map): @@ -630,12 +650,26 @@ class Scene: for node in around_nodes: x, y = self.real2map(node[0], node[1]) - if node not in self.visited and map[x, y] == 0: + if not self.isOutMap((node[0], node[1])) and node not in self.visited and map[x, y] == 0: return True if (pos[0], pos[1]) in self.all_frontier_list: self.all_frontier_list.remove((pos[0], pos[1])) return False + def getDistance(self, pos1, pos2): + return math.sqrt((pos1[0] - pos2[0]) ** 2 + (pos1[1] - pos2[1]) ** 2) + + def getNearestFrontier(self, cur_pos, frontiers): + dis_min = sys.maxsize + frontier_best = None + for frontier in frontiers: + dis = self.getDistance(frontier, cur_pos) + if dis <= dis_min: + dis_min = dis + frontier_best = frontier + return frontier_best + + def cal_distance_to_robot(self,objx,objy,objz): scene = self.status diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py index 82870d7..fb04a0f 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/robowaiter/scene/tasks/AEM.py @@ -2,6 +2,7 @@ 环境主动探索和记忆 要求输出探索结果（语义地图）对环境重点信息记忆。生成环境的语义拓扑地图，和不少于10个环境物品的识别和位置记忆，可以是图片或者文字或者格式化数据。 """ +import pickle from robowaiter.scene.scene import Scene class SceneAEM(Scene): @@ -12,9 +13,25 @@ class SceneAEM(Scene): pass def _run(self): cur_objs = [] + objs_name_set = set() + file_name = '../../proto/map_1.pkl' + if os.path.exists(file_name): + with open(file_name, 'rb') as file: + map = pickle.load(file) print('------------ 自主探索 ------------') - cur_objs = self.semantic_map.navigation_move(cur_objs, 0, 11) - print("物品列表如下：") + # cur_objs = self.semantic_map.navigation_move(cur_objs, 0, 11) + # print("物品列表如下：") + # print(cur_objs) + # cur_pos = [int(scene.location.X), int(scene.location.Y)] + # print(reachable([237,490])) + # navigation_move([[237,490]], i, map_id) + # navigation_test(i,map_id) + while True: + goal = self.explore(map, 120) # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取 + if goal is None: + break + cur_objs, objs_name_set = self.navigation_move(cur_objs, objs_name_set, [[goal[0], goal[1]]], 0, 11) + print("------------物品信息--------------") print(cur_objs) pass 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)