Merge remote-tracking branch 'origin/main'

robowaiter/behavior_lib/_base/Behavior.py

@@ -34,6 +34,8 @@ class Bahavior(ptree.behaviour.Behaviour):
         'Dessert':'Plate'
     }
 
+
+    
     @classmethod
     def get_ins_name(cls,*args):
         name = cls.__name__

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

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<min_dis:
                             min_dis = dis
                             obj_id = id
@@ -64,7 +62,7 @@ class MoveTo(Act):
             if obj_id == -1:
                 return ptree.common.Status.FAILURE
 
-            print("self.target_place:",self.target_place,"id:",obj_id,"dis:",min_dis)
+            # print("self.target_place:",self.target_place,"id:",obj_id,"dis:",min_dis)
             self.scene.move_to_obj(obj_id=obj_id)
 
             # 为了演示，写死咖啡位置

robowaiter/behavior_lib/act/PutDown.py

@@ -22,6 +22,8 @@ class PutDown(Act):
         info["pre"] = {f'Holding({arg[0]})',f'At(Robot,{arg[1]})'}
         info["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'}
         info["del_set"] = {f'Holding({arg[0]})'}
+
+        info['cost'] = 100
         return info
 
 
@@ -36,4 +38,6 @@ 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"])
         return Status.RUNNING

robowaiter/behavior_lib/cond/On.py

@@ -16,6 +16,9 @@ class On(Cond):
     def _update(self) -> 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:

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"]:
+                    # if c <= self.scene.state["condition_set"]:
-                        return True
+                    #     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':
+
+                    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
 

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()

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)\"}"}}

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)""}"

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

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:

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)
+        x = math.floor((x + 200))
-        y = math.floor((y + 400) / 5)
+        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

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)
+        # cur_objs = self.semantic_map.navigation_move(cur_objs, 0, 11)
-        print("物品列表如下：")
+        # 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

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):

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()

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)