feat: genAI urdf parser

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

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.")

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: