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):