File size: 3,812 Bytes
07d7978 ed7d49e 07d7978 ed7d49e 07d7978 ed7d49e 07d7978 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 |
"""Reachy Mini Hand Tracker App
This app makes Reachy Mini follow your hand.
It uses the robot camera to detect the hand position and adjusts the head pose accordingly.
It uses mediapipe to track the hand and OpenCV for image processing.
"""
import threading
import cv2
import numpy as np
from reachy_mini import ReachyMini, ReachyMiniApp
from reachy_mini.io.cam_utils import find_camera
from scipy.spatial.transform import Rotation as R
from reachy_mini_hand_tracker_app.hand_tracker import HandTracker
gui = False # Set to True if you want to use the GUI for debugging
class HandTrackerApp(ReachyMiniApp):
# Proportional gain for the controller
# Reduce/Increase to make the head movement smoother or more responsive)
kp = 0.2
# Maximum delta for the head position adjustments
# This limits how much the head can move in one iteration to prevent abrupt movements
max_delta = 0.3
# Proportional gains for the head height adjustment
# Adjust this value to control how much the head moves up/down based on vertical error
kz = 0.04
def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
cap = find_camera()
if cap is None:
raise RuntimeError("No camera found. Please connect a camera.")
hand_tracker = HandTracker()
head_pose = np.eye(4)
euler_rot = np.array([0.0, 0.0, 0.0])
while not stop_event.is_set():
success, img = cap.read()
if not success:
print("Failed to capture image from camera.")
continue
hands = hand_tracker.get_hands_positions(img)
if hands:
hand = hands[0] # Assuming we only track the first detected hand
if gui:
draw_hand(img, hand)
error = np.array([0, 0]) - hand
error = np.clip(
error, -self.max_delta, self.max_delta
) # Limit error to avoid extreme movements
euler_rot += np.array(
[0.0, -self.kp * 0.1 * error[1], self.kp * error[0]]
)
head_pose[:3, :3] = R.from_euler(
"xyz", euler_rot, degrees=False
).as_matrix()
head_pose[:3, 3][2] = (
error[1] * self.kz
) # Adjust height based on vertical error
reachy_mini.set_target(head=head_pose)
if gui:
cv2.imshow("Reachy Mini Hand Tracker App", img)
if cv2.waitKey(1) & 0xFF == ord("q"):
break
def draw_hand(img, hand):
"""Draw debug information on the image."""
h, w, _ = img.shape
draw_palm = [(-hand[0] + 1) / 2, (hand[1] + 1) / 2] # [0, 1]
cv2.circle(
img,
(int(w - draw_palm[0] * w), int(draw_palm[1] * h)),
radius=5,
color=(0, 0, 255),
thickness=-1,
)
_target = [0.5, 0.5]
cv2.circle(
img,
(int(_target[0] * w), int(_target[1] * h)),
radius=5,
color=(255, 0, 0),
thickness=-1,
)
cv2.line(
img,
(int(draw_palm[0] * w), int(draw_palm[1] * h)),
(int(_target[0] * w), int(_target[1] * h)),
color=(0, 255, 0),
thickness=2,
)
if __name__ == "__main__":
# You can run the app directly from this script
with ReachyMini() as mini:
app = HandTrackerApp()
stop = threading.Event()
try:
print("Running '{{ app_name }}' a ReachyMiniApp...")
print("Press Ctrl+C to stop the app.")
app.run(mini, stop)
print("App has stopped.")
except KeyboardInterrupt:
print("Stopping the app...")
stop.set()
|