From 4721fcdf377a0ce048b583a2e3761913585210e6 Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Thu, 16 Nov 2023 22:31:08 +0800 Subject: [PATCH 01/14] test --- run_robowaiter.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/run_robowaiter.py b/run_robowaiter.py index dda629e..222b0bc 100644 --- a/run_robowaiter.py +++ b/run_robowaiter.py @@ -10,6 +10,8 @@ behavior_lib_path = os.path.join(project_path, 'behavior_lib') robot = Robot(ptml_path, behavior_lib_path) + + # create task task = task_map[TASK_NAME](robot) task.reset() From a5a4a1badce81cb46636d038ad381250da1fa307 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Thu, 16 Nov 2023 22:48:54 +0800 Subject: [PATCH 02/14] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=A3=80=E6=B5=8B?= =?UTF-8?q?=E8=A1=8C=E4=BA=BA=E5=88=B0=20Bar=20=E5=89=8D=E5=B9=B6=E8=87=AA?= =?UTF-8?q?=E5=8A=A8=20=E6=89=93=E6=8B=9B=E5=91=BC=20=E7=9A=84=E6=9D=A1?= =?UTF-8?q?=E4=BB=B6=E8=8A=82=E7=82=B9=E5=92=8C=E5=8A=A8=E4=BD=9C=E8=8A=82?= =?UTF-8?q?=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/act/ExploreEnv.py | 5 +- robowaiter/behavior_lib/act/GreatCustomer.py | 25 ++++++++ .../behavior_lib/cond/DetectCustomer.py | 31 ++++++++++ robowaiter/robot/Default.ptml | 11 +++- robowaiter/scene/scene.py | 8 --- robowaiter/scene/tasks/VLM/VLM2.py | 60 +------------------ 6 files changed, 71 insertions(+), 69 deletions(-) create mode 100644 robowaiter/behavior_lib/act/GreatCustomer.py create mode 100644 robowaiter/behavior_lib/cond/DetectCustomer.py diff --git a/robowaiter/behavior_lib/act/ExploreEnv.py b/robowaiter/behavior_lib/act/ExploreEnv.py index 3b832b1..8179d2b 100644 --- a/robowaiter/behavior_lib/act/ExploreEnv.py +++ b/robowaiter/behavior_lib/act/ExploreEnv.py @@ -3,7 +3,8 @@ from typing import Any from robowaiter.behavior_lib._base.Act import Act class ExploreEnv(Act): - can_be_expanded = True + # can_be_expanded = True + can_be_expanded = False num_args=0 valid_args=() @@ -20,8 +21,6 @@ class ExploreEnv(Act): def _update(self) -> ptree.common.Status: # explore algorithm - - self.scene.state["condition_set"]|= self.info["add"] return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/act/GreatCustomer.py b/robowaiter/behavior_lib/act/GreatCustomer.py new file mode 100644 index 0000000..ce186ed --- /dev/null +++ b/robowaiter/behavior_lib/act/GreatCustomer.py @@ -0,0 +1,25 @@ +import py_trees as ptree +from robowaiter.behavior_lib._base.Act import Act +from robowaiter.algos.navigator.navigate import Navigator + +class GreatCustomer(Act): + can_be_expanded = False + num_args = 0 + valid_args = () + + def __init__(self, *args): + super().__init__(*args) + + @classmethod + def get_info(cls): + info = {} + info['pre'] = set() + info["add"] = set() + info["del_set"] = set() + info['cost']=0 + return info + + def _update(self) -> ptree.common.Status: + + self.scene.chat_bubble("欢迎光临！请问有什么可以帮您？") + return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/cond/DetectCustomer.py b/robowaiter/behavior_lib/cond/DetectCustomer.py new file mode 100644 index 0000000..76d2232 --- /dev/null +++ b/robowaiter/behavior_lib/cond/DetectCustomer.py @@ -0,0 +1,31 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Cond import Cond +import itertools + +class DetectCustomer(Cond): + can_be_expanded = False + num_params = 0 + valid_args = () + + def __init__(self,*args): + super().__init__(*args) + + + def _update(self) -> ptree.common.Status: + + # 获取customer的位置 + # bar (247.0, 520.0, 100.0) + close_to_bar = False + scene = self.scene.status + for walker in scene.walkers: + x, y, yaw = walker.pose.X, walker.pose.Y, walker.pose.Yaw + # 到达一定区域就打招呼 + if y >= 450 and y <= 620 and x >= 40 and x <= 100 and yaw>=-10 and yaw <=10: + close_to_bar = True + break + + if close_to_bar: + return ptree.common.Status.SUCCESS + else: + return ptree.common.Status.FAILURE \ No newline at end of file diff --git a/robowaiter/robot/Default.ptml b/robowaiter/robot/Default.ptml index 62a677c..1a1c2b2 100644 --- a/robowaiter/robot/Default.ptml +++ b/robowaiter/robot/Default.ptml @@ -14,5 +14,14 @@ selector act SubTaskPlaceHolder() } } - + sequence + { + selector + { + cond At(Robot,Bar) + act MoveTo(Bar) + } + cond DetectCustomer() + act GreatCustomer + } } \ No newline at end of file diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index e0d18b2..567ba9d 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -700,11 +700,3 @@ class Scene: ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100] return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2) - # def test_yaw(self): - # walk_v = [247.0, 480.0, 180.0, 180, 0] - # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) - # time.sleep(4) - # walk_v = [247.0, 500.0, 0.0, 180, 0] - # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) \ No newline at end of file diff --git a/robowaiter/scene/tasks/VLM/VLM2.py b/robowaiter/scene/tasks/VLM/VLM2.py index fa3595f..7a13acc 100644 --- a/robowaiter/scene/tasks/VLM/VLM2.py +++ b/robowaiter/scene/tasks/VLM/VLM2.py @@ -11,28 +11,11 @@ class SceneVLM(Scene): 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) + self.gen_obj() pass def _run(self, op_type=10): @@ -42,17 +25,13 @@ class SceneVLM(Scene): # 带领行人去有太阳的地方 # 行人说 有点热 # 好的，这就去开空调 - - scene = self.add_walkers([[47, 920],[70,-200]]) + scene = self.add_walkers([[47, 920]]) self.control_walker( - [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=47, Y=520, Yaw=0)]) + [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=0)]) cont = scene.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳" self.control_robot_action(0,3,cont) - # self.clean_walker() - - # 共17个操作 # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 @@ -73,39 +52,6 @@ class SceneVLM(Scene): # 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 From c748810ec2dabab9fcce522712473c8315bef944 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Thu, 16 Nov 2023 23:05:11 +0800 Subject: [PATCH 03/14] =?UTF-8?q?=E8=A7=A3=E5=86=B3=E4=BA=86=E9=85=B8?= =?UTF-8?q?=E5=A5=B6=E6=8E=89=E5=9C=B0=E4=B8=8A=E7=9A=84bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Behavior.py | 3 +-- robowaiter/behavior_lib/act/GreatCustomer.py | 2 ++ robowaiter/behavior_lib/act/Make.py | 1 + robowaiter/behavior_lib/act/MoveTo.py | 2 +- robowaiter/behavior_lib/act/PutDown.py | 2 +- 5 files changed, 6 insertions(+), 4 deletions(-) diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 92eb9d4..8d5460e 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -29,8 +29,7 @@ class Bahavior(ptree.behaviour.Behaviour): '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), + 'Table1': (340.0, 900.0, 99.0), 'Table2': (-55.0, 0.0, 107), 'Table3':(-55.0, 150.0, 107) } diff --git a/robowaiter/behavior_lib/act/GreatCustomer.py b/robowaiter/behavior_lib/act/GreatCustomer.py index ce186ed..5ac8844 100644 --- a/robowaiter/behavior_lib/act/GreatCustomer.py +++ b/robowaiter/behavior_lib/act/GreatCustomer.py @@ -21,5 +21,7 @@ class GreatCustomer(Act): def _update(self) -> ptree.common.Status: + goal = Act.place_xyz_dic['Bar'] + self.scene.walk_to(goal[0]-5,goal[1], 180, 180, 0) self.scene.chat_bubble("欢迎光临！请问有什么可以帮您？") return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 88f828e..f3e9b38 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -34,6 +34,7 @@ class Make(Act): info["add"] |= {f'On({arg},WaterTable)'} elif arg == cls.valid_args[2]: info["add"] |= {f'On({arg},Bar)'} + info['cost'] = 10 return info def _update(self) -> ptree.common.Status: diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index 6c31ad5..3ca9167 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']=10 + info['cost']=5 return info diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py index 7187a69..48723ca 100644 --- a/robowaiter/behavior_lib/act/PutDown.py +++ b/robowaiter/behavior_lib/act/PutDown.py @@ -23,7 +23,7 @@ class PutDown(Act): info["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'} info["del_set"] = {f'Holding({arg[0]})'} - info['cost'] = 100 + info['cost'] = 1 return info From 40e1959d1b88d9adcfba9cfdc3767481cb528ad9 Mon Sep 17 00:00:00 2001 From: liwang zhang <2638950452@qq.com> Date: Fri, 17 Nov 2023 09:42:09 +0800 Subject: [PATCH 04/14] =?UTF-8?q?=E6=B7=BB=E5=8A=A0map=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/scene/scene.py | 188 ++++++++++++++++++++------------------ 1 file changed, 97 insertions(+), 91 deletions(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index ee888fe..f4389d3 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -1,3 +1,4 @@ +import pickle import sys import time import grpc @@ -9,9 +10,6 @@ import math from robowaiter.proto import GrabSim_pb2 from robowaiter.proto import GrabSim_pb2_grpc - - - channel = grpc.insecure_channel( "localhost:30001", options=[ @@ -32,13 +30,13 @@ def init_world(scene_num=1, mapID=11): def show_image(camera_data): print('------------------show_image----------------------') - #获取第0张照片 + # 获取第0张照片 im = camera_data.images[0] - #使用numpy(np) 数值类型矩阵的frombuffer，将im.data以流的形式（向量的形式）读入，在变型reshape成三位矩阵的形式(长度，宽度，深度)即三阶张量 + # 使用numpy(np) 数值类型矩阵的frombuffer，将im.data以流的形式（向量的形式）读入，在变型reshape成三位矩阵的形式(长度，宽度，深度)即三阶张量 d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels)) - #matplotlib中的plt方法 对矩阵d 进行图形绘制，如果 深度相机拍摄的带深度的图片（图片名字中有depth信息），则转换成黑白图即灰度图 + # matplotlib中的plt方法 对矩阵d 进行图形绘制，如果 深度相机拍摄的带深度的图片（图片名字中有depth信息），则转换成黑白图即灰度图 plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None) - #图像展示在屏幕上 + # 图像展示在屏幕上 plt.show() return d @@ -58,10 +56,10 @@ class Scene: "sub_goal_list": [], # 子目标列表 "status": None, # 仿真器中的观测信息，见下方详细解释 "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)', - 'Holding(Nothing)', - # 'Holding(Yogurt)' - 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)', - 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'} + 'Holding(Nothing)', + # 'Holding(Yogurt)' + 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)', + 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'} } """ status: @@ -76,7 +74,7 @@ class Scene: collision: str, info: str """ - def __init__(self,robot=None, sceneID=0): + def __init__(self, robot=None, sceneID=0): self.sceneID = sceneID self.use_offset = False self.start_time = time.time() @@ -91,26 +89,29 @@ class Scene: # myx op # 1-7 正常执行, 8-10 控灯操作移动到6, 11-12窗帘操作不需要移动, - self.op_dialog = ["","制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 - "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 - "调整空调开关","调高空调温度","调低空调温度", # 13-15 - "抓握物体","放置物体"] # 16-17 - self.op_act_num = [0,3,4,6,3,2,0,1, - 0,0,0,0,0, - 0,0,0, - 0,0] - self.op_v_list = [[0.0,0.0],[250.0, 310.0],[-70.0, 480.0],[250.0, 630.0],[-70.0, 740.0],[260.0, 1120.0],[300.0, -220.0], + self.op_dialog = ["", "制作咖啡", "倒水", "夹点心", "拖地", "擦桌子", "开筒灯", "搬椅子", # 1-7 + "关筒灯", "开大厅灯", "关大厅灯", "关闭窗帘", "打开窗帘", # 8-12 + "调整空调开关", "调高空调温度", "调低空调温度", # 13-15 + "抓握物体", "放置物体"] # 16-17 + self.op_act_num = [0, 3, 4, 6, 3, 2, 0, 1, + 0, 0, 0, 0, 0, + 0, 0, 0, + 0, 0] + self.op_v_list = [[0.0, 0.0], [250.0, 310.0], [-70.0, 480.0], [250.0, 630.0], [-70.0, 740.0], [260.0, 1120.0], + [300.0, -220.0], [0.0, -70.0]] - self.op_typeToAct = {8:[6,2],9:[6,3],10:[6,4],11:[8,1],12:[8,2]} + self.op_typeToAct = {8: [6, 2], 9: [6, 3], 10: [6, 4], 11: [8, 1], 12: [8, 2]} # 空调面板位置 - self.obj_loc = [300.5, -140.0,114] + self.obj_loc = [300.5, -140.0, 114] # AEM self.visited = set() self.all_frontier_list = set() self.semantic_map = semantic_map self.auto_map = np.ones((800, 1550)) - + self.filename = "../proto/map_1.pkl" + with open(self.filename, 'rb') as file: + self.map_file = pickle.load(file) def reset(self): # 基类reset，默认执行仿真器初始化操作 @@ -141,15 +142,15 @@ class Scene: self.robot.step() def deal_event(self): - if len(self.event_list)>0: + if len(self.event_list) > 0: next_event = self.event_list[0] - t,func = next_event + t, func = next_event if self.time >= t: print(f'event: {t}, {func.__name__}') self.event_list.pop(0) func() - def create_chat_event(self,sentence): + def create_chat_event(self, sentence): def customer_say(): print(f'顾客说：{sentence}') if self.show_bubble: @@ -165,7 +166,7 @@ class Scene: def reset_sim(self): # reset world init_world() - + stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) def _reset(self): @@ -180,10 +181,6 @@ class Scene: # 场景自定义的step pass - - - - def walker_control_generator(self, walkerID, autowalk, speed, X, Y, Yaw): if self.use_offset: X, Y = X + loc_offset[0], Y + loc_offset[1] @@ -195,7 +192,7 @@ class Scene: ) def walk_to(self, X, Y, Yaw=100, velocity=200, dis_limit=0): - walk_v = [X,Y,Yaw,velocity,dis_limit] + walk_v = [X, Y, Yaw, velocity, dis_limit] action = GrabSim_pb2.Action( scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v ) @@ -203,7 +200,6 @@ class Scene: return scene - def reachable_check(self, X, Y, Yaw): if self.use_offset: X, Y = X + loc_offset[0], Y + loc_offset[1] @@ -219,11 +215,11 @@ class Scene: else: return True - def add_walkers(self,walker_loc=[[0, 880], [250, 1200], [-55, 750], [70, -200]]): + def add_walkers(self, walker_loc=[[0, 880], [250, 1200], [-55, 750], [70, -200]]): print('------------------add_walkers----------------------') walker_list = [] for i in range(len(walker_loc)): - loc = walker_loc[i] + [0,0, 100] + loc = walker_loc[i] + [0, 0, 100] action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc) scene = stub.Do(action) print(scene.info) @@ -383,13 +379,14 @@ class Scene: def ik_control_joints(self, handNum=2, x=30, y=40, z=80): # print('------------------ik_control_joints----------------------') # IK控制,双手, 1左手, 2右手; 暂时只动右手 - HandPostureObject = [GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=handNum, x=x, y=y, z=z, roll=0, pitch=0, yaw=0), - # GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=1, x=0, y=0, z=0, roll=0, pitch=0, yaw=0), - ] - temp = stub.GetIKControlInfos(GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject)) + HandPostureObject = [ + GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=handNum, x=x, y=y, z=z, roll=0, pitch=0, yaw=0), + # GrabSim_pb2.HandPostureInfos.HandPostureObject(handNum=1, x=0, y=0, z=0, roll=0, pitch=0, yaw=0), + ] + temp = stub.GetIKControlInfos( + GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject)) - - def move_to_obj(self,obj_id): + def move_to_obj(self, obj_id): scene = self.status obj_info = scene.objects[obj_id] # Robot @@ -399,28 +396,27 @@ class Scene: walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0] obj_x += 3 obj_y += 2.5 - walk_v[0]+=1 - print("walk:",walk_v) + walk_v[0] += 1 + print("walk:", walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) - # 移动到进行操作任务的指定地点 - def move_task_area(self,op_type,obj_id=0, release_pos=[247.0, 520.0, 100.0]): + def move_task_area(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]): scene = self.status cur_pos = [scene.location.X, scene.location.Y, scene.rotation.Yaw] print("Current Position:", cur_pos, "开始任务:", self.op_dialog[op_type]) - if op_type==11 or op_type==12: # 开关窗帘不需要移动 + if op_type == 11 or op_type == 12: # 开关窗帘不需要移动 return print('------------------moveTo_Area----------------------') if op_type < 8: - walk_v = self.op_v_list[op_type] + [scene.rotation.Yaw, 180, 0] # 动画控制 - print("walk_v:",walk_v) - if op_type>=8 and op_type<=10: walk_v = self.op_v_list[6] + [scene.rotation.Yaw, 180, 0] # 控灯 - if op_type in [13,14,15]: walk_v = [240, -140.0] + [0, 180, 0] # 空调 - if op_type==16: # 抓握物体，移动到物体周围的可达区域 + walk_v = self.op_v_list[op_type] + [scene.rotation.Yaw, 180, 0] # 动画控制 + print("walk_v:", walk_v) + if op_type >= 8 and op_type <= 10: walk_v = self.op_v_list[6] + [scene.rotation.Yaw, 180, 0] # 控灯 + if op_type in [13, 14, 15]: walk_v = [240, -140.0] + [0, 180, 0] # 空调 + if op_type == 16: # 抓握物体，移动到物体周围的可达区域 scene = self.status obj_info = scene.objects[obj_id] # Robot @@ -430,11 +426,11 @@ class Scene: walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0] obj_x += 3 obj_y += 2.5 - if op_type==17: # 放置物体，移动到物体周围的可达区域 + if op_type == 17: # 放置物体，移动到物体周围的可达区域 walk_v = release_pos[:-1] + [180, 180, 0] if release_pos == [340.0, 900.0, 99.0]: walk_v[2] = 130 - print("walk_v:",walk_v) + print("walk_v:", walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) @@ -453,12 +449,12 @@ class Scene: print(scene.info) return False - def adjust_kongtiao(self,op_type): - print("self.obj_loc:",self.obj_loc) + def adjust_kongtiao(self, op_type): + print("self.obj_loc:", self.obj_loc) obj_loc = self.obj_loc[:] - print("obj_loc:",obj_loc,"self.obj_loc:", self.obj_loc) + print("obj_loc:", obj_loc, "self.obj_loc:", self.obj_loc) obj_loc[2] -= 5 - print("obj_loc:",obj_loc) + print("obj_loc:", obj_loc) if op_type == 13: obj_loc[1] -= 2 if op_type == 14: obj_loc[1] -= 0 if op_type == 15: obj_loc[1] += 2 @@ -467,42 +463,44 @@ class Scene: self.robo_recover() return True - def gen_obj(self,h=100): + def gen_obj(self, h=100): # 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 14:cake 61:甜牛奶 scene = self.status 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), - # 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), - ] + 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), + # 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), + ] scene = stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID)) time.sleep(1.0) - def grasp_obj(self,obj_id,hand_id=1): + def grasp_obj(self, obj_id, hand_id=1): scene = self.status obj_info = scene.objects[obj_id] obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z - if obj_info.name=="CoffeeCup": + if obj_info.name == "CoffeeCup": pass - if obj_info.name=="Glass": + if obj_info.name == "Glass": pass # Finger - self.ik_control_joints(2, obj_x-9, obj_y, obj_z) # -10, 0, 0 + self.ik_control_joints(2, obj_x - 9, obj_y, obj_z) # -10, 0, 0 time.sleep(3.0) # Grasp Obj print('------------------grasp_obj----------------------') - action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp, values=[hand_id, obj_id]) + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp, + values=[hand_id, obj_id]) scene = stub.Do(action) time.sleep(3.0) return True # robot的肢体恢复原位 def robo_recover(self): - action = GrabSim_pb2.Action(scene=self.sceneID,action=GrabSim_pb2.Action.ActionType.RotateJoints, # 恢复原位 + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, # 恢复原位 values=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) scene = stub.Do(action) - def robo_stoop_parallel(self): # 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊 scene = self.status @@ -510,14 +508,14 @@ class Scene: angle[0] = 15 angle[19] = -15 angle[20] = -30 - action = GrabSim_pb2.Action(scene=self.sceneID,action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰 + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰 values=angle) scene = stub.Do(action) time.sleep(1.0) - def release_obj(self,release_pos): + def release_obj(self, release_pos): print("------------------release_obj----------------------") - if release_pos==[340.0, 900.0, 99.0]: + if release_pos == [340.0, 900.0, 99.0]: self.ik_control_joints(2, 300.0, 935, release_pos[2]) time.sleep(2.0) else: @@ -533,30 +531,31 @@ class Scene: return True # 执行过程：输出"开始(任务名)" -> 按步骤数执行任务 -> Robot输出成功或失败的对话 - def op_task_execute(self,op_type,obj_id=0,release_pos=[247.0, 520.0, 100.0]): - self.control_robot_action(0, 1, "开始"+self.op_dialog[op_type]) # 开始制作咖啡 - if op_type<8: result = self.control_robot_action(op_type, 1) - if op_type>=8 and op_type<=12: result = self.control_robot_action(self.op_typeToAct[op_type][0], self.op_typeToAct[op_type][1]) - if op_type in [13,14,15]: # 调整空调:13代表按开关,14升温,15降温 + def op_task_execute(self, op_type, obj_id=0, release_pos=[247.0, 520.0, 100.0]): + self.control_robot_action(0, 1, "开始" + self.op_dialog[op_type]) # 开始制作咖啡 + if op_type < 8: result = self.control_robot_action(op_type, 1) + if op_type >= 8 and op_type <= 12: result = self.control_robot_action(self.op_typeToAct[op_type][0], + self.op_typeToAct[op_type][1]) + if op_type in [13, 14, 15]: # 调整空调:13代表按开关,14升温,15降温 result = self.adjust_kongtiao(op_type) - if op_type ==16: # 抓握物体 + if op_type == 16: # 抓握物体 result = self.grasp_obj(obj_id) - if op_type ==17: # 放置物体 + if op_type == 17: # 放置物体 result = self.release_obj(release_pos) self.control_robot_action(0, 2) if result: - if self.op_act_num[op_type]>0: - for i in range(2,2+self.op_act_num[op_type]): - self.control_robot_action(op_type,i) + if self.op_act_num[op_type] > 0: + for i in range(2, 2 + self.op_act_num[op_type]): + self.control_robot_action(op_type, i) self.control_robot_action(0, 2) # self.control_robot_action(0, 1, "成功"+self.op_dialog[op_type]) # else: # self.control_robot_action(0, 1, self.op_dialog[op_type]+"失败") - def find_obj(self,name): + def find_obj(self, name): for id, item in enumerate(self.status.objects): if item.name == name: - print("name:",name,"id:",id,"X:",item.location.X,"Y:",item.location.Y,"Z:",item.location.Z,) + print("name:", name, "id:", id, "X:", item.location.X, "Y:", item.location.Y, "Z:", item.location.Z, ) def test_move(self): v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] @@ -584,7 +583,8 @@ class Scene: 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) - cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, objs_name_set) + cur_objs, objs_name_set = camera.get_semantic_map(GrabSim_pb2.CameraName.Head_Segment, cur_objs, + objs_name_set) # if scene.info == "Unreachable": print(scene.info) return cur_objs, objs_name_set @@ -669,11 +669,9 @@ class Scene: frontier_best = frontier return frontier_best - - - def cal_distance_to_robot(self,objx,objy,objz): + def cal_distance_to_robot(self, objx, objy, objz): scene = self.status - ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100] + ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y), 100] return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2) # def test_yaw(self): @@ -683,4 +681,12 @@ class Scene: # time.sleep(4) # walk_v = [247.0, 500.0, 0.0, 180, 0] # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) \ No newline at end of file + # scene = stub.Do(action) + + # 读取map文件判断是否可达 + def reachable(self, pos): + x, y = self.real2map(pos[0], pos[1]) + if self.map_file[x, y] == 1: + return False + else: + return True From 956203300ba905cd8109d035c7a7bf44d8b45f49 Mon Sep 17 00:00:00 2001 From: liwang zhang <2638950452@qq.com> Date: Fri, 17 Nov 2023 09:57:58 +0800 Subject: [PATCH 05/14] =?UTF-8?q?=E6=B7=BB=E5=8A=A0map=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/scene/scene.py | 26 +++++++++++++++++++------- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 567ba9d..110e9e0 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -1,3 +1,4 @@ +import pickle import sys import time import grpc @@ -110,21 +111,24 @@ class Scene: self.all_frontier_list = set() self.semantic_map = semantic_map self.auto_map = np.ones((800, 1550)) + self.filename = "../proto/map_1.pkl" + with open(self.filename, 'rb') as file: + self.map_file = pickle.load(file) def reset(self): - # 基类reset，默认执行仿真器初始化操作 - self.reset_sim() + # 基类reset，默认执行仿真器初始化操作 + self.reset_sim() - # reset state - self.state = self.default_state + # reset state + self.state = self.default_state - print("场景初始化完成") - self._reset() + print("场景初始化完成") + self._reset() - self.running = True + self.running = True def run(self): # 基类run @@ -700,3 +704,11 @@ class Scene: ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100] return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2) + # 根据map文件判断是否可达 + def reachable(self, pos): + x, y = self.real2map(pos[0], pos[1]) + if self.map_file[x, y] == 0: + return True + else: + return False + From b899599b042a06e900a4378f1adf6c19f6d9b449 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Fri, 17 Nov 2023 12:53:16 +0800 Subject: [PATCH 06/14] =?UTF-8?q?=E6=9B=B4=E6=96=B0=20AEM=20=E5=9C=B0?= =?UTF-8?q?=E5=9B=BE=E8=B7=AF=E5=BE=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/robot/robot.py | 1 + robowaiter/scene/scene.py | 23 ++++++++++++----------- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index 85d135a..bc79121 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -37,6 +37,7 @@ class Robot(object): def set_scene(self,scene): self.scene = scene + def load_BT(self): self.bt = load_bt_from_ptml(self.scene, self.ptml_path,self.behavior_lib_path) sub_task_place_holder = find_node_by_name(self.bt.root,"SubTaskPlaceHolder()") diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 110e9e0..c625a9a 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -10,8 +10,9 @@ import math from robowaiter.proto import GrabSim_pb2 from robowaiter.proto import GrabSim_pb2_grpc - - +import os +from robowaiter.utils import get_root_path +root_path = get_root_path() channel = grpc.insecure_channel( "localhost:30001", @@ -111,24 +112,24 @@ class Scene: self.all_frontier_list = set() self.semantic_map = semantic_map self.auto_map = np.ones((800, 1550)) - self.filename = "../proto/map_1.pkl" + self.filename = os.path.join(root_path, 'robowaiter/proto/map_1.pkl') with open(self.filename, 'rb') as file: self.map_file = pickle.load(file) def reset(self): - # 基类reset，默认执行仿真器初始化操作 - self.reset_sim() + # 基类reset，默认执行仿真器初始化操作 + self.reset_sim() - # reset state - self.state = self.default_state + # reset state + self.state = self.default_state - print("场景初始化完成") - self._reset() + print("场景初始化完成") + self._reset() - self.running = True + self.running = True def run(self): # 基类run @@ -180,8 +181,8 @@ class Scene: def reset_sim(self): # reset world init_world() - stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) + stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) def _reset(self): # 场景自定义的reset From 30eb82f7a8d39de780b8bcfcf5f3f6a1d7f72e68 Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Fri, 17 Nov 2023 13:07:34 +0800 Subject: [PATCH 07/14] Update scene.py --- robowaiter/scene/scene.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index e0d18b2..b69c219 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -507,6 +507,9 @@ class Scene: obj_info = scene.objects[obj_id] obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z if obj_info.name=="CoffeeCup": + values = [0,0,0,0,0, -15,0,0,0,0] + # values= [-6, 0, 0, 0, 0, -6, 0, 45, 45, 45] + stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values)) pass if obj_info.name=="Glass": pass @@ -526,6 +529,11 @@ class Scene: values=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) scene = stub.Do(action) + def standard_finger(self): + values = [0,0,0,0,0, 0,0,0,0,0] + stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values)) + time.sleep(1.0) + def robo_stoop_parallel(self): # 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊 @@ -553,6 +561,7 @@ class Scene: scene = stub.Do(action) time.sleep(2.0) self.robo_recover() + self.standard_finger() return True @@ -700,11 +709,4 @@ class Scene: ginger_x, ginger_y, ginger_z = [int(scene.location.X), int(scene.location.Y),100] return math.sqrt((ginger_x - objx) ** 2 + (ginger_y - objy) ** 2 + (ginger_z - objz) ** 2) - # def test_yaw(self): - # walk_v = [247.0, 480.0, 180.0, 180, 0] - # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) - # time.sleep(4) - # walk_v = [247.0, 500.0, 0.0, 180, 0] - # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) \ No newline at end of file + From 1e5a05f48ffbaabc1600895d2e7a1a5572deea32 Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Fri, 17 Nov 2023 13:15:00 +0800 Subject: [PATCH 08/14] update --- run_robowaiter.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/run_robowaiter.py b/run_robowaiter.py index 222b0bc..dda629e 100644 --- a/run_robowaiter.py +++ b/run_robowaiter.py @@ -10,8 +10,6 @@ behavior_lib_path = os.path.join(project_path, 'behavior_lib') robot = Robot(ptml_path, behavior_lib_path) - - # create task task = task_map[TASK_NAME](robot) task.reset() From e71b1c217f1b0e3d7148eff3d057c2bca9d35ca7 Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Fri, 17 Nov 2023 13:18:09 +0800 Subject: [PATCH 09/14] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=8C=96=E6=97=B6?= =?UTF-8?q?=E6=B8=85=E7=A9=BA=E8=A1=8C=E4=BA=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/scene/scene.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 98d2041..05e4c72 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -180,9 +180,10 @@ class Scene: def reset_sim(self): # reset world + stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) init_world() stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) - stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) + def _reset(self): # 场景自定义的reset From 5e1801986ced51bf3654b00a6a0692882f86e3b3 Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Fri, 17 Nov 2023 16:12:34 +0800 Subject: [PATCH 10/14] Update scene.py --- robowaiter/scene/scene.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 05e4c72..8837713 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -464,7 +464,6 @@ class Scene: walk_v = release_pos[:-1] + [180, 180, 0] if release_pos == [340.0, 900.0, 99.0]: walk_v[2] = 130 - print("walk_v:",walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) scene = stub.Do(action) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) @@ -513,7 +512,9 @@ class Scene: obj_info = scene.objects[obj_id] obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z if obj_info.name=="CoffeeCup": - values = [0,0,0,0,0, -15,0,0,0,0] + # obj_x += 1 + # obj_y -= 1 + # values = [0,0,0,0,0, 10,-25,-45,-45,-45] # values= [-6, 0, 0, 0, 0, -6, 0, 45, 45, 45] stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values)) pass @@ -545,7 +546,7 @@ class Scene: # 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊 scene = self.status angle = [scene.joints[i].angle for i in range(21)] - angle[0] = 15 + angle[0] = 15 # 15 angle[19] = -15 angle[20] = -30 action = GrabSim_pb2.Action(scene=self.sceneID,action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰 @@ -556,7 +557,7 @@ class Scene: def release_obj(self,release_pos): print("------------------release_obj----------------------") if release_pos==[340.0, 900.0, 99.0]: - self.ik_control_joints(2, 300.0, 935, release_pos[2]) + self.ik_control_joints(2, release_pos[0]-40, release_pos[1]+35, release_pos[2]) time.sleep(2.0) else: self.ik_control_joints(2, release_pos[0] - 80, release_pos[1], release_pos[2]) @@ -725,3 +726,4 @@ class Scene: + From ea0a6fc041217638b78176bac5e33c199f2f4b6a Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Fri, 17 Nov 2023 16:22:31 +0800 Subject: [PATCH 11/14] tool_api --- robowaiter/llm_client/find_obj_utils.py | 122 ++++++++++++++++++++++++ robowaiter/llm_client/tool_api.py | 5 +- 2 files changed, 126 insertions(+), 1 deletion(-) create mode 100644 robowaiter/llm_client/find_obj_utils.py diff --git a/robowaiter/llm_client/find_obj_utils.py b/robowaiter/llm_client/find_obj_utils.py new file mode 100644 index 0000000..a4b541a --- /dev/null +++ b/robowaiter/llm_client/find_obj_utils.py @@ -0,0 +1,122 @@ +import random + +# all_obj_en = ['Mug', 'Banana', 'Toothpaste', 'Bread', 'Softdrink', 'Yogurt', 'ADMilk', 'VacuumCup', 'BottledDrink', 'PencilVase', +# 'Teacup', 'Caddy', 'Dictionary', 'Cake', 'Date', 'Stapler', 'LunchBox', 'Bracelet', 'MilkDrink', 'CocountWater', 'Walnut', 'HamSausage', +# 'GlueStick', 'AdhensiveTape', 'Calculator', 'Chess', 'Orange', 'Glass', 'Washbowl', 'Durian', 'Gum', 'Towl', 'OrangeJuice', 'Cardcase', +# 'RubikCube', 'StickyNotes', 'NFCJuice', 'SpringWater', 'Apple', 'Coffee', 'Gauze', 'Mangosteen', 'SesameSeedCake', 'Glove', 'Mouse', +# 'Kettle', 'Atomize', 'Chips', 'SpongeGourd', 'Garlic', 'Potato', 'Tray', 'Hemomanometer', 'TennisBall', 'ToyDog', 'ToyBear', 'TeaTray', +# 'Sock', 'Scarf', 'ToiletPaper', 'Milk', 'Soap', 'Novel', 'Watermelon', 'Tomato', 'CleansingFoam', 'CocountMilk', 'SugarlessGum', +# 'MedicalAdhensiveTape', 'SourMilkDrink', 'PaperCup', 'Tissue', 'YogurtDrink', 'Newspaper', 'Box', 'PaperCupStarbucks', 'CoffeeMachine', +# 'Straw', 'Cake', 'Tray', 'Bread', 'Glass', 'Door', 'Mug', 'Machine', 'PackagedCoffee', 'CubeSugar', +# 'Apple', 'Spoon', 'Drinks', 'Drink', 'Take-AwayCup', 'Saucer', 'TrashBin', 'Knife', 'Ginger', 'Floor', 'Roof', 'Wall'] + \ +# ['Broom', 'CoffeeCup', 'Door', 'Glass', 'Mug', 'ZhuZi', 'Towel', 'LaJiTong', 'CoffeeMachine', 'ZaoTai', 'Sofa', 'Cake', 'ChaTou', +# 'Plate', 'CoffeeBag', 'TuoBu', 'KaiGuan', 'Kettle', 'Apple', 'ChaZuo', 'Sugar', 'BaTai', 'BaoJing', 'DrinkMachine', 'KongTiao', +# 'Desk', 'Clip', 'Knife', 'TuoPan', 'BoJi', 'ZhiBeiHe', 'Bread', 'WaterCup', 'Box', 'Chair', 'Hand', 'XiGuan', 'Spoon', 'Container', +# 'IceMachine', 'KaoXiang', 'SaoBa', 'XiangGui'] + +# , '机器人左手', '机器人右手', 'Bernachon' +all_obj = ['马克杯', '香蕉', '牙膏', '面包', '软饮料', '酸奶', 'AD钙奶', '真空杯', '瓶装饮料', '铅笔花瓶', '茶杯', '茶匙', '词典', + '蛋糕', '日期', '订书机', '午餐盒', '手镯', '牛奶饮品', '椰水', '核桃', '火腿肠', '胶棒', '胶带', '计算器', '国际象棋', '橙子', + '玻璃杯', '洗碗盆', '榴莲', '口香糖', '毛巾', '橙汁', '卡片盒', '魔方', '便签', 'NFC汁', '矿泉水', '苹果', '咖啡', '纱布', '芒果', + '芝麻蛋糕', '手套', '鼠标', '水壶', '雾化器', '薯片', '丝瓜', '大蒜', '土豆', '托盘', '血压计', '网球', '玩具狗', '玩具熊', '茶盘', + '袜子', '围巾', '卫生纸', '牛奶', '肥皂', '小说', '西瓜', '番茄', '洁面泡沫', '椰奶', '无糖口香糖', '医用胶带', '酸奶饮品', '纸杯', + '纸巾', '酸奶饮品', '报纸', '盒子', '星巴克纸杯', '咖啡机', '吸管', '蛋糕', '托盘', '面包', '玻璃杯', '门', + '马克杯', '机器', '包装咖啡', '方糖', '苹果', '勺子', '饮料', '饮料', '外带杯', '茶杯', '垃圾桶', '刀', '姜', '地板', '屋顶', '墙'] + \ + ['扫帚', '咖啡杯', '门', '玻璃杯', '马克杯', '著子', '毛巾', '垃圾桶', '咖啡机', '灶台', '沙发', '蛋糕', '插头', '盘子', '咖啡袋', '拖布', '开关', + '水壶', '苹果', '插座', '糖', '吧台', '报警', '饮料机', '空调', '桌子', '夹子', '刀', '托盘', '簸箕', '纸杯盒', '面包', '水杯', '盒子', '椅子', + '手', '吸管', '勺子', '容器', '制冰机', '烤箱', '扫把', '香柜'] + +# 储藏室 +all_loc = ['吧台', '餐桌', '沙发', '灶台', '大门', '灯开关', '空调开关', '橱柜', '卫生间', '窗户', '音响', '休闲区', '工作台', '服务台', '收银台', '墙角', + '蛋糕柜', '充电处', '冰箱', '书架'] + +all_loc_en = ['bar', 'Table', 'sofa', 'stove', 'Gate', 'light switch', 'airconditioner switch', 'cabinet', 'bathroom', 'window', + 'audio', 'lounge area', 'workstation', 'service counter', 'cashier counter', 'corner', + 'cake display', 'ChargingPoint', 'refrigerator', 'bookshelf'] + +loc_map_en = {'bar': {'工作台', '服务台', '收银台', '蛋糕柜'}, + 'Table': {'沙发', '大门', '窗户', '休闲区', '墙角', '椅子', '书架'}, + 'sofa': {'餐桌', '窗户', '音响', '休闲区', '墙角', '书架'}, + 'stove': {'吧台', '橱柜', '工作台', '服务台', '收银台', '蛋糕柜', '冰箱'}, + 'Gate': {'吧台', '灯开关', '空调开关', '卫生间', '墙角'}, + 'light switch': {'大门', '空调开关', '卫生间', '墙角'}, + 'airconditioner switch': {'大门', '灯开关', '卫生间', '墙角'}, + 'cabinet': {'灶台', '吧台', '工作台', '服务台', '收银台', '蛋糕柜', '充电处', '冰箱'}, + 'bathroom': {'大门', '墙角'}, + 'window': {'餐桌', '沙发', '休闲区'}, + 'audio': {'餐桌', '沙发', '休闲区', '墙角', '书架'}, + 'lounge area': {'沙发', '餐桌', '墙角', '书架', '音响'}, + 'workstation': {'吧台', '服务台', '收银台'}, + 'service counter': {'吧台', '工作台', '收银台'}, + 'cashier counter': {'吧台', '工作台', '服务台'}, + 'corner': {'卫生间', '沙发', '灯开关', '空调开关', '音响', '休闲区', '书架'}, + 'cake display': {'吧台', '橱柜', '服务台', '收银台', '冰箱'}, + 'ChargingPoint': {'吧台', '餐桌', '沙发', '休闲区', '工作台', '服务台', '收银台', '墙角', '书架'}, + 'refrigerator': {'吧台', '服务台', '蛋糕柜'}, + 'bookshelf': {'餐桌', '沙发', '窗户', '休闲区', '墙角'} + } + + +loc_map = {'吧台': {'灶台', '橱柜', '工作台', '服务台', '收银台', '蛋糕柜', '充电处', '冰箱'}, + '餐桌': {'沙发', '大门', '窗户', '休闲区', '墙角', '椅子', '书架'}, + '沙发': {'餐桌', '窗户', '音响', '休闲区', '墙角', '书架'}, + '灶台': {'吧台', '橱柜', '工作台', '服务台', '收银台', '蛋糕柜', '冰箱'}, + '大门': {'吧台', '灯开关', '空调开关', '卫生间', '墙角'}, + '灯开关': {'大门', '空调开关', '卫生间', '墙角'}, + '空调开关': {'大门', '灯开关', '卫生间', '墙角'}, + '橱柜': {'灶台', '吧台', '工作台', '服务台', '收银台', '蛋糕柜', '充电处', '冰箱'}, + '卫生间': {'大门', '墙角'}, + '窗户': {'餐桌', '沙发', '休闲区'}, + '音响': {'餐桌', '沙发', '休闲区', '墙角', '书架'}, + '休闲区': {'沙发', '餐桌', '墙角', '书架', '音响'}, + '工作台': {'吧台', '服务台', '收银台'}, + '服务台': {'吧台', '工作台', '收银台'}, + '收银台': {'吧台', '工作台', '服务台'}, + '墙角': {'卫生间', '沙发', '灯开关', '空调开关', '音响', '休闲区', '书架'}, + '蛋糕柜': {'吧台', '橱柜', '服务台', '收银台', '冰箱'}, + '充电处': {'吧台', '餐桌', '沙发', '休闲区', '工作台', '服务台', '收银台', '墙角', '书架'}, + '冰箱': {'吧台', '灶台', '工作台', '服务台', '收银台', '蛋糕柜'}, + '书架': {'餐桌', '沙发', '窗户', '休闲区', '墙角'} + } + +# print("looc_map:", loc_map['吧台']) +# print("looc_map:", random.choice(list(loc_map['吧台']))) + + +# import synonyms +# def get_synonyms(word): +# synonyms_list = synonyms.nearby(word)[0] +# return synonyms_list +# +# target_word = "高兴" +# synonyms_result = get_synonyms(target_word) +# print("synonyms_result:",synonyms_result) + + +# def save_variable(self): +# scene = self.status +# init_obj_name_set = set() +# init_obj_info_dict = {} +# for item in scene.objects: +# name = item.name +# location = item.location +# init_obj_name_set.add(name) +# # 如果name已经存在，则添加一个唯一标识符 +# if name in init_obj_info_dict: +# count = 1 +# new_name = f"{name}_{count}" +# while new_name in init_obj_info_dict: +# count += 1 +# new_name = f"{name}_{count}" +# name = new_name +# init_obj_info_dict[name] = location +# # print("obj_info_dict:",obj_info_dict) +# # print("obj_name_set:",obj_name_set) +# import os +# import pickle +# absolute_path = r"E:\DATA\Projects\Robot1002 5.2\Plugins\LLM_prompt\data\init_obj_name_set.pkl" +# target_directory = os.path.dirname(absolute_path) +# os.makedirs(target_directory, exist_ok=True) +# # 将变量保存到二进制文件 +# with open(absolute_path, 'wb') as file: +# pickle.dump(init_obj_name_set, file) \ No newline at end of file diff --git a/robowaiter/llm_client/tool_api.py b/robowaiter/llm_client/tool_api.py index 7ab10a6..0745879 100644 --- a/robowaiter/llm_client/tool_api.py +++ b/robowaiter/llm_client/tool_api.py @@ -36,7 +36,6 @@ functions = get_tools() def run_conversation(query: str, stream=False, max_retry=5): params = dict(model="chatglm3", messages=[{"role": "user", "content": query}], stream=stream) params["functions"] = functions - # print(params) response = get_response(**params) for _ in range(max_retry): @@ -61,6 +60,7 @@ def run_conversation(query: str, stream=False, max_retry=5): "content": tool_response, # 调用函数返回结果 } ) + del params["functions"] else: reply = response["choices"][0]["message"]["content"] return { @@ -112,3 +112,6 @@ def run_conversation_for_test_only(query: str, stream=False, max_retry=5): if __name__ == "__main__": query = "可以带我去吗" print(run_conversation_for_test_only(query, stream=False)) + + query = "这里有吗" + print(run_conversation_for_test_only(query, stream=False)) \ No newline at end of file From cb637bf2f6b9b6a47aada8d61c582adcb78302ec Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Fri, 17 Nov 2023 17:13:27 +0800 Subject: [PATCH 12/14] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E5=9C=B0?= =?UTF-8?q?=E7=82=B9=E7=9A=84=E5=B7=A5=E5=85=B7=E8=B0=83=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/llm_client/find_obj_utils.py | 4 +- robowaiter/llm_client/tool_api.py | 22 ++++++--- robowaiter/llm_client/tool_register.py | 66 +++++++++++++++++++------ 3 files changed, 68 insertions(+), 24 deletions(-) diff --git a/robowaiter/llm_client/find_obj_utils.py b/robowaiter/llm_client/find_obj_utils.py index a4b541a..2d940ae 100644 --- a/robowaiter/llm_client/find_obj_utils.py +++ b/robowaiter/llm_client/find_obj_utils.py @@ -32,7 +32,7 @@ all_loc = ['吧台', '餐桌', '沙发', '灶台', '大门', '灯开关', '空 all_loc_en = ['bar', 'Table', 'sofa', 'stove', 'Gate', 'light switch', 'airconditioner switch', 'cabinet', 'bathroom', 'window', 'audio', 'lounge area', 'workstation', 'service counter', 'cashier counter', 'corner', - 'cake display', 'ChargingPoint', 'refrigerator', 'bookshelf'] + 'cake display', 'ChargingStations', 'refrigerator', 'bookshelf'] loc_map_en = {'bar': {'工作台', '服务台', '收银台', '蛋糕柜'}, 'Table': {'沙发', '大门', '窗户', '休闲区', '墙角', '椅子', '书架'}, @@ -51,7 +51,7 @@ loc_map_en = {'bar': {'工作台', '服务台', '收银台', '蛋糕柜'}, 'cashier counter': {'吧台', '工作台', '服务台'}, 'corner': {'卫生间', '沙发', '灯开关', '空调开关', '音响', '休闲区', '书架'}, 'cake display': {'吧台', '橱柜', '服务台', '收银台', '冰箱'}, - 'ChargingPoint': {'吧台', '餐桌', '沙发', '休闲区', '工作台', '服务台', '收银台', '墙角', '书架'}, + 'ChargingStations': {'吧台', '餐桌', '沙发', '休闲区', '工作台', '服务台', '收银台', '墙角', '书架'}, 'refrigerator': {'吧台', '服务台', '蛋糕柜'}, 'bookshelf': {'餐桌', '沙发', '窗户', '休闲区', '墙角'} } diff --git a/robowaiter/llm_client/tool_api.py b/robowaiter/llm_client/tool_api.py index 0745879..51be613 100644 --- a/robowaiter/llm_client/tool_api.py +++ b/robowaiter/llm_client/tool_api.py @@ -38,7 +38,7 @@ def run_conversation(query: str, stream=False, max_retry=5): params["functions"] = functions response = get_response(**params) - for _ in range(max_retry): + for retry in range(max_retry): if response["choices"][0]["message"].get("function_call"): function_call = response["choices"][0]["message"]["function_call"] logger.info(f"Function Call Response: {function_call}") @@ -60,7 +60,7 @@ def run_conversation(query: str, stream=False, max_retry=5): "content": tool_response, # 调用函数返回结果 } ) - del params["functions"] + # del params["functions"] else: reply = response["choices"][0]["message"]["content"] return { @@ -110,8 +110,18 @@ def run_conversation_for_test_only(query: str, stream=False, max_retry=5): if __name__ == "__main__": - query = "可以带我去吗" - print(run_conversation_for_test_only(query, stream=False)) + # query = "可以带我去吗" + # print(run_conversation_for_test_only(query, stream=False)) - query = "这里有吗" - print(run_conversation_for_test_only(query, stream=False)) \ No newline at end of file + query = "卫生间在哪里" # + print(run_conversation(query, stream=False)) + + query = "我想看看冰箱,请问哪里可以找到冰箱" + print(run_conversation(query, stream=False)) + + query = "我想找个充电的地方，你能告诉我在哪儿吗" + print(run_conversation(query, stream=False)) + + query = "我想找张桌子" # + print(run_conversation(query, stream=False)) + # for query in \ No newline at end of file diff --git a/robowaiter/llm_client/tool_register.py b/robowaiter/llm_client/tool_register.py index 4360677..b2f893e 100644 --- a/robowaiter/llm_client/tool_register.py +++ b/robowaiter/llm_client/tool_register.py @@ -4,9 +4,13 @@ from copy import deepcopy from pprint import pformat from types import GenericAlias from typing import get_origin, Annotated +import robowaiter.llm_client.find_obj_utils as find_obj_utils +import random +import spacy _TOOL_HOOKS = {} _TOOL_DESCRIPTIONS = {} +nlp = spacy.load('en_core_web_lg') def register_tool(func: callable): @@ -143,26 +147,56 @@ def create_sub_task( return goal +# @register_tool +# def get_object_info( +# obj: Annotated[str, '需要获取信息的物体名称', True] +# ) -> str: +# """ +# 获取场景中指定物体 `object` 在哪里，不涉及到具体的执行任务 +# 如果`object` 是一个地点，例如洗手间，则输出大门。 +# 如果`object`是咖啡，则输出桌子，咖啡在桌子上。 +# 如果`object` 是空桌子，则输出一号桌 +# """ +# near_object = None +# # if obj == "Table": +# # near_object = "Bar" +# # if obj == "洗手间": +# # near_object = "大门" +# # if obj == "空桌子": +# # near_object = "一号桌" +# if obj in find_obj_utils.all_loc: # object是一个地点 +# mp = list(find_obj_utils.loc_map[obj]) +# # near_object = random.choice(mp) +# near_object = mp +# if obj in find_obj_utils.all_obj: # object是一个物品 +# near_ls = find_obj_utils.all_loc + find_obj_utils.all_obj +# near_object = random.choices(near_ls,k=5) +# return near_object + @register_tool -def get_object_info( - obj: Annotated[str, '需要获取信息的物体名称', True] +def find_location( + location: Annotated[str, '客人咨询的地点', True] ) -> str: + """" + 获取的location为英文 + 用户想找某个地点 """ - 获取场景中指定物体 `object` 在哪里，不涉及到具体的执行任务 - 如果`object` 是一个地点，例如洗手间，则输出大门。 - 如果`object`是咖啡，则输出桌子，咖啡在桌子上。 - 如果`object` 是空桌子，则输出一号桌 - """ - near_object = None - if obj == "Table": - near_object = "Bar" - if obj == "洗手间": - near_object = "大门" - if obj == "空桌子": - near_object = "一号桌" - return near_object - + near_location = None + query_token = nlp(location) + max_similarity = 0 + similar_word = None + for w in find_obj_utils.all_loc_en: + word_token = nlp(w) + similarity = query_token.similarity(word_token) + if similarity > max_similarity: + max_similarity = similarity + similar_word = w + print("similarity:", max_similarity, "similar_word:", similar_word) + if similar_word: + mp = list(find_obj_utils.loc_map_en[similar_word]) + near_location = random.choice(mp) + return near_location if __name__ == "__main__": print(dispatch_tool("get_weather", {"city_name": "beijing"})) From 077bff8775ab8f357efde05045419fac854ad3ac Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Fri, 17 Nov 2023 18:56:58 +0800 Subject: [PATCH 13/14] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=8C=87=E5=BC=95?= =?UTF-8?q?=E9=A1=BE=E5=AE=A2=E7=9A=84=E5=A4=9A=E4=B8=AA=E5=9C=B0=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Behavior.py | 18 ++++++- robowaiter/behavior_lib/act/DealChat.py | 1 + robowaiter/behavior_lib/act/MoveTo.py | 4 +- robowaiter/behavior_lib/act/PickUp.py | 2 +- robowaiter/behavior_lib/act/PutDown.py | 2 +- robowaiter/behavior_lib/cond/At.py | 2 +- robowaiter/behavior_lib/cond/On.py | 2 +- robowaiter/llm_client/data/test_questions.txt | 2 +- .../llm_client/data_raw/test_questions.csv | 1 + robowaiter/robot/Default.ptml | 4 +- robowaiter/scene/scene.py | 50 +++++++++++-------- robowaiter/scene/tasks/Open_tasks_test.py | 9 +++- robowaiter/scene/tasks/VLM/VLM.py | 6 ++- robowaiter/scene/tasks/VLM/VLM2.py | 46 ++++++++++++++--- 14 files changed, 105 insertions(+), 44 deletions(-) diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 8d5460e..8baf2ca 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -13,10 +13,15 @@ class Bahavior(ptree.behaviour.Behaviour): ''' scene = None print_name_prefix = "" - all_place = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3'} + tables_for_placement = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3',"BrightTable6"} all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', 'VacuumCup'} + # BrightTable5 = Table4 + tables_for_guiding = {"QuietTable1","QuietTable2", + "BrightTable1","BrightTable2","BrightTable3","BrightTable4","BrightTable5","BrightTable6" + 'CoffeeTable','WaterTable','Table1', 'Table2', 'Table3'} + # all_place = {'Bar', 'WaterTable', 'CoffeeTable'} # all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'Yogurt'} # all_object = {'Coffee', 'Water'} @@ -31,7 +36,16 @@ class Bahavior(ptree.behaviour.Behaviour): 'CoffeeTable':(250.0, 310.0, 100.0), 'Table1': (340.0, 900.0, 99.0), 'Table2': (-55.0, 0.0, 107), - 'Table3':(-55.0, 150.0, 107) + 'Table3':(-55.0, 150.0, 107), + 'BrightTable6': (5, -315, 116.5), + + 'QuietTable1':(480,1300,90), + 'QuietTable2':(250,-240,-65), + 'BrightTable1':(230,1200,135), + 'BrightTable2': (65, 1000, 135), + 'BrightTable3': (-80, 850, 135), + 'BrightTable4': (-270, 520, 150), + 'BrightTable5': (-270, 420, -135) } container_dic={ 'Coffee':'CoffeeCup', diff --git a/robowaiter/behavior_lib/act/DealChat.py b/robowaiter/behavior_lib/act/DealChat.py index 04133d7..ea6c8da 100644 --- a/robowaiter/behavior_lib/act/DealChat.py +++ b/robowaiter/behavior_lib/act/DealChat.py @@ -20,6 +20,7 @@ class DealChat(Act): res_dict = ask_llm(chat) answer = res_dict["Answer"] + self.scene.chat_bubble(answer) # 机器人输出对话 self.chat_history += answer + '\n' goal = res_dict["Goal"] diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index 3ca9167..ac419f9 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -5,7 +5,7 @@ from robowaiter.algos.navigator.navigate import Navigator class MoveTo(Act): can_be_expanded = True num_args = 1 - valid_args = Act.all_object | Act.all_place + valid_args = Act.all_object | Act.tables_for_placement | Act.tables_for_guiding valid_args.add('Customer') def __init__(self, target_place): @@ -35,7 +35,7 @@ class MoveTo(Act): # 走到固定的地点 if self.target_place in Act.place_xyz_dic: goal = Act.place_xyz_dic[self.target_place] - self.scene.walk_to(goal[0]+1,goal[1]) + self.scene.walk_to(goal[0]+1,goal[1],goal[2]) # 走到物品边上 else: # 是否用容器装好 diff --git a/robowaiter/behavior_lib/act/PickUp.py b/robowaiter/behavior_lib/act/PickUp.py index 30aec3f..83b1fe0 100644 --- a/robowaiter/behavior_lib/act/PickUp.py +++ b/robowaiter/behavior_lib/act/PickUp.py @@ -18,7 +18,7 @@ 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: + for place in cls.valid_args: info["del_set"] |= {f'On({arg},{place})'} return info diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py index 48723ca..d9a5770 100644 --- a/robowaiter/behavior_lib/act/PutDown.py +++ b/robowaiter/behavior_lib/act/PutDown.py @@ -8,7 +8,7 @@ class PutDown(Act): can_be_expanded = True num_args = 2 - valid_args = tuple(itertools.product(Act.all_object, Act.all_place)) + valid_args = tuple(itertools.product(Act.all_object, Act.tables_for_placement)) def __init__(self, *args): super().__init__(*args) diff --git a/robowaiter/behavior_lib/cond/At.py b/robowaiter/behavior_lib/cond/At.py index 4797557..a217217 100644 --- a/robowaiter/behavior_lib/cond/At.py +++ b/robowaiter/behavior_lib/cond/At.py @@ -7,7 +7,7 @@ class At(Cond): can_be_expanded = True num_params = 2 - valid_args = list(itertools.product(('Robot','Customer'), tuple(Cond.all_object | Cond.all_place | {'Customer'}))) + valid_args = list(itertools.product(('Robot','Customer'), tuple(Cond.all_object | Cond.tables_for_placement | Cond.tables_for_guiding | {'Customer'}))) valid_args.remove(('Customer','Customer')) valid_args = tuple(valid_args) diff --git a/robowaiter/behavior_lib/cond/On.py b/robowaiter/behavior_lib/cond/On.py index 0060e2e..b2a6040 100644 --- a/robowaiter/behavior_lib/cond/On.py +++ b/robowaiter/behavior_lib/cond/On.py @@ -6,7 +6,7 @@ class On(Cond): can_be_expanded = True num_params = 2 valid_params = [tuple(Cond.all_object), - tuple(Cond.all_place)] + tuple(Cond.tables_for_placement)] def __init__(self,*args): diff --git a/robowaiter/llm_client/data/test_questions.txt b/robowaiter/llm_client/data/test_questions.txt index 6959b5f..a0ba46a 100644 --- a/robowaiter/llm_client/data/test_questions.txt +++ b/robowaiter/llm_client/data/test_questions.txt @@ -1 +1 @@ -{"做一杯咖啡": {"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)\"}"}} +{"做一杯咖啡": {"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)\"}"}, "请问可以带我去空位上嘛？我想晒太阳。": {"Answer": "没问题！请跟我来。", "Goal": "{\"At(Robot,BrightTable1)\"}"}} diff --git a/robowaiter/llm_client/data_raw/test_questions.csv b/robowaiter/llm_client/data_raw/test_questions.csv index 8a71f42..4ce218c 100644 --- a/robowaiter/llm_client/data_raw/test_questions.csv +++ b/robowaiter/llm_client/data_raw/test_questions.csv @@ -21,3 +21,4 @@ Question,Answer,Goal Դȥ,Ȼԣǰһ,"{""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)""}" +ʿԴȥλɹ̫,û⣡,"{""At(Robot,BrightTable1)""}" diff --git a/robowaiter/robot/Default.ptml b/robowaiter/robot/Default.ptml index 1a1c2b2..e6addee 100644 --- a/robowaiter/robot/Default.ptml +++ b/robowaiter/robot/Default.ptml @@ -16,12 +16,12 @@ selector } sequence { + cond DetectCustomer() selector { cond At(Robot,Bar) act MoveTo(Bar) } - cond DetectCustomer() - act GreatCustomer + act GreatCustomer() } } \ No newline at end of file diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 8837713..daa4c76 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -60,10 +60,9 @@ class Scene: "sub_goal_list": [], # 子目标列表 "status": None, # 仿真器中的观测信息，见下方详细解释 "condition_set": {'At(Robot,Bar)', 'Is(AC,Off)', - 'Holding(Nothing)', - # 'Holding(Yogurt)' - 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)', - 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'} + 'Holding(Nothing)','Exist(Yogurt)','Exist(BottledDrink)','On(Yogurt,Bar)','On(BottledDrink,Table1)', + 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)', + 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'} } """ status: @@ -135,7 +134,6 @@ class Scene: # 基类run self._run() - # 运行并由robot打印每步信息 while True: self.step() @@ -199,8 +197,6 @@ class Scene: - - def walker_control_generator(self, walkerID, autowalk, speed, X, Y, Yaw): if self.use_offset: X, Y = X + loc_offset[0], Y + loc_offset[1] @@ -236,22 +232,31 @@ class Scene: else: return True + + def add_walker(self,id,x,y,yaw=0,v=0,scope=100): + loc = [x,y,yaw,v,scope] + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc) + scene = stub.Do(action) + # print(scene.info) + walker_list=[] + if (str(scene.info).find('unreachable') > -1): + print('当前位置不可达,无法初始化NPC') + else: + walker_list.append( + GrabSim_pb2.WalkerList.Walker(id=id+5, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=loc[2]))) + stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID)) + def add_walkers(self,walker_loc=[[0, 880], [250, 1200], [-55, 750], [70, -200]]): print('------------------add_walkers----------------------') - walker_list = [] - for i in range(len(walker_loc)): - loc = walker_loc[i] + [0,0, 100] - action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc) - scene = stub.Do(action) - print(scene.info) - if (str(scene.info).find('unreachable') > -1): - print('当前位置不可达,无法初始化NPC') - else: - walker_list.append( - GrabSim_pb2.WalkerList.Walker(id=i + 5, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=90))) - - scene = stub.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=self.sceneID)) - return scene + for id,walker in enumerate(walker_loc): + if len(walker)==2: + self.add_walker(id,walker[0],walker[1]) + elif len(walker)==3: + self.add_walker(id, walker[0], walker[1],walker[2]) + elif len(walker) == 4: + self.add_walker(id, walker[0], walker[1], walker[2], walker[3]) + elif len(walker) == 5: + self.add_walker(id, walker[0], walker[1], walker[2], walker[3], walker[4]) def remove_walker(self, *args): # take single walkerID or a list of walkerIDs remove_list = [] @@ -272,12 +277,13 @@ 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)): + for i in range(len(walker_loc)): loc = walker_loc[i] is_autowalk = is_autowalk pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180) diff --git a/robowaiter/scene/tasks/Open_tasks_test.py b/robowaiter/scene/tasks/Open_tasks_test.py index 0408d60..87d1f02 100644 --- a/robowaiter/scene/tasks/Open_tasks_test.py +++ b/robowaiter/scene/tasks/Open_tasks_test.py @@ -18,10 +18,17 @@ class SceneOT(Scene): super().__init__(robot) # 在这里加入场景中发生的事件 self.event_list = [ - (5, self.set_goal("At(Robot,Table2)")) + # (5, self.set_goal("At(Robot,WaterTable)")) + # (5, self.set_goal("On(Yogurt,Table4)")) + (5, self.set_goal("At(Robot,BrightTable4)")) ] def _reset(self): + self.gen_obj() + self.state["condition_set"] = {'At(Robot,Bar)', 'Is(AC,Off)', + 'Holding(Nothing)','Exist(Yogurt)','Exist(BottledDrink)','On(Yogurt,Bar)','On(BottledDrink,Table1)', + 'Is(HallLight,Off)', 'Is(TubeLight,On)', 'Is(Curtain,On)', + 'Is(Table1,Dirty)', 'Is(Floor,Dirty)', 'Is(Chairs,Dirty)'} pass diff --git a/robowaiter/scene/tasks/VLM/VLM.py b/robowaiter/scene/tasks/VLM/VLM.py index 5631c27..d0ba4a5 100644 --- a/robowaiter/scene/tasks/VLM/VLM.py +++ b/robowaiter/scene/tasks/VLM/VLM.py @@ -41,9 +41,11 @@ class SceneVLM(Scene): '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]]) + # 随机生成4个自由行走，一个在 BrightTable4,BrightTable5(-20,220) + self.add_walkers([[0, 880], [250, 1200], [-55, 750], [70, -200],[-290, 400, 0],[20, -150,180]]) 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]] diff --git a/robowaiter/scene/tasks/VLM/VLM2.py b/robowaiter/scene/tasks/VLM/VLM2.py index 7a13acc..f83416c 100644 --- a/robowaiter/scene/tasks/VLM/VLM2.py +++ b/robowaiter/scene/tasks/VLM/VLM2.py @@ -11,11 +11,10 @@ class SceneVLM(Scene): super().__init__(robot) # 在这里加入场景中发生的事件， (事件发生的时间，事件函数) self.event_list = [ - + (5, self.create_chat_event("请问可以带我去空位上嘛？我想晒太阳。")), ] def _reset(self): - self.gen_obj() pass def _run(self, op_type=10): @@ -25,13 +24,34 @@ class SceneVLM(Scene): # 带领行人去有太阳的地方 # 行人说 有点热 # 好的，这就去开空调 - scene = self.add_walkers([[47, 920]]) - self.control_walker( - [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=0)]) - cont = scene.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳" + self.gen_obj() + self.add_walkers([[47, 920]]) + self.control_walker( + [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=180)]) + time.sleep(1) + cont = self.status.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳。" self.control_robot_action(0,3,cont) + # time.sleep(3) + # self.event_list.append((5, self.set_goal("At(Robot,BrightTable1)"))) # "请问可以带我去空位上嘛？我想晒太阳" + # self.chat_bubble("没问题！请跟我来。") + + + # 跟随机器人 + # cont = self.status.walkers[0].name + "好的！" + # self.control_robot_action(0, 3, cont) + # + # start = [self.status.location.X, self.status.location.Y] + # time.sleep(0.2) + # end = [self.status.location.X, self.status.location.Y] + # while abs(start[0]-end[0])>=1 or abs(start[1]-end[1])>=1: + # self.control_walker( + # [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=0)]) + # + # cont = self.status.walkers[0].name+"谢谢！" + # self.control_robot_action(0,3,cont) + # 共17个操作 # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 @@ -51,11 +71,21 @@ class SceneVLM(Scene): # 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] - - pass def _step(self): + + # 如果机器人不在 吧台 + # if "At(Robot,Bar)" not in self.state['condition_set']: + end = [self.status.location.X, self.status.location.Y] + print("end:",end) + if end[1]>=600 or end[1]<=450 or end[0]>=250: + # if int(self.status.location.X)!=247 or int(self.status.location.X)!=520: + self.control_walker( + [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=-90)]) + + cont = self.status.walkers[0].name+"谢谢！" + self.control_robot_action(0,3,cont) pass From 68fcf72d62627ae14be47070f6e46fbf36ff5c58 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Fri, 17 Nov 2023 19:08:00 +0800 Subject: [PATCH 14/14] =?UTF-8?q?=E6=B3=A8=E9=87=8A=E4=BA=86=E5=B7=A5?= =?UTF-8?q?=E5=85=B7=E8=B0=83=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/llm_client/tool_register.py | 52 ++++++------- robowaiter/scene/tasks/VLM/VLM2.py | 58 +++----------- robowaiter/scene/tasks/VLM/VLM3.py | 101 +++++++++++++++++++++++++ 3 files changed, 137 insertions(+), 74 deletions(-) create mode 100644 robowaiter/scene/tasks/VLM/VLM3.py diff --git a/robowaiter/llm_client/tool_register.py b/robowaiter/llm_client/tool_register.py index b2f893e..36f204b 100644 --- a/robowaiter/llm_client/tool_register.py +++ b/robowaiter/llm_client/tool_register.py @@ -6,11 +6,11 @@ from types import GenericAlias from typing import get_origin, Annotated import robowaiter.llm_client.find_obj_utils as find_obj_utils import random -import spacy +# import spacy _TOOL_HOOKS = {} _TOOL_DESCRIPTIONS = {} -nlp = spacy.load('en_core_web_lg') +# nlp = spacy.load('en_core_web_lg') def register_tool(func: callable): @@ -173,30 +173,30 @@ def create_sub_task( # near_object = random.choices(near_ls,k=5) # return near_object -@register_tool -def find_location( - location: Annotated[str, '客人咨询的地点', True] -) -> str: - """" - 获取的location为英文 - 用户想找某个地点 - """ - near_location = None - query_token = nlp(location) - max_similarity = 0 - similar_word = None - for w in find_obj_utils.all_loc_en: - word_token = nlp(w) - similarity = query_token.similarity(word_token) - - if similarity > max_similarity: - max_similarity = similarity - similar_word = w - print("similarity:", max_similarity, "similar_word:", similar_word) - if similar_word: - mp = list(find_obj_utils.loc_map_en[similar_word]) - near_location = random.choice(mp) - return near_location +# @register_tool +# def find_location( +# location: Annotated[str, '客人咨询的地点', True] +# ) -> str: +# """" +# 获取的location为英文 +# 用户想找某个地点 +# """ +# near_location = None +# query_token = nlp(location) +# max_similarity = 0 +# similar_word = None +# for w in find_obj_utils.all_loc_en: +# word_token = nlp(w) +# similarity = query_token.similarity(word_token) +# +# if similarity > max_similarity: +# max_similarity = similarity +# similar_word = w +# print("similarity:", max_similarity, "similar_word:", similar_word) +# if similar_word: +# mp = list(find_obj_utils.loc_map_en[similar_word]) +# near_location = random.choice(mp) +# return near_location if __name__ == "__main__": print(dispatch_tool("get_weather", {"city_name": "beijing"})) diff --git a/robowaiter/scene/tasks/VLM/VLM2.py b/robowaiter/scene/tasks/VLM/VLM2.py index f83416c..1f23690 100644 --- a/robowaiter/scene/tasks/VLM/VLM2.py +++ b/robowaiter/scene/tasks/VLM/VLM2.py @@ -15,6 +15,8 @@ class SceneVLM(Scene): ] def _reset(self): + self.gen_obj() + self.add_walkers([[47, 920]]) pass def _run(self, op_type=10): @@ -24,61 +26,21 @@ class SceneVLM(Scene): # 带领行人去有太阳的地方 # 行人说 有点热 # 好的，这就去开空调 - - self.gen_obj() - self.add_walkers([[47, 920]]) - self.control_walker( - [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=180)]) - time.sleep(1) - cont = self.status.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳。" - self.control_robot_action(0,3,cont) - - # time.sleep(3) - # self.event_list.append((5, self.set_goal("At(Robot,BrightTable1)"))) # "请问可以带我去空位上嘛？我想晒太阳" - # self.chat_bubble("没问题！请跟我来。") - - - # 跟随机器人 - # cont = self.status.walkers[0].name + "好的！" - # self.control_robot_action(0, 3, cont) - # - # start = [self.status.location.X, self.status.location.Y] - # time.sleep(0.2) - # end = [self.status.location.X, self.status.location.Y] - # while abs(start[0]-end[0])>=1 or abs(start[1]-end[1])>=1: - # self.control_walker( - # [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=0)]) - # - # cont = self.status.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] pass def _step(self): + self.control_walker( + [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=180)]) + time.sleep(3) + cont = self.status.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳。" + self.control_robot_action(0,3,cont) + + # 如果机器人不在 吧台 # if "At(Robot,Bar)" not in self.state['condition_set']: end = [self.status.location.X, self.status.location.Y] - print("end:",end) + # print("end:",end) if end[1]>=600 or end[1]<=450 or end[0]>=250: # if int(self.status.location.X)!=247 or int(self.status.location.X)!=520: self.control_walker( diff --git a/robowaiter/scene/tasks/VLM/VLM3.py b/robowaiter/scene/tasks/VLM/VLM3.py new file mode 100644 index 0000000..f83416c --- /dev/null +++ b/robowaiter/scene/tasks/VLM/VLM3.py @@ -0,0 +1,101 @@ +""" +视觉语言操作 +机器人根据指令人的指令调节空调，自主探索环境导航到目标点，通过手臂的运动规划能力操作空调，比如开关按钮、调温按钮、显示面板 +""" + +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("请问可以带我去空位上嘛？我想晒太阳。")), + ] + + def _reset(self): + pass + + def _run(self, op_type=10): + # 一个行人从门口走到 吧台 + # 打招呼需要什么 + # 行人说 哪里有位置，想晒个太阳 + # 带领行人去有太阳的地方 + # 行人说 有点热 + # 好的，这就去开空调 + + self.gen_obj() + self.add_walkers([[47, 920]]) + self.control_walker( + [self.walker_control_generator(walkerID=0, autowalk=False, speed=200, X=60, Y=520, Yaw=180)]) + time.sleep(1) + cont = self.status.walkers[0].name+":请问可以带我去空位上嘛？我想晒太阳。" + self.control_robot_action(0,3,cont) + + # time.sleep(3) + # self.event_list.append((5, self.set_goal("At(Robot,BrightTable1)"))) # "请问可以带我去空位上嘛？我想晒太阳" + # self.chat_bubble("没问题！请跟我来。") + + + # 跟随机器人 + # cont = self.status.walkers[0].name + "好的！" + # self.control_robot_action(0, 3, cont) + # + # start = [self.status.location.X, self.status.location.Y] + # time.sleep(0.2) + # end = [self.status.location.X, self.status.location.Y] + # while abs(start[0]-end[0])>=1 or abs(start[1]-end[1])>=1: + # self.control_walker( + # [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=0)]) + # + # cont = self.status.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] + pass + + def _step(self): + + # 如果机器人不在 吧台 + # if "At(Robot,Bar)" not in self.state['condition_set']: + end = [self.status.location.X, self.status.location.Y] + print("end:",end) + if end[1]>=600 or end[1]<=450 or end[0]>=250: + # if int(self.status.location.X)!=247 or int(self.status.location.X)!=520: + self.control_walker( + [self.walker_control_generator(walkerID=0, autowalk=False, speed=100, X=end[0], Y=end[1], Yaw=-90)]) + + cont = self.status.walkers[0].name+"谢谢！" + self.control_robot_action(0,3,cont) + pass + + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneVLM(robot) + task.reset() + task.run()