xt = 0; 
yt = 0; 
v = 1.0; 
d = 0.75;
%d = 0.7;
z0 = 0; 
k1 = 3;  
k2 = 4; 
r_i = 0.5;
r_o = 1.40;
% r_i = 0.75;
% r_o = 1.75;
% r_i = 0.5;
% r_o = 1.3;
kb = d - r_i;
ka = r_o - d;
x0 = [1.15; 0.20; 25 * pi / 180];
tspan = [0 50];
opts = odeset('RelTol',1e-5,'AbsTol',1e-7);
function dxdt = dynamics_unknown(~, x, v, k1, d, k2, ka, kb)
    % States
    x_p = x(1); 
    y_p = x(2); 
    theta = x(3); 
    %z = x(4); 
    % Calculate the control term
    control_term = calculate_control_term(x, theta, v, k1, d,k2, ka,kb);
    
    %z_dot = -k2 * z + 0.5 * (x_p^2 + y_p^2);
    dxdt = [
        v * cos(theta); % dx/dt
        v * sin(theta); % dy/dt
        control_term;   % dtheta/dt
        %z_dot           % dz/dt
    ];
end
function control_term = calculate_control_term(x, theta, v, k1,d, ~,ka, kb)
    % States
    x1 = x(1); 
    y1 = x(2); 
    
    %xp=x1-d*sin(theta);
    %yp=y1+d*cos(theta);
    %xt=0;
    %yt=0;
    %e_x=xp-0;
    %e_y=yp-0;
    
    %z = x(4); 
    %ke = sqrt((x1-d*sin(theta)- 0).^2 + (y1+d*cos(theta)- 0).^2); 
    % Error terms
    %e_x = x_p - 0; % Target x-coordinate is 0
    %e_y = y_p - 0; % Target y-coordinate is 0
    %radius = sqrt(e_x^2 + e_y^2 + d^2 + 2*d*(e_x*sin(theta) - e_y*cos(theta)));
    r=sqrt(x1.^2+y1.^2);%&& radius > 1
   % s_fun = sqrt(e_x^2 + e_y^2);
    pk = r-d;
    if pk > 0
         q = 1;
    else
         q = 0;
    end
    px = cos(theta)*(x1-d*sin(theta)- 0) + sin(theta)*(y1+d*cos(theta)- 0);
    % Sigma function
    sigma = @(s) s ;
    c = sigma(v*px);
    control_term = q*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(ka^2-(r-d)^2)))+v*c))+(1-q)*((1/d)*(v+(((r-d)*(v/r))/(d*k1*(kb^2-(r-d)^2)))+v*c));
    %control_term = q*((k1 * v * c / (ka^2 - ke^2)) + (v / d)) + (1 - q) * ((k1 * v * c / (kb^2 - ke^2)) + (v / d));
    %control_term = q*((k1 * v * c / (ka^2 - e_x^2 - e_y^2)) + (v / d)) + (1 - q) * ((k1 * v * c / (kb^2 - e_x^2 - e_y^2)) + (v / d));
end
use above information to edit below code according to above code rewrite  below code 
"""
    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 = 3003
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.111")

    # 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 = "Khepera103"


    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 = 5
            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
            k2 = 2
            ka = 1.0
            #print(z_o,h)
    
            if elapsed_time >= 0.05:
                counter = time.time()
                h = time.time() - Prev_time
                z_dot = -(k2 * z_o) + (0.5 * ((x-x_d)**2 + (y-y_d)**2))
                z_o = z_o + z_dot*h
                Prev_time=time.time()
                k1 = 1.0
                d = 0.7
                c = (-k2 * z_o + 0.5 * ((x-x_d)**2 + (y-y_d)**2))/math.sqrt(1+(-k2 * z_o + 0.5 * ((x-x_d)**2 + (y-y_d)**2))**2)
                w = k1 * V * c /((ka)**2 - ((x-x_d)**2 + (y-y_d)**2))+ V/d
                #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)