From ad5e8cbd126e2defb1195518fedbe3ef44f2a0f0 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Wed, 15 Nov 2023 12:04:49 +0800 Subject: [PATCH 1/7] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E9=83=A8?= =?UTF-8?q?=E5=88=86=E5=8A=A8=E4=BD=9C=E8=8A=82=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/algos/explore/AEM.py | 8 +++- robowaiter/behavior_lib/_base/Behavior.py | 5 ++- robowaiter/behavior_lib/act/Clean.py | 4 +- robowaiter/behavior_lib/act/ExploreEnv.py | 13 +++++- robowaiter/behavior_lib/act/Make.py | 12 +++++- robowaiter/behavior_lib/act/MoveTo.py | 33 +++++++++------ robowaiter/behavior_lib/act/PickUp.py | 5 ++- robowaiter/behavior_lib/act/PutDown.py | 2 +- robowaiter/scene/scene.py | 49 ++++++++++++++++------- robowaiter/scene/tasks/AEM.py | 17 ++++++++ robowaiter/scene/tasks/GQA.py | 12 ++++++ robowaiter/scene/tasks/Open_tasks.py | 13 +++++- robowaiter/scene/tasks/VLM.py | 20 ++++++--- 13 files changed, 150 insertions(+), 43 deletions(-) diff --git a/robowaiter/algos/explore/AEM.py b/robowaiter/algos/explore/AEM.py index fd7c945..9b00980 100644 --- a/robowaiter/algos/explore/AEM.py +++ b/robowaiter/algos/explore/AEM.py @@ -14,8 +14,8 @@ import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable -import GrabSim_pb2_grpc -import GrabSim_pb2 +from robowaiter.proto import GrabSim_pb2 +from robowaiter.proto import GrabSim_pb2_grpc channel = grpc.insecure_channel('localhost:30001', options=[ ('grpc.max_send_message_length', 1024 * 1024 * 1024), @@ -141,6 +141,10 @@ if __name__ == '__main__': print('------------ 自主探索 ------------') while True: + + # scene = sim_client.Observe(GrabSim_pb2.SceneID(value=0)) + # cur_pos =[int(scene.location.X), int(scene.location.Y), int(scene.rotation.Yaw)] + goal = explorer.explore(cur_pos) # cur_pos 指的是当前机器人的位置，场景中应该也有接口可以获取 if goal is None: break diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 5966286..ccf6c26 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -13,10 +13,11 @@ 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', # 'VacuumCup'} - all_place = {'Bar', 'WaterTable', 'CoffeeTable'} + # all_place = {'Bar', 'WaterTable', 'CoffeeTable'} + # all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'Yogurt'} all_object = {'Coffee'} place_xyz_dic={ 'Bar': (247.0, 520.0, 100.0), diff --git a/robowaiter/behavior_lib/act/Clean.py b/robowaiter/behavior_lib/act/Clean.py index cb7db73..39df987 100644 --- a/robowaiter/behavior_lib/act/Clean.py +++ b/robowaiter/behavior_lib/act/Clean.py @@ -42,6 +42,6 @@ class Clean(Act): self.scene.move_task_area(self.op_type) self.scene.op_task_execute(self.op_type) - self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] |= (self.info["add"]) self.scene.state["condition_set"] -= self.info["del_set"] - return Status.RUNNING \ No newline at end of file + return Status.SUCCESS \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/ExploreEnv.py b/robowaiter/behavior_lib/act/ExploreEnv.py index 763fd7c..3b832b1 100644 --- a/robowaiter/behavior_lib/act/ExploreEnv.py +++ b/robowaiter/behavior_lib/act/ExploreEnv.py @@ -3,14 +3,25 @@ from typing import Any from robowaiter.behavior_lib._base.Act import Act class ExploreEnv(Act): + can_be_expanded = True + num_args=0 + valid_args=() def __init__(self, *args): super().__init__(*args) + @classmethod + def get_info(cls): + info = {} + info["pre"] = set() + info["add"] = {"EnvExplored()"} + info["del_set"] = set() + return info + def _update(self) -> ptree.common.Status: # explore algorithm - self.scene.state["condition_set"].add("EnvExplored()") + self.scene.state["condition_set"]|= self.info["add"] return ptree.common.Status.RUNNING diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index ebe3b7b..8fcc533 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -43,6 +43,16 @@ class Make(Act): # self.scene.gen_obj(type=40) - self.scene.state["condition_set"].union(self.info["add"]) + obj_dict = self.scene.status.objects + if len(obj_dict) != 0: + # 获取obj_id + for id, obj in enumerate(obj_dict): + if obj.name == "CoffeeCup": + obj_info = obj_dict[id] + obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z + print(id,obj.name,obj_x,obj_y,obj_z) + + self.scene.state["condition_set"] |= (self.info["add"]) self.scene.state["condition_set"] -= self.info["del_set"] + return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index 995a074..ed8fb4a 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -36,20 +36,27 @@ class MoveTo(Act): goal = Act.place_xyz_dic[self.target_place] self.scene.walk_to(goal[0],goal[1]) else: # 走到物品边上 - obj_id = -1 - obj_dict = self.scene.status.objects - if len(obj_dict)!=0: - # 获取obj_id - 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) + self.scene.move_task_area(16, obj_id=273) + self.scene.op_task_execute(16, obj_id=273) + + # obj_id = -1 + # obj_dict = self.scene.status.objects + # if len(obj_dict)!=0: + # # 获取obj_id + # for id,obj in enumerate(obj_dict): + # if obj.name == self.target_place: + # obj_id = id + # break + # # 为了演示，写死咖啡位置 + # if self.target_place=="Coffee": + # obj_id = 273 + # if obj_id == -1: + # return ptree.common.Status.FAILURE + # obj_info = obj_dict[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) + # print("MoveTo",obj_x, obj_y, obj_z," obj_id:",obj_id," obj_info:",obj_info.name) # goal = self.scene.state['map']['obj_pos'][self.args[0]] diff --git a/robowaiter/behavior_lib/act/PickUp.py b/robowaiter/behavior_lib/act/PickUp.py index 4dae241..f4d2e9a 100644 --- a/robowaiter/behavior_lib/act/PickUp.py +++ b/robowaiter/behavior_lib/act/PickUp.py @@ -29,8 +29,11 @@ class PickUp(Act): obj_id = 0 # 遍历场景里的所有物品，根据名字匹配位置最近的 obj-id + if self.args=="Coffee": + obj_id = 273 + self.scene.op_task_execute(op_type, obj_id=obj_id) - self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] |= (self.info["add"]) self.scene.state["condition_set"] -= self.info["del_set"] return Status.RUNNING diff --git a/robowaiter/behavior_lib/act/PutDown.py b/robowaiter/behavior_lib/act/PutDown.py index 009a8f8..a7da726 100644 --- a/robowaiter/behavior_lib/act/PutDown.py +++ b/robowaiter/behavior_lib/act/PutDown.py @@ -33,6 +33,6 @@ class PutDown(Act): # # 桌子2:[-55.0, 0.0, 107],桌子3:[-55.0, 150.0, 107] self.scene.op_task_execute(op_type, release_pos=release_pos) - self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] |= (self.info["add"]) self.scene.state["condition_set"] -= self.info["del_set"] return Status.RUNNING diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index cea7822..1a371b8 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -52,7 +52,11 @@ class Scene: "chat_list": [], # 未处理的顾客的对话, (顾客的位置,顾客对话的内容) "sub_goal_list": [], # 子目标列表 "status": None, # 仿真器中的观测信息，见下方详细解释 - "condition_set": set() + "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)'} } """ status: @@ -179,7 +183,7 @@ class Scene: pose=GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw), ) - def walk_to(self, X, Y, Yaw=None, velocity=200, dis_limit=0): + def walk_to(self, X, Y, Yaw=100, velocity=200, dis_limit=0): walk_v = [X,Y,Yaw,velocity,dis_limit] action = GrabSim_pb2.Action( scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v @@ -358,10 +362,27 @@ class Scene: temp = stub.GetIKControlInfos(GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject)) # 移动到进行操作任务的指定地点 - def move_task_area(self,op_type): + def move_task_area(self,op_type,obj_id=0): if op_type==11 or op_type==12: # 开关窗帘不需要移动 return scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) + + if op_type == 16: + print('------------------moveTo_obj----------------------') + obj_info = scene.objects[obj_id] + # Robot + obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z + walk_v = [obj_x + 50, obj_y] + [180, 180, 0] + if obj_y >= 820 and obj_y <= 1200 and obj_x >= 240 and obj_x <= 500: # 物品位于斜的抹布桌上 ([240,500],[820,1200]) + walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0] + obj_x += 3 + obj_y += 2.5 + # walk_v = [obj_x,obj_y-30,130, 180, 0] + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) + scene = stub.Do(action) + time.sleep(1.0) + return + walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] if op_type < 8: @@ -424,20 +445,20 @@ class Scene: def grasp_obj(self,obj_id,hand_id=1): # Move to Obj - print('------------------moveTo_obj----------------------') + # print('------------------moveTo_obj----------------------') scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) obj_info = scene.objects[obj_id] - # Robot + # # Robot obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z - walk_v = [obj_x+50, obj_y] + [180, 180, 0] - if obj_y>=820 and obj_y<= 1200 and obj_x>=240 and obj_x<= 500: # 物品位于斜的抹布桌上 ([240,500],[820,1200]) - walk_v = [obj_x+40, obj_y-35, 130, 180, 0] - obj_x += 3 - obj_y += 2.5 - # walk_v = [obj_x,obj_y-30,130, 180, 0] - action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - scene = stub.Do(action) - time.sleep(1.0) + # walk_v = [obj_x+50, obj_y] + [180, 180, 0] + # if obj_y>=820 and obj_y<= 1200 and obj_x>=240 and obj_x<= 500: # 物品位于斜的抹布桌上 ([240,500],[820,1200]) + # walk_v = [obj_x+40, obj_y-35, 130, 180, 0] + # obj_x += 3 + # obj_y += 2.5 + # # walk_v = [obj_x,obj_y-30,130, 180, 0] + # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) + # scene = stub.Do(action) + # time.sleep(1.0) # Finger self.ik_control_joints(2, obj_x-9, obj_y, obj_z) # -10, 0, 0 diff --git a/robowaiter/scene/tasks/AEM.py b/robowaiter/scene/tasks/AEM.py index a0229f6..c060341 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/robowaiter/scene/tasks/AEM.py @@ -11,3 +11,20 @@ class SceneAEM(Scene): self.event_list = [ (5, self.create_chat_event("测试AEM")), ] + + def _reset(self): + pass + def _run(self): + pass + + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneAEM(robot) + task.reset() + task.run() \ No newline at end of file diff --git a/robowaiter/scene/tasks/GQA.py b/robowaiter/scene/tasks/GQA.py index 8f69641..14f2323 100644 --- a/robowaiter/scene/tasks/GQA.py +++ b/robowaiter/scene/tasks/GQA.py @@ -28,3 +28,15 @@ class SceneGQA(Scene): def _run(self): pass + + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneGQA(robot) + task.reset() + task.run() \ No newline at end of file diff --git a/robowaiter/scene/tasks/Open_tasks.py b/robowaiter/scene/tasks/Open_tasks.py index 4ed52eb..ca49491 100644 --- a/robowaiter/scene/tasks/Open_tasks.py +++ b/robowaiter/scene/tasks/Open_tasks.py @@ -18,7 +18,8 @@ class SceneOT(Scene): super().__init__(robot) # 在这里加入场景中发生的事件 self.event_list = [ - (5,self.create_chat_event("给我一杯咖啡")) # (事件发生的时间，事件函数) + # (5,self.create_chat_event("给我一杯咖啡")) # (事件发生的时间，事件函数) + (5, self.create_chat_event("测试VLM：拖地")) # (事件发生的时间，事件函数) ] def _reset(self): @@ -31,3 +32,13 @@ class SceneOT(Scene): pass +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneOT(robot) + task.reset() + task.run() \ No newline at end of file diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index 5bfe7f6..053fe35 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -11,7 +11,7 @@ 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：关空调")), @@ -22,13 +22,11 @@ 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：做一杯咖啡放到吧台上")), ] def _reset(self): - self.state["condition_set"] = {'At(Robot,Bar)','Is(AC,Off)', 'Holding(Nothing)', # 'Holding(Yogurt)', #'Holding(Nothing)', - '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.op_task_execute(op_type=16, obj_id=0) @@ -57,3 +55,15 @@ class SceneVLM(Scene): 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() \ No newline at end of file From df425d9615c811862674f32cc7f09665515983aa Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Wed, 15 Nov 2023 13:14:49 +0800 Subject: [PATCH 2/7] Update VLM.py --- robowaiter/scene/tasks/VLM.py | 52 +++++++++++++++++++++++++++++------ 1 file changed, 43 insertions(+), 9 deletions(-) diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index 053fe35..a818842 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -41,16 +41,50 @@ class SceneVLM(Scene): # "调整空调开关","调高空调温度","调低空调温度", # 13-15 # "抓握物体","放置物体" # 16-17 - # 16: 抓操作需要传入物品id，17: 放操作需要传入放置位置周围的空地区域(桌边而不是桌上) - # if op_type == 16: - # self.gen_obj() - # self.op_task_execute(op_type, obj_id=0) - # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 70.0], 水杯桌:[-70.0, 500.0, 107] - # # 桌子1:[-55.0, 0.0, 107],桌子2:[-55.0, 150.0, 107], 抹布桌:[340.0, 900.0, 98.0] - # if op_type == 17: self.op_task_execute(op_type, release_pos=[340.0, 900.0, 99.0]) #[325.0, 860.0, 100] - # if op_type not in [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) pass def _step(self): @@ -66,4 +100,4 @@ if __name__ == '__main__': # create task task = SceneVLM(robot) task.reset() - task.run() \ No newline at end of file + task.run() From 1748507f8f59d8cec865586a1976f1b9134e91cc Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Wed, 15 Nov 2023 13:15:47 +0800 Subject: [PATCH 3/7] Update scene.py --- robowaiter/scene/scene.py | 106 ++++++++++++++------------------------ 1 file changed, 40 insertions(+), 66 deletions(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 1a371b8..9e69a31 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -362,13 +362,18 @@ class Scene: temp = stub.GetIKControlInfos(GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject)) # 移动到进行操作任务的指定地点 - def move_task_area(self,op_type,obj_id=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: # 开关窗帘不需要移动 return - scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) - - if op_type == 16: - print('------------------moveTo_obj----------------------') + print('------------------moveTo_Area----------------------') + if op_type < 8: walk_v = self.op_v_list[op_type] + [scene.rotation.Yaw, 180, 0] # 动画控制 + 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: # 抓握物体，移动到物体周围的可达区域 obj_info = scene.objects[obj_id] # Robot obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z @@ -377,31 +382,14 @@ class Scene: walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0] obj_x += 3 obj_y += 2.5 - # walk_v = [obj_x,obj_y-30,130, 180, 0] - action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - scene = stub.Do(action) - time.sleep(1.0) - return + 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 - walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] - - if op_type < 8: - v_list = self.op_v_list[op_type] - if op_type>=8 and op_type<=10: # 控灯 - v_list = self.op_v_list[6] - if op_type in [13,14,15]: # 空调 - v_list = [[240, -140.0]] # KongTiao [300.5, -140.0] # 250 - - print("------------------move_task_area----------------------") - print("Current Position:", walk_value,"开始任务:",self.op_dialog[op_type]) - for walk_v in v_list: - walk_v = walk_v + [scene.rotation.Yaw, 180, 0] - walk_v[2] = 0 if (op_type in [13,14,15]) else scene.rotation.Yaw # 空调操作朝向墙面 - 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]) + 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 control_robot_action(self, type=0, action=0, message="你好"): @@ -418,7 +406,9 @@ class Scene: return False def adjust_kongtiao(self,op_type): - obj_loc = self.obj_loc[0][:] + print("self.obj_loc:",self.obj_loc) + 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) if op_type == 13: obj_loc[1] -= 2 @@ -429,37 +419,25 @@ class Scene: self.robo_recover() return True - def gen_obj(self,type=5,h=100): + def gen_obj(self,h=100): # 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 14:cake 61:甜牛奶 - # type= 5 #9 - scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) + 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=type), + 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), + 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): - - # Move to Obj - # print('------------------moveTo_obj----------------------') - scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) + scene = self.status obj_info = scene.objects[obj_id] - # # Robot obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z - # walk_v = [obj_x+50, obj_y] + [180, 180, 0] - # if obj_y>=820 and obj_y<= 1200 and obj_x>=240 and obj_x<= 500: # 物品位于斜的抹布桌上 ([240,500],[820,1200]) - # walk_v = [obj_x+40, obj_y-35, 130, 180, 0] - # obj_x += 3 - # obj_y += 2.5 - # # walk_v = [obj_x,obj_y-30,130, 180, 0] - # action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - # scene = stub.Do(action) - # time.sleep(1.0) - + if obj_info.name=="CoffeeCup": + pass + if obj_info.name=="Glass": + pass # Finger self.ik_control_joints(2, obj_x-9, obj_y, obj_z) # -10, 0, 0 time.sleep(3.0) @@ -467,7 +445,7 @@ class Scene: print('------------------grasp_obj----------------------') action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Grasp, values=[hand_id, obj_id]) scene = stub.Do(action) - time.sleep(4) + time.sleep(3.0) return True # robot的肢体恢复原位 @@ -479,7 +457,7 @@ class Scene: def robo_stoop_parallel(self): # 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊 - scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) + scene = self.status angle = [scene.joints[i].angle for i in range(21)] angle[0] = 15 angle[19] = -15 @@ -490,12 +468,6 @@ class Scene: time.sleep(1.0) def release_obj(self,release_pos): - print("------------------Move to Realese Position----------------------") - walk_v = [release_pos[i] for i in range(2)] + [180,180,0] - if release_pos==[340.0, 900.0, 99.0]: - walk_v[2] = 130 - action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - scene = stub.Do(action) print("------------------release_obj----------------------") if release_pos==[340.0, 900.0, 99.0]: self.ik_control_joints(2, 300.0, 935, release_pos[2]) @@ -515,17 +487,14 @@ class Scene: # 执行过程：输出"开始(任务名)" -> 按步骤数执行任务 -> Robot输出成功或失败的对话 def op_task_execute(self,op_type,obj_id=0,release_pos=[240,-140]): 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) - elif op_type ==16: + if op_type ==16: # 抓握物体 result = self.grasp_obj(obj_id) - elif op_type ==17: + if op_type ==17: # 放置物体 result = self.release_obj(release_pos) - elif op_type>=8: - result = self.control_robot_action(self.op_typeToAct[op_type][0], self.op_typeToAct[op_type][1]) - print("result:",result) - else: - result = self.control_robot_action(op_type, 1) # self.control_robot_action(0, 2) if result: if self.op_act_num[op_type]>0: @@ -536,6 +505,11 @@ class Scene: # else: # self.control_robot_action(0, 1, self.op_dialog[op_type]+"失败") + 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,) + def test_move(self): v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] scene = self.status From 67f501182e9a613c1074551205664d1e784dafd8 Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Wed, 15 Nov 2023 13:16:40 +0800 Subject: [PATCH 4/7] Update VLM.py --- robowaiter/scene/tasks/VLM.py | 56 +++++++++++++++++------------------ 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index a818842..98b6a23 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -57,34 +57,34 @@ class SceneVLM(Scene): # 流程测试 # 抓握放置:抓吧台前生成的酸奶，放到抹布桌上 - 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.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) pass def _step(self): From ecb89247d4dd2dafeb44118a3f17d2d2277e7be2 Mon Sep 17 00:00:00 2001 From: Netceor <45135347+Netceor@users.noreply.github.com> Date: Wed, 15 Nov 2023 13:40:46 +0800 Subject: [PATCH 5/7] Update scene.py --- robowaiter/scene/scene.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 9e69a31..3c135e7 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -94,11 +94,11 @@ class Scene: 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_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.obj_loc = [[300.5, -140.0,114]] + self.obj_loc = [300.5, -140.0,114] def reset(self): From 0e55e18cbf91ae390edc7e25eea6675d7028d80f Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Wed, 15 Nov 2023 14:30:55 +0800 Subject: [PATCH 6/7] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E5=BC=80?= =?UTF-8?q?=E6=94=BE=E4=BB=BB=E5=8A=A1=E5=92=8C=20VLM?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/algos/navigate/test.py | 5 +- robowaiter/llm_client/data/test_questions.txt | 2 +- .../llm_client/data_raw/test_questions.csv | 1 + robowaiter/scene/tasks/Auto_tasks.py | 18 ++++++- robowaiter/scene/tasks/Open_tasks.py | 11 +++-- robowaiter/scene/tasks/VLM.py | 48 +++++++++---------- 6 files changed, 55 insertions(+), 30 deletions(-) diff --git a/robowaiter/algos/navigate/test.py b/robowaiter/algos/navigate/test.py index 525ad10..c8c4d91 100644 --- a/robowaiter/algos/navigate/test.py +++ b/robowaiter/algos/navigate/test.py @@ -44,9 +44,12 @@ if __name__ == '__main__': scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)]) scene.control_walker([scene.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)]) - goal = (-100, 700) + # goal = (-100, 700) # goal = (-300) + # goal = (340.0, 900.0) + goal = (240.0, 1160.0) + '''场景4: 行人自由移动 robot到达指定目标''' # # TODO: autowalk=True仿真器会闪退 ??? # scene.clean_walker() diff --git a/robowaiter/llm_client/data/test_questions.txt b/robowaiter/llm_client/data/test_questions.txt index cc0bb66..22b4854 100644 --- a/robowaiter/llm_client/data/test_questions.txt +++ b/robowaiter/llm_client/data/test_questions.txt @@ -1 +1 @@ -{"测试VLM：做一杯咖啡": {"Answer": "测试VLM：做一杯咖啡", "Goal": "{\"On(Coffee,CoffeeTable)\"}"}, "测试VLM：做一杯咖啡放到吧台上": {"Answer": "测试VLM：做一杯咖啡放到吧台上", "Goal": "{\"On(Coffee,Bar)\"}"}, "测试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)\"}"}, "我有点热，能开个空调吗？": {"Answer": "当然可以，我现在就开！", "Goal": "{\"Is(AC,On)\"}"}} diff --git a/robowaiter/llm_client/data_raw/test_questions.csv b/robowaiter/llm_client/data_raw/test_questions.csv index 1a7573a..694d30c 100644 --- a/robowaiter/llm_client/data_raw/test_questions.csv +++ b/robowaiter/llm_client/data_raw/test_questions.csv @@ -16,3 +16,4 @@ Question,Answer,Goal VLM,VLM,"{""Is(Table1,Clean)""}" VLM,VLM,"{""Is(Chairs,Clean)""}" VLMѱŵTable2,VLMѱŵTable2,"{""On(BottledDrink,Table2)""}" +еȣܿյ,ȻԣھͿ,"{""Is(AC,On)""}" diff --git a/robowaiter/scene/tasks/Auto_tasks.py b/robowaiter/scene/tasks/Auto_tasks.py index 3a5bf43..b926cf2 100644 --- a/robowaiter/scene/tasks/Auto_tasks.py +++ b/robowaiter/scene/tasks/Auto_tasks.py @@ -20,7 +20,23 @@ class SceneAT(Scene): self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) def _run(self): + + self.move_task_area(7) + self.op_task_execute(7) + self.move_task_area(6) + self.op_task_execute(6) pass def _step(self): - pass \ No newline at end of file + pass + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneAT(robot) + task.reset() + task.run() diff --git a/robowaiter/scene/tasks/Open_tasks.py b/robowaiter/scene/tasks/Open_tasks.py index ca49491..ba3c8f3 100644 --- a/robowaiter/scene/tasks/Open_tasks.py +++ b/robowaiter/scene/tasks/Open_tasks.py @@ -19,16 +19,21 @@ class SceneOT(Scene): # 在这里加入场景中发生的事件 self.event_list = [ # (5,self.create_chat_event("给我一杯咖啡")) # (事件发生的时间，事件函数) - (5, self.create_chat_event("测试VLM：拖地")) # (事件发生的时间，事件函数) + (5, self.create_chat_event("我有点热，能开个空调吗？")) # (事件发生的时间，事件函数) ] def _reset(self): - pass - # self.add_walker(1085, 2630, 220) + scene = self.add_walker(0, 0, 0) + print("我有点热，能开个空调吗？") + # print("scene.walkers:",scene.walkers) + # cont = scene.walkers[0].name+":我有点热，能开个空调吗？" + # self.control_robot_action(0,3,cont) # self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) + pass def _run(self): + pass diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index 98b6a23..19938fc 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -13,7 +13,7 @@ class SceneVLM(Scene): 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：开大厅灯")), # (5, self.create_chat_event("测试VLM：拖地")), @@ -22,7 +22,7 @@ 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：做一杯咖啡放到吧台上")), ] def _reset(self): @@ -57,28 +57,28 @@ class SceneVLM(Scene): # 流程测试 # 抓握放置:抓吧台前生成的酸奶，放到抹布桌上 - # 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.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) From f2df712f2e6230595168d67ba93ff567ef7014f4 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Wed, 15 Nov 2023 14:33:18 +0800 Subject: [PATCH 7/7] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E9=83=A8?= =?UTF-8?q?=E5=88=86=E5=8A=A8=E4=BD=9C=E8=8A=82=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/algos/navigate/test.py | 3 +++ robowaiter/behavior_lib/act/Make.py | 2 +- robowaiter/behavior_lib/act/MoveTo.py | 22 +++++++++++++++++-- .../llm_client/data_raw/test_questions.csv | 1 + robowaiter/scene/scene.py | 19 +++++++++++++++- robowaiter/scene/tasks/Open_tasks.py | 2 -- robowaiter/scene/tasks/VLM.py | 3 ++- 7 files changed, 45 insertions(+), 7 deletions(-) diff --git a/robowaiter/algos/navigate/test.py b/robowaiter/algos/navigate/test.py index c8c4d91..bee148a 100644 --- a/robowaiter/algos/navigate/test.py +++ b/robowaiter/algos/navigate/test.py @@ -46,6 +46,9 @@ if __name__ == '__main__': # goal = (-100, 700) # goal = (-300) + # goal = (340.0, 900.0) + + goal = (240.0, 1000.0) # goal = (340.0, 900.0) goal = (240.0, 1160.0) diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 8fcc533..bc58583 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -47,7 +47,7 @@ class Make(Act): if len(obj_dict) != 0: # 获取obj_id for id, obj in enumerate(obj_dict): - if obj.name == "CoffeeCup": + if obj.name == "Coffee": obj_info = obj_dict[id] obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z print(id,obj.name,obj_x,obj_y,obj_z) diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index ed8fb4a..db1cb7f 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -36,10 +36,28 @@ class MoveTo(Act): goal = Act.place_xyz_dic[self.target_place] self.scene.walk_to(goal[0],goal[1]) else: # 走到物品边上 + obj_id = -1 + min_dis = float('inf') + obj_dict = self.scene.status.objects + if len(obj_dict)!=0: + # 获取obj_id + for id,obj in enumerate(obj_dict): + if obj.name == self.target_place: + obj_id = id + # obj_info = obj_dict[id] + # obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z + # ginger_x,ginger_y,ginger_z = [int(self.scene.location.X), int(self.scene.location.Y), int(self.scene.rotation.Yaw)] + break + if self.target_place == "CoffeeCup": + obj_id = 273 + if obj_id == -1: + return ptree.common.Status.FAILURE - self.scene.move_task_area(16, obj_id=273) - self.scene.op_task_execute(16, obj_id=273) + self.scene.move_to_obj(obj_id=obj_id) + # 为了演示，写死咖啡位置 + # if self.target_place=="Coffee": + # obj_id = 273 # obj_id = -1 # obj_dict = self.scene.status.objects # if len(obj_dict)!=0: diff --git a/robowaiter/llm_client/data_raw/test_questions.csv b/robowaiter/llm_client/data_raw/test_questions.csv index 694d30c..2e64a0b 100644 --- a/robowaiter/llm_client/data_raw/test_questions.csv +++ b/robowaiter/llm_client/data_raw/test_questions.csv @@ -1,6 +1,7 @@ Question,Answer,Goal VLMһ,VLMһ,"{""On(Coffee,CoffeeTable)""}" VLMһȷŵ̨,VLMһȷŵ̨,"{""On(Coffee,Bar)""}" +VLMһȷŵˮϲˮ,VLMһȷŵˮϲˮ,"{""On(Coffee,WaterTable)""}" VLNǰ2,VLNǰ2,"{""At(Robot,Table2)""}" AEM,AEM,"{""EnvExplored()""}" VLMһˮ,VLMһˮ,"{""On(Water,WaterTable)""}" diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 3c135e7..bd105c2 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -361,6 +361,20 @@ class Scene: ] temp = stub.GetIKControlInfos(GrabSim_pb2.HandPostureInfos(scene=self.sceneID, handPostureObjects=HandPostureObject)) + + def move_to_obj(self,obj_id): + scene = self.status + obj_info = scene.objects[obj_id] + # Robot + obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z + walk_v = [obj_x + 50, obj_y] + [180, 180, 0] + if obj_y >= 820 and obj_y <= 1200 and obj_x >= 240 and obj_x <= 500: # 物品位于斜的抹布桌上 ([240,500],[820,1200]) + walk_v = [obj_x + 40, obj_y - 35, 130, 180, 0] + obj_x += 3 + obj_y += 2.5 + action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) + scene = stub.Do(action) + # 移动到进行操作任务的指定地点 def move_task_area(self,op_type,obj_id=0, release_pos=[247.0, 520.0, 100.0]): scene = self.status @@ -370,10 +384,13 @@ class Scene: 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] # 动画控制 + 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: # 抓握物体，移动到物体周围的可达区域 + scene = self.status obj_info = scene.objects[obj_id] # Robot obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z diff --git a/robowaiter/scene/tasks/Open_tasks.py b/robowaiter/scene/tasks/Open_tasks.py index ba3c8f3..418c809 100644 --- a/robowaiter/scene/tasks/Open_tasks.py +++ b/robowaiter/scene/tasks/Open_tasks.py @@ -31,9 +31,7 @@ class SceneOT(Scene): # self.control_walker([self.walker_control_generator(0, False, 100, 755, 1900, 180)]) pass - def _run(self): - pass diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index 19938fc..5cf3789 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -21,8 +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：做一杯咖啡放到吧台上")), + # (5, self.create_chat_event("测试VLM：做一杯咖啡放到水杯桌上并倒水")), ] def _reset(self): @@ -85,6 +85,7 @@ class SceneVLM(Scene): # pos = [-55.0, 0.0, 107] # self.move_task_area(17, release_pos=pos) # self.op_task_execute(17, release_pos=pos) + pass def _step(self):