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/algos/navigate/test.py b/robowaiter/algos/navigate/test.py index 525ad10..bee148a 100644 --- a/robowaiter/algos/navigate/test.py +++ b/robowaiter/algos/navigate/test.py @@ -44,8 +44,14 @@ 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, 1000.0) + + # goal = (340.0, 900.0) + goal = (240.0, 1160.0) '''场景4: 行人自由移动 robot到达指定目标''' # # TODO: autowalk=True仿真器会闪退 ??? 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..bc58583 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 == "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) + + 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..db1cb7f 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -37,19 +37,44 @@ class MoveTo(Act): 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 - 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_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: + # # 获取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/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..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)""}" @@ -16,3 +17,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/scene.py b/robowaiter/scene/scene.py index 0ed6374..fec9b6b 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: @@ -90,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): @@ -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 @@ -374,30 +378,52 @@ 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): + 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)) - walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] - + print('------------------moveTo_Area----------------------') 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 + 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 + 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 + 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("------------------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="你好"): @@ -414,7 +440,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 @@ -425,37 +453,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) @@ -463,7 +479,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的肢体恢复原位 @@ -475,7 +491,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 @@ -486,12 +502,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]) @@ -511,17 +521,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: @@ -532,6 +539,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 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/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 d21448b..7f10da3 100644 --- a/robowaiter/scene/tasks/Open_tasks.py +++ b/robowaiter/scene/tasks/Open_tasks.py @@ -18,15 +18,18 @@ class SceneOT(Scene): super().__init__(robot) # 在这里加入场景中发生的事件 self.event_list = [ - (5,self.create_chat_event("来二号桌")), - # (5,self.create_chat_event("感觉有点冷，可以关一下空调吗")), + # (5,self.create_chat_event("给我一杯咖啡")) # (事件发生的时间，事件函数) + (5, self.create_chat_event("我有点热，能开个空调吗？")) # (事件发生的时间，事件函数) ] def _reset(self): - self.add_walker(50, 300, 0) - # 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 5bfe7f6..5cf3789 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -11,9 +11,9 @@ 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：关空调")), # (5, self.create_chat_event("测试VLM：开大厅灯")), # (5, self.create_chat_event("测试VLM：拖地")), @@ -21,14 +21,12 @@ 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): - 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) @@ -43,17 +41,64 @@ 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): pass + + +if __name__ == '__main__': + import os + from robowaiter.robot.robot import Robot + + robot = Robot() + + # create task + task = SceneVLM(robot) + task.reset() + task.run()