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] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E9=83=A8=E5=88=86?= =?UTF-8?q?=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