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

更新了部分动作节点

This commit is contained in:
Caiyishuai 2023-11-15 12:04:49 +08:00
parent f06b87e4fb
commit ad5e8cbd12
13 changed files with 150 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
robowaiter/algos/explore/AEM.py
View File

@ -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

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

@ -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),

4
robowaiter/behavior_lib/act/Clean.py
View File

@ -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
return Status.SUCCESS

13
robowaiter/behavior_lib/act/ExploreEnv.py
View File

@ -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

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

@ -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

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

@ -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]]

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

@ -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

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

@ -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

49
robowaiter/scene/scene.py
View File

@ -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

17
robowaiter/scene/tasks/AEM.py
View File

@ -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()

12
robowaiter/scene/tasks/GQA.py
View File

@ -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()

13
robowaiter/scene/tasks/Open_tasks.py
View File

@ -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()

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

@ -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()