MediaPipeから角度を検出してみる

サーボを動かす数値を、MediaPipe側から生成してみましょう。肩と肘の座標をもとに、atan()を使って計算します。出てきた値は弧度法なのでESPのプログラムに合わせてmath.degreesで度数法に変換し、更にintで整数にキャストします。

サーボの動く範囲を0～180度に限定するものとし、マイナスなら0に、180以上なら180にクリッピングしてみました。

• cv2.VideoCapture(0)の引数は皆さんの環境によって変えてください。
• 配列landmarkは、インデックス12が右肩、14が右肘です。

import cv2
import math
import mediapipe as mp

mp_drawing = mp.solutions.drawing_utils
mp_drawing_styles = mp.solutions.drawing_styles
mp_pose = mp.solutions.pose
pre_angle_s = 0#Previous Angle of Shoulder

cap = cv2.VideoCapture(0)
with mp_pose.Pose(
    min_detection_confidence=0.5,
    min_tracking_confidence=0.5) as pose:
  while cap.isOpened():
    success, image = cap.read()
    if not success:
      print("Ignoring empty camera frame.")
      continue

    image.flags.writeable = False
    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    results = pose.process(image)

    image.flags.writeable = True
    image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
    mp_drawing.draw_landmarks(
        image,
        results.pose_landmarks,
        mp_pose.POSE_CONNECTIONS,
        landmark_drawing_spec=mp_drawing_styles.get_default_pose_landmarks_style())
    if results.pose_landmarks:
      x1=results.pose_landmarks.landmark[12].x #x1,y1 = shoulder
      x2=results.pose_landmarks.landmark[14].x #x2,y2 = elbow
      y1=results.pose_landmarks.landmark[12].y
      y2=results.pose_landmarks.landmark[14].y
      
      if x2<x1:
        angle_s = int(math.degrees(math.atan((y2-y1)/(x1-x2))))
      else:# x2>=x1
        angle_s = int(180+math.degrees(math.atan((y2-y1)/(x1-x2))))

      if angle_s < 0:
        angle_s = 0
      elif angle_s > 180:
        angle_s = 180

      print("\r"+str(angle_s))

    cv2.imshow('SIGACI', cv2.flip(image, 1))
    if cv2.waitKey(5) & 0xFF == 27: #ESCで終了
      break
cap.release()

適当な（適切な）ファイル名で保存し、実行してみると、画面上の腕の角度に応じて標準出力に出ている数値が変化するかと思います。

venvで仮想環境を構築した人は「….\python\venv\Mediapipe\Scripts\Activate」 の実行を忘れずに…暫く間があくと忘れますよね。「3日前の自分は他人」だと思いましょう。