added foundationpose folder and llm folder

Author: LuigiPenco93

ihmc-interfaces/ros2ws/README.md

@@ -17,7 +17,7 @@ Ensure you've set `ROS_DOMAIN_ID` equal to `RTPSDomainID` in `~/.ihmc/IHMCNetwor
 you have not created or edited this file, run `NetworkParametersCreator` and set `RTPSDomainID` and
 `RTPSSubnet`.
 ```
-$ export ROS_DOMAIN_ID=99 # Change to the domain ID you've set in IHMCNetworkParameters.ini
+$ export ROS_DOMAIN_ID=32 # Change to the domain ID you've set in IHMCNetworkParameters.ini
 ```
 
 ## Running examples
@@ -39,4 +39,4 @@ You can also test the scene graph only by running
 ```
 $ python3 scene_graph_example.py
 ```
-Observe that some scene nodes are added in the scene graph and in the scene and removed.
+Observe that some scene nodes are added in the scene graph and in the scene and removed.

ihmc-interfaces/ros2ws/__pycache__/llm_interface.cpython-310.pyc

@@ -0,0 +1,165 @@
+import os
+import time
+import threading
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Quaternion
+from behavior_msgs.msg import AI2RCommandMessage
+from behavior_msgs.msg import AI2RObjectMessage
+from behavior_msgs.msg import AI2RStatusMessage
+from behavior_msgs.msg import AI2RNavigationMessage
+from behavior_msgs.msg import AI2RReceiveObjectMessage
+
+import cv2
+import numpy as np
+from typing import List
+import json
+
+ros2 = {}
+initialized = False
+loggedFailure = False
+
+behavior_counter = 0
+behaviors_baseline = ["SCAN", "GOTO", "RECEIVE OBJECT", "GOTO", "PLACE CHARGE ON DOOR", "GOTO"]
+
+goto_person_param = ("Person1", AI2RNavigationMessage.DEFAULT, "")
+goto_door_param = ("DoorPanel1", AI2RNavigationMessage.FRONT, "")
+goto_barrier_param = ("Barrier1", AI2RNavigationMessage.BEHIND, "")
+receive_charge_param = ("Charge1",)
+parameters = [None, goto_person_param, receive_charge_param, goto_door_param, None, goto_barrier_param]
+
+def behavior_message_callback(msg):
+    global initialized  # Access the global variables
+    global loggedFailure
+    global behavior_counter
+    global behaviors_baseline
+    global parameters
+    robot_pose = msg.robot_mid_feet_under_pelvis_pose_in_world
+
+    if not initialized:
+        # --------- Scene -----------
+        scene_objects = msg.objects
+        print("Objects in the scene:")
+        if scene_objects:  # This checks if the list is not empty
+           for obj in scene_objects:
+               id = obj.object_name
+               print(f"{id}")
+               pose_in_world = obj.object_pose_in_world
+               pose_wrt_robot = obj.object_pose_in_robot_frame # This is the pose specified wrt to robot_pose
+        else:
+           print("-")
+
+        # --------- Behaviors -----------
+        behaviors = msg.available_behaviors
+        print("Available behaviors:")
+        if behaviors:
+            for behavior in behaviors:
+                print(behavior)
+        else:
+            print("-")
+
+    # --------- Monitoring -----------
+    #if (msg.completed_behavior != "-"):
+    print("Completed Behavior: " , msg.completed_behavior, " behavior_in_progress: " , msg.behavior_in_progress)
+    failed_behavior = msg.failed_behavior
+    failure = msg.failure
+    if failed_behavior != "-" and loggedFailure == False:
+        print("[FAILURE] -----------")
+        print("Failed behavior: " + failed_behavior)
+
+        failure_info = {
+            "Failed behavior": failed_behavior,
+            "Description": failure.action_name,
+            "Type": failure.action_type
+        }
+
+        if failure.missing_frame:
+            failure_info["Missing Frame"] = failure.reference_frame
+        if failure.collision_name != "-":
+            failure_info["Collision with"] = failure.collision_name
+        position_error = failure.position_error
+        error_vector = np.array([position_error.x, position_error.y, position_error.z])
+        norm = np.linalg.norm(error_vector)
+        if norm > failure.position_tolerance:
+            failure_info["Position error"] = norm
+
+        json_filename = 'failure_info.json'
+        with open(json_filename, 'a') as json_file:
+            json.dump(failure_info, json_file, indent=4)
+        loggedFailure = True
+
+    # --------- Coordination -----------
+    waiting_for_command = False
+    if msg.behavior_in_progress == "-":
+       waiting_for_command  = True
+
+    if waiting_for_command or not initialized:
+        behavior_command = AI2RCommandMessage()
+        # DECIDE what behavior to execute based on reasoning. For example can decide to scan environment to detect objects
+        behavior_command.behavior_to_execute = behaviors_baseline[behavior_counter]
+        behavior_command.adapting_behavior = False
+
+        if (behavior_command.behavior_to_execute == "GOTO"):
+            behavior_command.adapting_behavior = True
+            new_goto_behavior = AI2RNavigationMessage()
+            goto_parameters = parameters[behavior_counter]
+
+            # Set the reference frame name - can copy from scene_objects.obj_name
+            new_goto_behavior.target_object = goto_parameters[0]
+            # Set the distance to the object
+            new_goto_behavior.distance_to_object = 1.0
+            new_goto_behavior.pov_object = goto_parameters[2]
+            new_goto_behavior.spatial_relation = goto_parameters[1]
+            if new_goto_behavior.spatial_relation == AI2RNavigationMessage.DEFAULT or goto_parameters[2] == "":
+                new_goto_behavior.pov_object = "walkingFrame"
+
+            behavior_command.navigation = new_goto_behavior
+
+        if (behavior_command.behavior_to_execute == "RECEIVE OBJECT"):
+            behavior_command.adapting_behavior = True
+            new_receive_behavior = AI2RReceiveObjectMessage()
+            new_receive_behavior.object_name = parameters[behavior_counter][0]
+            new_receive_behavior.side =  bytes([1])
+
+            behavior_command.receive_object = new_receive_behavior
+
+        print("Commanded Behavior: " + behavior_command.behavior_to_execute)
+        ros2["behavior_publisher"].publish(behavior_command)
+        initialized = True
+        loggedFailure = False
+        behavior_counter += 1
+        print("Completed Behavior: " , msg.completed_behavior, " behavior_in_progress: " , msg.behavior_in_progress)
+
+def main(args=None):
+    rclpy.init(args=args)
+    
+    qos_profile_reliable = QoSProfile(
+        reliability=QoSReliabilityPolicy.BEST_EFFORT,
+        history=QoSHistoryPolicy.KEEP_LAST,
+        depth=1
+    )
+
+    node = rclpy.create_node('behavior_coordination_node')
+    ros2["node"] = node
+
+    behavior_subscriber = node.create_subscription(AI2RStatusMessage,
+                                            '/ihmc/behavior_tree/ai2r_status',
+                                            behavior_message_callback, qos_profile_reliable)
+    ros2["behavior_subscriber"] = behavior_subscriber
+
+    behavior_publisher = node.create_publisher(AI2RCommandMessage,
+                                    '/ihmc/behavior_tree/ai2r_command',
+                                    qos_profile_reliable)
+    ros2["behavior_publisher"] = behavior_publisher
+
+    # Wait until interrupt
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.destroy_node()
+
+if __name__ == '__main__':
+    main()

ihmc-interfaces/ros2ws/behavior_coordination_test.py

@@ -15,6 +15,14 @@
 
 import cv2
 import numpy as np
+from llm.llm_interface import LLMInterface
+
+# Example usage:
+print(" Calling the LLM")
+llm = LLMInterface(config_file="llm/config.json")
+
+
+
 
 ros2 = {}
 
@@ -45,17 +53,34 @@ def behavior_message_callback(msg):
 
    # --------- Reasoning -----------
    # CAN DO SOME REASONING HERE based on objects in the scene and available behaviors
+   behaviors_str = str(behaviors)
+#    messages = [
+#    {
+#        "role": "assistant",
+#        "content": behaviors_str
+#    },
+#    {
+#        "role": "user",
+#        "content": "Summarize all the behaviors in one paragraph"
+#    }
+#    ]
+#    answer = llama32(messages)
+   response = llm.call_model(behaviors_str)
+   print("Summarized behaviors: ",response)
 
    # --------- Monitoring -----------
    completed_behavior = msg.completed_behavior
+   print("Completed Behavior: " + completed_behavior)
    failed_behavior = msg.failed_behavior
    if failed_behavior:
-       print("[FAILURE] Failed behavior: " + failure_behavior)
+       print("[FAILURE] Failed behavior: " + failed_behavior)
        # Failure details
        failure = msg.failure
        print("Description: " + failure.action_name)
        print("Type: " + failure.action_type)
        print("Frame: " + failure.action_frame)
+       print("Missing Frame: " + failure.missing_frame)
+       print("Navigation Collision Frame Name: " + failure.collision_name)
 
        position_error = failure.position_error
        # Convert Point to numpy array
@@ -74,24 +99,22 @@ def behavior_message_callback(msg):
    # --------- Coordination / Adaptation -----------
    behavior_command = AI2RCommandMessage()
    # DECIDE what behavior to execute based on reasoning. For example can decide to navigate to a specific object
-   behavior_command.behavior_to_execute = "goto"
+   behavior_command.behavior_to_execute = "GOTO"
 
    # Update the go to behavior to navigate to whatever object or whenever in space according to reasoning
    new_goto_behavior = AI2RNavigationMessage()
    # Set the reference frame name - can copy from scene_objects.obj_name
-   new_goto_behavior.reference_frame_name = "your_reference_frame"
-   # Set the goal stance point - where the robot stance is positioned (position only, no orientation) wrt to the reference_frame_name
-   new_goto_behavior.goal_stance_point = Point(x=1.0, y=2.0, z=0.0)
-   # Set the goal focal point - where the stance is facing (how it is oriented) wrt to the reference_frame_name
-   new_goto_behavior.goal_focal_point = Point(x=3.0, y=4.0, z=0.0)
+   new_goto_behavior.reference_frame_name = "Charge1"
+   # Set the distance to the object
+   new_goto_behavior.distance_to_frame = 0.6
    behavior_command.navigation = new_goto_behavior
 
    # CAN EDIT HAND POSE ACTION, IF failed behavior has failed because of that action
    new_hand_pose_action = AI2RHandPoseAdaptationMessage()
    # Set the name of the action. COPY THAT from failed_behavior message
    new_hand_pose_action.action_name = "action_to_modify"
    # Set the reference frame name - can copy from scene_objects.obj_name
-   new_hand_pose_action.reference_frame_name = "your_reference_frame"
+   new_hand_pose_action.reference_frame_name = "Barrier1"
    # Set the new position
    new_hand_pose_action.new_position = Point(x=1.0, y=2.0, z=3.0)
    # Set the new orientation
@@ -105,7 +128,7 @@ def main(args=None):
     rclpy.init(args=args)
 
     qos_profile_reliable = QoSProfile(
-        reliability=QoSReliabilityPolicy.RELIABLE,
+        reliability=QoSReliabilityPolicy.BEST_EFFORT,
         history=QoSHistoryPolicy.KEEP_LAST,
         depth=1
     )