"""
    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._handlers[type_](data)
  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