import cv2 import mediapipe as mp import time import math from cvzone.SerialModule import SerialObject from time import sleep class poseDetector(): def __init__(self, mode=False, smooth=True, detectionCon=0.5, trackCon=0.5): self.mode = mode self.smooth = smooth self.detectionCon = detectionCon self.trackCon = trackCon self.mpDraw = mp.solutions.drawing_utils self.mpPose = mp.solutions.pose self.pose = self.mpPose.Pose(static_image_mode=self.mode, smooth_landmarks=self.smooth, min_detection_confidence=self.detectionCon, min_tracking_confidence=self.trackCon) # Define specific connections for landmarks 11-16 and 23-28 self.specific_connections = [ (11, 12), (12, 14), (14, 16), (11, 13), (13, 15), (23, 24), (24, 26), (26, 28), (23, 25), (25, 27) ] def findPose(self, img, draw=True): imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) self.results = self.pose.process(imgRGB) # if self.results.pose_landmarks: # if draw: # self.mpDraw.draw_landmarks(img, self.results.pose_landmarks, # self.mpPose.POSE_CONNECTIONS) for connection in self.specific_connections: start_idx, end_idx = connection if self.results.pose_landmarks.landmark[start_idx].visibility > 0.5 and \ self.results.pose_landmarks.landmark[end_idx].visibility > 0.5: start_point = self.results.pose_landmarks.landmark[start_idx] end_point = self.results.pose_landmarks.landmark[end_idx] h, w, c = img.shape start_coords = (int(start_point.x * w), int(start_point.y * h)) end_coords = (int(end_point.x * w), int(end_point.y * h)) cv2.line(img, start_coords, end_coords, (255, 255, 255), 3) return img def findPosition(self, img, draw=True): self.lmList = [] if self.results.pose_landmarks: for id, lm in enumerate(self.results.pose_landmarks.landmark): h, w, c = img.shape cx, cy = int(lm.x * w), int(lm.y * h) self.lmList.append([id, cx, cy]) if draw: cv2.circle(img, (cx, cy), 5, (255, 0, 0), cv2.FILLED) return self.lmList def findAngle(self, img, p1, p2, p3, draw=True): x1, y1 = self.lmList[p1][1:] x2, y2 = self.lmList[p2][1:] x3, y3 = self.lmList[p3][1:] angle = math.degrees(math.atan2(y3 - y2, x3 - x2) - math.atan2(y1 - y2, x1 - x2)) if angle < 0: angle += 360 Raduis = 2 if draw: cv2.line(img, (x1, y1), (x2, y2), (255, 255, 255), 3) #cv2.line(img, (x3, y3), (x2, y2), (255, 255, 255), 3) cv2.circle(img, (x1, y1), Raduis , (0, 0, 255), cv2.FILLED) cv2.circle(img, (x1, y1), Raduis , (0, 0, 255), 2) cv2.circle(img, (x2, y2), Raduis , (0, 0, 255), cv2.FILLED) cv2.circle(img, (x2, y2), Raduis , (0, 0, 255), 2) cv2.circle(img, (x3, y3), Raduis , (0, 0, 255), cv2.FILLED) cv2.circle(img, (x3, y3), Raduis , (0, 0, 255), 2) cv2.putText(img, str(int(angle)), (x2 - 50, y2 + 50), cv2.FONT_HERSHEY_PLAIN, 1.5, (0, 0, 255), 2) return angle def get_angles_from_pose(pose_number): switcher = { 1: [180, 180, 180, 180, 135, 135, 125, 125], 2: [235, 235, 100, 100, 180, 180, 165, 165], 3: [180, 180, 180, 110, 180, 180, 130, 170] } return switcher.get(pose_number, "Invalid pose number") def compare_angles(desired_angles, current_angles, tolerance): if len(desired_angles) != len(current_angles): print("Error: The lists do not have the same length.") return success = True for angle1, angle2 in zip(desired_angles, current_angles): if abs(angle1 - angle2) >= tolerance: success = False break if success: print("Success") else: print("Failed") return success def main(): cap = cv2.VideoCapture(0) detector = poseDetector() # Create a named window with the full-screen property cv2.namedWindow('Image', cv2.WND_PROP_FULLSCREEN) cv2.setWindowProperty('Image', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN) # Variables to track start time and duration start_time = time.time() duration = 3 # in seconds while time.time() - start_time < duration: success, img = cap.read() img = cv2.flip(img, 1) img = detector.findPose(img) lmList = detector.findPosition(img, draw=False) if len(lmList) != 0: # Calculate angles and store them in a list/Display Angle angle1 = detector.findAngle(img, 12, 14, 16) angle2 = detector.findAngle(img, 15, 13, 11) angle3 = detector.findAngle(img, 24, 26, 28) angle4 = detector.findAngle(img, 27, 25, 23) angle5 = detector.findAngle(img, 11, 12, 14) angle6 = detector.findAngle(img, 13, 11, 12) angle7 = detector.findAngle(img, 23, 24, 26) angle8 = detector.findAngle(img, 25, 23, 24) angles = [angle1, angle2, angle3, angle4, angle5, angle6, angle7,angle8] # Display the angles on the image #for i, angle in enumerate(angles): # cv2.putText(img, f'Angle{i + 1}: {int(angle)}', (10, 50 + i * 50), cv2.FONT_HERSHEY_PLAIN, 2, (255, 0, 0), 2) Raduis=2; #Display Joints #Left Arm cv2.circle(img, (lmList[12][1], lmList[12][2]), Raduis, (0, 0, 255), cv2.FILLED) cv2.circle(img, (lmList[14][1], lmList[14][2]), Raduis, (0, 0, 255), cv2.FILLED) cv2.circle(img, (lmList[16][1], lmList[16][2]), Raduis, (0, 0, 255), cv2.FILLED) # Right Arm cv2.circle(img, (lmList[11][1], lmList[11][2]), Raduis, (0, 255, 0), cv2.FILLED) cv2.circle(img, (lmList[13][1], lmList[13][2]), Raduis, (0, 255, 0), cv2.FILLED) cv2.circle(img, (lmList[15][1], lmList[15][2]), Raduis, (0, 255, 0), cv2.FILLED) # Left Leg cv2.circle(img, (lmList[24][1], lmList[24][2]), Raduis, (0, 0, 255), cv2.FILLED) cv2.circle(img, (lmList[26][1], lmList[26][2]), Raduis, (0, 0, 255), cv2.FILLED) cv2.circle(img, (lmList[28][1], lmList[28][2]), Raduis, (0, 0, 255), cv2.FILLED) # Right Leg cv2.circle(img, (lmList[23][1], lmList[23][2]), Raduis, (0, 255, 0), cv2.FILLED) cv2.circle(img, (lmList[25][1], lmList[25][2]), Raduis, (0, 255, 0), cv2.FILLED) cv2.circle(img, (lmList[27][1], lmList[27][2]), Raduis, (0, 255, 0), cv2.FILLED) # Display the resulting frame in full screen cv2.imshow("Image", img) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows() # Convert angles to a comma-separated string arduino = SerialObject() angles_int = [int(angle) for angle in angles] print("Angles as integers:", angles_int) while True: try: pose_number = int(input("Enter a pose number (1-5) to get the angles, or 0 to exit: ")) if pose_number == 0: print("Exiting...") break standardAngles = get_angles_from_pose(pose_number) if standardAngles == "Invalid case number": print(standardAngles ) else: print(f"Angles for pose {pose_number}: {standardAngles }") except ValueError: print("Please enter a valid integer.") comparison=compare_angles(standardAngles, angles_int , 100) if comparison: arduino.sendData([1]) sleep(1) else: arduino.sendData([pose_number,2]) """" if angles_str: print("Sending angles to Arduino:", angles_str) angles_str_without_commas = angles_str.replace(',', '') #arduino.sendData(angles_str_without_commas) else: print("No angles to send to Arduino.") """ if __name__ == "__main__": main()