EmbodiedAI
/
RoboWaiter
mirror of https://github.com/HPCL-EI/RoboWaiter.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' of github.com:HPCL-EI/RoboWaiter

This commit is contained in:
ChenXL97 2023-11-15 09:41:17 +08:00
parent 05f790e635 8637135281
commit 70af2a7eaa
13 changed files with 137 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
robowaiter/behavior_lib/_base/Behavior.py
View File

@ -13,15 +13,17 @@ class Bahavior(ptree.behaviour.Behaviour):
'''
scene = None
print_name_prefix = ""
all_place = {'Bar', 'Bar2', 'WaterTable', 'CoffeeTable', 'Table1', 'Table2', 'Table3'}
all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk',
'VacuumCup'}
# 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_object = {'Coffee'}
place_xyz_dic={
'Bar': (247.0, 520.0, 100.0),
'Bar2': (240.0, 40.0, 70.0),
'WaterTable':(-70.0, 500.0, 107),
'CoffeeTable':(247.0, 520.0, 100.0), # 位置需要更改！！！
'Table1': (247.0, 520.0, 100.0),# 位置需要更改！！！
'CoffeeTable':(250.0, 310.0, 100.0),
'Table1': (340.0, 900.0, 98.0),
'Table2': (-55.0, 0.0, 107),
'Table3':(-55.0, 150.0, 107)
}

9
robowaiter/behavior_lib/act/Make.py
View File

@ -27,12 +27,13 @@ class Make(Act):
info = {}
info["pre"]= {f'Holding(Nothing)'}
info['del_set'] = set()
info['add'] = {f'Exist({arg})'}
if arg == "Coffee":
info["add"]= {f'On(Coffee,CoffeeTable)'}
info["add"] |= {f'On(Coffee,CoffeeTable)'}
elif arg == "Water":
info["add"] = {f'On(Water,WaterTable)'}
info["add"] |= {f'On(Water,WaterTable)'}
elif arg == "Dessert":
info["add"] = {f'On(Dessert,Bar)'}
info["add"] |= {f'On(Dessert,Bar)'}
return info
def _update(self) -> ptree.common.Status:
@ -40,6 +41,8 @@ class Make(Act):
self.scene.move_task_area(self.op_type)
self.scene.op_task_execute(self.op_type)
# self.scene.gen_obj(type=40)
self.scene.state["condition_set"].union(self.info["add"])
self.scene.state["condition_set"] -= self.info["del_set"]
return Status.RUNNING

21
robowaiter/behavior_lib/act/MoveTo.py
View File

@ -17,6 +17,8 @@ class MoveTo(Act):
def get_info(cls,arg):
info = {}
info['pre'] = set()
if arg in Act.all_object:
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}
return info
@ -29,20 +31,27 @@ class MoveTo(Act):
# goal = self.scene.state['map']['obj_pos'][self.args[0]]
# navigator.navigate(goal, animation=False)
# 走到固定的地点
if self.target_place in Act.place_xyz_dic:
goal = Act.place_xyz_dic[self.target_place]
self.scene.walk_to(goal[0],goal[1])
else:
# 获取obj_id
for id,obj in enumerate(self.scene.objects):
if obj.name == self.target_place:
obj_id = id
break
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)
# goal = self.scene.state['map']['obj_pos'][self.args[0]]
# self.scene.walk_to(goal[0],goal[1]) # X, Y, Yaw=None, velocity=200, dis_limit=0

2
robowaiter/behavior_lib/act/PickUp.py
View File

@ -18,6 +18,8 @@ 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:
info["del_set"] |= {f'On({arg},{place})'}
return info

2
robowaiter/behavior_lib/act/PutDown.py
View File

@ -20,7 +20,7 @@ class PutDown(Act):
def get_info(cls,*arg):
info = {}
info["pre"] = {f'Holding({arg[0]})',f'At(Robot,{arg[1]})'}
info["add"] = {f'Holding(Nothing)',f'At({arg[0]},{arg[1]})'}
info["add"] = {f'Holding(Nothing)',f'On({arg[0]},{arg[1]})'}
info["del_set"] = {f'Holding({arg[0]})'}
return info

26
robowaiter/behavior_lib/cond/Exist.py Normal file
View File

@ -0,0 +1,26 @@
import py_trees as ptree
from typing import Any
from robowaiter.behavior_lib._base.Cond import Cond
import itertools
class Exist(Cond):
can_be_expanded = True
num_params = 2
valid_args = tuple(Cond.all_object)
def __init__(self,*args):
super().__init__(*args)
def _update(self) -> ptree.common.Status:
# if self.scene.status?
if self.name in self.scene.state["condition_set"]:
return ptree.common.Status.SUCCESS
else:
return ptree.common.Status.FAILURE
# if self.scene.state['chat_list'] == []:
# return ptree.common.Status.FAILURE
# else:
# return ptree.common.Status.SUCCESS

80
robowaiter/behavior_tree/dataset/goal_states.txt
View File

@ -747,7 +747,7 @@
{At(Robot,Table1)}
{At(Robot,Table2)}
{At(Robot,Table3)}
{At(Robot, Table1)}
{At(Robot, CoffeeTable)}
{On(Softdrink,Bar)}
{On(Softdrink,WaterTable)}
{On(Softdrink,CoffeeTable)}
@ -1483,21 +1483,21 @@
{On(VacuumCup,Table1)}
{On(VacuumCup,Table2)}
{On(VacuumCup,Table3)}
{On(Milk, Table1)}
{On(ADMilk, CoffeeTable)}
{On(VacuumCup, Bar2)}
{On(MilkDrink, Table2)}
{On(MilkDrink, Bar2)}
{On(Yogurt, WaterTable)}
{On(Softdrink, Bar2)}
{On(Softdrink, WaterTable)}
{On(Softdrink, Bar)}
{On(Softdrink, Table2)}
{On(Softdrink, Table1)}
{On(ADMilk, Bar2)}
{On(Milk, Table1)}
{On(Milk, Table1)}
{On(Milk, Bar2)}
{On(VacuumCup, Bar)}
{On(MilkDrink, WaterTable)}
{On(ADMilk, Bar)}
{On(MilkDrink, Table3)}
{On(MilkDrink, WaterTable)}
{On(BottledDrink, CoffeeTable)}
{On(VacuumCup, Table2)}
{On(MilkDrink, Bar)}
{On(ADMilk, WaterTable)}
{On(BottledDrink, Bar2)}
{On(ADMilk, Table3)}
{On(VacuumCup, Bar2)}
{Is(AC,Off)}
{Is(AC,On)}
{Is(ACTemperature,Up)}
@ -2234,20 +2234,20 @@
{Is(Floor,Clean)}
{Is(Table1,Dirty)}
{Is(Table1,Clean)}
{Is(ACTemperature, 0)}
{Is(Chairs, 1)}
{Is(TubeLight, 0)}
{Is(HallLight, 0)}
{Is(Chairs, 0)}
{Is(Floor, 0)}
{Is(Floor, 0)}
{Is(AC, 0)}
{Is(HallLight, 1)}
{Is(Chairs, 0)}
{Is(TubeLight, 1)}
{Is(ACTemperature, 0)}
{Is(Floor, 1)}
{Is(AC, 1)}
{Is(Floor, 1)}
{Is(Floor, 1)}
{Is(TubeLight, 0)}
{Is(Table1, 0)}
{Is(AC, 0)}
{Is(ACTemperature, 1)}
{Is(Table1, 0)}
{Is(HallLight, 0)}
{Is(HallLight, 1)}
{Is(Chairs, 1)}
{Holding(Softdrink)}
{Holding(BottledDrink)}
{Holding(Yogurt)}
@ -2744,10 +2744,10 @@
{Holding(Milk)}
{Holding(VacuumCup)}
{Holding(Nothing)}
{Holding(Softdrink)}
{Holding(VacuumCup)}
{Holding(Yogurt)}
{Holding(VacuumCup)}
{Holding(MilkDrink)}
{Holding(ADMilk)}
{Holding(ADMilk)}
{On(Coffee,Bar)}
{On(Coffee,WaterTable)}
{On(Coffee,CoffeeTable)}
@ -3483,21 +3483,21 @@
{On(Dessert,Table1)}
{On(Dessert,Table2)}
{On(Dessert,Table3)}
{On(Dessert, Table2)}
{On(Dessert, Bar2)}
{On(Water, Table2)}
{On(Dessert, Table3)}
{On(Coffee, WaterTable)}
{On(Coffee, CoffeeTable)}
{On(Coffee, WaterTable)}
{On(Dessert, Bar2)}
{On(Water, CoffeeTable)}
{On(Water, Bar2)}
{On(Dessert, Table2)}
{On(Coffee, Table3)}
{On(Water, WaterTable)}
{On(Coffee, Table2)}
{On(Coffee, Table2)}
{On(Water, Table3)}
{On(Coffee, Bar2)}
{On(Coffee, Bar)}
{On(Dessert, Bar)}
{On(Water, Table2)}
{On(Coffee, WaterTable)}
{On(Water, CoffeeTable)}
{On(Dessert, CoffeeTable)}
{On(Water, Table3)}
{On(Water, Table3)}
{On(Coffee, Table1)}
{On(Dessert, WaterTable)}
{On(Water, Bar2)}
{On(Water, Bar2)}
{At(Robot,Bar),On(Softdrink,Bar)}
{At(Robot,Bar),On(Softdrink,WaterTable)}
{At(Robot,Bar),On(Softdrink,CoffeeTable)}

2
robowaiter/llm_client/data/test_questions.txt
View File

@ -1 +1 @@
{"测试VLM：做一杯咖啡": {"Answer": "测试VLM：做一杯咖啡", "Goal": "{\"At(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)\"}"}}

3
robowaiter/llm_client/data_raw/test_questions.csv
View File

@ -1,5 +1,6 @@
Question,Answer,Goal
测试VLM：做一杯咖啡,测试VLM：做一杯咖啡,"{""At(Coffee,Bar)""}"
测试VLM：做一杯咖啡,测试VLM：做一杯咖啡,"{""On(Coffee,CoffeeTable)""}"
测试VLM：做一杯咖啡放到吧台上,测试VLM：做一杯咖啡放到吧台上,"{""On(Coffee,Bar)""}"
测试VLN：前往2号桌,测试VLN：前往2号桌,"{""At(Robot,Table2)""}"
测试AEM,测试AEM,"{""EnvExplored()""}"
测试VLM：倒一杯水,测试VLM：倒一杯水,"{""On(Water,WaterTable)""}"

1 Question Answer Goal
2 测试VLM：做一杯咖啡 测试VLM：做一杯咖啡 {"At(Coffee,Bar)"} {"On(Coffee,CoffeeTable)"}
3 测试VLM：做一杯咖啡放到吧台上 测试VLM：做一杯咖啡放到吧台上 {"On(Coffee,Bar)"}
4 测试VLN：前往2号桌 测试VLN：前往2号桌 {"At(Robot,Table2)"}
5 测试AEM 测试AEM {"EnvExplored()"}
6 测试VLM：倒一杯水 测试VLM：倒一杯水 {"On(Water,WaterTable)"}

25
robowaiter/scene/scene.py
View File

@ -180,21 +180,13 @@ class Scene:
)
def walk_to(self, X, Y, Yaw=None, velocity=200, dis_limit=0):
if self.use_offset:
X, Y = X + loc_offset[0], Y + loc_offset[1]
if Yaw is None:
Yaw = self.status.rotation.Yaw
v = [X, Y, Yaw - 90, velocity, dis_limit]
print(v)
walk_v = [X,Y,Yaw,velocity,dis_limit]
action = GrabSim_pb2.Action(
scene=self.sceneID,
action=GrabSim_pb2.Action.ActionType.WalkTo,
values=v
scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v
)
scene_info = stub.Do(action)
return scene_info
scene = stub.Do(action)
return scene
def reachable_check(self, X, Y, Yaw):
@ -418,13 +410,14 @@ class Scene:
def gen_obj(self,type=5,h=100):
# 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 14:cake 61:甜牛奶
type= 5 #9
# type= 5 #9
scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID))
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),
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),
# 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)

4
robowaiter/scene/tasks/VLM.py
View File

@ -21,6 +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：做一杯咖啡放到吧台上")),
]
def _reset(self):
@ -28,7 +30,7 @@ class SceneVLM(Scene):
'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.gen_obj(type=9)
# self.op_task_execute(op_type=16, obj_id=0)
# self.move_task_area(op_type=4)
pass

2
run_robowaiter.py
View File

@ -1,7 +1,7 @@
import os
from robowaiter import Robot, task_map
TASK_NAME = 'OT'
TASK_NAME = 'VLM'
# create robot
project_path = "./robowaiter"

26
sub_task.ptml
View File

@ -1,4 +1,26 @@
selector{
cond At(Robot,Table2)
act MoveTo(Table2)
cond On(Coffee,CoffeeTable)
sequence{
cond Holding(Nothing)
act Make(Coffee)
}
sequence{
cond At(Robot,CoffeeTable)
cond Holding(Coffee)
act PutDown(Coffee,CoffeeTable)
}
sequence{
cond At(Robot,Bar)
cond Holding(Coffee)
act PutDown(Coffee,Bar)
}
sequence{
cond At(Robot,WaterTable)
cond Holding(Coffee)
act PutDown(Coffee,WaterTable)
}
sequence{
cond Holding(Coffee)
act MoveTo(CoffeeTable)
}
}