Merge branch 'main' of github.com:HPCL-EI/RoboWaiter

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_place = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3'}
-    all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk',
+    # all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk',
-                  'VacuumCup'}
+    #               '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), # 位置需要更改！！！
+        'CoffeeTable':(250.0, 310.0, 100.0),
-        'Table1': (247.0, 520.0, 100.0),# 位置需要更改！！！
+        'Table1': (340.0, 900.0, 98.0),
         'Table2': (-55.0, 0.0, 107),
         'Table3':(-55.0, 150.0, 107)
     }

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

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:
+        else: # 走到物品边上
+            obj_id = -1
+            obj_dict = self.scene.status.objects
+            if len(obj_dict)!=0:
                 # 获取obj_id
-            for id,obj in enumerate(self.scene.objects):
+                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
 

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
 
 

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
 

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

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(MilkDrink, Table2)}
-{On(ADMilk, CoffeeTable)}
+{On(MilkDrink, Bar2)}
-{On(VacuumCup, Bar2)}
 {On(Yogurt, WaterTable)}
+{On(Softdrink, Bar2)}
 {On(Softdrink, WaterTable)}
-{On(Softdrink, Bar)}
+{On(MilkDrink, WaterTable)}
-{On(Softdrink, Table2)}
+{On(ADMilk, Bar)}
-{On(Softdrink, Table1)}
+{On(MilkDrink, Table3)}
-{On(ADMilk, Bar2)}
+{On(MilkDrink, WaterTable)}
-{On(Milk, Table1)}
+{On(BottledDrink, CoffeeTable)}
-{On(Milk, Table1)}
+{On(VacuumCup, Table2)}
-{On(Milk, Bar2)}
+{On(MilkDrink, Bar)}
-{On(VacuumCup, 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(ACTemperature, 0)}
-{Is(TubeLight, 1)}
+{Is(Floor, 1)}
+{Is(AC, 1)}
+{Is(Floor, 1)}
+{Is(Floor, 1)}
 {Is(TubeLight, 0)}
 {Is(Table1, 0)}
-{Is(AC, 0)}
+{Is(Table1, 0)}
-{Is(ACTemperature, 1)}
+{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(Coffee, Table3)}
-{On(Dessert, Bar2)}
+{On(Water, WaterTable)}
-{On(Water, Table2)}
+{On(Coffee, Table2)}
-{On(Dessert, Table3)}
+{On(Coffee, Table2)}
-{On(Coffee, WaterTable)}
+{On(Water, Table3)}
-{On(Coffee, CoffeeTable)}
-{On(Coffee, WaterTable)}
-{On(Dessert, Bar2)}
-{On(Water, CoffeeTable)}
-{On(Water, Bar2)}
-{On(Dessert, Table2)}
 {On(Coffee, Bar2)}
-{On(Coffee, Bar)}
+{On(Coffee, WaterTable)}
-{On(Dessert, Bar)}
+{On(Water, CoffeeTable)}
-{On(Water, Table2)}
+{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)}

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

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

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:
+        walk_v = [X,Y,Yaw,velocity,dis_limit]
-            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)
         action = GrabSim_pb2.Action(
-            scene=self.sceneID,
+            scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v
-            action=GrabSim_pb2.Action.ActionType.WalkTo,
-            values=v
         )
-        scene_info = stub.Do(action)
+        scene = stub.Do(action)
-        return scene_info
+
+        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)
 

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

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"

sub_task.ptml

@@ -1,4 +1,26 @@
 selector{
-cond At(Robot,Table2)
+cond On(Coffee,CoffeeTable)
-act MoveTo(Table2)
+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)
+}
 }