diff --git a/README.md b/README.md index 0a3eb56..89cfeda 100644 --- a/README.md +++ b/README.md @@ -14,45 +14,29 @@ pip install -e . ### 快速入门 1. 安装UE及Harix插件，打开默认项目并运行 -2. 运行 run_robowaiter.py 文件即可实现机器人控制端与仿真器的交互 +2. 运行 tasks 文件夹下的任意场景即可实现机器人控制端与仿真器的交互 -# 运行流程介绍 -run_robowaiter.py 入口文件如下： -```python -import os -from robowaiter import Robot, task_map +# 代码框架介绍 -TASK_NAME = 'GQA' - -# create robot -project_path = "./robowaiter" -ptml_path = os.path.join(project_path, 'robot/Default.ptml') -behavior_lib_path = os.path.join(project_path, 'behavior_lib') - -robot = Robot(ptml_path,behavior_lib_path) - -# create task -task = task_map[TASK_NAME](robot) -task.reset() -task.run() -``` ## Robot Robot是机器人类，包括从ptml加载行为树的方法，以及执行行为树的方法等 -## task_map -task_map是任务字典，通过任务缩写来返回相应的场景类。 +## tasks +tasks文件夹中存放的场景定义及运行代码。 -| 缩写 | 任务 | -|----|---------| -| AEM | 主动探索和记忆 | -| GQA | 具身多轮对话 | -| VLN | 视觉语言导航 | -| VLM | 视觉语言操作 | -| OT | 复杂开放任务 | -| AT | 自主任务 | +| 缩写 | 任务 | +|---------------------|-------------| +| AEM | 主动探索和记忆 | +| GQA | 具身多轮对话 | +| VLN | 视觉语言导航 | +| VLM | 视觉语言操作 | +| OT | 复杂开放任务 | +| AT | 自主任务 | +| CafeDailyOperations | 整体展示：咖啡厅的一天 | +| Interact | 命令行自由交互 | ## Scene diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index c60ff51..e704d5a 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -103,7 +103,7 @@ class Bahavior(ptree.behaviour.Behaviour): - # let behavior node interact with the scene + # let behavior node Interact with the scene def set_scene(self, scene=None): if scene: self.scene = scene diff --git a/robowaiter/llm_client/data/fix_questions.txt b/robowaiter/llm_client/data/fix_questions.txt index 053d559..8962f86 100644 --- a/robowaiter/llm_client/data/fix_questions.txt +++ b/robowaiter/llm_client/data/fix_questions.txt @@ -9,7 +9,7 @@ ok, 有需要您再找我。 做一杯咖啡 好的，我马上做咖啡 create_sub_task -{"goal":"On(Coffee,CoffeeTable)"}00 +{"goal":"On(Coffee,CoffeeTable)"} 不用了。 好的，您有需要再跟我说 diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index 5c70b29..0670a3e 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -306,9 +306,7 @@ class Scene: return set_sub_task - def new_set_goal(self, goal): - g = eval("{'" + goal + "'}") - self.state['chat_list'].append(("Goal", g)) + def new_set_goal(self,goal): self.state['chat_list'].append(("Goal",goal)) diff --git a/robowaiter/scene/tasks/VLN.py b/robowaiter/scene/tasks/VLN.py deleted file mode 100644 index ce59d79..0000000 --- a/robowaiter/scene/tasks/VLN.py +++ /dev/null @@ -1,106 +0,0 @@ -""" -视觉语言导航 -识别顾客（NPC）靠近、打招呼、对话、领位导航到适合人数的空闲餐桌 -开始条件：监测到顾客靠近 -结束条件：完成领位，语音：“请问您想喝点什么？”，并等待下一步指令 -""" -import os -import pickle -import time -import random - -import matplotlib.pyplot as plt -import numpy as np - -from robowaiter.scene.scene import Scene,init_world # TODO: 文件名改成Scene.py - -from robowaiter.scene.scene import Scene -from robowaiter.utils import get_root_path -from robowaiter.algos.navigator.navigate import Navigator -from robowaiter.algos.navigator import test - -class SceneVLN(Scene): - def __init__(self, robot): - super().__init__(robot) - # 在这里加入场景中发生的事件， (事件发生的时间，事件函数) - self.event_list = [ - (5, self.create_chat_event("测试VLN：前往2号桌")), - ] - - def _reset(self): - root_path = get_root_path() - file_name = os.path.join(root_path,'robowaiter/algos/navigator/map_5.pkl') - with open(file_name, 'rb') as file: - map = pickle.load(file) - - self.state['map']['2d'] = map - self.state['map']['obj_pos']['Table'] = np.array((-100, 700)) - - def _run(self): - file_name = '../../algos/navigator/map_5.pkl' - if os.path.exists(file_name): - with open(file_name, 'rb') as file: - map = pickle.load(file) - - # self.init_world(1, 11) - # scene = self.status - - navigator = Navigator(scene=self, area_range=[-350, 600, -400, 1450], map=map) - - '''场景1: 无行人环境 robot到达指定目标''' - # goal = np.array((-100, 700)) - - '''场景2: 静止行人环境 robot到达指定目标''' - # scene.clean_walker() - # scene.add_walker(50, 300, 0) - # scene.add_walker(-50, 500, 0) - # goal = np.array((-100, 700)) - - '''场景3: 移动行人环境 robot到达指定目标''' - self.walk_to(100, 0, -90, dis_limit=10) - self.clean_walker() - self.add_walkers([[50, 300], [-50, 500], [0, 700]]) - # scene.add_walker(50, 300, 0) - # scene.add_walker(-50, 500, 0) - # scene.add_walker(0, 700, 0) - self.control_walker( - [self.walker_control_generator(walkerID=0, autowalk=False, speed=50, X=-50, Y=600, Yaw=0)]) - self.control_walker( - [self.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)]) - self.control_walker([self.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)]) - - # 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仿真器会闪退 ??? - # scene.clean_walker() - # scene.add_walker(50, 300, 0) - # scene.add_walker(-50, 500, 0) - # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=True, speed=20, X=0, Y=0, Yaw=0)]) - # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=True, speed=20, X=0, Y=0, Yaw=0)]) - # time.sleep(5) - # goal = np.array((-100, 700)) - - navigator.navigate(goal, animation=False) - - self.clean_walker() - print(self.status.collision) # TODO: 不显示碰撞信息 ??? - pass - - -if __name__ == '__main__': - import os - from robowaiter.robot.robot import Robot - - robot = Robot() - - # create task - task = SceneVLN(robot) - task.reset() - task.run() diff --git a/robowaiter/utils/draw_bt/Default.ptml b/robowaiter/utils/draw_bt/Default.ptml index 03745e6..55a6f88 100644 --- a/robowaiter/utils/draw_bt/Default.ptml +++ b/robowaiter/utils/draw_bt/Default.ptml @@ -1,17 +1,10 @@ -selector - sequence - cond CustomerChatting() - act DealChat() - sequence - cond HasSubTask() - sequence - act SubTaskPlaceHolder() - sequence - cond FocusingCustomer() - act ServeCustomer() - sequence - cond NewCustomerComing() - selector - cond At(Robot,Bar) - act MoveTo(Bar) - act GreetCustomer() +sequence{ +cond Holding(Nothing) +sequence{ +cond Is(Curtain,Off) +act Turn(Curtain,On) +} +sequence{ +cond Is(AC,Off) +act Turn(AC,On) +}} \ No newline at end of file diff --git a/robowaiter/utils/draw_bt/Default_bracket.ptml b/robowaiter/utils/draw_bt/Default_bracket.ptml index e18182b..844db49 100644 --- a/robowaiter/utils/draw_bt/Default_bracket.ptml +++ b/robowaiter/utils/draw_bt/Default_bracket.ptml @@ -2,27 +2,28 @@ selector { sequence { - cond CustomerChatting() + cond Chatting() act DealChat() - -} sequence + } sequence { cond HasSubTask() sequence { act SubTaskPlaceHolder() - -} sequence + } } sequence +{ cond FocusingCustomer() act ServeCustomer() - -} sequence + } sequence { - cond NewCustomerComing() + cond NewCustomer() selector { cond At(Robot,Bar) act MoveTo(Bar) - -} act GreetCustomer() -}} \ No newline at end of file + } act GreetCustomer() + } sequence +{ + cond AnomalyDetected() + act ResolveAnomaly() + }} \ No newline at end of file diff --git a/robowaiter/utils/draw_bt/test.dot b/robowaiter/utils/draw_bt/test.dot index bd79eac..cf2b2d7 100644 --- a/robowaiter/utils/draw_bt/test.dot +++ b/robowaiter/utils/draw_bt/test.dot @@ -3,37 +3,19 @@ ordering=out; graph [fontname="times-roman"]; node [fontname="times-roman"]; edge [fontname="times-roman"]; -"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=diamond, style=filled, width=0.01]; -"717a1290-56bc-41e1-9b4b-e6df93d40e64" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "717a1290-56bc-41e1-9b4b-e6df93d40e64"; -"b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2" [fillcolor=yellow, fontcolor=black, fontsize=20, label="CustomerChatting()", shape=ellipse, style=filled]; -"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2"; -"28015225-b7a4-4f38-b26f-4dbe5eea4154" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="DealChat()", shape=box, style=filled]; -"717a1290-56bc-41e1-9b4b-e6df93d40e64" -> "28015225-b7a4-4f38-b26f-4dbe5eea4154"; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"a3ac082f-ad46-4cc9-86b4-3e76c3c564d3" -> "ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e"; -"2f77f976-499c-4601-9d8c-86dd80d66dfa" [fillcolor=yellow, fontcolor=black, fontsize=20, label="HasSubTask()", shape=ellipse, style=filled]; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2f77f976-499c-4601-9d8c-86dd80d66dfa"; -"2428792c-3896-443d-8744-f5e286644fad" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "2428792c-3896-443d-8744-f5e286644fad"; -"4bb7a9c3-521b-408f-b403-1fd25d54b192" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="SubTaskPlaceHolder()", shape=box, style=filled]; -"2428792c-3896-443d-8744-f5e286644fad" -> "4bb7a9c3-521b-408f-b403-1fd25d54b192"; -"64b2362a-d99e-4e99-8772-ad1419e53a2e" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "64b2362a-d99e-4e99-8772-ad1419e53a2e"; -"fd7124ba-c9d1-4fde-8063-e8337d335121" [fillcolor=yellow, fontcolor=black, fontsize=20, label="FocusingCustomer()", shape=ellipse, style=filled]; -"64b2362a-d99e-4e99-8772-ad1419e53a2e" -> "fd7124ba-c9d1-4fde-8063-e8337d335121"; -"169ebec9-3645-4fbb-a533-5186a8e5967b" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="ServeCustomer()", shape=box, style=filled]; -"64b2362a-d99e-4e99-8772-ad1419e53a2e" -> "169ebec9-3645-4fbb-a533-5186a8e5967b"; -"bea12066-ecbd-49b0-8934-efcb2c38b5f5" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; -"ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e" -> "bea12066-ecbd-49b0-8934-efcb2c38b5f5"; -"a847a0cc-c5af-4757-aa8c-8baef72788dc" [fillcolor=yellow, fontcolor=black, fontsize=20, label="NewCustomerComing()", shape=ellipse, style=filled]; -"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "a847a0cc-c5af-4757-aa8c-8baef72788dc"; -"99354f05-1716-46b2-9151-d88eac0a5b27" [fillcolor=cyan, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=diamond, style=filled, width=0.01]; -"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "99354f05-1716-46b2-9151-d88eac0a5b27"; -"19de10a3-7554-43a3-b892-34cb2e32ab9a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="At(Robot,Bar)", shape=ellipse, style=filled]; -"99354f05-1716-46b2-9151-d88eac0a5b27" -> "19de10a3-7554-43a3-b892-34cb2e32ab9a"; -"4286d652-c4ef-4522-a7f1-b5c865dcc4c9" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="MoveTo(Bar)", shape=box, style=filled]; -"99354f05-1716-46b2-9151-d88eac0a5b27" -> "4286d652-c4ef-4522-a7f1-b5c865dcc4c9"; -"a3766a34-5152-4f82-8fba-8bf1f6b3830b" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="GreetCustomer()", shape=box, style=filled]; -"bea12066-ecbd-49b0-8934-efcb2c38b5f5" -> "a3766a34-5152-4f82-8fba-8bf1f6b3830b"; +"c9a8671e-b0b8-4587-a4de-a094fd678334" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Selector, shape=octagon, style=filled, width=0.01]; +"bc597ff6-b3cb-4573-9dea-944fdb38d045" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Holding(Nothing)", shape=ellipse, style=filled]; +"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "bc597ff6-b3cb-4573-9dea-944fdb38d045"; +"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "a8e26121-f5cc-4ba5-b98e-86bd84de06d6"; +"676d8bda-2c30-44a5-bca2-ade68761403a" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(Curtain,Off)", shape=ellipse, style=filled]; +"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "676d8bda-2c30-44a5-bca2-ade68761403a"; +"c3d096cf-f848-46a3-9497-5398614798e8" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(Curtain,On)", shape=box, style=filled]; +"a8e26121-f5cc-4ba5-b98e-86bd84de06d6" -> "c3d096cf-f848-46a3-9497-5398614798e8"; +"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" [fillcolor=orange, fontcolor=black, fontsize=20, height=0.01, label=Sequence, shape=octagon, style=filled, width=0.01]; +"c9a8671e-b0b8-4587-a4de-a094fd678334" -> "400b4104-ff96-4d64-89b2-49dc6e4e3cbd"; +"3fc48afc-1cef-45f5-826d-2571d7979a2f" [fillcolor=yellow, fontcolor=black, fontsize=20, label="Is(AC,Off)", shape=ellipse, style=filled]; +"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "3fc48afc-1cef-45f5-826d-2571d7979a2f"; +"47af4df0-8811-45a5-95e2-f0fc9458ddfd" [fillcolor=lawngreen, fontcolor=black, fontsize=20, label="Turn(AC,On)", shape=box, style=filled]; +"400b4104-ff96-4d64-89b2-49dc6e4e3cbd" -> "47af4df0-8811-45a5-95e2-f0fc9458ddfd"; } diff --git a/robowaiter/utils/draw_bt/test.png b/robowaiter/utils/draw_bt/test.png index 773238d..15a3183 100644 Binary files a/robowaiter/utils/draw_bt/test.png and b/robowaiter/utils/draw_bt/test.png differ diff --git a/robowaiter/utils/draw_bt/test.svg b/robowaiter/utils/draw_bt/test.svg index 2ed2acf..29211a1 100644 --- a/robowaiter/utils/draw_bt/test.svg +++ b/robowaiter/utils/draw_bt/test.svg @@ -4,208 +4,100 @@ - - + + pastafarianism - - + + -a3ac082f-ad46-4cc9-86b4-3e76c3c564d3 - -Selector +c9a8671e-b0b8-4587-a4de-a094fd678334 + +Selector - + -717a1290-56bc-41e1-9b4b-e6df93d40e64 - -Sequence +bc597ff6-b3cb-4573-9dea-944fdb38d045 + +Holding(Nothing) - + -a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->717a1290-56bc-41e1-9b4b-e6df93d40e64 - - +c9a8671e-b0b8-4587-a4de-a094fd678334->bc597ff6-b3cb-4573-9dea-944fdb38d045 + + - - -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e - -Sequence - - - -a3ac082f-ad46-4cc9-86b4-3e76c3c564d3->ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e - - - - + -b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2 - -CustomerChatting() +a8e26121-f5cc-4ba5-b98e-86bd84de06d6 + +Sequence - + -717a1290-56bc-41e1-9b4b-e6df93d40e64->b3236b13-d5f2-4643-ae7c-6f27bd4ab6f2 - - +c9a8671e-b0b8-4587-a4de-a094fd678334->a8e26121-f5cc-4ba5-b98e-86bd84de06d6 + + - - -28015225-b7a4-4f38-b26f-4dbe5eea4154 - -DealChat() - - - -717a1290-56bc-41e1-9b4b-e6df93d40e64->28015225-b7a4-4f38-b26f-4dbe5eea4154 - - - - + -2f77f976-499c-4601-9d8c-86dd80d66dfa - -HasSubTask() +400b4104-ff96-4d64-89b2-49dc6e4e3cbd + +Sequence - + -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2f77f976-499c-4601-9d8c-86dd80d66dfa - - +c9a8671e-b0b8-4587-a4de-a094fd678334->400b4104-ff96-4d64-89b2-49dc6e4e3cbd + + - + + +676d8bda-2c30-44a5-bca2-ade68761403a + +Is(Curtain,Off) + + + +a8e26121-f5cc-4ba5-b98e-86bd84de06d6->676d8bda-2c30-44a5-bca2-ade68761403a + + + + + +c3d096cf-f848-46a3-9497-5398614798e8 + +Turn(Curtain,On) + + + +a8e26121-f5cc-4ba5-b98e-86bd84de06d6->c3d096cf-f848-46a3-9497-5398614798e8 + + + + -2428792c-3896-443d-8744-f5e286644fad - -Sequence +3fc48afc-1cef-45f5-826d-2571d7979a2f + +Is(AC,Off) - + -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->2428792c-3896-443d-8744-f5e286644fad - - +400b4104-ff96-4d64-89b2-49dc6e4e3cbd->3fc48afc-1cef-45f5-826d-2571d7979a2f + + - - -64b2362a-d99e-4e99-8772-ad1419e53a2e - -Sequence - - - -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->64b2362a-d99e-4e99-8772-ad1419e53a2e - - - - - -bea12066-ecbd-49b0-8934-efcb2c38b5f5 - -Sequence - - - -ff83c742-a2ba-4aa5-8b3a-39ecd7c03b0e->bea12066-ecbd-49b0-8934-efcb2c38b5f5 - - - - + -4bb7a9c3-521b-408f-b403-1fd25d54b192 - -SubTaskPlaceHolder() +47af4df0-8811-45a5-95e2-f0fc9458ddfd + +Turn(AC,On) - + -2428792c-3896-443d-8744-f5e286644fad->4bb7a9c3-521b-408f-b403-1fd25d54b192 - - - - - -fd7124ba-c9d1-4fde-8063-e8337d335121 - -FocusingCustomer() - - - -64b2362a-d99e-4e99-8772-ad1419e53a2e->fd7124ba-c9d1-4fde-8063-e8337d335121 - - - - - -169ebec9-3645-4fbb-a533-5186a8e5967b - -ServeCustomer() - - - -64b2362a-d99e-4e99-8772-ad1419e53a2e->169ebec9-3645-4fbb-a533-5186a8e5967b - - - - - -a847a0cc-c5af-4757-aa8c-8baef72788dc - -NewCustomerComing() - - - -bea12066-ecbd-49b0-8934-efcb2c38b5f5->a847a0cc-c5af-4757-aa8c-8baef72788dc - - - - - -99354f05-1716-46b2-9151-d88eac0a5b27 - -Selector - - - -bea12066-ecbd-49b0-8934-efcb2c38b5f5->99354f05-1716-46b2-9151-d88eac0a5b27 - - - - - -a3766a34-5152-4f82-8fba-8bf1f6b3830b - -GreetCustomer() - - - -bea12066-ecbd-49b0-8934-efcb2c38b5f5->a3766a34-5152-4f82-8fba-8bf1f6b3830b - - - - - -19de10a3-7554-43a3-b892-34cb2e32ab9a - -At(Robot,Bar) - - - -99354f05-1716-46b2-9151-d88eac0a5b27->19de10a3-7554-43a3-b892-34cb2e32ab9a - - - - - -4286d652-c4ef-4522-a7f1-b5c865dcc4c9 - -MoveTo(Bar) - - - -99354f05-1716-46b2-9151-d88eac0a5b27->4286d652-c4ef-4522-a7f1-b5c865dcc4c9 - - +400b4104-ff96-4d64-89b2-49dc6e4e3cbd->47af4df0-8811-45a5-95e2-f0fc9458ddfd + + diff --git a/run_robowaiter.py b/run_robowaiter.py deleted file mode 100644 index dda629e..0000000 --- a/run_robowaiter.py +++ /dev/null @@ -1,16 +0,0 @@ -import os -from robowaiter import Robot, task_map - -TASK_NAME = 'VLM' - -# create robot -project_path = "./robowaiter" -ptml_path = os.path.join(project_path, 'robot/Default.ptml') -behavior_lib_path = os.path.join(project_path, 'behavior_lib') - -robot = Robot(ptml_path, behavior_lib_path) - -# create task -task = task_map[TASK_NAME](robot) -task.reset() -task.run() diff --git a/sub_task.ptml b/sub_task.ptml deleted file mode 100644 index 33895cd..0000000 --- a/sub_task.ptml +++ /dev/null @@ -1,31 +0,0 @@ -selector{ - -sequence{ - cond On(Coffee,CoffeeTable) - 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) -} -} diff --git a/robowaiter/scene/tasks/AEM.py b/tasks/AEM/AEM.py similarity index 98% rename from robowaiter/scene/tasks/AEM.py rename to tasks/AEM/AEM.py index 3ab63f1..e441598 100644 --- a/robowaiter/scene/tasks/AEM.py +++ b/tasks/AEM/AEM.py @@ -40,7 +40,7 @@ class SceneAEM(Scene): map_ratio = self.map_ratio db = DBSCAN(eps=map_ratio, min_samples=int(map_ratio / 2)) - file_name = '../../proto/map_1.pkl' + file_name = '../../robowaiter/proto/map_1.pkl' if os.path.exists(file_name): with open(file_name, 'rb') as file: map = pickle.load(file) @@ -117,7 +117,7 @@ class SceneAEM(Scene): {"id": f"{i}", "name": f"{cur_objs[i].name}", "location": f"{cur_objs[i].location}", "height": f"{cur_objs[i].location.Z}"}) - with open('../../proto/objs.json', 'w') as file: + with open('../../robowaiter/proto/objs.json', 'w') as file: json.dump(obj_json_data, file) # for i in range(-350, 600): diff --git a/robowaiter/scene/interact/__init__.py b/tasks/AEM/__init__.py similarity index 100% rename from robowaiter/scene/interact/__init__.py rename to tasks/AEM/__init__.py diff --git a/robowaiter/scene/tasks/AT/Auto_tasks_light.py b/tasks/AT/Auto_tasks_light.py similarity index 100% rename from robowaiter/scene/tasks/AT/Auto_tasks_light.py rename to tasks/AT/Auto_tasks_light.py diff --git a/robowaiter/scene/tasks/AT/__init__.py b/tasks/AT/__init__.py similarity index 100% rename from robowaiter/scene/tasks/AT/__init__.py rename to tasks/AT/__init__.py diff --git a/robowaiter/scene/tasks/CafeDailyOperations/CafeOneDay.py b/tasks/CafeDailyOperations/CafeOneDay.py similarity index 100% rename from robowaiter/scene/tasks/CafeDailyOperations/CafeOneDay.py rename to tasks/CafeDailyOperations/CafeOneDay.py diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py b/tasks/CafeDailyOperations/VLN_all.py similarity index 100% rename from robowaiter/scene/tasks/CafeDailyOperations/VLN_all.py rename to tasks/CafeDailyOperations/VLN_all.py diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py b/tasks/CafeDailyOperations/VLN_greet_and_order.py similarity index 100% rename from robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order.py rename to tasks/CafeDailyOperations/VLN_greet_and_order.py diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1118.py b/tasks/CafeDailyOperations/VLN_greet_and_order_1118.py similarity index 100% rename from robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1118.py rename to tasks/CafeDailyOperations/VLN_greet_and_order_1118.py diff --git a/robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1119.py b/tasks/CafeDailyOperations/VLN_greet_and_order_1119.py similarity index 100% rename from robowaiter/scene/tasks/CafeDailyOperations/VLN_greet_and_order_1119.py rename to tasks/CafeDailyOperations/VLN_greet_and_order_1119.py diff --git a/robowaiter/scene/tasks/GQA/__init__.py b/tasks/CafeDailyOperations/__init__.py similarity index 100% rename from robowaiter/scene/tasks/GQA/__init__.py rename to tasks/CafeDailyOperations/__init__.py diff --git a/robowaiter/scene/tasks/GQA/GQA.py b/tasks/GQA/GQA.py similarity index 100% rename from robowaiter/scene/tasks/GQA/GQA.py rename to tasks/GQA/GQA.py diff --git a/robowaiter/scene/tasks/GQA/GQA_1_ask_order_place.py b/tasks/GQA/GQA_1_ask_order_place.py similarity index 100% rename from robowaiter/scene/tasks/GQA/GQA_1_ask_order_place.py rename to tasks/GQA/GQA_1_ask_order_place.py diff --git a/robowaiter/scene/tasks/GQA/GQA_NLP.py b/tasks/GQA/GQA_NLP.py similarity index 100% rename from robowaiter/scene/tasks/GQA/GQA_NLP.py rename to tasks/GQA/GQA_NLP.py diff --git a/robowaiter/scene/tasks/OT/__init__.py b/tasks/GQA/__init__.py similarity index 100% rename from robowaiter/scene/tasks/OT/__init__.py rename to tasks/GQA/__init__.py diff --git a/robowaiter/scene/tasks/VLM/__init__.py b/tasks/Interact/__init__.py similarity index 100% rename from robowaiter/scene/tasks/VLM/__init__.py rename to tasks/Interact/__init__.py diff --git a/robowaiter/scene/interact/system.py b/tasks/Interact/system.py similarity index 54% rename from robowaiter/scene/interact/system.py rename to tasks/Interact/system.py index 6cbd372..fc5517b 100644 --- a/robowaiter/scene/interact/system.py +++ b/tasks/Interact/system.py @@ -1,10 +1,5 @@ """ -人提出请求，机器人完成任务 -1. 做咖啡（固定动画）：接收到做咖啡指令、走到咖啡机、拿杯子、操作咖啡机、取杯子、送到客人桌子上 -2. 倒水 -3. 夹点心 - -具体描述：设计一套点单规则（如菜单包含咖啡、水、点心等），按照规则拟造随机的订单。在收到订单后，通过大模型让机器人输出合理的备餐计划，并尝试在模拟环境中按照这个规划实现任务。 +交互式场景，输入 """ @@ -27,9 +22,9 @@ class SubScene(Scene): question = input("请输入指令：") if question[-1] == ")": print(f"设置目标:{question}") - self.set_goal(question)() + self.new_set_goal(question) else: - self.state['chat_list'].append(question) + self.customer_say("System",question) diff --git a/robowaiter/scene/tasks/OT/OT_coffee_water_dessert.py b/tasks/OT/OT_coffee_water_dessert.py similarity index 100% rename from robowaiter/scene/tasks/OT/OT_coffee_water_dessert.py rename to tasks/OT/OT_coffee_water_dessert.py diff --git a/robowaiter/scene/tasks/OT/Open_tasks.py b/tasks/OT/Open_tasks.py similarity index 100% rename from robowaiter/scene/tasks/OT/Open_tasks.py rename to tasks/OT/Open_tasks.py diff --git a/robowaiter/scene/tasks/OT/Open_tasks_bright_table.py b/tasks/OT/Open_tasks_bright_table.py similarity index 100% rename from robowaiter/scene/tasks/OT/Open_tasks_bright_table.py rename to tasks/OT/Open_tasks_bright_table.py diff --git a/robowaiter/scene/tasks/OT/Open_tasks_test.py b/tasks/OT/Open_tasks_test.py similarity index 100% rename from robowaiter/scene/tasks/OT/Open_tasks_test.py rename to tasks/OT/Open_tasks_test.py diff --git a/robowaiter/scene/tasks/OT/Open_tasks_walkers.py b/tasks/OT/Open_tasks_walkers.py similarity index 100% rename from robowaiter/scene/tasks/OT/Open_tasks_walkers.py rename to tasks/OT/Open_tasks_walkers.py diff --git a/robowaiter/scene/tasks/__init__.py b/tasks/OT/__init__.py similarity index 100% rename from robowaiter/scene/tasks/__init__.py rename to tasks/OT/__init__.py diff --git a/robowaiter/scene/tasks/VLM/VLM.py b/tasks/VLM/VLM.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM.py rename to tasks/VLM/VLM.py diff --git a/robowaiter/scene/tasks/VLM/VLM3.py b/tasks/VLM/VLM3.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM3.py rename to tasks/VLM/VLM3.py diff --git a/robowaiter/scene/tasks/VLM/VLM_1_order.py b/tasks/VLM/VLM_1_order.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_1_order.py rename to tasks/VLM/VLM_1_order.py diff --git a/robowaiter/scene/tasks/VLM/VLM_2_AC.py b/tasks/VLM/VLM_2_AC.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_2_AC.py rename to tasks/VLM/VLM_2_AC.py diff --git a/robowaiter/scene/tasks/VLM/VLM_AC.py b/tasks/VLM/VLM_AC.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_AC.py rename to tasks/VLM/VLM_AC.py diff --git a/robowaiter/scene/tasks/VLM/VLM_cafe_shutdown.py b/tasks/VLM/VLM_cafe_shutdown.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_cafe_shutdown.py rename to tasks/VLM/VLM_cafe_shutdown.py diff --git a/robowaiter/scene/tasks/VLM/VLM_order.py b/tasks/VLM/VLM_order.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLM_order.py rename to tasks/VLM/VLM_order.py diff --git a/robowaiter/scene/tasks/VLM/VLN_greet_lead.py b/tasks/VLM/VLN_greet_lead.py similarity index 100% rename from robowaiter/scene/tasks/VLM/VLN_greet_lead.py rename to tasks/VLM/VLN_greet_lead.py diff --git a/zoo/__init__.py b/tasks/VLM/__init__.py similarity index 100% rename from zoo/__init__.py rename to tasks/VLM/__init__.py diff --git a/robowaiter/scene/tasks/VLN/VLN_edge_map.py b/tasks/VLN/VLN_edge_map.py similarity index 99% rename from robowaiter/scene/tasks/VLN/VLN_edge_map.py rename to tasks/VLN/VLN_edge_map.py index f2897f0..0a88c3a 100644 --- a/robowaiter/scene/tasks/VLN/VLN_edge_map.py +++ b/tasks/VLN/VLN_edge_map.py @@ -3,8 +3,6 @@ 测试导航算法的动态避障(Obstacle Detection and Avoidance, ODA)能力 - - """ import os import pickle diff --git a/robowaiter/scene/tasks/VLN/VLN_edge_obj.py b/tasks/VLN/VLN_edge_obj.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_edge_obj.py rename to tasks/VLN/VLN_edge_obj.py diff --git a/robowaiter/scene/tasks/VLN/VLN_greet_lead.py b/tasks/VLN/VLN_greet_lead.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_greet_lead.py rename to tasks/VLN/VLN_greet_lead.py diff --git a/robowaiter/scene/tasks/VLN/VLN_oda_eazy.py b/tasks/VLN/VLN_oda_eazy.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_oda_eazy.py rename to tasks/VLN/VLN_oda_eazy.py diff --git a/robowaiter/scene/tasks/VLN/VLN_oda_hard.py b/tasks/VLN/VLN_oda_hard.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_oda_hard.py rename to tasks/VLN/VLN_oda_hard.py diff --git a/robowaiter/scene/tasks/VLN/VLN_oda_normal.py b/tasks/VLN/VLN_oda_normal.py similarity index 100% rename from robowaiter/scene/tasks/VLN/VLN_oda_normal.py rename to tasks/VLN/VLN_oda_normal.py diff --git a/zoo/behavior_lib/__init__.py b/tasks/VLN/__init__.py similarity index 100% rename from zoo/behavior_lib/__init__.py rename to tasks/VLN/__init__.py diff --git a/zoo/behavior_tree/__init__.py b/tasks/__init__.py similarity index 100% rename from zoo/behavior_tree/__init__.py rename to tasks/__init__.py diff --git a/zoo/behavior_lib/Behavior.py b/zoo/behavior_lib/Behavior.py deleted file mode 100644 index 850459a..0000000 --- a/zoo/behavior_lib/Behavior.py +++ /dev/null @@ -1,27 +0,0 @@ -import py_trees as ptree -from typing import Any - -# _base Behavior -class Bahavior(ptree.behaviour.Behaviour): - scene = None - def __init__(self, name: str, scene): - super().__init__(name) - self.scene = scene - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def _update(self) -> ptree.common.Status: - print("this is just a _base behavior node.") - - - def update(self) -> ptree.common.Status: - re = self._update() - print(f"{self.__class__.__name__}: {re.value}") - return re - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) diff --git a/zoo/behavior_lib/Chatting.py b/zoo/behavior_lib/Chatting.py deleted file mode 100644 index 563c23f..0000000 --- a/zoo/behavior_lib/Chatting.py +++ /dev/null @@ -1,23 +0,0 @@ -import py_trees as ptree -from typing import Any -from robowaiter.behavior_lib._base.Behavior import Bahavior - -class Chatting(Bahavior): - def __init__(self, name: str, scene): - super().__init__(name, scene) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def _update(self) -> ptree.common.Status: - # if self.scene.status? - if self.scene.state['chat_list'] == []: - return ptree.common.Status.FAILURE - else: - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) diff --git a/zoo/behavior_lib/CoffeeCupFound.py b/zoo/behavior_lib/CoffeeCupFound.py deleted file mode 100644 index e7130ec..0000000 --- a/zoo/behavior_lib/CoffeeCupFound.py +++ /dev/null @@ -1,20 +0,0 @@ -import py_trees as ptree -from typing import Any -from robowaiter.behavior_lib._base.Behavior import Bahavior - -class CoffeeCupFound(Bahavior): - def __init__(self, name: str, scene): - super().__init__(name, scene) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print("Start checking IsChatting...") - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) diff --git a/zoo/behavior_lib/CoffeeCupGrasped.py b/zoo/behavior_lib/CoffeeCupGrasped.py deleted file mode 100644 index 32c010a..0000000 --- a/zoo/behavior_lib/CoffeeCupGrasped.py +++ /dev/null @@ -1,22 +0,0 @@ -import py_trees as ptree -from typing import Any -from robowaiter.behavior_lib._base.Behavior import Bahavior - -class CoffeeCupGrasped(Bahavior): - def __init__(self, name: str, scene): - super().__init__(name, scene) - - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/CoffeeCupPlaced.py b/zoo/behavior_lib/CoffeeCupPlaced.py deleted file mode 100644 index d4cdb35..0000000 --- a/zoo/behavior_lib/CoffeeCupPlaced.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class CoffeeCupPlaced(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/DealChat.py b/zoo/behavior_lib/DealChat.py deleted file mode 100644 index c1fee07..0000000 --- a/zoo/behavior_lib/DealChat.py +++ /dev/null @@ -1,26 +0,0 @@ -import py_trees as ptree -from typing import Any -from robowaiter.behavior_lib._base.Behavior import Bahavior -from robowaiter.llm_client.ask_llm import ask_llm - -class DealChat(Bahavior): - def __init__(self, name: str, scene): - super().__init__(name, scene) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def _update(self) -> ptree.common.Status: - # if self.scene.status? - chat = self.scene.state['chat_list'].pop() - answer = ask_llm(chat) - print(f"机器人回答：{answer}") - self.scene.chat_bubble(f"机器人回答：{answer}") - - return ptree.common.Status.RUNNING - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) diff --git a/zoo/behavior_lib/DestinationAReached.py b/zoo/behavior_lib/DestinationAReached.py deleted file mode 100644 index 9092023..0000000 --- a/zoo/behavior_lib/DestinationAReached.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class DestinationAReached(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/FindCoffeeCup.py b/zoo/behavior_lib/FindCoffeeCup.py deleted file mode 100644 index 6781c50..0000000 --- a/zoo/behavior_lib/FindCoffeeCup.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class FindCoffeeCup(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/Grasp.py b/zoo/behavior_lib/Grasp.py deleted file mode 100644 index 57c5708..0000000 --- a/zoo/behavior_lib/Grasp.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class Grasp(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def _update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/GraspCoffeeCup.py b/zoo/behavior_lib/GraspCoffeeCup.py deleted file mode 100644 index 285804e..0000000 --- a/zoo/behavior_lib/GraspCoffeeCup.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class GraspCoffeeCup(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/Istask.py b/zoo/behavior_lib/Istask.py deleted file mode 100644 index 4a1922c..0000000 --- a/zoo/behavior_lib/Istask.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class Istask(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/MoveTo.py b/zoo/behavior_lib/MoveTo.py deleted file mode 100644 index 516e9dc..0000000 --- a/zoo/behavior_lib/MoveTo.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class MoveTo(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene, a, b, c, d): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def _update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/PlaceCoffeeCup.py b/zoo/behavior_lib/PlaceCoffeeCup.py deleted file mode 100644 index 7cf0bbd..0000000 --- a/zoo/behavior_lib/PlaceCoffeeCup.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class PlaceCoffeeCup(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/ReachDestinationA.py b/zoo/behavior_lib/ReachDestinationA.py deleted file mode 100644 index 18fbe03..0000000 --- a/zoo/behavior_lib/ReachDestinationA.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class ReachDestinationA(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/SeqTest.py b/zoo/behavior_lib/SeqTest.py deleted file mode 100644 index e07ab74..0000000 --- a/zoo/behavior_lib/SeqTest.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class SeqTest(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_lib/TestTask.py b/zoo/behavior_lib/TestTask.py deleted file mode 100644 index c9c53d7..0000000 --- a/zoo/behavior_lib/TestTask.py +++ /dev/null @@ -1,21 +0,0 @@ -import py_trees as ptree -from typing import Any - -class TestTask(ptree.behaviour.Behaviour): - - def __init__(self, name: str, scene): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> ptree.common.Status: - print('Start checking IsChatting...') - return ptree.common.Status.SUCCESS - - def terminate(self, new_status: ptree.common.Status) -> None: - return super().terminate(new_status) - \ No newline at end of file diff --git a/zoo/behavior_tree/behavior_library.py b/zoo/behavior_tree/behavior_library.py deleted file mode 100644 index 113cc49..0000000 --- a/zoo/behavior_tree/behavior_library.py +++ /dev/null @@ -1,124 +0,0 @@ -""" - 顶层行为树中的动作与条件节点 -""" - -from typing import * -import py_trees -from py_trees import common -from py_trees.common import Status - - -############################################################## -# 条件节点 -############################################################## - -class IsChatting(py_trees.behaviour.Behaviour): - def __init__(self, name: str = ""): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> Status: - print('Start checking IsChatting...') - return common.Status.SUCCESS - - def terminate(self, new_status: Status) -> None: - return super().terminate(new_status) - - -class IsTakingAction(py_trees.behaviour.Behaviour): - def __init__(self, name: str = ""): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> Status: - print('Start checking IsTakingAction...') - return common.Status.SUCCESS - - def terminate(self, new_status: Status) -> None: - return super().terminate(new_status) - - -class IsSomethingMore(py_trees.behaviour.Behaviour): - def __init__(self, name: str = ""): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> Status: - print('Start checking IsSomethingMore...') - return common.Status.SUCCESS - - def terminate(self, new_status: Status) -> None: - return super().terminate(new_status) - - -############################################################## -# 动作节点 -############################################################## - -class Chatting(py_trees.behaviour.Behaviour): - def __init__(self, name: str = ""): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> Status: - print('Start executing Chatting...') - return common.Status.SUCCESS - - def terminate(self, new_status: Status) -> None: - return super().terminate(new_status) - - -class TakingAction(py_trees.behaviour.Behaviour): - def __init__(self, name: str = ""): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> Status: - print('Start executing TakingAction...') - return common.Status.SUCCESS - - def terminate(self, new_status: Status) -> None: - return super().terminate(new_status) - - -class TakingMoreAction(py_trees.behaviour.Behaviour): - def __init__(self, name: str = ""): - super().__init__(name) - - def setup(self, **kwargs: Any) -> None: - return super().setup(**kwargs) - - def initialise(self) -> None: - return super().initialise() - - def update(self) -> Status: - print('Start executing TakingMoreAction...') - return common.Status.SUCCESS - - def terminate(self, new_status: Status) -> None: - return super().terminate(new_status) \ No newline at end of file diff --git a/zoo/behavior_tree/main.py b/zoo/behavior_tree/main.py deleted file mode 100644 index 78cdb99..0000000 --- a/zoo/behavior_tree/main.py +++ /dev/null @@ -1,66 +0,0 @@ -import py_trees -from behavior_library import * - - -def LoadMainTree() -> py_trees.trees.BehaviourTree: - """ - 此方法用于加载固定的顶层行为树（不包括实际执行） - - Args: None - """ - - seq_subtree_0 = py_trees.composites.Sequence( - name='seq_subtree_0', - memory=False, - children=[IsChatting(), Chatting()] - ) - - seq_subtree_1 = py_trees.composites.Sequence( - name='seq_subtree_1', - memory=False, - children=[IsTakingAction(), TakingAction()] - ) - - seq_subtree_2 = py_trees.composites.Sequence( - name='seq_subtree_2', - memory=False, - children=[IsSomethingMore(), TakingMoreAction()] - ) - - root = py_trees.composites.Selector( - name='selector_root', - memory=False, - children=[seq_subtree_0, seq_subtree_1, seq_subtree_2] - ) - - return py_trees.trees.BehaviourTree(root) - - -def LoadSubTree(path: str) -> py_trees.behaviour.Behaviour: - """ - 此方法用于从ptml文件中加载行为树（不包括实际执行） - - Args: - -- path: ptml文件的路径 - """ - # TODO - pass - - -if __name__ == '__main__': - btree = LoadMainTree() - - - def print_tree(tree): - print(py_trees.display.unicode_tree(root=tree.root, show_status=True)) - - - try: - btree.tick_tock( - period_ms=500, - number_of_iterations=py_trees.trees.CONTINUOUS_TICK_TOCK, - pre_tick_handler=None, - post_tick_handler=print_tree - ) - except KeyboardInterrupt: - btree.interrupt() diff --git a/zoo/demo/__init__.py b/zoo/demo/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/zoo/demo/关节控制.py b/zoo/demo/关节控制.py deleted file mode 100644 index fbbff16..0000000 --- a/zoo/demo/关节控制.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -import sys -import time -import grpc - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from proto import GrabSim_pb2 -from proto import GrabSim_pb2_grpc - -channel = grpc.insecure_channel('localhost:30001',options=[ - ('grpc.max_send_message_length', 1024*1024*1024), - ('grpc.max_receive_message_length', 1024*1024*1024) - ]) - -sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) - -def map_test(map_id=0, scene_num=1): - initworld = sim_client.Init(GrabSim_pb2.NUL()) - print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())) - initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - -def joint_test(scene_id=0): - print('------------------joint_test----------------------') - action_list = [[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.37, 37.2, -92.4, 4.13, -0.62, 0.4], - [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.62, 34.75, -94.80, 3.22, -0.26, 0.85], - [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 32.63, -32.80, 15.15, -110.70, 6.86, 2.36, 0.40], - [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 28.18, -27.92, 6.75, -115.02, 9.46, 4.28, 1.35], - [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 4.09, -13.15, -11.97, -107.35, 13.08, 8.58, 3.33]] - - for value in action_list: - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value) - scene = sim_client.Do(action) - - for i in range(8, 21): # arm - print( - f"{scene.joints[i].name}:{scene.joints[i].angle} location:{scene.joints[i].location.X},{scene.joints[i].location.Y},{scene.joints[i].location.Z}" - ) - print('') - for i in range(5, 10): # Right hand - print( - f"{scene.fingers[i].name} angle:{scene.fingers[i].angle} location:{scene.fingers[i].location[0].X},{scene.fingers[i].location[0].Y},{scene.fingers[i].location[0].Z}" - ) - print('----------------------------------------') - time.sleep(0.03) - time.sleep(1) - -if __name__ == '__main__': - map_id = 3 # 地图编号: 3: 咖啡厅 - scene_num = 1 # 场景数量 - map_test(map_id, scene_num) # 场景加载测试 - time.sleep(5) - - for i in range(scene_num): - print("------------------", i, "----------------------") - joint_test(i) # 关节控制测试 \ No newline at end of file diff --git a/zoo/demo/动画控制.py b/zoo/demo/动画控制.py deleted file mode 100644 index 457bbbc..0000000 --- a/zoo/demo/动画控制.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -# enconding = utf8 -import sys -import time -import grpc - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from proto import GrabSim_pb2 -from proto import GrabSim_pb2_grpc - -channel = grpc.insecure_channel('localhost:30001', options=[ - ('grpc.max_send_message_length', 1024 * 1024 * 1024), - ('grpc.max_receive_message_length', 1024 * 1024 * 1024) -]) - -sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) - - -def map_test(map_id=0, scene_num=1): - initworld = sim_client.Init(GrabSim_pb2.NUL()) - print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())) - initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - - -def control_robot_action(scene_id=0, type=0, action=0, message="你好"): - scene = sim_client.ControlRobot(GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message)) - if (scene.info == "action success"): - return True - else: - return False - - -if __name__ == '__main__': - map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室 - scene_num = 1 # 场景数量 - map_test(map_id, scene_num) # 场景加载测试 - time.sleep(5) - - # 制作咖啡 - control_robot_action(0, 0, 1, "开始制作咖啡") - result = control_robot_action(0, 1, 1) - if (result): - control_robot_action(0, 1, 2) - control_robot_action(0, 1, 3) - control_robot_action(0, 1, 4) - else: - control_robot_action(0, 0, 1, "制作咖啡失败") - - # 倒水 - control_robot_action(0, 0, 1, "开始倒水") - result = control_robot_action(0, 2, 1) - if (result): - control_robot_action(0, 2, 2) - control_robot_action(0, 2, 3) - control_robot_action(0, 2, 4) - control_robot_action(0, 2, 5) - else: - control_robot_action(0, 0, 1, "倒水失败") - - # 夹点心 - control_robot_action(0, 0, 1, "开始夹点心") - result = control_robot_action(0, 3, 1) - if (result): - control_robot_action(0, 3, 2) - control_robot_action(0, 3, 3) - control_robot_action(0, 3, 4) - control_robot_action(0, 3, 5) - control_robot_action(0, 3, 6) - control_robot_action(0, 3, 7) - else: - control_robot_action(0, 0, 1, "夹点心失败") - - # 拖地 - control_robot_action(0, 0, 1, "开始拖地") - result = control_robot_action(0, 4, 1) - if (result): - control_robot_action(0, 4, 2) - control_robot_action(0, 4, 3) - control_robot_action(0, 4, 4) - else: - control_robot_action(0, 0, 1, "拖地失败") - - # 擦桌子 - control_robot_action(0, 0, 1, "开始擦桌子") - result = control_robot_action(0, 5, 1) - if (result): - control_robot_action(0, 5, 2) - control_robot_action(0, 5, 3) - else: - control_robot_action(0, 0, 1, "擦桌子失败") \ No newline at end of file diff --git a/zoo/demo/场景操作.py b/zoo/demo/场景操作.py deleted file mode 100644 index cc48eb7..0000000 --- a/zoo/demo/场景操作.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -import sys -import time -import grpc - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from proto import GrabSim_pb2 -from proto import GrabSim_pb2_grpc - -channel = grpc.insecure_channel('localhost:30001',options=[ - ('grpc.max_send_message_length', 1024*1024*1024), - ('grpc.max_receive_message_length', 1024*1024*1024) - ]) - -sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) - -def map_test(map_id=0, scene_num=1): - initworld = sim_client.Init(GrabSim_pb2.NUL()) - print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())) - initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - -def reset(scene_id=0): - scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id)) - -def show_env_info(scene_id=0): - scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - print('------------------show_env_info----------------------') - print( - f"location:{[scene.location.X, scene.location.Y]}, rotation:{scene.rotation.Yaw}\n", - f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", f"objects number: {len(scene.objects)}\n" - f"rotation:{scene.rotation}, timestep:{scene.timestep}\n" - f"timestamp:{scene.timestamp}, collision:{scene.collision}, info:{scene.info}") - -if __name__ == '__main__': - map_id = 3 # 地图编号: 3: 咖啡厅 - scene_num = 1 # 场景数量 - map_test(map_id, scene_num) # 场景加载测试 - time.sleep(5) - - for i in range(scene_num): - print("------------------", i, "----------------------") - reset(i) # 场景重置测试 - show_env_info(i) # 场景信息测试 \ No newline at end of file diff --git a/zoo/demo/导航寻路.py b/zoo/demo/导航寻路.py deleted file mode 100644 index 21cf412..0000000 --- a/zoo/demo/导航寻路.py +++ /dev/null @@ -1,139 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -# enconding = utf8 -import sys -import time -import grpc - -sys.path.append('./') -sys.path.append('../') - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from robowaiter.proto import GrabSim_pb2_grpc,GrabSim_pb2 - - -channel = grpc.insecure_channel('localhost:30001', options=[ - ('grpc.max_send_message_length', 1024 * 1024 * 1024), - ('grpc.max_receive_message_length', 1024 * 1024 * 1024) -]) - -sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) - -''' -初始化，卸载已经加载的关卡，清除所有机器人 -''' - - -def Init(): - sim_client.Init(GrabSim_pb2.NUL()) - - -''' -获取当前可加载的地图信息(地图名字、地图尺寸) -''' - - -def AcquireAvailableMaps(): - AvailableMaps = sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()) - print(AvailableMaps) - - -''' -1、根据mapID加载指定地图 -2、如果scene_num>1,则根据地图尺寸偏移后加载多个相同地图 -3、这样就可以在一个关卡中训练多个地图 -''' - - -def SetWorld(map_id=0, scene_num=1): - print('------------------SetWorld----------------------') - world = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - - -''' -返回场景的状态信息 -1、返回机器人的位置和旋转 -2、返回各个关节的名字和旋转 -3、返回场景中标记的物品信息(名字、类型、位置、旋转) -4、返回场景中行人的信息(名字、位置、旋转、速度) -5、返回机器人手指和双臂的碰撞信息 -''' - - -def Observe(scene_id=0): - print('------------------show_env_info----------------------') - scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - print( - f"location:{[scene.location]}, rotation:{scene.rotation}\n", - f"joints number:{len(scene.joints)}, fingers number:{len(scene.fingers)}\n", - f"objects number: {len(scene.objects)}, walkers number: {len(scene.walkers)}\n" - f"timestep:{scene.timestep}, timestamp:{scene.timestamp}\n" - f"collision:{scene.collision}, info:{scene.info}") - - -''' -重置场景 -1、重置桌子的宽度和高度 -2、清除生成的行人和物品 -3、重置关节角度、位置旋转 -4、清除碰撞信息 -5、重置场景中标记的物品 -''' - - -def Reset(scene_id=0): - print('------------------Reset----------------------') - scene = sim_client.Reset(GrabSim_pb2.ResetParams(scene=scene_id)) - print(scene) - - # 如果场景支持调整桌子 - # sim_client.Reset(GrabSim_pb2.ResetParams(scene = scene_id, adjust = True, height = 100.0, width = 100.0))" - - -""" -导航移动 -yaw:机器人朝向; -velocity:速度,>0代表移动,<0代表瞬移,=0代表只查询; -dis:最终达到的位置距离目标点最远距离,如果超过此距离则目标位置不可达 -""" - - -def navigation_move(scene_id=0, map_id=0): - print('------------------navigation_move----------------------') - scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - walk_value = [scene.location.X, scene.location.Y, scene.rotation.Yaw] - print("position:", walk_value) - - if map_id == 11: # coffee - v_list = [[0, 880], [250, 1200], [-55, 750], [70, -200]] - else: - v_list = [[0.0, 0.0]] - - for walk_v in v_list: - walk_v = walk_v + [scene.rotation.Yaw - 90, 600, 100] - print("walk_v", walk_v) - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) - scene = sim_client.Do(action) - print(scene.info) - - -if __name__ == '__main__': - map_id = 11 # 地图编号 - scene_num = 1 # 场景数量 - - print('------------ 初始化加载场景 ------------') - Init() - AcquireAvailableMaps() - SetWorld(map_id, scene_num) - time.sleep(5.0) - - for i in range(scene_num): - print('------------ 场景操作 ------------') - Observe(i) - Reset(i) - - print('------------ 导航移动 ------------') - navigation_move(i, map_id) \ No newline at end of file diff --git a/zoo/demo/文字冒泡.py b/zoo/demo/文字冒泡.py deleted file mode 100644 index 370ffb8..0000000 --- a/zoo/demo/文字冒泡.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -# enconding = utf8 -import sys -import time -import grpc - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from proto import GrabSim_pb2 -from proto import GrabSim_pb2_grpc - -channel = grpc.insecure_channel('localhost:30001', options=[ - ('grpc.max_send_message_length', 1024 * 1024 * 1024), - ('grpc.max_receive_message_length', 1024 * 1024 * 1024) -]) - -sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) - - -def map_test(map_id=0, scene_num=1): - initworld = sim_client.Init(GrabSim_pb2.NUL()) - print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())) - initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - - -def control_robot_action(scene_id=0, type=0, action=0, message="你好"): - scene = sim_client.ControlRobot(GrabSim_pb2.ControlInfo(scene=scene_id, type=type, action=action, content=message)) - if (scene.info == "action success"): - return True - else: - return False - - -if __name__ == '__main__': - map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室 - scene_num = 1 # 场景数量 - map_test(map_id, scene_num) # 场景加载测试 - time.sleep(5) - - # 文字冒泡 - control_robot_action(0, 0, 1, "你好，欢迎光临") \ No newline at end of file diff --git a/zoo/demo/物品操作.py b/zoo/demo/物品操作.py deleted file mode 100644 index f63a4c7..0000000 --- a/zoo/demo/物品操作.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -import sys -import time -import grpc - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from proto import GrabSim_pb2 -from proto import GrabSim_pb2_grpc - -channel = grpc.insecure_channel('localhost:30001',options=[ - ('grpc.max_send_message_length', 1024*1024*1024), - ('grpc.max_receive_message_length', 1024*1024*1024) - ]) - -sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) - -def map_test(map_id=0, scene_num=1): - initworld = sim_client.Init(GrabSim_pb2.NUL()) - print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())) - initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - -def joint_test(scene_id=0): - print('------------------joint_test----------------------') - action_list = [[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.37, 37.2, -92.4, 4.13, -0.62, 0.4], - [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.62, 34.75, -94.80, 3.22, -0.26, 0.85], - [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 32.63, -32.80, 15.15, -110.70, 6.86, 2.36, 0.40], - [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 28.18, -27.92, 6.75, -115.02, 9.46, 4.28, 1.35], - [0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 4.09, -13.15, -11.97, -107.35, 13.08, 8.58, 3.33]] - - for value in action_list: - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value) - scene = sim_client.Do(action) - - for i in range(8, 21): # arm - print( - f"{scene.joints[i].name}:{scene.joints[i].angle} location:{scene.joints[i].location.X},{scene.joints[i].location.Y},{scene.joints[i].location.Z}" - ) - print('') - for i in range(5, 10): # Right hand - print( - f"{scene.fingers[i].name} angle:{scene.fingers[i].angle} location:{scene.fingers[i].location[0].X},{scene.fingers[i].location[0].Y},{scene.fingers[i].location[0].Z}" - ) - print('----------------------------------------') - time.sleep(0.03) - time.sleep(1) - -def gen_obj(scene_id, h=80): - print('------------------gen objs----------------------') - scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z] - - obj_list = [ - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 90, y=ginger_loc[1] + 30, yaw=10, z=h, type=4), - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 80, y=ginger_loc[1] + 31, z=h, type=5), - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 33, y=ginger_loc[1] - 10.5, z=h+20, type=7), - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 70, y=ginger_loc[1] + 33, z=h, type=9), - GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 60, y=ginger_loc[1] + 34, z=h, type=13) - ] - scene = sim_client.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=scene_id)) - print(scene.collision) - time.sleep(5) - -def remove_obj(scene_id=0, id_list=[1]): - print('------------------remove objs----------------------') - remove_obj_list = id_list - scene = sim_client.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_obj_list, scene=scene_id)) - print(f"remove objects {id_list}. current obj:") - time.sleep(1) - -def clean_obj(scene_id=0): - print('------------------clean objs----------------------') - scene = sim_client.CleanObjects(GrabSim_pb2.SceneID(value=scene_id)) - -def obj_test(scene_id=0): - gen_obj(scene_id) - # remove_obj(scene_id, id_list=[0]) - # clean_obj(scene_id) - -def grasp_test(hand_id, obj_scene_id, scene_id=0): - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Grasp, values=[hand_id, obj_scene_id]) - scene = sim_client.Do(action) - -def release_test(hand_id, scene_id=0): - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Release, values=[hand_id]) - scene = sim_client.Do(action) - -if __name__ == '__main__': - map_id = 3 # 地图编号: 3: 咖啡厅 - scene_num = 1 # 场景数量 - map_test(map_id, scene_num) # 场景加载测试 - time.sleep(5) - - for i in range(scene_num): - print("------------------", i, "----------------------") - joint_test(i) # 关节控制测试 - obj_test(i) # 物品生成测试 - grasp_test(1, 2) # 抓取物品测试 - joint_test(i) # 关节控制测试 - release_test(1) # 释放物品测试 \ No newline at end of file diff --git a/zoo/demo/相机操作.py b/zoo/demo/相机操作.py deleted file mode 100644 index 42e400c..0000000 --- a/zoo/demo/相机操作.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -import sys -import time -import grpc - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from proto import GrabSim_pb2 -from proto import GrabSim_pb2_grpc - -channel = grpc.insecure_channel('localhost:30001',options=[ - ('grpc.max_send_message_length', 1024*1024*1024), - ('grpc.max_receive_message_length', 1024*1024*1024) - ]) - -sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) - -def map_test(map_id=0, scene_num=1): - initworld = sim_client.Init(GrabSim_pb2.NUL()) - print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())) - initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - -def get_camera(part, scene_id=0): - action = GrabSim_pb2.CameraList(cameras=part, scene=scene_id) - return sim_client.Capture(action) - -def show_image(img_data): - im = img_data.images[0] - d = np.frombuffer(im.data, dtype=im.dtype).reshape((im.height, im.width, im.channels)) - plt.imshow(d, cmap="gray" if "depth" in im.name.lower() else None) - plt.show() - -def camera_test(scene_id=0): - for camera_name in [GrabSim_pb2.CameraName.Head_Color, GrabSim_pb2.CameraName.Head_Depth, GrabSim_pb2.CameraName.Head_Segment]: - img_data = get_camera([camera_name], scene_id) - show_image(img_data) - -if __name__ == '__main__': - map_id = 3 # 地图编号: 0：空房间 1：室内 2:咖啡厅1.0 3: 咖啡厅2.0 4:餐厅 5:养老院 6：会议室 - scene_num = 1 # 场景数量 - map_test(map_id, scene_num) # 场景加载测试 - time.sleep(5) - - for i in range(scene_num): - print("------------------", i, "----------------------") - camera_test(i) # 相机操作测试 \ No newline at end of file diff --git a/zoo/demo/行人控制.py b/zoo/demo/行人控制.py deleted file mode 100644 index 32efcb2..0000000 --- a/zoo/demo/行人控制.py +++ /dev/null @@ -1,86 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -import sys -import time -import grpc - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - -from proto import GrabSim_pb2 -from proto import GrabSim_pb2_grpc - -channel = grpc.insecure_channel('localhost:30001',options=[ - ('grpc.max_send_message_length', 1024*1024*1024), - ('grpc.max_receive_message_length', 1024*1024*1024) - ]) - -sim_client = GrabSim_pb2_grpc.GrabSimStub(channel) - -def map_test(map_id=0, scene_num=1): - initworld = sim_client.Init(GrabSim_pb2.NUL()) - print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL())) - initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id)) - -def add_walker(scene_id=0): - """pose:表示行人的初始位置姿态""" - print('------------------add walker----------------------') - s = sim_client.Observe(GrabSim_pb2.SceneID(value=0)) - - walker_loc = [[0, 880], [250, 1200], [-55, 750], [70, -200]] - walker_list = [] - for i in range(len(walker_loc)): - loc = walker_loc[i] + [0, 600, 100] - action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.WalkTo, values=loc) - scene = sim_client.Do(action) - print(scene.info) - # 只有可达的位置才能成功初始化行人，显示unreachable的地方不能初始化行人 - walker_list.append(GrabSim_pb2.WalkerList.Walker(id=i, pose=GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=90))) - - scene = sim_client.AddWalker(GrabSim_pb2.WalkerList(walkers=walker_list, scene=scene_id)) - return scene - -def control_walkers(scene_id=0): - """pose:表示行人的终止位置姿态""" - s = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - - walker_loc = [[-55, 750], [70, -200], [250, 1200], [0, 880]] - controls = [] - for i in range(len(s.walkers)): - loc = walker_loc[i] - is_autowalk = False - pose = GrabSim_pb2.Pose(X=loc[0], Y=loc[1], Yaw=180) - controls.append(GrabSim_pb2.WalkerControls.WControl(id=i, autowalk=is_autowalk, speed=200, pose=pose)) - scene = sim_client.ControlWalkers(GrabSim_pb2.WalkerControls(controls=controls, scene=scene_id)) - time.sleep(10) - return scene - -def remove_walkers(scene_id=0): - s = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)) - scene = sim_client.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=[1, 3], scene=scene_id)) - time.sleep(2) - return - -def clean_walkers(scene_id=0): - scene = sim_client.CleanWalkers(GrabSim_pb2.SceneID(value=scene_id)) - return scene - -def walker_test(scene_id=0): - add_walker(scene_id) - control_walkers(scene_id) - print(sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) - remove_walkers(scene_id) - print(sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) - clean_walkers(scene_id) - return - -if __name__ == '__main__': - map_id = 3 # 地图编号: 3: 咖啡厅 - scene_num = 1 # 场景数量 - map_test(map_id, scene_num) # 场景加载测试 - time.sleep(5) - - for i in range(scene_num): - print("------------------", i, "----------------------") - walker_test(i) # 行人控制测试 \ No newline at end of file diff --git a/zoo/install_pytorch.sh b/zoo/install_pytorch.sh deleted file mode 100644 index a2b5b5c..0000000 --- a/zoo/install_pytorch.sh +++ /dev/null @@ -1 +0,0 @@ -conda install pytorch==1.11.0 torchvision==0.12.0 torchaudio==0.11.0 cudatoolkit=11.3 -c pytorch \ No newline at end of file diff --git a/zoo/navigator_DstarLite/__init__.py b/zoo/navigator_DstarLite/__init__.py deleted file mode 100644 index 1ceb32a..0000000 --- a/zoo/navigator_DstarLite/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ - -from . import navigate -from . import dstar_lite - diff --git a/zoo/navigator_DstarLite/dstar_lite.py b/zoo/navigator_DstarLite/dstar_lite.py deleted file mode 100644 index 6a83108..0000000 --- a/zoo/navigator_DstarLite/dstar_lite.py +++ /dev/null @@ -1,510 +0,0 @@ -''' -基于两个D* lite的实现 -按照原始算法的伪代码 -自己写的D* lite -''' - -import math -import queue -from functools import partial -import numpy as np -import heapq - -from matplotlib import pyplot as plt - - -def diagonal_distance(start, end): # - return max(abs(start[0] - end[0]), abs(start[1] - end[1])) - - -def manhattan_distance(start, end): # 曼哈顿距离 - return abs(start[0] - end[0]) + abs(start[1] - end[1]) - - -def euclidean_distance(start, end): # 欧式距离 - # return np.linalg.norm(start-end) - return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2) - - -def heuristic(start, end, name='euclidean'): - if name == 'diagonal': - return diagonal_distance(start, end) - elif name == 'euclidean': - return euclidean_distance(start, end) - elif name == 'manhattan': - return manhattan_distance(start, end) - else: - raise Exception('Error heuristic name!') - - -class Priority: - ''' - 优先级类 - ''' - - def __init__(self, k1, k2): - self.k1 = k1 - self.k2 = k2 - - def __lt__(self, other): # 定义对象的 < 运算 - return self.k1 < other.k1 or (self.k1 == other.k1 and self.k2 < other.k2) - - def __le__(self, other): # 定于对象的 <= 运算 - return self.k1 < other.k1 or (self.k1 == other.k1 and self.k2 <= other.k2) - - -class Node: - ''' - 节点类 - ''' - - def __init__(self, pos: (int, int), priority: Priority): - self.pos = pos # X Y - self.priority = priority - - def __lt__(self, other): # 定义对象的 < 运算 - return self.priority < other.priority - - def __le__(self, other): # 定于对象的 <= 运算 - return self.priority <= other.priority - - -class PriorityQueue: - def __init__(self): - self.queue = [] # 节点队列 - self.nodes = [] # 节点位置列表 - - def is_empty(self): - # 队列判空 - return len(self.queue) == 0 - - def top(self): - return self.queue[0].pos - - def top_key(self): - if self.is_empty(): - return Priority(float('inf'), float('inf')) - return self.queue[0].priority - - def pop(self): - # 取出第一个节点 - node = heapq.heappop(self.queue) - self.nodes.remove(node.pos) - return node - - def insert(self, pos, priority): - # 创建并添加节点 - node = Node(pos, priority) - heapq.heappush(self.queue, node) - self.nodes.append(pos) - - def remove(self, pos): - # 删除指定节点并重新排序 - self.queue = [n for n in self.queue if n.pos != pos] - heapq.heapify(self.queue) # 重排序 - self.nodes.remove(pos) - - def update(self, pos, priority): - # 更新指定位置的priority值 - for n in self.queue: - if n.pos == pos: - n.priority = priority - break - - -class DStarLite: - def __init__(self, - map: np.array([int, int]), # [X, Y] - area_range, # [x_min, x_max, y_min, y_max] 实际坐标范围 - scale_ratio=5, # 地图缩放率 - dyna_obs_radius=20 # dyna_obs实际身位半径 - ): - - # self.area_bounds = area - self.map = map - self.background = map - self.X = map.shape[0] - self.Y = map.shape[1] - (self.x_min, self.x_max, self.y_min, self.y_max) = area_range - self.scale_ratio = scale_ratio - self.dyna_obs_list = [] # 动态障碍物位置列表( 当前地图 ) [(x, y)] - self.dyna_obs_radius = math.ceil(dyna_obs_radius/scale_ratio) # dyna_obs缩放后身位半径 - - # free:0, obs:1, dyna_obs:2 - self.idx_to_object = { - 0: "free", - 1: "obstacle", - 2: "dynamic obstacle" - } - self.object_to_idx = dict(zip(self.idx_to_object.values(), self.idx_to_object.keys())) - self.object_to_cost = { - "free": 0, - "obstacle": float('inf'), - "dynamic obstacle": 50 - } - - self.compute_cost_map() - - self.s_start = None # (int,int) 必须是元组(元组可以直接当作矩阵索引) - self.s_goal = None # (int,int) - self.s_last = None # (int,int) - self.U = PriorityQueue() - self.k_m = 0 - self.rhs = np.ones((self.X, self.Y)) * np.inf - self.g = self.rhs.copy() - self.path = [] - - def set_map(self, map_): - ''' - 设置map 和 cost_map - ''' - self.map = map_ - self.X = map_.shape[0] - self.Y = map_.shape[1] - self.compute_cost_map() - - def reset(self): - ''' - (完成一次导航后) - 重置 1.环境变量 - 2.dstar_lite变量 - ''' - # env reset - self.map = self.background.copy() - self.compute_cost_map() - self.dyna_obs_list.clear() - self.path.clear() - # dstar_lite reset - self.s_start = None - self.s_goal = None - self.s_last = None - self.U = PriorityQueue() - self.k_m = 0 - self.rhs = np.ones((self.X, self.Y)) * np.inf - self.g = self.rhs.copy() - - def calculate_key(self, s: (int, int)): - ''' - 计算 位置s 的 key1, key2 - :returns： - Priority(k1, k2): 可比较的对象 - ''' - k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start, s) + self.k_m - k2 = min(self.g[s], self.rhs[s]) - return Priority(k1, k2) - - def c(self, u: (int, int), v: (int, int), v_old=None) -> float: - ''' - 计算节点间的 路径代价 和 目标位置代价 (目标位置代价为0时采用路径代价) - (因为是终点->起点的扩展方向，因此v是node，u是v扩展的neighbor) - Args: - u: from pos - v: to pos - v_old: 指定的v的类型 - ''' - if v_old: - obj_cost = self.object_to_cost[v_old] - else: - obj_cost = self.cost_map[v] - if obj_cost > 0: - return obj_cost - return heuristic(u, v) - - def contain(self, u: (int, int)): - ''' - 判断 节点u 是否在队列中 - ''' - return u in self.U.nodes - - def update_vertex(self, u: (int, int)): - ''' - 判定节点状态, 更新队列 - 不一致且在队列 --> 更新key - 不一致且不在队列 --> 计算key并加入队列 - 一致且在队列 --> 移出队列 - ''' - if self.g[u] != self.rhs[u] and self.contain(u): - self.U.update(u, self.calculate_key(u)) - elif self.g[u] != self.rhs[u] and not self.contain(u): - self.U.insert(u, self.calculate_key(u)) - elif self.g[u] == self.rhs[u] and self.contain(u): - self.U.remove(u) - - def compute_shortest_path(self): - ''' - 计算最短路径 - ''' - while self.U.top_key() < self.calculate_key(self.s_start) or self.rhs[self.s_start] > self.g[self.s_start]: - u = self.U.top() - k_old = self.U.top_key() - k_new = self.calculate_key(u) - if k_old < k_new: - self.U.update(u, k_new) - elif self.g[u] > self.rhs[u]: # 过一致 - self.g[u] = self.rhs[u] - self.U.remove(u) - pred = self.get_neighbors(u) - for s in pred: - if s != self.s_goal: - self.rhs[s] = min(self.rhs[s], self.c(s, u) + self.g[u]) - self.update_vertex(s) - else: # 欠一致 - g_old = self.g[u] - self.g[u] = float('inf') - pred = self.get_neighbors(u) - for s in pred + [u]: - if self.rhs[s] == self.c(s, u) + g_old: - if s != self.s_goal: - succ = self.get_neighbors(s) - self.rhs[s] = min([self.c(s, s_) + self.g[s_] for s_ in succ]) - self.update_vertex(s) - - def _planning(self, s_start, s_goal, dyna_obs, step_num=None, debug=False): - ''' - 规划路径(实际实现) - Args: - dyna_obs: 动态障碍物位置列表 - step_num: 单次移动步数 - ''' - # 确保目标合法 - if not self.in_bounds_without_obstacle(s_goal): - return None - # 第一次规划需要初始化rhs并将goal加入队列，计算最短路径 - if self.s_goal is None: - self.s_start = tuple(s_start) - self.s_goal = tuple(s_goal) - self.s_last = tuple(s_start) - self.rhs[tuple(s_goal)] = 0 - self.U.insert(tuple(s_goal), Priority(k1=heuristic(tuple(s_start), tuple(s_goal)), k2=0)) - self.compute_shortest_path() # 计算最短路径 - self.path = self.get_path() - # 后续规划只更新起点，直接使用原路径(去掉已走过部分) - else: - self.s_start = tuple(s_start) - self.path = self.path[step_num:] - # 根据obs更新map, cost_map, edge_cost - changed_pos = self.update_map(dyna_obs=dyna_obs) - if changed_pos: - self.update_cost_map(changed_pos) - self.update_edge_costs(changed_pos) - # 若地图改变，重新计算最短路径 - self.compute_shortest_path() - self.path = self.get_path() - - # TODO: 误差抖动使robot没有到达路径上的点，导致新起点的rhs=∞，可能导致get_path失败 ( 当前版本没有该问题 ) - # assert (self.rhs[self.s_start] != float('inf')), "There is no known path!" - # self.path = self.get_path(step_num) - # # debug - # if debug: - # pass - return self.path - - def planning(self, s_start, s_goal, dyna_obs, step_num=None, debug=False): - ''' - 路径规划(供外部调用，处理实际坐标和地图坐标的转换) - ''' - # 实际坐标 -> 地图坐标 - s_start = self.real2map(s_start) - s_goal = self.real2map(s_goal) - for i in range(len(dyna_obs)): - dyna_obs[i] = self.real2map(dyna_obs[i]) - - self._planning(s_start, s_goal, dyna_obs, step_num, debug) - - # 地图坐标->实际坐标 - path = [self.map2real(node) for node in self.path] - return path - - def get_path(self): - ''' - 得到路径 - Args: - step_num: 路径步数 (None表示返回完整路径) - return: - path: [(x, y), ...] - ''' - if self.s_start is None or self.s_goal == self.s_start: - return [] - path = [] - cur = self.s_start - # if step_num is None: # 得到完整路径 - while cur != self.s_goal: - succ = self.get_neighbors(cur) - cur = succ[np.argmin([self.c(cur, s_) + self.g[s_] for s_ in succ])] - path.append(cur) - # else: - # for i in range(step_num): - # if cur == self.s_goal: - # break - # succ = self.get_neighbors(cur) - # cur = succ[np.argmin([self.c(cur, s_) + self.g[s_] for s_ in succ])] - # path.append(cur) - return path - - def in_bounds_without_obstacle(self, pos): - ''' - 判断位置在地图范围内 且 不是静态障碍物 - ''' - (x, y) = pos - return 0 <= x < self.X and 0 <= y < self.Y and self.map[x, y] != self.object_to_idx["obstacle"] - - def get_neighbors(self, pos, mode=8): - ''' - 获取邻居节点, 地图范围内 - ''' - (x_, y_) = pos - # results = [(x_+1,y_), (x_-1,y_), (x_, y_+1), (x_,y_-1)] - # if mode == 8: - neighbors = [(x_ + 1, y_), (x_ - 1, y_), (x_, y_ + 1), (x_, y_ - 1), (x_ + 1, y_ + 1), (x_ - 1, y_ + 1), - (x_ + 1, y_ - 1), (x_ - 1, y_ - 1)] - neighbors = filter(self.in_bounds_without_obstacle, neighbors) # 确保位置在地图范围内 且 不是静态障碍物 - return list(neighbors) - - def compute_cost_map(self): - # 计算当前地图的cost_map - self.cost_map = np.zeros(self.map.shape) - for idx, obj in self.idx_to_object.items(): - self.cost_map[self.map == idx] = self.object_to_cost[obj] - - def update_map(self, dyna_obs): - ''' - 更新地图 和 动态障碍物列表 - Args: - dyna_obs: 当前动态障碍物位置列表 [(x,y), ...] - return: - update_obj: 改变的位置列表 [(x, y, obj_idx, obj_idx_old), ...] - ''' - # dyna_obs没有变化 (集合set可以忽略元素在列表中的位置) - if set(dyna_obs) == set(self.dyna_obs_list): - return [] - - # 新旧dyna_obs占用位置列表 - old_obs_occupy = [] - new_obs_occupy = [] - for pos in self.dyna_obs_list: - old_obs_occupy.extend(self.get_occupy_pos(pos)) - for pos in dyna_obs: - new_obs_occupy.extend(self.get_occupy_pos(pos)) - old_obs_occupy = [pos for i, pos in enumerate(old_obs_occupy) if pos not in old_obs_occupy[:i]] # 去除重复位置 - new_obs_occupy = [pos for i, pos in enumerate(new_obs_occupy) if pos not in new_obs_occupy[:i]] # 去除重复位置 - - # 转变为free 和 转变为obs的位置列表 - changed_free = [pos for pos in old_obs_occupy if pos not in new_obs_occupy] - changed_obs = [pos for pos in new_obs_occupy if pos not in old_obs_occupy] - - # 更新地图，计算changed_pos - changed_pos = [] - for (x, y) in changed_free: - self.map[x, y] = self.object_to_idx['free'] - changed_pos.append((x, y, self.object_to_idx["free"], self.object_to_idx["dynamic obstacle"])) - for (x, y) in changed_obs: - self.map[x, y] = self.object_to_idx['dynamic obstacle'] - changed_pos.append((x, y, self.object_to_idx["dynamic obstacle"], self.object_to_idx["free"])) - - # 更新动态障碍物列表 - self.dyna_obs_list = dyna_obs - - return changed_pos - - def get_occupy_pos(self, obs_pos): - ''' - 根据dyna_obs中心位置，计算其占用的所有网格位置 - ''' - (x, y) = obs_pos - occupy_pos = [] - for i in range(x-self.dyna_obs_radius, x+self.dyna_obs_radius+1): - for j in range(y-self.dyna_obs_radius, y+self.dyna_obs_radius+1): - occupy_pos.append((i, j)) - occupy_pos = filter(self.in_bounds_without_obstacle, occupy_pos) # 确保位置在地图范围内 且 不是静态障碍物 - return list(occupy_pos) - - def update_cost_map(self, changed_pos): - ''' - 更新cost_map - Args: - changed_pos: 改变的位置列表 [x, y, idx] - ''' - for (x, y, idx, _) in changed_pos: - self.cost_map[x, y] = self.object_to_cost[self.idx_to_object[idx]] - - def update_edge_costs(self, changed_pos): - ''' - 重新计算edge_cost，更新受影响节点的rhs - Args: - changed_pos: 改变的位置列表 [(x, y, obj_idx_new, obj_idx_old)] - ''' - if not changed_pos: return - self.k_m += heuristic(self.s_last, self.s_start, name='euclidean') # 使用欧式距离 更新km - self.s_last = self.s_start - for pos in changed_pos: - v = (pos[0], pos[1]) # to pos - v_old = self.idx_to_object[pos[3]] # 位置v的旧类型 - pred_v = self.get_neighbors(v) - for u in pred_v: - c_old = self.c(u, v, v_old=v_old) - c_new = self.c(u, v) - if c_old > c_new and u != self.s_goal: - self.rhs[u] = min(self.rhs[u], self.c(u, v) + self.g[v]) - elif self.rhs[u] == c_old + self.g[v] and u != self.s_goal: - succ_u = self.get_neighbors(u) - self.rhs[u] = min([self.c(u, s_) + self.g[s_] for s_ in succ_u]) - self.update_vertex(u) - - def map2real(self, pos): - ''' - 地图坐标->实际坐标 - ''' - x = pos[0] * self.scale_ratio + self.x_min - y = pos[1] * self.scale_ratio + self.y_min - return tuple((x, y)) - - def real2map(self, pos, approximation='round'): - ''' - 实际坐标->地图坐标 - ''' - if approximation == 'round': # 四舍五入 - x = round((pos[0] - self.x_min) / self.scale_ratio) - y = round((pos[1] - self.y_min) / self.scale_ratio) - elif approximation == 'low': # 向下取整 - x = math.floor((pos[0] - self.x_min) / self.scale_ratio) - y = math.floor((pos[1] - self.y_min) / self.scale_ratio) - else: - raise Exception("Wrong approximation!") - return tuple((x, y)) - - def draw_graph(self, step_num): - # 清空当前figure内容，保留figure对象 - plt.clf() - # for stopping simulation with the esc key. 按esc结束 - plt.gcf().canvas.mpl_connect( - 'key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - - # 缩放坐标偏移量 - offset = (self.x_min / self.scale_ratio, self.x_max / self.scale_ratio, - self.y_min / self.scale_ratio, self.y_max / self.scale_ratio,) - - # 画地图: X行Y列，第一行在下面 - # 范围: 横向Y[-80,290] 纵向X[-70,120] - plt.imshow(self.map, cmap='binary', alpha=0.5, origin='lower', - extent=(offset[2], offset[3], - offset[0], offset[1])) - # 画起点和目标 - plt.plot(self.s_start[1] + offset[2], self.s_start[0] + offset[0], "xr") - plt.plot(self.s_goal[1] + offset[2], self.s_goal[0] + offset[0], "xr") - - # 画搜索路径 - plt.plot([y + offset[2] for (x, y) in self.path], - [x + offset[0] for (x, y) in self.path], "-g") - - # 画移动路径 - next_step = min(step_num, len(self.path)) - # plt.plot([self.s_start[1] + offset[2], self.path[next_step-1][1] + offset[2]], - # [self.s_start[0] + offset[0], self.path[next_step-1][0] + offset[0]], "-r") - plt.plot([y + offset[2] for (x, y) in self.path[:next_step]], - [x + offset[0] for (x, y) in self.path[:next_step]], "-r") - - plt.xlabel('y', loc='right') - plt.ylabel('x', loc='top') - plt.grid(color='black', linestyle='-', linewidth=0.5) - plt.pause(0.3) diff --git a/zoo/navigator_DstarLite/map_5.pkl b/zoo/navigator_DstarLite/map_5.pkl deleted file mode 100644 index 7b49370..0000000 Binary files a/zoo/navigator_DstarLite/map_5.pkl and /dev/null differ diff --git a/zoo/navigator_DstarLite/navigate.py b/zoo/navigator_DstarLite/navigate.py deleted file mode 100644 index bbb0a16..0000000 --- a/zoo/navigator_DstarLite/navigate.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 -# -*- encoding: utf-8 -*- -import math -import sys -import time - -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable -from scene_utils import Scene - - -from dstar_lite import DStarLite - -class Navigator: - ''' - 导航类 - ''' - - def __init__(self, scene, area_range, map, scale_ratio=5): - self.scene = scene - self.area_range = area_range # 地图实际坐标范围 xmin, xmax, ymin, ymax - self.map = map # 缩放并离散化的地图 array(X,Y) - self.scale_ratio = scale_ratio # 地图缩放率 - self.step_length = 50 # 步长(单次移动) - self.step_num = self.step_length // self.scale_ratio # 单次移动地图格数 - self.v = 200 # 速度 - self.step_time = self.step_length/self.v + 0.1 # 单步移动时长 - - - self.planner = DStarLite(area_range=area_range, map=map, scale_ratio=scale_ratio) - - - @staticmethod - def is_reached(pos: np.array((float, float)), goal: np.array((float, float)), dis_limit=25): - ''' - 判断是否到达目标 - ''' - dis = np.linalg.norm(pos - goal) - return dis < dis_limit - - def reset_goal(self, goal:(float, float)): - # TODO: 使目标可达 - # 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点 - # 目标在地图外面：起点和目标连线最靠近目标的可行点 - pass - - def navigate(self, goal: (float, float), animation=True): - ''' - 单次导航，直到到达目标 - ''' - - pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) # 机器人当前: 位置 和 朝向 - yaw = self.scene.status.rotation.Yaw - print('------------------navigation_start----------------------') - while not self.is_reached(pos, goal): - dyna_obs = [(walker.pose.X, walker.pose.Y) for walker in self.scene.status.walkers] # 动态障碍物(顾客)位置列表 - path = self.planner.planning(pos, goal, dyna_obs, step_num=self.step_num) - if path: - next_step = min(self.step_num, len(path)) - (next_x, next_y) = path[next_step-1] - print('plan pos:', (next_x, next_y), end=' ') - self.scene.walk_to(next_x, next_y, yaw, velocity=self.v) - if animation: - self.planner.draw_graph(self.step_num) # 画出搜索路径 - time.sleep(self.step_time) - pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) - print('reach pos:', pos) - - self.planner.reset() # 完成一轮导航，重置变量 - - if self.is_reached(pos, goal): - print('The robot has achieved goal !!') - - - - - diff --git a/zoo/navigator_DstarLite/readme.md b/zoo/navigator_DstarLite/readme.md deleted file mode 100644 index 3a77674..0000000 --- a/zoo/navigator_DstarLite/readme.md +++ /dev/null @@ -1,4 +0,0 @@ -### dstar_lite.py ——Dstar lite算法文件 -### navigate.py ——导航类 -### test.py ——测试文件 -### map_5.pkl ——离散化地图文件 \ No newline at end of file diff --git a/zoo/navigator_DstarLite/test.py b/zoo/navigator_DstarLite/test.py deleted file mode 100644 index 66ef1e7..0000000 --- a/zoo/navigator_DstarLite/test.py +++ /dev/null @@ -1,72 +0,0 @@ -import os -import pickle -import time -import random - -import matplotlib.pyplot as plt -import numpy as np - -from scene_utils import Scene # TODO: 文件名改成Scene.py -from navigate import Navigator - - - - -if __name__ == '__main__': - - file_name = 'map_5.pkl' - if os.path.exists(file_name): - with open(file_name, 'rb') as file: - map = pickle.load(file) - - Scene.init_world(1, 3) - scene = Scene.Scene(sceneID=0) - - navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map) - - '''场景1: 无行人环境 robot到达指定目标''' - # goal = np.array((-100, 700)) - - - '''场景2: 静止行人环境 robot到达指定目标''' - # scene.clean_walker() - # scene.add_walker(50, 300, 0) - # scene.add_walker(-50, 500, 0) - # goal = np.array((-100, 700)) - - '''场景3: 移动行人环境 robot到达指定目标''' - scene.clean_walker() - scene.add_walker(50, 300, 0) - scene.add_walker(-50, 500, 0) - scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=20, X=-50, Y=600, Yaw=0)]) - scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=20, X=100, Y=150, Yaw=0)]) - goal = np.array((-100, 700)) - - '''场景4: 行人自由移动 robot到达指定目标''' - # # TODO: autowalk=True仿真器会闪退 ??? - # scene.clean_walker() - # scene.add_walker(50, 300, 0) - # scene.add_walker(-50, 500, 0) - # scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=True, speed=20, X=0, Y=0, Yaw=0)]) - # scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=True, speed=20, X=0, Y=0, Yaw=0)]) - # time.sleep(5) - # goal = np.array((-100, 700)) - - navigator.navigate(goal, animation=False) - - scene.clean_walker() - print(scene.status.collision) # TODO: 不显示碰撞信息 ??? - - - - - - - - - - - - - - diff --git a/zoo/opt_bt_expansion/BehaviorTree.py b/zoo/opt_bt_expansion/BehaviorTree.py deleted file mode 100644 index 4513a20..0000000 --- a/zoo/opt_bt_expansion/BehaviorTree.py +++ /dev/null @@ -1,95 +0,0 @@ - -#叶结点 -class Leaf: - def __init__(self,type,content,mincost): - self.type=type - self.content=content #conditionset or action - self.parent=None - self.parent_index=0 - self.mincost=mincost - - # tick 叶节点，返回返回值以及对应的条件或行动对象self.content - def tick(self,state): - if self.type=='cond': - if self.content <= state: - return 'success',self.content - else: - return 'failure',self.content - if self.type=='act': - if self.content.pre<=state: - return 'running',self.content #action - else: - return 'failure',self.content - - def __str__(self): - print( self.content) - return '' - - def print_nodes(self): - print(self.content) - - def count_size(self): - return 1 - - -#可能包含控制结点的行为树 -class ControlBT: - def __init__(self,type): - self.type=type - self.children=[] - self.parent=None - self.parent_index=0 - - - def add_child(self,subtree_list): - for subtree in subtree_list: - self.children.append(subtree) - subtree.parent=self - subtree.parent_index=len(self.children)-1 - - # tick行为树，根据不同控制结点逻辑tick子结点 - def tick(self,state): - if len(self.children) < 1: - print("error,no child") - if self.type =='?':#选择结点，即或结点 - for child in self.children: - val,obj=child.tick(state) - if val=='success': - return val,obj - if val=='running': - return val,obj - return 'failure','?fails' - if self.type =='>':#顺序结点，即与结点 - for child in self.children: - val,obj=child.tick(state) - if val=='failure': - return val,obj - if val=='running': - return val,obj - return 'success', '>success' - if self.type =='act':#行动结点 - return self.children[0].tick(state) - if self.type =='cond':#条件结点 - return self.children[0].tick(state) - - def getFirstChild(self): - return self.children[0] - - def __str__(self): - print(self.type+'\n') - for child in self.children: - print (child) - return '' - - def print_nodes(self): - print(self.type) - for child in self.children: - child.print_nodes() - - # 递归统计树中结点数 - def count_size(self): - result=1 - for child in self.children: - result+= child.count_size() - return result - diff --git a/zoo/opt_bt_expansion/MakeCoffee.ptml b/zoo/opt_bt_expansion/MakeCoffee.ptml deleted file mode 100644 index 40fc6a4..0000000 --- a/zoo/opt_bt_expansion/MakeCoffee.ptml +++ /dev/null @@ -1,59 +0,0 @@ -selector{ -cond At(Table,Coffee) -selector{ -cond At(Robot,Table), Holding(Coffee) -act PutDown(Table,Coffee) -} -selector{ -cond At(Robot,Coffee), NotHolding, At(Robot,Table) -act PickUp(Coffee) -} -selector{ -cond Available(Table), Holding(Coffee) -act MoveTo(Table) -} -selector{ -cond At(Robot,Coffee), At(Robot,Table), Holding(VacuumCup) -act PutDown(Table,VacuumCup) -} -selector{ -cond At(Robot,CoffeeMachine), NotHolding, At(Robot,Table) -act OpCoffeeMachine -} -selector{ -cond At(Robot,Coffee), Available(Table), NotHolding -act PickUp(Coffee) -} -selector{ -cond At(Robot,CoffeeMachine), At(Robot,Table), Holding(VacuumCup) -act PutDown(Table,VacuumCup) -} -selector{ -cond Available(Table), Available(Coffee), NotHolding -act MoveTo(Coffee) -} -selector{ -cond Available(Table), At(Robot,CoffeeMachine), NotHolding -act OpCoffeeMachine -} -selector{ -cond Available(Table), Available(Coffee), At(Robot,Table), Holding(VacuumCup) -act PutDown(Table,VacuumCup) -} -selector{ -cond Available(Table), NotHolding, Available(CoffeeMachine) -act MoveTo(CoffeeMachine) -} -selector{ -cond Available(Table), Available(Coffee), Holding(VacuumCup) -act MoveTo(Table) -} -selector{ -cond Available(Table), Available(CoffeeMachine), At(Robot,Table), Holding(VacuumCup) -act PutDown(Table,VacuumCup) -} -selector{ -cond Available(Table), Available(CoffeeMachine), Holding(VacuumCup) -act MoveTo(Table) -} -} diff --git a/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py b/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py deleted file mode 100644 index b886f8d..0000000 --- a/zoo/opt_bt_expansion/OptimalBTExpansionAlgorithm.py +++ /dev/null @@ -1,210 +0,0 @@ -import copy -import random -from robowaiter.behavior_tree.obtea.BehaviorTree import Leaf,ControlBT - -class CondActPair: - def __init__(self, cond_leaf,act_leaf): - self.cond_leaf = cond_leaf - self.act_leaf = act_leaf - -#定义行动类，行动包括前提、增加和删除影响 -class Action: - def __init__(self,name='anonymous action',pre=set(),add=set(),del_set=set(),cost=1): - self.pre=copy.deepcopy(pre) - self.add=copy.deepcopy(add) - self.del_set=copy.deepcopy(del_set) - self.name=name - self.cost=cost - - def __str__(self): - return self.name - -#生成随机状态 -def generate_random_state(num): - result = set() - for i in range(0,num): - if random.random()>0.5: - result.add(i) - return result -#从状态和行动生成后继状态 -def state_transition(state,action): - if not action.pre <= state: - print ('error: action not applicable') - return state - new_state=(state | action.add) - action.del_set - return new_state - - -#本文所提出的完备规划算法 -class OptBTExpAlgorithm: - def __init__(self,verbose=False): - self.bt = None - self.nodes=[] - self.traversed=[] - self.mounted=[] - self.conditions=[] - self.conditions_index=[] - self.verbose=verbose - - def clear(self): - self.bt = None - self.nodes = [] - self.traversed = [] - self.conditions = [] - self.conditions_index = [] - - #运行规划算法，从初始状态、目标状态和可用行动，计算行为树self.bt - def run_algorithm(self,goal,actions): - if self.verbose: - print("\n算法开始！") - - - self.bt = ControlBT(type='cond') - # 初始行为树只包含目标条件 - gc_node = Leaf(type='cond', content=goal,mincost=0) # 为了统一，都成对出现 - ga_node = Leaf(type='act', content=None, mincost=0) - subtree = ControlBT(type='?') - subtree.add_child([copy.deepcopy(gc_node)]) # 子树首先保留所扩展结 - self.bt.add_child([subtree]) - - # self.conditions.append(goal) - cond_anc_pair = CondActPair(cond_leaf=gc_node,act_leaf=ga_node) - self.nodes.append(copy.deepcopy(cond_anc_pair)) # the set of explored but unexpanded conditions - self.traversed = [goal] # the set of expanded conditions - - - while len(self.nodes)!=0: - - # Find the condition for the shortest cost path - pair_node = None - min_cost = float ('inf') - index= -1 - for i,cond_anc_pair in enumerate(self.nodes): - if cond_anc_pair.cond_leaf.mincost < min_cost: - min_cost = cond_anc_pair.cond_leaf.mincost - pair_node = copy.deepcopy(cond_anc_pair) - index = i - break - - if self.verbose: - print("选择扩展条件结点：",pair_node.cond_leaf.content) - # Update self.nodes and self.traversed - self.nodes.pop(index) # the set of explored but unexpanded conditions. self.nodes.remove(pair_node) - c = pair_node.cond_leaf.content # 子树所扩展结点对应的条件（一个文字的set） - - # Mount the action node and extend BT. T = Eapand(T,c,A(c)) - if c!=goal: - sequence_structure = ControlBT(type='>') - sequence_structure.add_child( - [copy.deepcopy(pair_node.cond_leaf), copy.deepcopy(pair_node.act_leaf)]) - subtree.add_child([copy.deepcopy(sequence_structure)]) # subtree 是回不断变化的，它的父亲是self.bt - if self.verbose: - print("完成扩展 a_node= %s,对应的新条件 c_attr= %s,mincost=%d" \ - % (cond_anc_pair.act_leaf.content.name, cond_anc_pair.cond_leaf.content, - cond_anc_pair.cond_leaf.mincost)) - - if self.verbose: - print("遍历所有动作, 寻找符合条件的动作") - # 遍历所有动作, 寻找符合条件的动作 - current_mincost = pair_node.cond_leaf.mincost # 当前的最短路径是多少 - - for i in range(0, len(actions)): - if not c & ((actions[i].pre | actions[i].add) - actions[i].del_set) <= set(): - if (c - actions[i].del_set) == c: - if self.verbose: - print("———— 满足条件可以扩展") - c_attr = (actions[i].pre | c) - actions[i].add - - # 剪枝操作,现在的条件是以前扩展过的条件的超集 - valid = True - for j in self.traversed: # 剪枝操作 - if j <= c_attr: - valid = False - if self.verbose: - print("———— --被剪枝") - break - - if valid: - # 把符合条件的动作节点都放到列表里 - if self.verbose: - print("———— -- %s 符合条件放入列表" % actions[i].name) - c_attr_node = Leaf(type='cond', content=c_attr, mincost=current_mincost + actions[i].cost) - a_attr_node = Leaf(type='act', content=actions[i], mincost=current_mincost + actions[i].cost) - cond_anc_pair = CondActPair(cond_leaf=c_attr_node, act_leaf=a_attr_node) - self.nodes.append(copy.deepcopy(cond_anc_pair)) # condition node list - self.traversed.append(c_attr) # 重点 the set of expanded conditions - - if self.verbose: - print("算法结束！\n") - return True - - def print_solution(self): - print("========= BT ==========") # 树的bfs遍历 - nodes_ls = [] - nodes_ls.append(self.bt) - while len(nodes_ls) != 0: - parnode = nodes_ls[0] - print("Parrent:", parnode.type) - for child in parnode.children: - if isinstance(child, Leaf): - print("---- Leaf:", child.content) - elif isinstance(child, ControlBT): - print("---- ControlBT:", child.type) - nodes_ls.append(child) - print() - nodes_ls.pop(0) - print("========= BT ==========\n") - - # 返回所有能到达目标状态的初始状态 - def get_all_state_leafs(self): - state_leafs=[] - - nodes_ls = [] - nodes_ls.append(self.bt) - while len(nodes_ls) != 0: - parnode = nodes_ls[0] - for child in parnode.children: - if isinstance(child, Leaf): - if child.type == "cond": - state_leafs.append(child.content) - elif isinstance(child, ControlBT): - nodes_ls.append(child) - nodes_ls.pop(0) - - return state_leafs - - - # 树的dfs - def dfs_ptml(self,parnode): - for child in parnode.children: - if isinstance(child, Leaf): - if child.type == 'cond': - self.ptml_string += "cond " - c_set_str = ', '.join(map(str, child.content)) + "\n" - self.ptml_string += c_set_str - elif child.type == 'act': - self.ptml_string += 'act ' + child.content.name + "\n" - elif isinstance(child, ControlBT): - if parnode.type == '?': - self.ptml_string += "selector{\n" - self.dfs_ptml(parnode=child) - elif parnode.type == '>': - self.ptml_string += "sequence{\n" - self.dfs_ptml( parnode=child) - self.ptml_string += '}\n' - - - def get_ptml(self): - self.ptml_string = "selector{\n" - self.dfs_ptml(self.bt.children[0]) - self.ptml_string += '}\n' - return self.ptml_string - - - def save_ptml_file(self,file_name): - self.ptml_string = "selector{\n" - self.dfs_ptml(self.bt.children[0]) - self.ptml_string += '}\n' - with open(f'./{file_name}.ptml', 'w') as file: - file.write(self.ptml_string) - return self.ptml_string diff --git a/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png b/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png deleted file mode 100644 index 6453647..0000000 Binary files a/zoo/opt_bt_expansion/README.assets/image-20231103191141047.png and /dev/null differ diff --git a/zoo/opt_bt_expansion/README.md b/zoo/opt_bt_expansion/README.md deleted file mode 100644 index 1ddd4a6..0000000 --- a/zoo/opt_bt_expansion/README.md +++ /dev/null @@ -1,125 +0,0 @@ - - -## 代码说明 - -### 1. `BehaviorTree.py` 实现行为树叶子结点和非叶子结点的定义 - -- **Leaf**：表示叶节点，可以是动作（`act`）或条件（`cond`）。 -- **ControlBT**：代表可能包含控制节点的行为树。它们可以是选择器（`?`）、序列（`>`）、动作节点（`act`）或条件节点（`cond`）。 -- 上述两个类都包含 `tick` 方法。 - -### 2. `OptimalBTExpansionAlgorithm.py` 实现最优行为树扩展算法 - -![image-20231103191141047](README.assets/image-20231103191141047.png) - -定义行动类 -```python -#定义行动类，行动包括前提、增加和删除影响 -class Action: - def __init__(self,name='anonymous action',pre=set(),add=set(),del_set=set(),cost=1): - self.pre=copy.deepcopy(pre) - self.add=copy.deepcopy(add) - self.del_set=copy.deepcopy(del_set) - self.name=name - self.cost=cost - - def __str__(self): - return self.name -``` - -调用算法 -```python -algo = OptBTExpAlgorithm(verbose=True) -algo.clear() -algo.run_algorithm(start, goal, actions) # 使用算法得到行为树在 algo.bt -algo.print_solution() # 打印行为树 -val, obj = algo.bt.tick(state) # 执行行为树 -algo.save_ptml_file("bt.ptml") # 保存行为树为 ptml 文件 -``` - -### 3. **`tools.py`** 实现打印数据、行为树测试等模块 - -使用方法 - -```python -print_action_data_table(goal,start,actions) # 打印所有变量 - -# 行为树鲁棒性测试，随机生成规划问题 -# 设置生成规划问题集的超参数：文字数、解深度、迭代次数 -seed=1 -literals_num=10 -depth = 10 -iters= 10 -BTTest(seed=seed,literals_num=literals_num,depth=depth,iters=iters) -``` - -### 4. `example.py` 中设计规划案例 goals, start, actions - -```python -def MoveBtoB (): - actions=[] - a = Action(name="Move(b,ab)") - a.pre={'Free(ab)','WayClear'} - a.add={'At(b,ab)'} - a.del_set= {'Free(ab)','At(b,pb)'} - a.cost = 1 - actions.append(a) - - a=Action(name="Move(s,ab)") - a.pre={'Free(ab)'} - a.add={'Free(ab)','WayClear'} - a.del_set={'Free(ab)','At(s,ps)'} - a.cost = 1 - actions.append(a) - - a=Action(name="Move(s,as)") - a.pre={'Free(as)'} - a.add={'At(s,ps)','WayClear'} - a.del_set={'Free(as)','At(s,ps)'} - a.cost = 1 - actions.append(a) - - start = {'Free(ab)','Free(as)','At(b,pb)','At(s,ps)'} - goal= {'At(b,ab)'} - return goal,start,actions -``` - -### 5. `opt_bt_exp_main.py` 为主函数，在此演示如何调用最优行为树扩展算法得到完全扩展最优行为树 - -初始化的时候：传入 actions (包含 pre,add,del,cost). -调用的时候，传入 goal 状态集合 (set类型)，返回完全最优扩展行为树的 ptml 形式 (string类型) - -```python -actions=[ - Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1) - ………… -] -algo = BTOptExpInterface(actions) - -goal = {'At(Table,Coffee)'} -ptml_string = algo.process(goal,start) -print(ptml_string) - -``` -两种检测方法，用于检测当前状态 `start` 能否到达目标状态 `goal` - -```python -# 判断初始状态能否到达目标状态 -start = {'At(Robot,Bar)', 'Holding(VacuumCup)', 'Available(Table)', 'Available(CoffeeMachine)','Available(FrontDesk)'} - -# 方法一：算法返回所有可能的初始状态，在里面看看有没有对应的初始状态 -right_bt = algo.find_all_leaf_states_contain_start(start) -if not right_bt: - print("ERROR1: The current state cannot reach the goal state!") -else: - print("Right1: The current state can reach the goal state!") - -# 方法二：预先跑一边行为树，看能否到达目标状态 -right_bt2 = algo.run_bt_from_start(goal,start) -if not right_bt2: - print("ERROR2: The current state cannot reach the goal state!") -else: - print("Right2: The current state can reach the goal state!") - -``` - diff --git a/zoo/opt_bt_expansion/__init__.py b/zoo/opt_bt_expansion/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/zoo/opt_bt_expansion/examples.py b/zoo/opt_bt_expansion/examples.py deleted file mode 100644 index 019af9b..0000000 --- a/zoo/opt_bt_expansion/examples.py +++ /dev/null @@ -1,174 +0,0 @@ - -from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action - - - -def MakeCoffee(): - actions=[ - Action(name='Put(Table,Coffee)', pre={'Holding(Coffee)','At(Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), - Action(name='Put(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), - - Action(name='Grasp(Coffee)', pre={'NotHolding','At(Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), - - Action(name='MoveTo(Table)', pre={'Exist(Table)'}, add={'At(Table)'}, del_set={'At(FrontDesk)','At(Coffee)','At(CoffeeMachine)'}, cost=1), - Action(name='MoveTo(Coffee)', pre={'Exist(Coffee)'}, add={'At(Coffee)'}, del_set={'At(FrontDesk)','At(Table)','At(CoffeeMachine)'}, cost=1), - Action(name='MoveTo(CoffeeMachine)', pre={'Exist(CoffeeMachine)'}, add={'At(CoffeeMachine)'}, del_set={'At(FrontDesk)','At(Coffee)','At(Table)'}, cost=1), - - Action(name='OpCoffeeMachine', pre={'At(CoffeeMachine)','NotHolding'}, add={'Exist(Coffee)','At(Coffee)'}, del_set=set(), cost=1), - ] - - start = {'At(FrontDesk)','Holding(VacuumCup)','Exist(Table)','Exist(CoffeeMachine)','Exist(FrontDesk)'} - goal = {'At(Table,Coffee)'} - return goal,start,actions - -# 本例子中，将 VacuumCup 放到 FrontDesk，比 MoveTo(Table) 再 Put(Table,VacuumCup) 的 cost 要小 -def MakeCoffeeCost(): - actions=[ - Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), - Action(name='PutDown(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Robot,Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), - - Action(name='PickUp(Coffee)', pre={'NotHolding','At(Robot,Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), - - Action(name='MoveTo(Table)', pre={'Available(Table)'}, add={'At(Robot,Table)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,CoffeeMachine)'}, cost=1), - Action(name='MoveTo(Coffee)', pre={'Available(Coffee)'}, add={'At(Robot,Coffee)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Table)','At(Robot,CoffeeMachine)'}, cost=1), - Action(name='MoveTo(CoffeeMachine)', pre={'Available(CoffeeMachine)'}, add={'At(Robot,CoffeeMachine)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,Table)'}, cost=1), - - Action(name='OpCoffeeMachine', pre={'At(Robot,CoffeeMachine)','NotHolding'}, add={'Available(Coffee)','At(Robot,Coffee)'}, del_set=set(), cost=1), - ] - - start = {'At(Robot,Bar)','Holding(VacuumCup)','Available(Table)','Available(CoffeeMachine)','Available(FrontDesk)'} - goal = {'At(Table,Coffee)'} - - return goal,start,actions - - -# test -def Test(): - actions=[ - Action(name='a1', pre={6}, add={0,2,4}, del_set={1,5}, cost=1), - Action(name='a2', pre=set(), add={0,1}, del_set=set(), cost=1), - Action(name='a3', pre={1,6}, add={0,2,3,5}, del_set={1,6}, cost=1), - Action(name='a4', pre={0,2,3}, add={4,5}, del_set={0,6}, cost=1), - Action(name='a5', pre={0,1,4}, add={2,3,6}, del_set={0}, cost=1), - ] - - start = {1,2,6} - goal={0,1,2,4,6} - return goal,start,actions - -# def Test(): -# actions=[ -# Action(name='a1', pre={2}, add={1}, del_set=set(), cost=1), -# Action(name='a2', pre=set(), add={1}, del_set={0,2}, cost=1), -# Action(name='a3', pre={1}, add=set(), del_set={0,2}, cost=1), -# Action(name='a4', pre=set(), add={0}, del_set=set(), cost=1), -# Action(name='a5', pre={1}, add={0,2}, del_set={1}, cost=1), -# Action(name='a6', pre={1}, add=set(), del_set={0,1,2}, cost=1), -# Action(name='a7', pre={1}, add={2}, del_set={0, 2}, cost=1), -# ] -# -# start = {1,2} -# goal={0,1} -# return goal,start,actions - - -# todo: 最原始的例子 -def MoveBtoB_num (): - actions=[] - a = Action(name='a1') - a.pre={1,4} - a.add={"c_goal"} - a.del_set={1,4} - a.cost = 1 - actions.append(a) - - a=Action(name='a2') - a.pre={1,2,3} - a.add={"c_goal"} - a.del_set={1,2,3} - a.cost = 1 - actions.append(a) - - a=Action(name='a3') - a.pre={1,2} - a.add={4} - a.del_set={2} - a.cost = 1 - actions.append(a) - - a=Action(name='a4') - a.pre={"c_start"} - a.add={1,2,3} - a.del_set={"c_start",4} - a.cost = 1 - actions.append(a) - - start = {"c_start"} - goal={"c_goal"} - return goal,start,actions - - -# todo: 最原始的例子 -def MoveBtoB (): - actions=[] - a = Action(name="Move(b,ab)") #'movebtob' - a.pre={'Free(ab)','WayClear'} #{1,2} - a.add={'At(b,ab)'} #{3} - a.del_set= {'Free(ab)','At(b,pb)'} #{1,4} - a.cost = 1 - actions.append(a) - - a=Action(name="Move(s,ab)") #'moveatob' - a.pre={'Free(ab)'} #{1} - a.add={'Free(ab)','WayClear'} #{5,2} - a.del_set={'Free(ab)','At(s,ps)'} #{1,6} - a.cost = 1 - actions.append(a) - - a=Action(name="Move(s,as)") #'moveatoa' - a.pre={'Free(as)'} #{7} - a.add={'At(s,ps)','WayClear'} #{8,2} - a.del_set={'Free(as)','At(s,ps)'} #{7,6} - a.cost = 1 - actions.append(a) - - start = {'Free(ab)','Free(as)','At(b,pb)','At(s,ps)'} #{1,7,4,6} - goal= {'At(b,ab)'} #{3} - return goal,start,actions - - -# 小蔡师兄论文里的例子 -def Cond2BelongsToCond3(): - actions=[] - a = Action(name='a1') - a.pre={1,4} - a.add={"c_goal"} - a.del_set={1,4} - a.cost = 1 - actions.append(a) - - a=Action(name='a2') - a.pre={1,2,3} - a.add={"c_goal"} - a.del_set={1,2,3} - a.cost = 100 - actions.append(a) - - a=Action(name='a3') - a.pre={1,2} - a.add={4} - a.del_set={2} - a.cost = 1 - actions.append(a) - - a=Action(name='a4') - a.pre={"c_start"} - a.add={1,2,3} - a.del_set={"c_start",4} - a.cost = 1 - actions.append(a) - - start = {"c_start"} - goal={"c_goal"} - return goal,start,actions - diff --git a/zoo/opt_bt_expansion/opt_bt_exp_main.py b/zoo/opt_bt_expansion/opt_bt_exp_main.py deleted file mode 100644 index 87329b6..0000000 --- a/zoo/opt_bt_expansion/opt_bt_exp_main.py +++ /dev/null @@ -1,118 +0,0 @@ -from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm,state_transition # 调用最优行为树扩展算法 -from opt_bt_expansion.examples import * - - -# 封装好的主接口 -class BTOptExpInterface: - def __init__(self, action_list): - """ - Initialize the BTOptExpansion with a list of actions. - :param action_list: A list of actions to be used in the behavior tree. - """ - # self.actions = [] - # for act in action_list: - # a = Action(name=act.name) - # a.pre=act['pre'] - # a.add=act['add'] - # a.del_set= act['del_set'] - # a.cost = 1 - # self.actions.append(a) - self.actions = action_list - self.has_processed = False - - def process(self, goal): - """ - Process the input sets and return a string result. - :param input_set: The set of goal states and the set of initial states. - :return: A PTML string representing the outcome of the behavior tree. - """ - self.goal = goal - self.algo = OptBTExpAlgorithm(verbose=False) - self.algo.clear() - self.algo.run_algorithm(self.goal, self.actions) # 调用算法得到行为树保存至 algo.bt - self.ptml_string = self.algo.get_ptml() - self.has_processed = True - # algo.print_solution() # print behavior tree - - return self.ptml_string - - # 方法一：查找所有初始状态是否包含当前状态 - def find_all_leaf_states_contain_start(self,start): - if not self.has_processed: - raise RuntimeError("The process method must be called before find_all_leaf_states_contain_start!") - # 返回所有能到达目标状态的初始状态 - state_leafs = self.algo.get_all_state_leafs() - for state in state_leafs: - if start >= state: - return True - return False - - # 方法二：模拟跑一遍行为树，看 start 能够通过执行一系列动作到达 goal - def run_bt_from_start(self,goal,start): - if not self.has_processed: - raise RuntimeError("The process method must be called before run_bt_from_start!") - # 检查是否能到达目标 - right_bt = True - state = start - steps = 0 - val, obj = self.algo.bt.tick(state) - while val != 'success' and val != 'failure': - state = state_transition(state, obj) - val, obj = self.algo.bt.tick(state) - if (val == 'failure'): - # print("bt fails at step", steps) - right_bt = False - steps += 1 - if not goal <= state: - # print("wrong solution", steps) - right_bt = False - else: - pass - # print("right solution", steps) - return right_bt - - - - -if __name__ == '__main__' : - - # todo: Example Cafe - # todo: Define goal, start, actions - actions=[ - Action(name='PutDown(Table,Coffee)', pre={'Holding(Coffee)','At(Robot,Table)'}, add={'At(Table,Coffee)','NotHolding'}, del_set={'Holding(Coffee)'}, cost=1), - Action(name='PutDown(Table,VacuumCup)', pre={'Holding(VacuumCup)','At(Robot,Table)'}, add={'At(Table,VacuumCup)','NotHolding'}, del_set={'Holding(VacuumCup)'}, cost=1), - - Action(name='PickUp(Coffee)', pre={'NotHolding','At(Robot,Coffee)'}, add={'Holding(Coffee)'}, del_set={'NotHolding'}, cost=1), - - Action(name='MoveTo(Table)', pre={'Available(Table)'}, add={'At(Robot,Table)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,CoffeeMachine)'}, cost=1), - Action(name='MoveTo(Coffee)', pre={'Available(Coffee)'}, add={'At(Robot,Coffee)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Table)','At(Robot,CoffeeMachine)'}, cost=1), - Action(name='MoveTo(CoffeeMachine)', pre={'Available(CoffeeMachine)'}, add={'At(Robot,CoffeeMachine)'}, del_set={'At(Robot,FrontDesk)','At(Robot,Coffee)','At(Robot,Table)'}, cost=1), - - Action(name='OpCoffeeMachine', pre={'At(Robot,CoffeeMachine)','NotHolding'}, add={'Available(Coffee)','At(Robot,Coffee)'}, del_set=set(), cost=1), - ] - algo = BTOptExpInterface(actions) - - - goal = {'At(Table,Coffee)'} - ptml_string = algo.process(goal) - print(ptml_string) - - file_name = "MakeCoffee" - with open(f'./{file_name}.ptml', 'w') as file: - file.write(ptml_string) - - - # 判断初始状态能否到达目标状态 - start = {'At(Robot,Bar)', 'Holding(VacuumCup)', 'Available(Table)', 'Available(CoffeeMachine)','Available(FrontDesk)'} - # 方法一：算法返回所有可能的初始状态，在里面看看有没有对应的初始状态 - right_bt = algo.find_all_leaf_states_contain_start(start) - if not right_bt: - print("ERROR1: The current state cannot reach the goal state!") - else: - print("Right1: The current state can reach the goal state!") - # 方法二：预先跑一边行为树，看能否到达目标状态 - right_bt2 = algo.run_bt_from_start(goal,start) - if not right_bt2: - print("ERROR2: The current state cannot reach the goal state!") - else: - print("Right2: The current state can reach the goal state!") diff --git a/zoo/opt_bt_expansion/tools.py b/zoo/opt_bt_expansion/tools.py deleted file mode 100644 index c2e0695..0000000 --- a/zoo/opt_bt_expansion/tools.py +++ /dev/null @@ -1,167 +0,0 @@ - - -from tabulate import tabulate -import numpy as np -import random -from zoo.opt_bt_expansion.OptimalBTExpansionAlgorithm import Action,OptBTExpAlgorithm -import time - - -def print_action_data_table(goal,start,actions): - data = [] - for a in actions: - data.append([a.name , a.pre , a.add , a.del_set , a.cost]) - data.append(["Goal" ,goal ," " ,"Start" ,start]) - print(tabulate(data, headers=["Name", "Pre", "Add" ,"Del" ,"Cost"], tablefmt="fancy_grid")) # grid plain simple github fancy_grid - - -# 从状态随机生成一个行动 -def generate_from_state(act,state,num): - for i in range(0,num): - if i in state: - if random.random() >0.5: - act.pre.add(i) - if random.random() >0.5: - act.del_set.add(i) - continue - if random.random() > 0.5: - act.add.add(i) - continue - if random.random() >0.5: - act.del_set.add(i) - -def print_action(act): - print (act.pre) - print(act.add) - print(act.del_set) - - - -#行为树测试代码 -def BTTest(seed=1,literals_num=10,depth=10,iters=10,total_count=1000): - print("============= BT Test ==============") - random.seed(seed) - # 设置生成规划问题集的超参数：文字数、解深度、迭代次数 - literals_num=literals_num - depth = depth - iters= iters - total_tree_size = [] - total_action_num = [] - total_state_num = [] - total_steps_num=[] - #fail_count=0 - #danger_count=0 - success_count =0 - failure_count = 0 - planning_time_total = 0.0 - # 实验1000次 - for count in range (total_count): - - action_num = 1 - - # 生成一个规划问题，包括随机的状态和行动，以及目标状态 - states = [] - actions = [] - start = generate_random_state(literals_num) - state = start - states.append(state) - #print (state) - for i in range (0,depth): - a = Action() - generate_from_state(a,state,literals_num) - if not a in actions: - a.name = "a"+str(action_num) - action_num+=1 - actions.append(a) - state = state_transition(state,a) - if state in states: - pass - else: - states.append(state) - #print(state) - - goal = states[-1] - state = start - for i in range (0,iters): - a = Action() - generate_from_state(a,state,literals_num) - if not a in actions: - a.name = "a"+str(action_num) - action_num+=1 - actions.append(a) - state = state_transition(state,a) - if state in states: - pass - else: - states.append(state) - state = random.sample(states,1)[0] - - # 选择测试本文算法btalgorithm，或对比算法weakalgorithm - algo = OptBTExpAlgorithm() - #algo = Weakalgorithm() - start_time = time.time() - # print_action_data_table(goal, start, list(actions)) - if algo.run_algorithm(start, goal, actions):#运行算法，规划后行为树为algo.bt - total_tree_size.append( algo.bt.count_size()-1) - # algo.print_solution() # 打印行为树 - else: - print ("error") - end_time = time.time() - planning_time_total += (end_time-start_time) - - #开始从初始状态运行行为树，测试 - state=start - steps=0 - val, obj = algo.bt.tick(state)#tick行为树，obj为所运行的行动 - while val !='success' and val !='failure':#运行直到行为树成功或失败 - state = state_transition(state,obj) - val, obj = algo.bt.tick(state) - if(val == 'failure'): - print("bt fails at step",steps) - steps+=1 - if(steps>=500):#至多运行500步 - break - if not goal <= state:#错误解，目标条件不在执行后状态满足 - #print ("wrong solution",steps) - failure_count+=1 - - else:#正确解，满足目标条件 - #print ("right solution",steps) - success_count+=1 - total_steps_num.append(steps) - algo.clear() - total_action_num.append(len(actions)) - total_state_num.append(len(states)) - - print ("success:",success_count,"failure:",failure_count)#算法成功和失败次数 - print("Total Tree Size: mean=",np.mean(total_tree_size), "std=",np.std(total_tree_size, ddof=1))#1000次测试树大小 - print ("Total Steps Num: mean=",np.mean(total_steps_num),"std=",np.std(total_steps_num,ddof=1)) - print ("Average number of states:",np.mean(total_state_num))#1000次问题的平均状态数 - print ("Average number of actions",np.mean(total_action_num))#1000次问题的平均行动数 - print("Planning Time Total:",planning_time_total,planning_time_total/1000.0) - print("============ End BT Test ===========") - - # xiao cai - # success: 1000 failure: 0 - # Total Tree Size: mean= 35.303 std= 29.71336526001515 - # Total Steps Num: mean= 1.898 std= 0.970844240101644 - # Average number of states: 20.678 - # Average number of actions 20.0 - # Planning Time Total: 0.6280641555786133 0.0006280641555786133 - - # our start - # success: 1000 failure: 0 - # Total Tree Size: mean= 17.945 std= 12.841997192488865 - # Total Steps Num: mean= 1.785 std= 0.8120556843187752 - # Average number of states: 20.678 - # Average number of actions 20.0 - # Planning Time Total: 1.4748523235321045 0.0014748523235321046 - - # our - # success: 1000 failure: 0 - # Total Tree Size: mean= 48.764 std= 20.503626574406358 - # Total Steps Num: mean= 1.785 std= 0.8120556843187752 - # Average number of states: 20.678 - # Average number of actions 20.0 - # Planning Time Total: 3.3271877765655518 0.0033271877765655516 -