From a7007aeea7f8538d2ec30d014e3cc150a40c17e4 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Mon, 13 Nov 2023 14:48:58 +0800 Subject: [PATCH 1/7] =?UTF-8?q?=E6=9B=B4=E6=94=B9=E4=BA=86obea=E7=9A=84seq?= =?UTF-8?q?uence?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py | 4 ++-- sub_task.ptml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py index 6ec11d5..7f0010f 100644 --- a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py +++ b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py @@ -185,10 +185,10 @@ class OptBTExpAlgorithm: elif child.type == 'act': self.ptml_string += 'act '+child.content.name+"\n" elif isinstance(child, ControlBT): - if parnode.type == '?': + if child.type == '?': self.ptml_string += "selector{\n" self.dfs_ptml(parnode=child) - elif parnode.type == '>': + elif child.type == '>': self.ptml_string += "sequence{\n" self.dfs_ptml( parnode=child) self.ptml_string += '}\n' diff --git a/sub_task.ptml b/sub_task.ptml index b8bf4e1..adcea35 100644 --- a/sub_task.ptml +++ b/sub_task.ptml @@ -1,6 +1,6 @@ selector{ cond At(Coffee,Bar) -selector{ +sequence{ cond At(Robot,CoffeeMachine) act MakeCoffee() } From 9a3143cafcfa42395c0989421164e9359f6f2e12 Mon Sep 17 00:00:00 2001 From: ChenXL97 <908926798@qq.com> Date: Mon, 13 Nov 2023 14:53:00 +0800 Subject: [PATCH 2/7] =?UTF-8?q?=E7=8E=B0=E5=9C=A8=E5=8F=AF=E4=BB=A5?= =?UTF-8?q?=E8=87=AA=E5=8A=A8=E7=94=9F=E6=88=90=E8=A1=8C=E4=B8=BA=E6=A0=91?= =?UTF-8?q?=E6=89=A9=E5=B1=95=E5=8A=A8=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Act.py | 9 ++--- robowaiter/behavior_lib/_base/Behavior.py | 6 +++ .../act/{MakeCoffee.py => Make.py} | 24 ++++++++---- robowaiter/behavior_lib/act/MoveTo.py | 20 +++++++++- robowaiter/behavior_lib/cond/At.py | 1 - robowaiter/behavior_lib/cond/On.py | 28 ++++++++++++++ robowaiter/robot/robot.py | 35 ++++++++++++----- robowaiter/utils/__init__.py | 9 +---- robowaiter/utils/basic.py | 6 +++ robowaiter/utils/bt/load.py | 38 ++++++++++++++++++- sub_task.ptml | 4 -- 11 files changed, 143 insertions(+), 37 deletions(-) rename robowaiter/behavior_lib/act/{MakeCoffee.py => Make.py} (54%) create mode 100644 robowaiter/behavior_lib/cond/On.py create mode 100644 robowaiter/utils/basic.py diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py index b402ed9..34ee91c 100644 --- a/robowaiter/behavior_lib/_base/Act.py +++ b/robowaiter/behavior_lib/_base/Act.py @@ -7,8 +7,7 @@ class Act(Bahavior): def __init__(self,*args): super().__init__(*args) - def get_conds(self): - pre = set() - add = set() - de = set() - return pre, add, de \ No newline at end of file + @classmethod + def get_info(self,*arg): + return None + diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 2a99f03..129be1d 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -19,7 +19,13 @@ class Bahavior(ptree.behaviour.Behaviour): if len(args)>0: name = f'{name}({",".join(list(args))})' self.name = name + #get valid args + # self.valid_arg_list = [] + # lines = self.valid_params.strip().splitlines() + # for line in lines: + # self.valid_arg_list.append((x.strip for x in line.split(","))) self.args = args + super().__init__(self.name) def _update(self) -> ptree.common.Status: diff --git a/robowaiter/behavior_lib/act/MakeCoffee.py b/robowaiter/behavior_lib/act/Make.py similarity index 54% rename from robowaiter/behavior_lib/act/MakeCoffee.py rename to robowaiter/behavior_lib/act/Make.py index d4903fe..2d0be6c 100644 --- a/robowaiter/behavior_lib/act/MakeCoffee.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -3,17 +3,27 @@ from typing import Any from robowaiter.behavior_lib._base.Act import Act from robowaiter.behavior_lib._base.Behavior import Status -class MakeCoffee(Act): +class Make(Act): + can_be_expanded = True + num_args = 1 + valid_args = ( + "Coffee", + ) def __init__(self, *args): super().__init__(*args) + self.target_obj = self.args[0] + + + @classmethod + def get_info(cls,arg): + info = None + if arg == "Coffee": + info = { + "add": {f'On(Coffee,Table)'}, + } + return info - @property - def cond_sets(self): - pre = {"At(Robot,Bar)"} - add = {"At(Coffee,Bar)"} - de = {} - return pre,add,de def _update(self) -> ptree.common.Status: op_type = 1 diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index e01eb53..000869d 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -4,9 +4,25 @@ from robowaiter.behavior_lib._base.Act import Act from robowaiter.algos.navigate.DstarLite.navigate import Navigator class MoveTo(Act): + can_be_expanded = True + num_args = 1 + valid_args = ( + "Bar", + "Table", + ) + + def __init__(self, target_place): + super().__init__(target_place) + self.target_place = target_place + + + @classmethod + def get_info(self,arg): + info = { + "add": {f'At(Robot,{arg})'}, + } + return info - def __init__(self, *args): - super().__init__(*args) def _update(self) -> ptree.common.Status: # self.scene.test_move() diff --git a/robowaiter/behavior_lib/cond/At.py b/robowaiter/behavior_lib/cond/At.py index a660621..1c88605 100644 --- a/robowaiter/behavior_lib/cond/At.py +++ b/robowaiter/behavior_lib/cond/At.py @@ -6,7 +6,6 @@ class At(Cond): can_be_expanded = True num_params = 2 valid_params = ''' - Coffee, Bar Robot, Bar ''' diff --git a/robowaiter/behavior_lib/cond/On.py b/robowaiter/behavior_lib/cond/On.py new file mode 100644 index 0000000..e679cfd --- /dev/null +++ b/robowaiter/behavior_lib/cond/On.py @@ -0,0 +1,28 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Cond import Cond + +class On(Cond): + can_be_expanded = True + num_params = 2 + valid_params = ''' + Robot, Bar + ''' + + def __init__(self,*args): + super().__init__(*args) + + + def _update(self) -> ptree.common.Status: + # if self.scene.status? + arg_str = self.arg_str + + if f'At({arg_str})' in self.scene.state["condition_set"]: + return ptree.common.Status.SUCCESS + else: + return ptree.common.Status.FAILURE + + # if self.scene.state['chat_list'] == []: + # return ptree.common.Status.FAILURE + # else: + # return ptree.common.Status.SUCCESS diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index 880d55b..8133fcb 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -1,5 +1,7 @@ import io import contextlib +import os +import importlib.util from robowaiter.utils.bt.load import load_bt_from_ptml,find_node_by_name,print_tree_from_root from robowaiter.utils.bt.visitor import StatusVisitor @@ -9,7 +11,7 @@ from robowaiter.behavior_tree.obtea.opt_bt_exp_main import BTOptExpInterface from robowaiter.behavior_lib.act.DelSubTree import DelSubTree from robowaiter.behavior_lib._base.Sequence import Sequence - +from robowaiter.utils.bt.load import load_behavior_tree_lib class Robot(object): scene = None @@ -67,15 +69,30 @@ class Robot(object): print("当前行为树为：") print_tree_from_root(self.bt.root) + # 获取所有action的pre,add,del列表 def collect_action_nodes(self): - action_list = [ - Action(name='MakeCoffee', pre={'At(Robot,CoffeeMachine)'}, - add={'At(Coffee,Bar)'}, del_set=set(), cost=1), - Action(name='MoveTo(Table)', pre={'At(Robot,Bar)'}, - add={'At(Robot,Table)'}, del_set=set(), cost=1), - Action(name='ExploreEnv()', pre={'At(Robot,Bar)'}, - add={'EnvExplored()'}, del_set=set(), cost=1), - ] + action_list = [] + behavior_dict = load_behavior_tree_lib() + for cls in behavior_dict["act"].values(): + if cls.can_be_expanded: + if cls.num_args == 0: + action_list.append(Action(name=cls.__name__,**cls.get_info())) + if cls.num_args == 1: + for arg in cls.valid_args: + action_list.append(Action(name=cls.__name__, **cls.get_info(arg))) + if cls.num_args > 1: + for args in cls.valid_args: + action_list.append(Action(name=cls.__name__,**cls.get_info(*args))) + + print(action_list) + # action_list = [ + # Action(name='MakeCoffee', pre={'At(Robot,CoffeeMachine)'}, + # add={'At(Coffee,Bar)'}, del_set=set(), cost=1), + # Action(name='MoveTo(Table)', pre={'At(Robot,Bar)'}, + # add={'At(Robot,Table)'}, del_set=set(), cost=1), + # Action(name='ExploreEnv()', pre=set(), + # add={'EnvExplored()'}, del_set=set(), cost=1), + # ] return action_list def step(self): diff --git a/robowaiter/utils/__init__.py b/robowaiter/utils/__init__.py index 5b043f1..119bc84 100644 --- a/robowaiter/utils/__init__.py +++ b/robowaiter/utils/__init__.py @@ -2,11 +2,4 @@ import os from robowaiter.utils import * from robowaiter.utils import * - - - - -def get_root_path(): - return os.path.abspath( - os.path.join(__file__, "../../..") - ) +from robowaiter.utils.basic import get_root_path \ No newline at end of file diff --git a/robowaiter/utils/basic.py b/robowaiter/utils/basic.py new file mode 100644 index 0000000..2d7c60d --- /dev/null +++ b/robowaiter/utils/basic.py @@ -0,0 +1,6 @@ +import os + +def get_root_path(): + return os.path.abspath( + os.path.join(__file__, "../../..") + ) diff --git a/robowaiter/utils/bt/load.py b/robowaiter/utils/bt/load.py index cb7f61a..bf90ea9 100644 --- a/robowaiter/utils/bt/load.py +++ b/robowaiter/utils/bt/load.py @@ -1,6 +1,8 @@ import py_trees as ptree from robowaiter.behavior_tree.ptml import ptmlCompiler - +import os +import importlib.util +from robowaiter.utils.basic import get_root_path def load_bt_from_ptml(scene, ptml_path, behavior_lib_path): ptml_bt = ptmlCompiler.load(scene, ptml_path, behavior_lib_path) @@ -48,6 +50,40 @@ def find_node_by_name(tree, name): +def get_classes_from_folder(folder_path): + cls_dict = {} + for filename in os.listdir(folder_path): + if filename.endswith('.py'): + # 构建模块的完整路径 + module_path = os.path.join(folder_path, filename) + # 获取模块名（不含.py扩展名） + module_name = os.path.splitext(filename)[0] + + # 动态导入模块 + spec = importlib.util.spec_from_file_location(module_name, module_path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + + # 获取模块中定义的所有类 + for name, obj in module.__dict__.items(): + if isinstance(obj, type): + cls_dict[module_name] = obj + + return cls_dict + + +def load_behavior_tree_lib(): + root_path = get_root_path() + type_list = ["act","cond"] + behavior_dict = {} + for type in type_list: + path = os.path.join(root_path,"robowaiter","behavior_lib",type) + behavior_dict[type] = get_classes_from_folder(path) + + return behavior_dict + +if __name__ == '__main__': + print(load_behavior_tree_lib()) # class BehaviorTree(ptree): # def __init__(self): # super().__init__() \ No newline at end of file diff --git a/sub_task.ptml b/sub_task.ptml index 1b4d81e..d9cb899 100644 --- a/sub_task.ptml +++ b/sub_task.ptml @@ -1,7 +1,3 @@ selector{ cond EnvExplored() -selector{ -cond At(Robot,Bar) -act ExploreEnv() -} } From 2d933bf7a42d4ce9726fa3406e3a95d587cb81c6 Mon Sep 17 00:00:00 2001 From: ChenXL97 <908926798@qq.com> Date: Mon, 13 Nov 2023 15:05:21 +0800 Subject: [PATCH 3/7] =?UTF-8?q?=E7=8E=B0=E5=9C=A8=E6=97=A0=E5=8F=82?= =?UTF-8?q?=E6=95=B0=E8=8A=82=E7=82=B9=E6=89=93=E5=8D=B0=E6=97=B6=E4=B9=9F?= =?UTF-8?q?=E4=BC=9A=E5=B8=A6=E7=A9=BA=E6=8B=AC=E5=8F=B7()?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Behavior.py | 13 ++++++++++++- robowaiter/robot/robot.py | 13 +++++++++---- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 129be1d..84ed74b 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -14,6 +14,15 @@ class Bahavior(ptree.behaviour.Behaviour): scene = None print_name_prefix = "" + @classmethod + def get_ins_name(cls,*args): + name = cls.__name__ + if len(args) > 0: + ins_name = f'{name}({",".join(list(args))})' + else: + ins_name = f'{name}()' + return ins_name + def __init__(self,*args): name = self.__class__.__name__ if len(args)>0: @@ -34,7 +43,9 @@ class Bahavior(ptree.behaviour.Behaviour): @property def print_name(self): - return f'{self.print_name_prefix}{self.name}' + return f'{self.print_name_prefix}{self.get_ins_name(*self.args)}' + + # let behavior node interact with the scene def set_scene(self, scene): diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index 8133fcb..6d7b275 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -45,7 +45,12 @@ class Robot(object): def expand_sub_task_tree(self,goal): if self.action_list is None: self.action_list = self.collect_action_nodes() - print(f"首次运行行为树扩展算法，收集到{len(self.action_list)}个有效动作") + print("\n--------------------") + print(f"首次运行行为树扩展算法，收集到{len(self.action_list)}个有效动作:") + for a in self.action_list: + print(a.name) + print("--------------------\n") + algo = BTOptExpInterface(self.action_list) @@ -76,13 +81,13 @@ class Robot(object): for cls in behavior_dict["act"].values(): if cls.can_be_expanded: if cls.num_args == 0: - action_list.append(Action(name=cls.__name__,**cls.get_info())) + action_list.append(Action(name=cls.get_ins_name(),**cls.get_info())) if cls.num_args == 1: for arg in cls.valid_args: - action_list.append(Action(name=cls.__name__, **cls.get_info(arg))) + action_list.append(Action(name=cls.get_ins_name(arg), **cls.get_info(arg))) if cls.num_args > 1: for args in cls.valid_args: - action_list.append(Action(name=cls.__name__,**cls.get_info(*args))) + action_list.append(Action(name=cls.get_ins_name(*args),**cls.get_info(*args))) print(action_list) # action_list = [ From f43abea0ac7a077af660a774c0df5dafe3cf1674 Mon Sep 17 00:00:00 2001 From: ChenXL97 <908926798@qq.com> Date: Mon, 13 Nov 2023 15:36:07 +0800 Subject: [PATCH 4/7] =?UTF-8?q?=E5=B0=86sub=5Ftask.ptml=E5=8A=A0=E5=85=A5g?= =?UTF-8?q?itignore?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 9ed45df..85e33e4 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,8 @@ MANIFEST MO-VLN/ GLIP/ +sub_task.ptml + # PyInstaller # Usually these files are written by a python script from a template # before PyInstaller builds the exe, so as to inject date/other infos into it. From 806847ac15aec9f00aca8a2bb9b5fed606ed56a3 Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Mon, 13 Nov 2023 15:40:19 +0800 Subject: [PATCH 5/7] =?UTF-8?q?=E6=9B=B4=E6=94=B9=E4=BA=86memgpt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Act.py | 8 ++-- robowaiter/behavior_lib/act/MakeCoffee.py | 4 +- robowaiter/behavior_lib/act/PourWater.py | 23 ++++++++++++ .../obtea/OptimalBTExpansionAlgorithm.py | 16 +++++--- robowaiter/llm_client/MemGPT_simple/.env | 4 +- robowaiter/llm_client/MemGPT_simple/agent.py | 37 ++++++++++++++++--- .../llm_client/MemGPT_simple/functions_zh.py | 17 +++++++++ robowaiter/llm_client/MemGPT_simple/main.py | 11 +++++- robowaiter/llm_client/data/test_questions.txt | 3 +- .../llm_client/data_raw/test_questions.csv | 1 + robowaiter/robot/robot.py | 6 ++- robowaiter/scene/tasks/VLM.py | 3 +- sub_task.ptml | 10 +++-- 13 files changed, 118 insertions(+), 25 deletions(-) create mode 100644 robowaiter/behavior_lib/act/PourWater.py create mode 100644 robowaiter/llm_client/MemGPT_simple/functions_zh.py diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py index b402ed9..244a060 100644 --- a/robowaiter/behavior_lib/_base/Act.py +++ b/robowaiter/behavior_lib/_base/Act.py @@ -8,7 +8,7 @@ class Act(Bahavior): super().__init__(*args) def get_conds(self): - pre = set() - add = set() - de = set() - return pre, add, de \ No newline at end of file + self.pre = set() + self.add = set() + self.de = set() + return self.pre, self.add, self.de \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/MakeCoffee.py b/robowaiter/behavior_lib/act/MakeCoffee.py index d4903fe..b14727f 100644 --- a/robowaiter/behavior_lib/act/MakeCoffee.py +++ b/robowaiter/behavior_lib/act/MakeCoffee.py @@ -10,7 +10,7 @@ class MakeCoffee(Act): @property def cond_sets(self): - pre = {"At(Robot,Bar)"} + pre = {"At(Robot,CoffeeMachine)","NotHolding"} add = {"At(Coffee,Bar)"} de = {} return pre,add,de @@ -19,5 +19,5 @@ class MakeCoffee(Act): op_type = 1 self.scene.move_task_area(op_type) self.scene.op_task_execute(op_type) - self.scene.state["condition_set"].add("At(Coffee,Bar)") + self.scene.state["condition_set"].add(self.add) return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/PourWater.py b/robowaiter/behavior_lib/act/PourWater.py new file mode 100644 index 0000000..23db1fe --- /dev/null +++ b/robowaiter/behavior_lib/act/PourWater.py @@ -0,0 +1,23 @@ +import py_trees as ptree +from typing import Any +from robowaiter.behavior_lib._base.Act import Act +from robowaiter.behavior_lib._base.Behavior import Status + +class PourWater(Act): + + def __init__(self, *args): + super().__init__(*args) + + @property + def cond_sets(self): + self.pre = {"At(Robot,WaterTable)","NotHolding"} + self.add = {"On(Water,WaterTable)"} + self.de = {} + return self.pre,self.add,self.de + + def _update(self) -> ptree.common.Status: + op_type = 2 + self.scene.move_task_area(op_type) + self.scene.op_task_execute(op_type) + self.scene.state["condition_set"].update(self.add) + return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py index 7f0010f..333a644 100644 --- a/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py +++ b/robowaiter/behavior_tree/obtea/OptimalBTExpansionAlgorithm.py @@ -94,10 +94,13 @@ class OptBTExpAlgorithm: # 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 c!=set(): + 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 + else: + subtree.add_child([copy.deepcopy(pair_node.act_leaf)]) 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, @@ -183,7 +186,10 @@ class OptBTExpAlgorithm: 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" + if '(' not in child.content.name: + self.ptml_string += 'act '+child.content.name+"()\n" + else: + self.ptml_string += 'act ' + child.content.name + "\n" elif isinstance(child, ControlBT): if child.type == '?': self.ptml_string += "selector{\n" diff --git a/robowaiter/llm_client/MemGPT_simple/.env b/robowaiter/llm_client/MemGPT_simple/.env index 1ce720e..1c9626b 100644 --- a/robowaiter/llm_client/MemGPT_simple/.env +++ b/robowaiter/llm_client/MemGPT_simple/.env @@ -1,2 +1,4 @@ -OPENAI_API_BASE= +#BACKEND_TYPE=webui +#OPENAI_API_BASE=https://45.125.46.134:25344/v1/chat/completions +OPENAI_API_BASE=https://45.125.46.134:25344/v1/chat/completions OPENAI_API_KEY= \ No newline at end of file diff --git a/robowaiter/llm_client/MemGPT_simple/agent.py b/robowaiter/llm_client/MemGPT_simple/agent.py index e49ab4e..c46758f 100644 --- a/robowaiter/llm_client/MemGPT_simple/agent.py +++ b/robowaiter/llm_client/MemGPT_simple/agent.py @@ -170,7 +170,8 @@ class Agent: ) else: # Standard non-function reply - print("### Internal monologue: " + (response_message.content if response_message.content else "")) + # print("### Internal monologue: " + (response_message.content if response_message.content else "")) + print("### Internal monologue: " + (response_message['content'] if response_message['content'] else "")) messages.append(response_message) function_failed = None @@ -178,13 +179,37 @@ class Agent: def step(self, user_message): input_message_sequence = self.messages + [{"role": "user", "content": user_message}] - response = openai.ChatCompletion.create(model=self.model, messages=input_message_sequence, - functions=self.functions_description, function_call="auto") - response_message = response.choices[0].message - response_message_copy = response_message.copy() + # 原来的通信方式 + # response = openai.ChatCompletion.create(model=self.model, messages=input_message_sequence, + # functions=self.functions_description, function_call="auto") + # + # response_message = response.choices[0].message + # response_message_copy = response_message.copy() + + # ===我们的通信方式 "tools": self.functions_description 不起作用=== + import requests + url = "https://45.125.46.134:25344/v1/chat/completions" + headers = {"Content-Type": "application/json"} + data = { + "model": "RoboWaiter", + "messages": input_message_sequence, + # "functions":self.functions_description, + # "function_call":"auto" + # "function_call":self.functions_description + "tools": self.functions_description + } + response = requests.post(url, headers=headers, json=data, verify=False) + if response.status_code == 200: + result = response.json() + response_message = result['choices'][0]['message'] + else: + response_message = "大模型请求失败:"+ str(response.status_code) + response_message_copy = response_message + # ===我们的通信方式 "tools": self.functions_description 不起作用=== + + all_response_messages, function_failed = self.handle_ai_response(response_message) - assert "api_response" not in all_response_messages[0], f"api_response already in {all_response_messages[0]}" all_response_messages[0]["api_response"] = response_message_copy assert "api_args" not in all_response_messages[0], f"api_args already in {all_response_messages[0]}" diff --git a/robowaiter/llm_client/MemGPT_simple/functions_zh.py b/robowaiter/llm_client/MemGPT_simple/functions_zh.py new file mode 100644 index 0000000..2e06f63 --- /dev/null +++ b/robowaiter/llm_client/MemGPT_simple/functions_zh.py @@ -0,0 +1,17 @@ +FUNCTIONS = [ + { + "name": "get_current_weather", + "description": "Get the current weather in a given location", + "parameters": { + "type": "object", + "properties": { + "location": { + "type": "string", + "description": "The city and state, e.g. San Francisco, CA", + }, + "unit": {"type": "string"}, + }, + "required": ["location"], + }, + } +] diff --git a/robowaiter/llm_client/MemGPT_simple/main.py b/robowaiter/llm_client/MemGPT_simple/main.py index 1b84d03..86fc9f1 100644 --- a/robowaiter/llm_client/MemGPT_simple/main.py +++ b/robowaiter/llm_client/MemGPT_simple/main.py @@ -2,10 +2,14 @@ from dotenv import load_dotenv load_dotenv() import utils -from functions import FUNCTIONS +# from functions import FUNCTIONS +from functions_zh import FUNCTIONS from agent import Agent +import urllib3 +urllib3.disable_warnings(urllib3.exceptions.InsecureRequestWarning) + def run_agent_loop(agent): while True: user_input = input("You: ") @@ -20,6 +24,11 @@ if __name__ == "__main__": persona = utils.get_persona_text() human = utils.get_human_text() system = utils.get_system_text() + + print("system:",system) + print("persona:", persona) + print("human:", human) + agent = Agent(model="gpt-3.5-turbo-16k-0613", system=system, functions_description=FUNCTIONS, persona_notes=persona, human_notes=human) run_agent_loop(agent) diff --git a/robowaiter/llm_client/data/test_questions.txt b/robowaiter/llm_client/data/test_questions.txt index 823fa74..aa9f62c 100644 --- a/robowaiter/llm_client/data/test_questions.txt +++ b/robowaiter/llm_client/data/test_questions.txt @@ -1 +1,2 @@ -{"测试VLM：做一杯咖啡": {"Answer": "测试VLM：做一杯咖啡", "Goal": "{\"At(Coffee,Bar)\"}"}, "测试VLN：前往桌子": {"Answer": "测试VLN：前往桌子", "Goal": "{\"At(Robot,Table)\"}"}, "测试AEM": {"Answer": "测试AEM", "Goal": "{\"EnvExplored()\"}"}} +{"测试VLM：做一杯咖啡": {"Answer": "测试VLM：做一杯咖啡", "Goal": "{\"At(Coffee,Bar)\"}"}, "测试VLN：前往桌子": {"Answer": "测试VLN：前往桌子", "Goal": "{\"At(Robot,Table)\"}"}, "测试VLM：倒一杯水": {"Answer": "测试VLM：倒一杯水", "Goal": "{\"At(Water,WaterTable)\"}"}} + diff --git a/robowaiter/llm_client/data_raw/test_questions.csv b/robowaiter/llm_client/data_raw/test_questions.csv index 7523658..de611cc 100644 --- a/robowaiter/llm_client/data_raw/test_questions.csv +++ b/robowaiter/llm_client/data_raw/test_questions.csv @@ -2,3 +2,4 @@ Question,Answer,Goal VLMһ,VLMһ,"{""At(Coffee,Bar)""}" VLNǰ,VLNǰ,"{""At(Robot,Table)""}" AEM,AEM,"{""EnvExplored()""}" +VLMһˮ,VLMһˮ,"{""At(Water,WaterTable)""}" diff --git a/robowaiter/robot/robot.py b/robowaiter/robot/robot.py index 2e1253d..313e562 100644 --- a/robowaiter/robot/robot.py +++ b/robowaiter/robot/robot.py @@ -69,10 +69,14 @@ class Robot(object): action_list = [ Action(name='MakeCoffee()', pre={'At(Robot,CoffeeMachine)'}, add={'At(Coffee,Bar)'}, del_set=set(), cost=1), - Action(name='MoveTo(Table)', pre={'At(Robot,Bar)'}, + Action(name='MoveTo(Table)', pre={''}, add={'At(Robot,Table)'}, del_set=set(), cost=1), + Action(name='MoveTo(WaterTable)', pre={''}, + add={'At(Robot,WaterTable)'}, del_set=set(), cost=1), Action(name='ExploreEnv()', pre={'At(Robot,Bar)'}, add={'EnvExplored()'}, del_set=set(), cost=1), + Action(name='PourWater()', pre={'At(Robot,WaterTable)'}, + add={'At(Water,WaterTable)'}, del_set=set(), cost=1), ] return action_list diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index 89fd7fc..9b4c5cb 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -11,7 +11,8 @@ class SceneVLM(Scene): super().__init__(robot) # 在这里加入场景中发生的事件， (事件发生的时间，事件函数) self.event_list = [ - (5, self.create_chat_event("测试VLM：做一杯咖啡")), + # (5, self.create_chat_event("测试VLM：做一杯咖啡")), + (5, self.create_chat_event("测试VLM：倒一杯水")), ] def _reset(self): diff --git a/sub_task.ptml b/sub_task.ptml index adcea35..f534388 100644 --- a/sub_task.ptml +++ b/sub_task.ptml @@ -1,7 +1,11 @@ selector{ -cond At(Coffee,Bar) +cond At(Water,WaterTable) sequence{ -cond At(Robot,CoffeeMachine) -act MakeCoffee() +cond At(Robot,WaterTable) +act PourWater() +} +sequence{ +cond +act MoveTo(WaterTable) } } From 04b5c733558314f8e13ed1524004040fd62f69f0 Mon Sep 17 00:00:00 2001 From: ChenXL97 <908926798@qq.com> Date: Mon, 13 Nov 2023 21:52:15 +0800 Subject: [PATCH 6/7] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E5=AF=BC?= =?UTF-8?q?=E8=88=AA=E7=AE=97=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../algos/navigate/DstarLite/__init__.py | 1 - .../navigate/DstarLite/discretize_map.py | 59 +++++++++++++ .../algos/navigate/DstarLite/dstar_lite.py | 41 +++++----- .../algos/navigate/DstarLite/navigate.py | 82 ++++++++++--------- robowaiter/algos/navigate/DstarLite/readme.md | 56 ++++++++++++- robowaiter/algos/navigate/DstarLite/test.py | 29 ++++--- robowaiter/behavior_lib/act/Make.py | 1 + 7 files changed, 195 insertions(+), 74 deletions(-) create mode 100644 robowaiter/algos/navigate/DstarLite/discretize_map.py diff --git a/robowaiter/algos/navigate/DstarLite/__init__.py b/robowaiter/algos/navigate/DstarLite/__init__.py index 1ceb32a..2802065 100644 --- a/robowaiter/algos/navigate/DstarLite/__init__.py +++ b/robowaiter/algos/navigate/DstarLite/__init__.py @@ -1,4 +1,3 @@ - from . import navigate from . import dstar_lite diff --git a/robowaiter/algos/navigate/DstarLite/discretize_map.py b/robowaiter/algos/navigate/DstarLite/discretize_map.py new file mode 100644 index 0000000..f16dd92 --- /dev/null +++ b/robowaiter/algos/navigate/DstarLite/discretize_map.py @@ -0,0 +1,59 @@ +# !/usr/bin/env python3 +# -*- encoding: utf-8 -*- + + +import matplotlib.pyplot as plt +import numpy as np +import pickle +import os + + + + + +def draw_grid_map(grid_map): + # 生成新的地图图像 + plt.imshow(grid_map, cmap='binary', alpha=0.5, origin='lower') # 黑白网格 + + # 绘制坐标轴 + plt.xlabel('y', loc='right') + plt.ylabel('x', loc='top') + + # 显示网格线 + plt.grid(color='black', linestyle='-', linewidth=0.5) + + # 显示图像 + plt.show() + #plt.pause(0.01) + + + + +if __name__ == '__main__': + # control.init_world(scene_num=1, mapID=3) + # scene = control.Scene(sceneID=0) + # + # X = int(950/5) # 采点数量 + # Y = int(1850/5) + # map = np.zeros((X, Y)) + # + # for x in range(X): + # for y in range(Y): + # if not scene.reachable_check(x*5-350, y*5-400, Yaw=0): + # map[x, y] = 1 + # print(x, y) + # + # + # file_name = 'map_5.pkl' + # if not os.path.exists(file_name): + # open(file_name, 'w').close() + # with open(file_name, 'wb') as file: + # pickle.dump(map, file) + # print('保存成功') + + + file_name = 'map_5.pkl' + if os.path.exists(file_name): + with open(file_name, 'rb') as file: + map = pickle.load(file) + draw_grid_map(map) \ No newline at end of file diff --git a/robowaiter/algos/navigate/DstarLite/dstar_lite.py b/robowaiter/algos/navigate/DstarLite/dstar_lite.py index 6a83108..e685a58 100644 --- a/robowaiter/algos/navigate/DstarLite/dstar_lite.py +++ b/robowaiter/algos/navigate/DstarLite/dstar_lite.py @@ -23,7 +23,8 @@ def manhattan_distance(start, end): # 曼哈顿距离 def euclidean_distance(start, end): # 欧式距离 # return np.linalg.norm(start-end) - return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2) + # return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2) + return math.hypot(start[0] - end[0], start[1] - end[1]) def heuristic(start, end, name='euclidean'): @@ -115,9 +116,9 @@ class PriorityQueue: 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实际身位半径 + area_range, # [x_min, x_max, y_min, y_max] 实际坐标范围 + scale_ratio=5, # 地图缩放率 + dyna_obs_radius=30, # dyna_obs实际身位半径 ): # self.area_bounds = area @@ -128,7 +129,7 @@ class DStarLite: (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缩放后身位半径 + self.dyna_obs_radius = math.ceil(dyna_obs_radius / scale_ratio) # dyna_obs缩放后身位半径 # free:0, obs:1, dyna_obs:2 self.idx_to_object = { @@ -140,7 +141,7 @@ class DStarLite: self.object_to_cost = { "free": 0, "obstacle": float('inf'), - "dynamic obstacle": 50 + "dynamic obstacle": 100 } self.compute_cost_map() @@ -259,7 +260,7 @@ class DStarLite: 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): + def _planning(self, s_start, s_goal, dyna_obs, debug=False): ''' 规划路径(实际实现) Args: @@ -268,7 +269,7 @@ class DStarLite: ''' # 确保目标合法 if not self.in_bounds_without_obstacle(s_goal): - return None + return [] # 第一次规划需要初始化rhs并将goal加入队列，计算最短路径 if self.s_goal is None: self.s_start = tuple(s_start) @@ -281,7 +282,6 @@ class DStarLite: # 后续规划只更新起点，直接使用原路径(去掉已走过部分) 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: @@ -299,17 +299,16 @@ class DStarLite: # pass return self.path - def planning(self, s_start, s_goal, dyna_obs, step_num=None, debug=False): + def planning(self, s_start, s_goal, dyna_obs, 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]) + dyna_obs = [self.real2map(obs) for obs in dyna_obs] - self._planning(s_start, s_goal, dyna_obs, step_num, debug) + self._planning(s_start, s_goal, dyna_obs, debug) # 地图坐标->实际坐标 path = [self.map2real(node) for node in self.path] @@ -319,7 +318,7 @@ class DStarLite: ''' 得到路径 Args: - step_num: 路径步数 (None表示返回完整路径) + step_num: 路径步数 return: path: [(x, y), ...] ''' @@ -327,7 +326,6 @@ class DStarLite: 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])] @@ -411,10 +409,13 @@ class DStarLite: 根据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)) + # 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 = [(i, j) 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) + if euclidean_distance((i, j), obs_pos) < self.dyna_obs_radius] + occupy_pos = filter(self.in_bounds_without_obstacle, occupy_pos) # 确保位置在地图范围内 且 不是静态障碍物 return list(occupy_pos) @@ -507,4 +508,4 @@ class DStarLite: plt.xlabel('y', loc='right') plt.ylabel('x', loc='top') plt.grid(color='black', linestyle='-', linewidth=0.5) - plt.pause(0.3) + plt.pause(0.2) diff --git a/robowaiter/algos/navigate/DstarLite/navigate.py b/robowaiter/algos/navigate/DstarLite/navigate.py index 79cdd70..89597ce 100644 --- a/robowaiter/algos/navigate/DstarLite/navigate.py +++ b/robowaiter/algos/navigate/DstarLite/navigate.py @@ -1,71 +1,80 @@ #!/usr/bin/env python3 # -*- encoding: utf-8 -*- import math -import sys -import time +from dstar_lite import DStarLite, euclidean_distance -import matplotlib.pyplot as plt -import numpy as np -from mpl_toolkits.axes_grid1 import make_axes_locatable - - -from robowaiter.algos.navigate.DstarLite.dstar_lite import DStarLite class Navigator: ''' 导航类 ''' - def __init__(self, scene, area_range, map, scale_ratio=5): + def __init__(self, scene, area_range, map, scale_ratio=5, step_length=150, velocity=150, react_radius=250): 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.area_range = area_range # 地图实际坐标范围 xmin, xmax, ymin, ymax + self.map = map # 缩放并离散化的地图 array(X,Y) + self.scale_ratio = scale_ratio # 地图缩放率 + self.step_length = step_length # 步长(单次移动) self.step_num = self.step_length // self.scale_ratio # 单次移动地图格数 - self.v = 200 # 速度 - self.step_time = self.step_length/self.v + 0.1 # 单步移动时长 - + self.v = velocity # 速度 + self.react_radius = react_radius # robot反应半径 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): + def is_reached(pos: (float, float), goal: (float, float), dis_limit=50): ''' 判断是否到达目标 ''' - dis = np.linalg.norm(pos - goal) + dis = math.hypot(pos[0]-goal[0], pos[1]-goal[1]) + # dis = np.linalg.norm(pos - goal) return dis < dis_limit - def reset_goal(self, goal:(float, float)): - # TODO: 使目标可达 - # 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点 - # 目标在地图外面：起点和目标连线最靠近目标的可行点 - pass + @staticmethod + def get_yaw(pos: (float, float), goal: (float, float)): + ''' + 得到移动方向 + ''' + return math.degrees(math.atan2(goal[0] - pos[0], -(goal[1] - pos[1]))) + + def legalize_goal(self, goal: (float, float)): + ''' + TODO: 处理非法目标 + 目标在障碍物上：从目标开始方形向外扩展，直到找到可行点 + 目标在地图外面：起点和目标连线最靠近目标的可行点 + ''' + return goal def navigate(self, goal: (float, float), animation=True): ''' 单次导航，直到到达目标 ''' + if not self.scene.reachable_check(goal[0], goal[1], 0): + goal = self.legalize_goal(goal) - pos = np.array((self.scene.status.location.X, self.scene.status.location.Y)) # 机器人当前: 位置 和 朝向 - yaw = self.scene.status.rotation.Yaw + pos = (self.scene.status.location.X, self.scene.status.location.Y) # 机器人当前: 位置 和 朝向 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) + dyna_obs = [obs for obs in dyna_obs if euclidean_distance(obs, pos) < self.react_radius] # 过滤观测范围外的dyna_obs + # 周围有dyna_obs则步长减半 + if dyna_obs: + step_num = self.step_num // 2 + else: + step_num = self.step_num + path = self.planner.planning(pos, goal, dyna_obs) 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=' ') - scene_info = self.scene.walk_to(next_x, next_y, yaw, velocity=self.v) - yaw = scene_info.rotation.Yaw - 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)) + self.planner.draw_graph(step_num) # 画出搜索路径 + next_step = min(step_num, len(path)) + next_pos = path[next_step - 1] + print('plan pos:', next_pos, end=' ') + yaw = self.get_yaw(pos, next_pos) + self.scene.walk_to(next_pos[0], next_pos[1], yaw, velocity=self.v, dis_limit=10) + # pos = (self.scene.status.location.X, self.scene.status.location.Y) + # if self.is_reached(pos, next_pos): + self.planner.path = self.planner.path[next_step - 1:] # 去除已走过的路径 + pos = (self.scene.status.location.X, self.scene.status.location.Y) print('reach pos:', pos) self.planner.reset() # 完成一轮导航，重置变量 @@ -76,4 +85,3 @@ class Navigator: - diff --git a/robowaiter/algos/navigate/DstarLite/readme.md b/robowaiter/algos/navigate/DstarLite/readme.md index 3a77674..585e602 100644 --- a/robowaiter/algos/navigate/DstarLite/readme.md +++ b/robowaiter/algos/navigate/DstarLite/readme.md @@ -1,4 +1,52 @@ -### dstar_lite.py ——Dstar lite算法文件 -### navigate.py ——导航类 -### test.py ——测试文件 -### map_5.pkl ——离散化地图文件 \ No newline at end of file +## 默认使用RRTStar+路径平滑 + +### apf.py: 势场法实现 + +### discretize_map.py: 地图离散化并压缩 + +### map_5.pkl: 地图文件(5倍压缩) + +### navigate.py: 导航类 + +### pathsmoothing.py: 路径平滑 + +### rrt.py: RRT实现 + +### rrt_star.py: RRTStar 实现 + +### test.py: 测试文件 + + + + +## TODO + +### 目标不合法 +#### 初始目标不合法 +目标在障碍物上：从目标开始方形向外扩展，直到找到可行点 +目标在地图外面：起点和目标连线最靠近目标的可行点 +对不合法的目标做单独处理，生成新的目标 + +#### 在移动过程中目标被占据 +给出目标但不会行移动，程序会继续运行，重新计算规划路径，给出新目标 + + +### 规划中断情况 + + +### 计算转向角 +`完成` + + +### 有些本来可达的位置却无法走到（系统bug?） +例如从初始位置(247, 500) 移动到(115, -10)，无法到达 +`(已解决)将dis_limit设为小值5而非0` +#### 构型空间膨胀？？ +`不需要` + +### 只考虑一定范围内的行人 +观测范围 / 反应半径 + +`完成` +#### 观测范围内有行人步长要减小 +`完成` diff --git a/robowaiter/algos/navigate/DstarLite/test.py b/robowaiter/algos/navigate/DstarLite/test.py index 2f0e4ed..525ad10 100644 --- a/robowaiter/algos/navigate/DstarLite/test.py +++ b/robowaiter/algos/navigate/DstarLite/test.py @@ -2,11 +2,12 @@ import os import pickle import time import random +import math import matplotlib.pyplot as plt import numpy as np -from robowaiter.scene.scene import Scene,init_world # TODO: 文件名改成Scene.py +from robowaiter.scene import scene from navigate import Navigator @@ -19,14 +20,13 @@ if __name__ == '__main__': with open(file_name, 'rb') as file: map = pickle.load(file) - init_world(1, 11) - scene = Scene(sceneID=0) + scene.init_world(1, 11) + scene = scene.Scene(sceneID=0) - navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map) + navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map) '''场景1: 无行人环境 robot到达指定目标''' - goal = np.array((-100, 700)) - + # goal = np.array((-100, 700)) '''场景2: 静止行人环境 robot到达指定目标''' # scene.clean_walker() @@ -35,12 +35,17 @@ if __name__ == '__main__': # 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)) + scene.walk_to(100, 0, -90, dis_limit=10) + scene.clean_walker() + scene.add_walker(50, 300, 0) + scene.add_walker(-50, 500, 0) + scene.add_walker(0, 700, 0) + scene.control_walker([scene.walker_control_generator(walkerID=0, autowalk=False, speed=50, X=-50, Y=600, Yaw=0)]) + scene.control_walker([scene.walker_control_generator(walkerID=1, autowalk=False, speed=50, X=100, Y=150, Yaw=0)]) + scene.control_walker([scene.walker_control_generator(walkerID=2, autowalk=False, speed=50, X=0, Y=0, Yaw=0)]) + + goal = (-100, 700) + # goal = (-300) '''场景4: 行人自由移动 robot到达指定目标''' # # TODO: autowalk=True仿真器会闪退 ??? diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 8388cc2..3c8fac3 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -23,6 +23,7 @@ class Make(Act): "add": {f'On(Coffee,Table)'}, } return info + def _update(self) -> ptree.common.Status: op_type = 1 self.scene.move_task_area(op_type) From 0284830b0acdc6f55133fe1c4be3ec284ff9b63f Mon Sep 17 00:00:00 2001 From: Caiyishuai <39987654+Caiyishuai@users.noreply.github.com> Date: Mon, 13 Nov 2023 22:14:55 +0800 Subject: [PATCH 7/7] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E9=83=A8?= =?UTF-8?q?=E5=88=86=E5=8A=A8=E4=BD=9C=E8=8A=82=E7=82=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robowaiter/behavior_lib/_base/Act.py | 4 ++++ robowaiter/behavior_lib/_base/Behavior.py | 1 + robowaiter/behavior_lib/act/Make.py | 17 +++++++++++------ robowaiter/behavior_lib/act/MakeCoffee.py | 23 ----------------------- robowaiter/behavior_lib/act/MoveTo.py | 12 +++++------- robowaiter/behavior_lib/act/PourWater.py | 23 ----------------------- robowaiter/scene/scene.py | 5 +++-- robowaiter/scene/tasks/VLM.py | 10 +++++----- 8 files changed, 29 insertions(+), 66 deletions(-) delete mode 100644 robowaiter/behavior_lib/act/MakeCoffee.py delete mode 100644 robowaiter/behavior_lib/act/PourWater.py diff --git a/robowaiter/behavior_lib/_base/Act.py b/robowaiter/behavior_lib/_base/Act.py index ee948d5..d2e95e5 100644 --- a/robowaiter/behavior_lib/_base/Act.py +++ b/robowaiter/behavior_lib/_base/Act.py @@ -3,9 +3,13 @@ from robowaiter.behavior_lib._base.Behavior import Bahavior class Act(Bahavior): print_name_prefix = "act " type = 'Act' + all_place = {'Bar', 'WaterTable', 'CoffeeTable', 'Bar2', 'Table1', 'Table2', 'Table3'} + all_object = {'Coffee', 'Water', 'Dessert', 'Softdrink', 'BottledDrink', 'Yogurt', 'ADMilk', 'MilkDrink', 'Milk', + 'VacuumCup'} def __init__(self,*args): super().__init__(*args) + self.info = self.get_info(*args) @classmethod def get_info(self,*arg): diff --git a/robowaiter/behavior_lib/_base/Behavior.py b/robowaiter/behavior_lib/_base/Behavior.py index 84ed74b..b80c634 100644 --- a/robowaiter/behavior_lib/_base/Behavior.py +++ b/robowaiter/behavior_lib/_base/Behavior.py @@ -35,6 +35,7 @@ class Bahavior(ptree.behaviour.Behaviour): # self.valid_arg_list.append((x.strip for x in line.split(","))) self.args = args + super().__init__(self.name) def _update(self) -> ptree.common.Status: diff --git a/robowaiter/behavior_lib/act/Make.py b/robowaiter/behavior_lib/act/Make.py index 8388cc2..ecb99f6 100644 --- a/robowaiter/behavior_lib/act/Make.py +++ b/robowaiter/behavior_lib/act/Make.py @@ -7,7 +7,7 @@ class Make(Act): can_be_expanded = True num_args = 1 valid_args = ( - "Coffee", + "Coffee","Water","Dessert" ) def __init__(self, *args): @@ -17,15 +17,20 @@ class Make(Act): @classmethod def get_info(cls,arg): - info = None + info = {} + info["pre"]= {f'Holding(Nothing)'} + info['del'] = set() if arg == "Coffee": - info = { - "add": {f'On(Coffee,Table)'}, - } + info["add"]= {f'On(Coffee,CoffeeTable)'} + elif arg == "Water": + info["add"] = {f'On(Water,WaterTable)'} + elif arg == "Dessert": + info["add"] = {f'On(Dessert,Bar)'} return info def _update(self) -> ptree.common.Status: op_type = 1 self.scene.move_task_area(op_type) self.scene.op_task_execute(op_type) - self.scene.state["condition_set"].add(self.add) + self.scene.state["condition_set"].union(self.info["add"]) + self.scene.state["condition_set"] -= self.info["del"] return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/MakeCoffee.py b/robowaiter/behavior_lib/act/MakeCoffee.py deleted file mode 100644 index b14727f..0000000 --- a/robowaiter/behavior_lib/act/MakeCoffee.py +++ /dev/null @@ -1,23 +0,0 @@ -import py_trees as ptree -from typing import Any -from robowaiter.behavior_lib._base.Act import Act -from robowaiter.behavior_lib._base.Behavior import Status - -class MakeCoffee(Act): - - def __init__(self, *args): - super().__init__(*args) - - @property - def cond_sets(self): - pre = {"At(Robot,CoffeeMachine)","NotHolding"} - add = {"At(Coffee,Bar)"} - de = {} - return pre,add,de - - def _update(self) -> ptree.common.Status: - op_type = 1 - self.scene.move_task_area(op_type) - self.scene.op_task_execute(op_type) - self.scene.state["condition_set"].add(self.add) - return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/behavior_lib/act/MoveTo.py b/robowaiter/behavior_lib/act/MoveTo.py index 000869d..2a1a78a 100644 --- a/robowaiter/behavior_lib/act/MoveTo.py +++ b/robowaiter/behavior_lib/act/MoveTo.py @@ -6,10 +6,8 @@ from robowaiter.algos.navigate.DstarLite.navigate import Navigator class MoveTo(Act): can_be_expanded = True num_args = 1 - valid_args = ( - "Bar", - "Table", - ) + valid_args = Act.all_object | Act.all_place + valid_args.add('Customer') def __init__(self, target_place): super().__init__(target_place) @@ -18,9 +16,9 @@ class MoveTo(Act): @classmethod def get_info(self,arg): - info = { - "add": {f'At(Robot,{arg})'}, - } + info = {} + info["add"] = {f'At(Robot,{arg})'} + info["del"] = {f'At(Robot,{place})' for place in self.valid_args if place != arg} return info diff --git a/robowaiter/behavior_lib/act/PourWater.py b/robowaiter/behavior_lib/act/PourWater.py deleted file mode 100644 index 23db1fe..0000000 --- a/robowaiter/behavior_lib/act/PourWater.py +++ /dev/null @@ -1,23 +0,0 @@ -import py_trees as ptree -from typing import Any -from robowaiter.behavior_lib._base.Act import Act -from robowaiter.behavior_lib._base.Behavior import Status - -class PourWater(Act): - - def __init__(self, *args): - super().__init__(*args) - - @property - def cond_sets(self): - self.pre = {"At(Robot,WaterTable)","NotHolding"} - self.add = {"On(Water,WaterTable)"} - self.de = {} - return self.pre,self.add,self.de - - def _update(self) -> ptree.common.Status: - op_type = 2 - self.scene.move_task_area(op_type) - self.scene.op_task_execute(op_type) - self.scene.state["condition_set"].update(self.add) - return Status.RUNNING \ No newline at end of file diff --git a/robowaiter/scene/scene.py b/robowaiter/scene/scene.py index b3819b1..b0db6e9 100644 --- a/robowaiter/scene/scene.py +++ b/robowaiter/scene/scene.py @@ -409,10 +409,11 @@ class Scene: return True def gen_obj(self,h=100): - # 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 + # 4;冰红(盒) 5;酸奶 7:保温杯 9;冰红(瓶) 13:代语词典 14:cake 61:甜牛奶 + type= 9 #9 scene = stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z] - obj_list = [GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z = h, roll=0, pitch=0, yaw=0, type=9)] + obj_list = [GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 50, y=ginger_loc[1] - 40, z = h, roll=0, pitch=0, yaw=0, type=type)] scene = stub.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=self.sceneID)) time.sleep(1.0) diff --git a/robowaiter/scene/tasks/VLM.py b/robowaiter/scene/tasks/VLM.py index 9b4c5cb..d8e646f 100644 --- a/robowaiter/scene/tasks/VLM.py +++ b/robowaiter/scene/tasks/VLM.py @@ -11,14 +11,14 @@ class SceneVLM(Scene): super().__init__(robot) # 在这里加入场景中发生的事件， (事件发生的时间，事件函数) self.event_list = [ - # (5, self.create_chat_event("测试VLM：做一杯咖啡")), - (5, self.create_chat_event("测试VLM：倒一杯水")), + (5, self.create_chat_event("测试VLM：做一杯咖啡")), + # (5, self.create_chat_event("测试VLM：倒一杯水")), ] def _reset(self): pass - def _run(self, op_type=2): + def _run(self, op_type=7): # 共17个操作 # "制作咖啡","倒水","夹点心","拖地","擦桌子","开筒灯","搬椅子", # 1-7 # "关筒灯","开大厅灯","关大厅灯","关闭窗帘","打开窗帘", # 8-12 @@ -30,8 +30,8 @@ class SceneVLM(Scene): # self.gen_obj() # self.op_task_execute(op_type, obj_id=0) # # 原始吧台处:[247.0, 520.0, 100.0], 空调开关旁吧台:[240.0, 40.0, 70.0], 水杯桌:[-70.0, 500.0, 107] - # # 桌子1:[-55.0, 0.0, 107],桌子1:[-55.0, 150.0, 107] - # elif op_type == 17: self.op_task_execute(op_type, release_pos=[-55.0, 150.0, 107]) + # # 桌子1:[-55.0, 0.0, 107],桌子2:[-55.0, 150.0, 107] + # elif op_type == 17: self.op_task_execute(op_type, release_pos=[247.0, 520.0, 100.0])#[-55.0, 150.0, 107] # else: # self.move_task_area(op_type) # self.op_task_execute(op_type)