Skip to content

Latest commit

 

History

History
819 lines (625 loc) · 34.7 KB

airsim_objectnavigation.md

File metadata and controls

819 lines (625 loc) · 34.7 KB
Imagine you are helping me interact with the AirSim simulator. We are controlling an embodied agent. At any given point of time, you have the following abilities. You are also required to output code for some of the requests.

Question - Ask me a clarification question Reason - Explain why you did something the way you did it. Code - Output a code command that achieves the desired goal.

The scene consists of several objects. We have access to the following functions, please use only these functions as much as possible:

Perception:

get_image() : Renders an image from the front facing camera of the agent detect_objects(img): Runs an object detection model on an image img, and returns two variables - obj_list, which is a list of the names of objects detected in the scene. obj_locs, a list of bounding box coordinates in the image for each object.

Action:

forward(): Move forward by 0.1 meters. turn_left(): Turn left by 90 degrees. turn_right(): Turn right by 90 degrees.

You are not to use any other hypothetical functions. You can use functions from Python libraries such as math, numpy etc. Are you ready?


Yes, I am ready. Please let me know how I can assist you.


I need you to help me find an object. Objects might be scattered around the scene, so if you don't find it right away, a good strategy is to turn around a few times to see if it becomes visible. Give me code to explore and find the bottle.


Sure, I'd be happy to help! Before we get started, can you confirm whether the object detection model has already been loaded, or do we need to load it first?


Assume it has already been loaded. Please use the functions I listed above.


Got it. Here's some code that will help you explore the environment and find the bottle:

import math

def search_for_bottle():
num_turns = 4 # Turn around 4 times to look for the bottle
for i in range(num_turns): # Turn left 90 degrees to face a new direction
turn_left()

        # Take a picture and detect objects in it
        img = get_image()
        obj_list, obj_locs = detect_objects(img)

        # Check if the bottle is in the list of detected objects
        if 'bottle' in obj_list:
            # Get the coordinates of the bottle in the image
            bottle_idx = obj_list.index('bottle')
            bbox = obj_locs[bottle_idx]

            # Compute the center of the bounding box
            center_x = (bbox[0] + bbox[2]) / 2
            center_y = (bbox[1] + bbox[3]) / 2

            # Print the center coordinates of the bottle
            print(f"Found the bottle at ({center_x}, {center_y})")

            # Exit the function, as we have found the bottle
            return

    # If we reach this point, we were unable to find the bottle
    print("Unable to find the bottle.")

This function turns the agent around in a circle, taking a picture and detecting objects in each direction. If the bottle is detected, it prints the center coordinates of the bottle and exits the function. If the bottle is not found after turning in a full circle, the function prints a message saying that it was unable to find the bottle.

You can call this function by running search_for_bottle(). Let me know if you have any questions!


Great! Now let's say we did find the bottle. Now I want to move towards it. The only information we have is the location of the bottle bounding box in the image. Can you give me code to make this happen?


Sure thing! Here's some code to move the agent towards the center of the bounding box of the bottle:

def move_to_bottle(): # Take a picture and detect objects in it
img = get_image()
obj_list, obj_locs = detect_objects(img)

    # Check if the bottle is in the list of detected objects
    if 'bottle' in obj_list:
        # Get the coordinates of the bottle in the image
        bottle_idx = obj_list.index('bottle')
        bbox = obj_locs[bottle_idx]

        # Compute the center of the bounding box
        center_x = (bbox[0] + bbox[2]) / 2
        center_y = (bbox[1] + bbox[3]) / 2

        # Compute the distance and angle to the center of the bounding box
        img_width, img_height = img.size
        angle = math.atan2(center_x - img_width / 2, img_height / 2 - center_y)
        distance = (center_x - img_width / 2) / (img_width / 2)

        # Print the distance and angle to the center of the bounding box
        print(f"Moving towards bottle, distance: {distance}, angle: {angle}")

        # Move the agent towards the center of the bounding box
        if abs(distance) > 0.05:
            if distance > 0:
                # Bottle is to the right, so turn right and move forward
                turn_right()
                forward()
            else:
                # Bottle is to the left, so turn left and move forward
                turn_left()
                forward()

        # Rotate the agent to face the bottle
        if abs(angle) > 0.05:
            if angle > 0:
                # Bottle is to the right, so turn right
                turn_right()
            else:
                # Bottle is to the left, so turn left
                turn_left()
    else:
        print("Bottle not found in image.")

This function first detects the bottle in the image and computes the center coordinates of its bounding box. It then computes the angle and distance between the center of the bounding box and the center of the image. Based on these values, the agent will turn left or right and move forward to move towards the center of the bounding box. Once it is close enough, it will stop moving and rotate to face the bottle.

You can call this function by running move_to_bottle(). Let me know if you have any questions!


**Thank you. Can you combine both pieces of code and give me one script, which will explore to find the object of interest (bottle for example but can be anything), and then also move the agent towards it? **


Certainly! Here's a script that combines the two functions:

import math

def search_for_bottle_and_move_to_it(): # Turn around in a circle to find the bottle
num_turns = 4
for i in range(num_turns): # Turn left 90 degrees to face a new direction
turn_left()

        # Take a picture and detect objects in it
        img = get_image()
        obj_list, obj_locs = detect_objects(img)

        # Check if the bottle is in the list of detected objects
        if 'bottle' in obj_list:
            # Get the coordinates of the bottle in the image
            bottle_idx = obj_list.index('bottle')
            bbox = obj_locs[bottle_idx]

            # Compute the center of the bounding box
            center_x = (bbox[0] + bbox[2]) / 2
            center_y = (bbox[1] + bbox[3]) / 2

            # Print the center coordinates of the bottle
            print(f"Found the bottle at ({center_x}, {center_y})")

            # Move towards the bottle
            while True:
                # Take a picture and detect objects in it
                img = get_image()
                obj_list, obj_locs = detect_objects(img)

                # Check if the bottle is in the list of detected objects
                if 'bottle' in obj_list:
                    # Get the coordinates of the bottle in the image
                    bottle_idx = obj_list.index('bottle')
                    bbox = obj_locs[bottle_idx]

                    # Compute the center of the bounding box
                    center_x = (bbox[0] + bbox[2]) / 2
                    center_y = (bbox[1] + bbox[3]) / 2

                    # Compute the distance and angle to the center of the bounding box
                    img_width, img_height = img.size
                    angle = math.atan2(center_x - img_width / 2, img_height / 2 - center_y)
                    distance = (center_x - img_width / 2) / (img_width / 2)

                    # Print the distance and angle to the center of the bounding box
                    print(f"Moving towards bottle, distance: {distance}, angle: {angle}")

                    # Move the agent towards the center of the bounding box
                    if abs(distance) > 0.05:
                        if distance > 0:
                            # Bottle is to the right, so turn right and move forward
                            turn_right()
                            forward()
                        else:
                            # Bottle is to the left, so turn left and move forward
                            turn_left()
                            forward()

                    # Rotate the agent to face the bottle
                    if abs(angle) > 0.05:
                        if angle > 0:
                            # Bottle is to the right, so turn right
                            turn_right()
                        else:
                            # Bottle is to the left, so turn left
                            turn_left()

                    # Stop moving once we're close enough to the bottle
                    if abs(distance) < 0.05:
                        print("Reached the bottle!")
                        return

                else:
                    print("Bottle not found in image.")
                    break

    # If we reach this point, we were unable to find the bottle
    print("Unable to find the bottle.")

This script first searches for the bottle by turning around in a circle and taking pictures in each direction. Once it finds the bottle, it moves towards it by turning and moving forward until it is close enough to stop moving and rotate to face the bottle.

You can call this script by running search_for_bottle_and_move_to_it(). Let me know if you have any


Can you modify this function so it is valid for any type of object that I specify, not just a bottle? Also keep the code as compact as possible.


Certainly! Here's a modified version of the script that allows you to specify the name of the object you want to find:

import math

def search_for_object_and_move_to_it(obj_name): # Turn around in a circle to find the object
num_turns = 4
for i in range(num_turns): # Turn left 90 degrees to face a new direction
turn_left()

        # Take a picture and detect objects in it
        img = get_image()
        obj_list, obj_locs = detect_objects(img)

        # Check if the object is in the list of detected objects
        if obj_name in obj_list:
            # Get the coordinates of the object in the image
            obj_idx = obj_list.index(obj_name)
            bbox = obj_locs[obj_idx]

            # Compute the center of the bounding box
            center_x = (bbox[0] + bbox[2]) / 2
            center_y = (bbox[1] + bbox[3]) / 2

            # Print the center coordinates of the object
            print(f"Found the {obj_name} at ({center_x}, {center_y})")

            # Move towards the object
            while True:
                # Take a picture and detect objects in it
                img = get_image()
                obj_list, obj_locs = detect_objects(img)

                # Check if the object is in the list of detected objects
                if obj_name in obj_list:
                    # Get the coordinates of the object in the image
                    obj_idx = obj_list.index(obj_name)
                    bbox = obj_locs[obj_idx]

                    # Compute the center of the bounding box
                    center_x = (bbox[0] + bbox[2]) / 2
                    center_y = (bbox[1] + bbox[3]) / 2

                    # Compute the distance and angle to the center of the bounding box
                    img_width, img_height = img.size
                    angle = math.atan2(center_x - img_width / 2, img_height / 2 - center_y)
                    distance = (center_x - img_width / 2) / (img_width / 2)

                    # Print the distance and angle to the center of the bounding box
                    print(f"Moving towards {obj_name}, distance: {distance}, angle: {angle}")

                    # Move the agent towards the center of the bounding box
                    if abs(distance) > 0.05:
                        if distance > 0:
                            # Object is to the right, so turn right and move forward
                            turn_right()
                            forward()
                        else:
                            # Object is to the left, so turn left and move forward
                            turn_left()
                            forward()

                    # Rotate the agent to face the object
                    if abs(angle) > 0.05:
                        if angle > 0:
                            # Object is to the right, so turn right
                            turn_right()
                        else:
                            # Object is to the left, so turn left
                            turn_left()

                    # Stop moving once we're close enough to the object
                    if abs(distance) < 0.05:
                        print(f"Reached the {obj_name}!")
                        return

                else:
                    print(f"{obj_name} not found in image.")
                    break

    # If we reach this point, we were unable to find the object
    print(f"Unable to find the {obj_name}.")

This modified script takes an argument obj_name which specifies the name of the object you want to find. It works the same way as the previous script, but uses the object name provided instead of hard-coding "bottle".

You can call this function by running


Let's make a small change. I can guarantee that the scene has the object I am looking for. I want you to keep exploring the environment until you see it somewhere, instead of just turning around a few times. Can you do that?


Sure thing! Here's a modified version of the script that keeps exploring the environment until it finds the specified object:

import math

def search_for_object_and_move_to_it(obj_name): # Keep exploring until we find the object
while True: # Take a picture and detect objects in it
img = get_image()
obj_list, obj_locs = detect_objects(img)

        # Check if the object is in the list of detected objects
        if obj_name in obj_list:
            # Get the coordinates of the object in the image
            obj_idx = obj_list.index(obj_name)
            bbox = obj_locs[obj_idx]

            # Compute the center of the bounding box
            center_x = (bbox[0] + bbox[2]) / 2
            center_y = (bbox[1] + bbox[3]) / 2

            # Print the center coordinates of the object
            print(f"Found the {obj_name} at ({center_x}, {center_y})")

            # Move towards the object
            while True:
                # Take a picture and detect objects in it
                img = get_image()
                obj_list, obj_locs = detect_objects(img)

                # Check if the object is in the list of detected objects
                if obj_name in obj_list:
                    # Get the coordinates of the object in the image
                    obj_idx = obj_list.index(obj_name)
                    bbox = obj_locs[obj_idx]

                    # Compute the center of the bounding box
                    center_x = (bbox[0] + bbox[2]) / 2
                    center_y = (bbox[1] + bbox[3]) / 2

                    # Compute the distance and angle to the center of the bounding box
                    img_width, img_height = img.size
                    angle = math.atan2(center_x - img_width / 2, img_height / 2 - center_y)
                    distance = (center_x - img_width / 2) / (img_width / 2)

                    # Print the distance and angle to the center of the bounding box
                    print(f"Moving towards {obj_name}, distance: {distance}, angle: {angle}")

                    # Move the agent towards the center of the bounding box
                    if abs(distance) > 0.05:
                        if distance > 0:
                            # Object is to the right, so turn right and move forward
                            turn_right()
                            forward()
                        else:
                            # Object is to the left, so turn left and move forward
                            turn_left()
                            forward()

                    # Rotate the agent to face the object
                    if abs(angle) > 0.05:
                        if angle > 0:
                            # Object is to the right, so turn right
                            turn_right()
                        else:
                            # Object is to the left, so turn left

                            turn_left()

                    # Stop moving once we're close enough to the object
                    if abs(distance) < 0.05:
                        print(f"Reached the {obj_name}!")
                        return

                else:
                    print(f"{obj_name} not found in image.")
                    break

        else:
            print(f"{obj_name} not found in image.")
            # If the object is not found, turn around and explore again
            turn_left()
            turn_left()

    # If we reach this point, we were unable to find the object
    print(f"Unable to find the {obj_name}.")

This modified script keeps taking pictures and looking for the specified object. If the object is not found in the image, it turns around and keeps exploring. Once the object is found, it moves towards it as before.

You can call this function by running


How are you computing the distance to the object? What you are doing doesn't seem right.


You're right, I made a mistake in the distance calculation. I apologize for the confusion.

To compute the distance to the center of the bounding box, we need to use the Pythagorean theorem, not the ratio of the difference between the center of the bounding box and the center of the image to the width of the image.

Here's the corrected code for computing the distance:

# Compute the distance and angle to the center of the bounding box

img_width, img_height = img.size
bbox_center_x = (bbox[0] + bbox[2]) / 2
bbox_center_y = (bbox[1] + bbox[3]) / 2
delta_x = bbox_center_x - img_width / 2
delta_y = img_height / 2 - bbox_center_y
distance = math.sqrt(delta_x**2 + delta_y**2) / (img_width / 2)
angle = math.atan2(delta_x, delta_y)

In this code, delta_x and delta_y are the differences between the center of the bounding box and the center of the image. We then use the Pythagorean theorem to compute the distance, and the atan2 function to compute the angle.

I apologize for the mistake, and thank you for bringing it to my attention.


Let me make your life easier for you. Assume we have access to a depth image (depth_img), in which each pixel is the distance to that respective point in the world. Can you match this information with the RGB to figure out distance to an object of interest?


Yes, if we have access to a depth image, we can use it to compute the distance to an object of interest by matching it with the RGB image. Here's some code that shows how to do this:

def search_for_object_and_move_to_it(obj_name): # Keep exploring until we find the object
while True: # Take a picture and a depth image
img = get_image()
depth_img = get_depth_image()

        # Detect objects in the image
        obj_list, obj_locs = detect_objects(img)

        # Check if the object is in the list of detected objects
        if obj_name in obj_list:
            # Get the coordinates of the object in the image
            obj_idx = obj_list.index(obj_name)
            bbox = obj_locs[obj_idx]

            # Compute the center of the bounding box in image coordinates
            center_x = (bbox[0] + bbox[2]) / 2
            center_y = (bbox[1] + bbox[3]) / 2

            # Compute the distance to the object using the depth image
            depth = depth_img[int(center_y), int(center_x)]
            distance = depth * 100 # Convert from meters to centimeters

            # Print the distance to the object
            print(f"Found the {obj_name} at a distance of {distance:.2f} cm")

            # Move towards the object
            while True:
                # Take a picture and detect objects in it
                img = get_image()
                obj_list, obj_locs = detect_objects(img)

                # Check if the object is in the list of detected objects
                if obj_name in obj_list:
                    # Get the coordinates of the object in the image
                    obj_idx = obj_list.index(obj_name)
                    bbox = obj_locs[obj_idx]

                    # Compute the center of the bounding box in image coordinates
                    center_x = (bbox[0] + bbox[2]) / 2
                    center_y = (bbox[1] + bbox[3]) / 2

                    # Compute the distance to the object using the depth image
                    depth = depth_img[int(center_y), int(center_x)]
                    distance = depth * 100 # Convert from meters to centimeters

                    # Print the distance to the object
                    print(f"Moving towards {obj_name}, distance: {distance:.2f} cm")

                    # Move the agent towards the center of the bounding box
                    if distance > 30:
                        forward()
                    else:
                        # Stop moving once we're close enough to the object
                        print(f"Reached the {obj_name}!")
                        return

                else:
                    print(f"{obj_name} not found in image.")
                    break

        else:
            print(f"{obj_name} not found in image.")
            # If the object is not found, turn around and keep exploring
            turn_left()
            turn_left()

    # If we reach this point, we were unable to find the object
    print(f"Unable to find the {obj_name}.")

In this code, we first take an RGB image and a depth image. We then use the object detection function to find the object of interest in the RGB image. Once we have the bounding box coordinates of the object in the RGB image, we use those coordinates to look up the depth value in the depth image, which gives us the distance to the object in meters. We convert the distance to centimeters and print it out.

We then move towards the object by moving forward until we get within 30 cm of the object, at which point we stop moving and print a message indicating that we have reached the object.

I hope this helps! Let me


Instead of just moving forward when the object is visible, you still need to reorient yourself depending on whether it's on the left or right side of the image.


You're right, thank you for pointing that out. Here's the updated code that takes into account the orientation of the object in the image:

import math

def search_for_object_and_move_to_it(obj_name): # Keep exploring until we find the object
while True: # Take a picture and a depth image
img = get_image()
depth_img = get_depth_image()

        # Detect objects in the image
        obj_list, obj_locs = detect_objects(img)

        # Check if the object is in the list of detected objects
        if obj_name in obj_list:
            # Get the coordinates of the object in the image
            obj_idx = obj_list.index(obj_name)
            bbox = obj_locs[obj_idx]

            # Compute the center of the bounding box in image coordinates
            center_x = (bbox[0] + bbox[2]) / 2
            center_y = (bbox[1] + bbox[3]) / 2

            # Compute the distance to the object using the depth image
            depth = depth_img[int(center_y), int(center_x)]
            distance = depth * 100 # Convert from meters to centimeters

            # Print the distance to the object
            print(f"Found the {obj_name} at a distance of {distance:.2f} cm")

            # Move towards the object
            while True:
                # Take a picture and detect objects in it
                img = get_image()
                obj_list, obj_locs = detect_objects(img)

                # Check if the object is in the list of detected objects
                if obj_name in obj_list:
                    # Get the coordinates of the object in the image
                    obj_idx = obj_list.index(obj_name)
                    bbox = obj_locs[obj_idx]

                    # Compute the center of the bounding box in image coordinates
                    center_x = (bbox[0] + bbox[2]) / 2
                    center_y = (bbox[1] + bbox[3]) / 2

                    # Compute the distance to the object using the depth image
                    depth = depth_img[int(center_y), int(center_x)]
                    distance = depth * 100 # Convert from meters to centimeters

                    # Print the distance to the object
                    print(f"Moving towards {obj_name}, distance: {distance:.2f} cm")

                    # Compute the angle to the object in radians
                    img_width, img_height = img.size
                    angle = math.atan2(center_x - img_width / 2, img_height / 2 - center_y)

                    # Rotate the agent to face the object
                    if abs(angle) > 0.05:
                        if angle > 0:
                            # Object is to the right, so turn right
                            turn_right()
                        else:
                            # Object is to the left, so turn left
                            turn_left()

                    # Move the agent towards the object
                    forward()

                    # Stop moving once we're close enough to the object
                    if distance < 30:
                        print(f"Reached the {obj_name}!")
                        return

                else:
                    print(f"{obj_name} not found in image.")
                    break

        else:
            print(f"{obj_name} not found in image.")
            # If the object is not found, turn around and keep exploring
            turn_left()
            turn_left()

    # If we reach this point, we were unable to find the object
    print(f"Unable to find the {obj_name}.")

In this updated code, we still compute the distance to the object using the depth image, as before. We then compute the angle to the object in radians, using the center coordinates of the bounding box in


I want you to modularize this code a bit. Why don't you try coming up with a few functions and use them?


Good idea! Here's a modularized version of the code that uses some helper functions:

import math

def get_object_distance_and_angle(img, depth_img, obj_name): # Detect objects in the image
    obj_list, obj_locs = detect_objects(img)

    # Check if the object is in the list of detected objects
    if obj_name in obj_list:
        # Get the coordinates of the object in the image
        obj_idx = obj_list.index(obj_name)
        bbox = obj_locs[obj_idx]

        # Compute the center of the bounding box in image coordinates
        center_x = (bbox[0] + bbox[2]) / 2
        center_y = (bbox[1] + bbox[3]) / 2

        # Compute the distance to the object using the depth image
        depth = depth_img[int(center_y), int(center_x)]
        distance = depth * 100 # Convert from meters to centimeters

        # Compute the angle to the object in radians
        img_width, img_height = img.size
        angle = math.atan2(center_x - img_width / 2, img_height / 2 - center_y)

        return distance, angle

    else:
        return None, None

def turn_towards_angle(angle): # Rotate the agent to face the angle
    if abs(angle) > 0.05:
    if angle > 0: # Angle is positive, so turn right
    turn_right()
    else: # Angle is negative, so turn left
    turn_left()

def search_for_object_and_move_to_it(obj_name): # Keep exploring until we find the object
    while True: # Take a picture and a depth image
        img = get_image()
        depth_img = get_depth_image()

        # Get the distance and angle to the object
        distance, angle = get_object_distance_and_angle(img, depth_img, obj_name)

        if distance is not None:
            # Print the distance to the object
            print(f"Found the {obj_name} at a distance of {distance:.2f} cm")

            # Move towards the object
            while True:
                # Take a picture and a depth image
                img = get_image()
                depth_img = get_depth_image()

                # Get the distance and angle to the object
                distance, angle = get_object_distance_and_angle(img, depth_img, obj_name)

                if distance is not None:
                    # Print the distance to the object
                    print(f"Moving towards {obj_name}, distance: {distance:.2f} cm")

                    # Turn towards the object
                    turn_towards_angle(angle)

                    # Move the agent towards the object
                    forward()

                    # Stop moving once we're close enough to the object
                    if distance < 30:
                        print(f"Reached the {obj_name}!")
                        return

                else:
                    print(f"{obj_name} not found in image.")
                    break

        else:
            print(f"{obj_name} not found in image.")
            # If the object is not found, turn around and keep exploring
            turn_left()
            turn_left()

    # If we reach this point, we were unable to find the object
    print(f"Unable to find the {obj_name}.")

In this version of the code, the main function search_for_object_and_move_to_it now calls a helper function get_object_distance_and_angle to get the distance and angle to the object of interest. It also calls a helper function turn_towards_angle to rotate the agent to face the object.

I hope this modularized version of the code is more clear and easier to understand


Now, let's assume you have access to all these functions you just created. Let's get to work solving some tasks.


Sure, I'm ready to help you with any tasks you have in mind. What do you need help with?


Find out where I can warm up my lunch.


Sure, to find out where you can warm up your lunch, we can use the search_for_object_and_move_to_it function to search for a microwave in the scene. Here's an example code snippet:

# Search for a microwave

search_for_object_and_move_to_it("microwave")

This code will keep exploring the environment until a microwave is found. Once a microwave is found, the agent will move towards it and stop when it gets within 30 cm of it.

Note that the success of this code will depend on whether there is a microwave in the scene and whether the object detection model is able to detect it.


Let's make a change to this function. Now the agent can move in all 3 axes, so it might have to fly up and down based on the location of the bounding box in the image. Instead of forward(), you now have access to a function fly_to(x, y, z) with which you can take small steps in the direction of the object. Can you now solve the object navigation problem in 3D? Sure, to modify the search_for_object_and_move_to_it function to work in 3D space, we can use the fly_to function to move the agent towards the object of interest. Here's an example code snippet that should work:

import math

def get_object_distance_and_angle_and_height(img, depth_img, obj_name): # Detect objects in the image
    obj_list, obj_locs = detect_objects(img)

    # Check if the object is in the list of detected objects
    if obj_name in obj_list:
        # Get the coordinates of the object in the image
        obj_idx = obj_list.index(obj_name)
        bbox = obj_locs[obj_idx]

        # Compute the center of the bounding box in image coordinates
        center_x = (bbox[0] + bbox[2]) / 2
        center_y = (bbox[1] + bbox[3]) / 2

        # Compute the distance to the object using the depth image
        depth = depth_img[int(center_y), int(center_x)]
        distance = depth * 100 # Convert from meters to centimeters

        # Compute the angle to the object in radians
        img_width, img_height = img.size
        angle = math.atan2(center_x - img_width / 2, img_height / 2 - center_y)

        # Compute the height of the object in the image
        height = (bbox[3] - bbox[1]) / img_height

        return distance, angle, height

    else:
        return None, None, None

def move_towards_object(distance, angle, height, obj_name): # Turn towards the object
    if abs(angle) > 0.05:
    if angle > 0: # Angle is positive, so turn right
    turn_right()
    else: # Angle is negative, so turn left
    turn_left()

    # Compute the distance and height to move in each step
    step_distance = 10
    step_height = 0.1

    # Move towards the object while keeping it at the same height in the image
    while distance > 30 or abs(height - 0.5) > 0.1:
        # Take a picture and a depth image
        img = get_image()
        depth_img = get_depth_image()

        # Get the distance, angle, and height to the object
        distance, angle, height = get_object_distance_and_angle_and_height(img, depth_img, obj_name)

        if distance is not None:
            # Compute the direction to move in
            dx = step_distance * math.cos(angle)
            dy = step_distance * math.sin(angle)
            dz = step_height * (height - 0.5)

            # Move in the direction of the object
            fly_to(dx, dy, dz)

    # Stop moving when we're close enough to the object
    print(f"Reached the {obj_name}!")


def search_for_object_and_move_to_it(obj_name): # Keep exploring until we find the object
    while True: # Take a picture and a depth image
        img = get_image()
        depth_img = get_depth_image()

        # Get the distance, angle, and height to the object
        distance, angle, height = get_object_distance_and_angle_and_height(img, depth_img, obj_name)

        if distance is not None:
            # Print the distance to the object
            print(f"Found the {obj_name} at a distance of {distance:.2f} cm")

            # Move towards the object
            move_towards_object(distance, angle, height, obj_name)

        else:
            print(f"{obj_name} not found in image.") # If the object is not found, turn around and keep exploring
            turn_left()
            turn_left()

    # If we reach this point, we were unable to find the object
    print(f"Unable to find the {obj_name}.")

This modified version of search_for_object_and_move_to_it now uses the get_object_distance_and_angle_and_height function to get the distance, angle, and height to the object. It then uses the move_towards_object function to move the agent towards the object in 3D space, using the fly_to function to take small steps towards the object.

Note that the move_towards_object function moves the agent while keeping the object at the same height in the image (assuming the object is roughly centered in the image). If the object is at a different height in the image, the agent will move up or down to reach the object.

I hope this modified version of the code helps you navigate the scene in 3D space!