From 76af85ad0606ef2a60390299c1e172a5e73d7293 Mon Sep 17 00:00:00 2001 From: gestures5 Date: Fri, 10 Jul 2026 10:07:27 +0300 Subject: [PATCH] dominika version --- gesture_control/arm_control.py | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/gesture_control/arm_control.py b/gesture_control/arm_control.py index 68f4b44..95a1cd6 100644 --- a/gesture_control/arm_control.py +++ b/gesture_control/arm_control.py @@ -6,7 +6,7 @@ class ArmController: #self.mirror = mirror #self.shoulder_idx = {'left': 11, 'right': 12} - #self.elbow_idx = {'left': 13, 'right': 14} + #self.elbow_idx = {'left': 13, 'ri ght': 14} #self.wrist_idx = {'left': 15, 'right': 16} #self.hip_idx = {'left': 23, 'right': 24} @@ -14,6 +14,9 @@ class ArmController: self.r_elbow_id = 13 #14 self.r_shoulder_id = 11 # 12 + self.error = 23 + self.bag = 27 + self.debug = config.get('debug', False) @@ -44,12 +47,27 @@ class ArmController: angle_shoulder_elbow = self._get_angle(right_shoulder_x, right_shoulder_y, right_elbow_x, right_elbow_y) - five_deg_in_rad = np.deg2rad(5) + right_wirst_point = landmarks[self.r_wirst_id] + right_wirst_x = right_wirst_point[0] + right_wirst_y = right_wirst_point[1] + + angle_elbow_wirst = self._get_angle(right_elbow_x, right_elbow_y, right_wirst_x, right_wirst_y) - print(f"Angle between shoulder and elbow is {angle_shoulder_elbow}") + five_deg_in_rad = np.deg2rad(self.error) - if (angle_shoulder_elbow < five_deg_in_rad) and (angle_shoulder_elbow > -five_deg_in_rad): - linear = 1. + + if (angle_shoulder_elbow < np.pi/2.5 + five_deg_in_rad) and (angle_shoulder_elbow > np.pi/2.5 -five_deg_in_rad): + print(f"Angle between shoulder and elbow is {angle_shoulder_elbow} {np.rad2deg(angle_shoulder_elbow)}") + #print(f"Angle between elbow and wirst is {angle_elbow_wirst}") + if (angle_elbow_wirst > np.deg2rad(-90 - self.error)) and (angle_elbow_wirst < np.deg2rad(-90 + self.error)): + linear = 1. + if (angle_elbow_wirst > np.deg2rad(90 - self.error)) and (angle_elbow_wirst < np.deg2rad(90 + self.error)): + linear = -1. + if (angle_elbow_wirst > np.deg2rad(-45 - self.bag)) and (angle_elbow_wirst < np.deg2rad(-45 + self.bag)): + angular = -1. + if (angle_elbow_wirst > np.deg2rad(-135 - self.bag)) and (angle_elbow_wirst < np.deg2rad(-135 + self.bag)): + angular = 1. return linear, angular +