import serial import time import numpy as np def connect(): ser = serial.Serial('/dev/ttyUSB0',38400,timeout=None) time.sleep(10) # Nach dem Öffnen 10 Sekunden warten ser.write('connect\n'.encode()) ser.readline().decode() return ser def move_pose(ser,w,gripServo,duration): BASE_MIN = -2.6112 BASE_MAX = 2.6061 SHOULDER_MIN = -1.5708 SHOULDER_MAX = 1.5708 ELBOW_MIN = -2.0043 ELBOW_MAX = 1.5555 WRIST_MIN = -1.6218 WRIST_MAX = 1.6779 WROT_MIN = -2.6112 WROT_MAX = 2.6061 GRIP_MIN = 0 GRIP_MAX = 511 DURATION_MIN = 250 DURATION_MAX = 65535 ###################### Code-Teil von Shefqet Grdlay v = np.zeros(5) d = [111.6, 0, 0, 0, 155] a = [0, 148.22, 147, 0, 0] v[0] = np.arctan2(w[1], w[0]) v234 = np.arctan2(-(np.cos(v[0]) * w[3] + np.sin(v[0]) * w[4]), -w[5]) b1 = np.cos(v[0]) * w[0] + np.sin(v[0]) * w[1] + d[4] * np.sin(v234) b2 = d[0] - d[4] * np.cos(v234) - w[2] v[2] = np.arccos((np.square(np.linalg.norm([[b1], [b2]])) - np.square(a[1]) - np.square(a[2])) / (2 * a[1] * a[2])) v[1] = np.arctan2((a[1] + a[2] * np.cos(v[2])) * b2 - a[2] * np.sin(v[2]) * b1, (a[1] + a[2] * np.cos(v[2])) * b1 + a[2] * np.sin(v[2]) * b2) v[3] = v234 - v[1] - v[2] v[4] = np.pi * np.log(np.sqrt(np.square(w[3]) + np.square(w[4]) + np.square(w[5]))) v[1] = v[1] + 1.315014068517125 v[2] = v[2] - 1.314998850007427 v[3] = v[3] + 1.570781108285199 ###################### # Überprüfung der zulässigen Werte if v[0] < BASE_MIN: return "False" if v[0] > BASE_MAX: return "False" if v[1] < SHOULDER_MIN: return "False" if v[1] > SHOULDER_MAX: return "False" if v[2] < ELBOW_MIN: return "False" if v[2] > ELBOW_MAX: return "False" if v[3] < WRIST_MIN: return "False" if v[3] > WRIST_MAX: return "False" if v[4] < WROT_MIN: return "False" if v[4] > WROT_MAX: return "False" if gripServo < GRIP_MIN: return "False" if gripServo > GRIP_MAX: return "False" if duration < DURATION_MIN: return "False" if duration > DURATION_MAX: return "False" if np.any(np.isinf(v)) or np.any(np.isnan(v)): # Probe auf Inf und NaN return "False" else: resp = set_joint_angles(ser,v[0],v[1],v[2],v[3],v[4],gripServo,duration) return resp def set_joint_angles(ser,baseServo,shoulderServo,elbowServo,wristServo,wristRotServo,gripServo,duration): msg = (f"move {baseServo:.4f} {shoulderServo:.4f} {elbowServo:.4f} {wristServo:.4f} {wristRotServo:.4f} {int(gripServo)} {int(duration)}\n") ser.write(msg.encode()) resp = ser.readline().decode() return resp def get_joint_angles(ser): ser.write('getJointAngles\n'.encode()) resp = list() for i in range(0,6): msg = ser.readline().decode() resp.append(msg) return resp def sleep(ser): ser.write('sleep\n'.encode()) def disconnect(ser): ser.write('disconnect\n'.encode()) ser.readline().decode() ser.close()