import cv2
import math
import time
import mediapipe as mp
from time import sleep
from robotControl import *
from kinematics import *
from os import listdir
import numpy as np
 
mp_drawing = mp.solutions.drawing_utils
mp_hands = mp.solutions.hands
 
#调用mediapipe库中的Hand函数，这个函数就可以识别手部
hands = mp_hands.Hands(min_detection_confidence=0.5, min_tracking_confidence=0.5,max_num_hands=1)
rc=robotControl()
cap = cv2.VideoCapture(0)
robot_x = 0.2
robot_y = 0.04
robot_z = 0.158
while cap.isOpened():
    success, image = cap.read()
    if not success:
        print("没有图像")
        continue
 
    image = cv2.cvtColor(cv2.flip(image, 1), cv2.COLOR_BGR2RGB)
    image.flags.writeable = False
    #result是调用hands函数得到的结果，里面包含了手的各点坐标，通过results.multi_hand_landmarks调用
    results = hands.process(image)
 
 
    image.flags.writeable = True
    image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
    h, w, c = image.shape#像素坐标
    xtotal = []
    ztotal = []
    if results.multi_hand_landmarks:
        #得到results.multi_hand_landmarks中所有点的坐标，并进行绘制
        #print(results.multi_hand_landmarks.landmark)
        for hand_landmarks in results.multi_hand_landmarks:
            for index,xy in enumerate(hand_landmarks.landmark):
                #print(index,xy)
                #imx,imy=int(xy.x*w),int(xy.y*h)#获得像素坐标
                imx,imz=xy.x*robot_x-0.1,xy.y*robot_z
                #print(index,imx,imz)
                xtotal.append(imx)
                ztotal.append(imz)
                mp_drawing.draw_landmarks(image, hand_landmarks, mp_hands.HAND_CONNECTIONS)
        ik=[]
        ik.append(ana_ik([np.mean(xtotal),robot_y,np.mean(ztotal)]))  
        rc.move_to_joints(ik[0])  
        

 
    cv2.imshow('result', image)
    if cv2.waitKey(5) & 0xFF == 27:
        break
cv2.destroyAllWindows()
hands.close()
cap.release()