""" Streaming 6Dof from QTM """ import asyncio import xml.etree.ElementTree as ET import…
- 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_indexdef 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
- 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
xandyare being assigned as floats which cannot be unpacked into two variablesx1andy1.Here's the relevant part of your code that's causing the error:
x1, y1 = x # This line is causing an errorxis already a float (a single value), so attempting to unpack it intox1andy1results in aTypeError. To fix this issue, you need to use the variablesxandydirectly 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)**2By making this adjustment, you assign the float values
xandydirectly tox1andy1, 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.
- User
please write full correct5 code
- 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
xandycoordinates: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:
- Assign
xandydirectly: Ensured thatx1andy1are directly assigned fromxandysince they are single float values. - Correct
wanted_index: The index of the wanted body is now correctly fetched from thebody_indexdictionary usingwanted_body. - Consistent Usage: Throughout the script, used consistent handling for the
x,y, andthetavariables. - Ensure Connection Release: Used a
try-finallyblock to ensure the robot sends a stop command even if the execution is interrupted. - 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. - Assign