From 373ac38bd4024b01687a76208959f8a181c9378e Mon Sep 17 00:00:00 2001 From: Maciej Majek Date: Tue, 1 Oct 2024 15:05:08 +0200 Subject: [PATCH 1/3] feat: implement urdf parsing in rai_cli --- src/rai/rai/cli/rai_cli.py | 43 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/src/rai/rai/cli/rai_cli.py b/src/rai/rai/cli/rai_cli.py index a185fab1..e302ca0c 100644 --- a/src/rai/rai/cli/rai_cli.py +++ b/src/rai/rai/cli/rai_cli.py @@ -21,7 +21,7 @@ import coloredlogs from langchain_community.vectorstores import FAISS -from langchain_core.messages import SystemMessage +from langchain_core.messages import HumanMessage, SystemMessage from rai.apps.talk_to_docs import ingest_documentation from rai.messages import preprocess_image @@ -35,7 +35,7 @@ def parse_whoami_package(): parser = argparse.ArgumentParser( - description="Parse robot whoami package. Script builds a vector store and creates a robot identity." + description="Parse robot whoami package. Script builds a vector store, creates a robot identity and a URDF description." ) parser.add_argument( "documentation_root", type=str, help="Path to the root of the documentation" @@ -53,6 +53,33 @@ def parse_whoami_package(): llm = get_llm_model(model_type="simple_model") embeddings_model = get_embeddings_model() + def calculate_urdf_tokens(): + combined_urdf = "" + xacro_files = glob.glob(args.documentation_root + "/urdf/*.xacro") + for xacro_file in xacro_files: + combined_urdf += f"# {xacro_file}\n" + combined_urdf += open(xacro_file, "r").read() + combined_urdf += "\n\n" + return len(combined_urdf) / 4 + + def build_urdf_description(): + logger.info("Building the URDF description...") + combined_urdf = "" + xacro_files = glob.glob(args.documentation_root + "/urdf/*.xacro") + for xacro_file in xacro_files: + combined_urdf += f"# {xacro_file}\n" + combined_urdf += open(xacro_file, "r").read() + combined_urdf += "\n\n" + + prompt = "You will be given a URDF file. Your task is to create a short and detailed description of links and joints. " + parsed_urdf = llm.invoke( + [SystemMessage(content=prompt), HumanMessage(content=str(combined_urdf))] + ).content + + with open(save_dir + "/robot_description.urdf.txt", "w") as f: + f.write(parsed_urdf) + logger.info("Done") + def build_docs_vector_store(): logger.info("Building the robot docs vector store...") faiss_index = FAISS.from_documents(docs, embeddings_model) @@ -115,6 +142,18 @@ def build_robot_identity(): f"You can do it manually by creating {save_dir}/robot_identity.txt" ) + logger.info( + f"Building the URDF description. The urdf's length is {calculate_urdf_tokens()} tokens" + ) + logger.warn("Do you want to continue? (y/n)") + if input() == "y": + build_urdf_description() + else: + logger.info( + f"Skipping the URDF description creation. " + f"You can do it manually by creating {save_dir}/robot_description.urdf.txt" + ) + def create_rai_ws(): parser = argparse.ArgumentParser(description="Creation of a robot package.") From 4cea9e9aaf5f01c4a621ad8a33d280155f8c1809 Mon Sep 17 00:00:00 2001 From: Maciej Majek Date: Tue, 1 Oct 2024 15:05:44 +0200 Subject: [PATCH 2/3] feat: serve urdf description in rai_whoami --- src/rai_whoami/rai_whoami/rai_whoami_node.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/rai_whoami/rai_whoami/rai_whoami_node.py b/src/rai_whoami/rai_whoami/rai_whoami_node.py index c82db088..ca5de21a 100644 --- a/src/rai_whoami/rai_whoami/rai_whoami_node.py +++ b/src/rai_whoami/rai_whoami/rai_whoami_node.py @@ -53,6 +53,11 @@ def __init__(self): "rai_whoami_identity_service", self.get_identity_callback, ) + self.srv = self.create_service( + Trigger, + "rai_whoami_urdf_service", + self.get_urdf_callback, + ) # parse robot_description_package path self.robot_description_package = ( @@ -84,6 +89,21 @@ def _load_documentation(self) -> FAISS: ) return faiss_index + def get_urdf_callback( + self, request: Trigger_Request, response: Trigger_Response + ) -> Trigger_Response: + """Return URDF description""" + urdf_path = ( + get_package_share_directory(self.robot_description_package) + + "/description/robot_description.urdf.txt" + ) + with open(urdf_path, "r") as f: + urdf = f.read() + response.message = urdf + response.success = True + self.get_logger().info("Incoming request for URDF description, responding") + return response + def get_constitution_callback( self, request: Trigger_Request, response: Trigger_Response ) -> Trigger_Response: From 3f3f9987cef55bcc8e9895b28cb5be88a6ca507a Mon Sep 17 00:00:00 2001 From: Maciej Majek Date: Tue, 1 Oct 2024 15:06:11 +0200 Subject: [PATCH 3/3] docs: update info about new feature --- docs/create_robots_whoami.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/create_robots_whoami.md b/docs/create_robots_whoami.md index dfe9f1f4..08a2946f 100644 --- a/docs/create_robots_whoami.md +++ b/docs/create_robots_whoami.md @@ -19,6 +19,7 @@ Your robot's `whoami` package serves as a configuration package for the `rai_who 3. Fill in the `src/examples/panda_whoami/description` folder with data:\ 2.1 Save [this image](https://robodk.com/robot/img/Franka-Emika-Panda-robot.png) into `src/examples/panda_whoami/description/images`\ 2.2 Save [this document](https://github.com/user-attachments/files/16417196/Franka.Emika.Panda.robot.-.RoboDK.pdf) in `src/examples/panda_whoami/description/documentation` + 2.3 Save [this urdf](https://github.com/frankaemika/franka_ros/blob/develop/franka_description/robots/panda/panda.urdf.xacro) in `src/examples/panda_whoami/description/urdf` 4. Run the `parse_whoami_package`. This will process the documentation, building it into a vector database, which is used by RAI agent to reason about its identity. @@ -46,6 +47,7 @@ ros2 run rai_whoami rai_whoami_node --ros-args -p robot_description_package:="pa ros2 service call /rai_whoami_identity_service std_srvs/srv/Trigger # ask for identity ros2 service call /rai_whoami_selfimages_service std_srvs/srv/Trigger # ask for images folder ros2 service call /rai_whoami_constitution_service std_srvs/srv/Trigger # ask for robot constitution +ros2 service call /rai_whoami_urdf_service std_srvs/srv/Trigger # ask for urdf description ros2 service call /rai_whoami_documentation_service rai_interfaces/srv/VectorStoreRetrieval "query: 'maximum load'" # ask for Panda's maximum load ```