Skip to content
TheChatArchiveThe Chat Archive
gpt-4o-2024-08-06· OpenAIAi2 WildChat-4.8M (AI2)

""" Streaming 6Dof from QTM """ import asyncio import xml.etree.ElementTree as ET import…

  1. User

    """ Streaming 6Dof from QTM """

    import asyncio import xml.etree.ElementTree as ET import math import qtm_rt

    from scipy.spatial.transform import Rotation as R import numpy as np

    import time

    import socket

    UDP_IP_ADDRESS = "192.168.0.105" # Replace with your server IP address UDP_PORT_NO = 3004 serverSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) serverSock.bind((UDP_IP_ADDRESS, UDP_PORT_NO))

    global z_o global Prev_time Prev_time = time.time() z_o = 0

    def create_body_index(xml_string): """ Extract a name to index dictionary from 6dof settings xml """ xml = ET.fromstring(xml_string)

    body_to_index = {}
    for index, body in enumerate(xml.findall("*/Body/Name")):
        body_to_index[body.text.strip()] = index
    
    return body_to_index
    

    def body_enabled_count(xml_string): xml = ET.fromstring(xml_string) return sum(enabled.text == "true" for enabled in xml.findall("*/Body/Enabled"))

    async def main():

    # Connect to qtm
    connection = await qtm_rt.connect("192.168.0.151")
    
    # Connection failed?
    if connection is None:
        print("Failed to connect")
        return
    
    # Take control of qtm, context manager will automatically release control after scope end
    async with qtm_rt.TakeControl(connection, "password"):
    
        realtime = True
    
        if realtime:
            # Start new realtime
            await connection.new()
        else:
            print("make real time = true")
    
    # Get 6dof settings from qtm
    xml_string = await connection.get_parameters(parameters=["6d"])
    body_index = create_body_index(xml_string)
    
    print("{} of {} 6DoF bodies enabled".format(body_enabled_count(xml_string), len(body_index)))
    global counter
    counter = time.time()
    wanted_body = "Khepera104"
    
    
    def on_packet(packet):
        info, bodies = packet.get_6d()
        # print(
        #     "Framenumber: {} - Body count: {}".format(
        #         packet.framenumber, info.body_count
        #     )
        # )
    
    
    
        if wanted_body is not None and wanted_body in body_index:
            # Extract one specific body
            wanted_index = 6
            position, rotation= bodies[wanted_index]
    
            x=position[0]/1000
            y=position[1]/1000
            z=position[2]/1000
    
    
            rotation = np.array(rotation)
            rotation = rotation.reshape(3,3)
            r = R.from_matrix(rotation)
            rot = r.as_euler('zyx', degrees=True)
    
            theta=-rot[0]
    
    
            #print(x,y,theta)
    
            theta_r=math.radians(theta)
    
            #print("{} - Pos: {} - Rot: {}".format(wanted_body, position, rot))
            # print(rot)
            # data, addr = serverSock.recvfrom(1024)  # buffer size is 1024 bytes
    
            x_d = 0  # values given in mm
            y_d = 0
    
    
            # V = k_p*e_p
            # w = k_theta*e_theta
    
            V = 0.600
    
            # Send the move command
            global counter
            global z_o
            elapsed_time = time.time() - counter
            global Prev_time
            # -------------------- Parameter Initialization --------------------
    

    Constants from MATLAB code

            v = 1.0          # Linear velocity
            d = 0.75         # Parameter d
    
            r_i = 0.5        # Inner radius
            r_o = 1.40       # Outer radius
    
            kb = d - r_i     # kb based on MATLAB code
            ka = r_o - d     # ka based on MATLAB code
    
            k1 = 3           # Control gain k1
            k2 = 4           # Control gain k2
            #print(z_o,h)
    
            if elapsed_time >= 0.05:
                counter = time.time()
                h = time.time() - Prev_time
                x1, y1 = x
                r = math.sqrt(x1**2 + y1**2)  # Distance from origin
    
                pk = r - d
                q = 1 if pk > 0 else 0
    
        # Compute px as defined in MATLAB
                px = math.cos(theta) * (x1 - d * math.sin(theta)) + math.sin(theta) * (y1 + d * math.cos(theta))
    
        # Sigma function (identity in MATLAB)
                c = v * px
    
        # Compute denominators, handle potential division by zero
                denominator_ka = ka**2 - (r - d)**2
                denominator_kb = kb**2 - (r - d)**2
    
        # To prevent division by zero, set a small epsilon
                epsilon = 1e-6
                if denominator_ka == 0:
                     denominator_ka = epsilon
                if denominator_kb == 0:
                    denominator_kb = epsilon
    
        # Control term as per MATLAB logic
                term_ka = ((r - d) * (v / r)) / (d * k1 * denominator_ka)
                term_kb = ((r - d) * (v / r)) / (d * k1 * denominator_kb)
                w = q * ((1 / d) * (v + term_ka + v * c)) + (1 - q) * ((1 / d) * (v + term_kb + v * c))
                #w = (V+k*((x-x_d)*math.cos(theta_r)+(y-y_d)*math.sin(theta_r)))/rd
                command = f"{-w}x{V*1000}".encode()
                serverSock.sendto(command, addr)
                print(z_o, h, w)
            #time.sleep(0.05)
    
    
            
        else:
            # Print all bodies
            for position, rotation in bodies:
                print("Pos: {} - Rot: {}".format(position, rotation))
    
    # Start streaming frames
    await connection.stream_frames(components=["6d"], on_packet=on_packet)
    counter = time.time()
    # Wait asynchronously 5 seconds
    await asyncio.sleep(150)
    
    # Stop streaming
    await connection.stream_frames_stop()
    

    if name == "main": # Run our asynchronous function until complete print("memed") data, addr = serverSock.recvfrom(1024) print("memed2")

    asyncio.get_event_loop().run_until_complete(main())
    print("stop")
    
    # Stop the robot
    stop_command = "0x0".encode()
    serverSock.sendto(stop_command, addr)
    

    please resolve below error in above code /bin/python3.10 /home/mocaplab/khepera-main/libkhepera-2.1/template/Python_Server/circumnavigation_with_range_rate_ablf.py (base) mocaplab@mocaplab-HP-Z1-G8-Tower-Desktop-PC:~$ /bin/python3.10 /home/mocaplab/khepera-main/libkhepera-2.1/template/Python_Server/circumnavigation_with_range_rate_ablf.py memed memed2 2024-10-18 15:34:19,161 - qtm_rt - INFO - Connected 2024-10-18 15:34:19,166 - qtm_rt - INFO - Took control 2024-10-18 15:34:19,169 - qtm_rt - INFO - Released control 6 of 10 6DoF bodies enabled 2024-10-18 15:34:19,248 - asyncio - ERROR - Fatal error: protocol.data_received() call failed. protocol: <qtm_rt.protocol.QTMProtocol object at 0x7c19831cdcf0> transport: <_SelectorSocketTransport fd=7 read=polling write=<idle, bufsize=0>> Traceback (most recent call last): File "/usr/lib/python3.10/asyncio/selector_events.py", line 876, in _read_ready__data_received self._protocol.data_received(data) File "/home/mocaplab/.local/lib/python3.10/site-packages/qtm_rt/protocol.py", line 133, in data_received self._receiver.data_received(data) File "/home/mocaplab/.local/lib/python3.10/site-packages/qtm_rt/receiver.py", line 25, in data_received self.parse_received(data[h_size:size], type) File "/home/mocaplab/.local/lib/python3.10/site-packages/qtm_rt/receiver.py", line 49, in _parse_received self._handlerstype_ File "/home/mocaplab/.local/lib/python3.10/site-packages/qtm_rt/protocol.py", line 148, in _on_data self.on_packet(packet) File "/home/mocaplab/khepera-main/libkhepera-2.1/template/Python_Server/circumnavigation_with_range_rate_ablf.py", line 142, in on_packet x1, y1 = x TypeError: cannot unpack non-iterable float object 2024-10-18 15:34:19,249 - qtm_rt - INFO - Disconnected

  2. Assistant

    The error message you're encountering indicates that you're trying to unpack a non-iterable float object. This issue is happening because the variables x and y are being assigned as floats which cannot be unpacked into two variables x1 and y1.

    Here's the relevant part of your code that's causing the error:

    x1, y1 = x  # This line is causing an error
    

    x is already a float (a single value), so attempting to unpack it into x1 and y1 results in a TypeError. To fix this issue, you need to use the variables x and y directly without attempting to unpack them.

    Here's a modified snippet of your code:

    # Correct use of x, y as single float variables
    x1 = x
    y1 = y
    
    r = math.sqrt(x1**2 + y1**2)  # Distance from origin
    
    pk = r - d
    q = 1 if pk > 0 else 0
    
    # Compute px as defined in MATLAB
    px = math.cos(theta) * (x1 - d * math.sin(theta)) + math.sin(theta) * (y1 + d * math.cos(theta))
    
    # Sigma function (identity in MATLAB)
    c = v * px
    
    # Compute denominators, handle potential division by zero
    denominator_ka = ka**2 - (r - d)**2
    denominator_kb = kb**2 - (r - d)**2
    

    By making this adjustment, you assign the float values x and y directly to x1 and y1, allowing the rest of your calculations to proceed correctly.

    Make sure to verify the logic of your program if x and y are indeed intended to represent some other data structure like a vector or a tuple. If they are meant to be single values for some reason, adjusting the code as shown will resolve the TypeError.

  3. User

    please write full correct5 code

  4. Assistant

    Based on your request and the error you encountered, I'll provide a revised version of your code with fixes applied to ensure correct handling of the positional data and other code optimizations. This will include focusing on the problem you faced with unpacking the x and y coordinates:

    import asyncio
    import xml.etree.ElementTree as ET
    import math
    import qtm_rt
    from scipy.spatial.transform import Rotation as R
    import numpy as np
    import socket
    import time
    
    UDP_IP_ADDRESS = "192.168.0.105"  # Replace with your server IP address
    UDP_PORT_NO = 3004
    serverSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    serverSock.bind((UDP_IP_ADDRESS, UDP_PORT_NO))
    
    global z_o
    global Prev_time
    Prev_time = time.time()
    z_o = 0
    
    def create_body_index(xml_string):
        """ Extract a name to index dictionary from 6dof settings xml """
        xml = ET.fromstring(xml_string)
        body_to_index = {body.text.strip(): index for index, body in enumerate(xml.findall("*/Body/Name"))}
        return body_to_index
    
    def body_enabled_count(xml_string):
        xml = ET.fromstring(xml_string)
        return sum(enabled.text == "true" for enabled in xml.findall("*/Body/Enabled"))
    
    async def main():
        connection = await qtm_rt.connect("192.168.0.151")
        if connection is None:
            print("Failed to connect")
            return
    
        async with qtm_rt.TakeControl(connection, "password"):
            realtime = True
            if realtime:
                await connection.new()
            else:
                print("Make real time = true")
    
        xml_string = await connection.get_parameters(parameters=["6d"])
        body_index = create_body_index(xml_string)
    
        print(f"{body_enabled_count(xml_string)} of {len(body_index)} 6DoF bodies enabled")
        global counter
        counter = time.time()
        wanted_body = "Khepera104"
    
        def on_packet(packet):
            info, bodies = packet.get_6d()
    
            if wanted_body is not None and wanted_body in body_index:
                wanted_index = body_index[wanted_body]
                position, rotation = bodies[wanted_index]
    
                x = position[0] / 1000
                y = position[1] / 1000
                z = position[2] / 1000
    
                rotation = np.array(rotation).reshape(3, 3)
                r = R.from_matrix(rotation)
                rot = r.as_euler('zyx', degrees=True)
    
                theta = -rot[0]
                theta_r = math.radians(theta)
    
                x1 = x
                y1 = y
    
                V = 0.600
                global counter
                global z_o
                elapsed_time = time.time() - counter
                global Prev_time
    
                v = 1.0       
                d = 0.75      
                r_i = 0.5     
                r_o = 1.40    
                kb = d - r_i  
                ka = r_o - d  
    
                k1 = 3
                k2 = 4
    
                if elapsed_time >= 0.05:
                    counter = time.time()
                    h = time.time() - Prev_time
                    r = math.sqrt(x1**2 + y1**2)
    
                    pk = r - d
                    q = 1 if pk > 0 else 0
    
                    px = math.cos(theta) * (x1 - d * math.sin(theta)) + math.sin(theta) * (y1 + d * math.cos(theta))
    
                    c = v * px
    
                    denominator_ka = ka**2 - (r - d)**2
                    denominator_kb = kb**2 - (r - d)**2
    
                    epsilon = 1e-6
                    if denominator_ka == 0:
                        denominator_ka = epsilon
                    if denominator_kb == 0:
                        denominator_kb = epsilon
    
                    term_ka = ((r - d) * (v / r)) / (d * k1 * denominator_ka)
                    term_kb = ((r - d) * (v / r)) / (d * k1 * denominator_kb)
                    w = q * ((1 / d) * (v + term_ka + v * c)) + (1 - q) * ((1 / d) * (v + term_kb + v * c))
                    
                    command = f"{-w}x{V*1000}".encode()
                    serverSock.sendto(command, addr)
                    print(z_o, h, w)
    
            else:
                for position, rotation in bodies:
                    print(f"Pos: {position} - Rot: {rotation}")
    
        await connection.stream_frames(components=["6d"], on_packet=on_packet)
        counter = time.time()
        await asyncio.sleep(150)
        await connection.stream_frames_stop()
    
    if __name__ == "__main__":
        print("memed")
        data, addr = serverSock.recvfrom(1024)
        print("memed2")
        try:
            asyncio.get_event_loop().run_until_complete(main())
        finally:
            stop_command = "0x0".encode()
            serverSock.sendto(stop_command, addr)
            print("stop")
    

    Key Adjustments Made:

    1. Assign x and y directly: Ensured that x1 and y1 are directly assigned from x and y since they are single float values.
    2. Correct wanted_index: The index of the wanted body is now correctly fetched from the body_index dictionary using wanted_body.
    3. Consistent Usage: Throughout the script, used consistent handling for the x, y, and theta variables.
    4. Ensure Connection Release: Used a try-finally block to ensure the robot sends a stop command even if the execution is interrupted.
    5. General Improvements: Added proper string formatting, and some general code tidying for better readability.

    This code assumes you have a target body named "Khepera104" defined in your QTM setup, and that all necessary dependencies are correctly installed. Adjust any IP addresses, ports, or other configuration details according to your specific environment.