Merge branch 'main' of github.com:HPCL-EI/RoboWaiter
This commit is contained in:
ChenXL97
commit
4aa6e8f282
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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仿真器会闪退 ???
|
||||
|
|
|
|||
|
|
@ -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),
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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]]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)\"}"}}
|
||||
|
|
|
|||
|
|
@ -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)""}"
|
||||
|
|
|
|||
|
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
@ -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
|
||||
|
||||
if __name__ == '__main__':
|
||||
import os
|
||||
from robowaiter.robot.robot import Robot
|
||||
|
||||
robot = Robot()
|
||||
|
||||
# create task
|
||||
task = SceneAT(robot)
|
||||
task.reset()
|
||||
task.run()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
Loading…
Reference in New Issue