サーボを動かす数値を、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日前の自分は他人」だと思いましょう。