sirius/gesture_rec
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

dominika version

Browse Source
This commit is contained in:
gestures5
2026-07-10 10:07:27 +03:00
parent b73f67182f
commit 76af85ad06
1 changed files with 23 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gesture_control/arm_control.py
+22 -4
View File
@@ -6,7 +6,7 @@ class ArmController:
#self.mirror = mirror #self.mirror = mirror
#self.shoulder_idx = {'left': 11, 'right': 12} #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.wrist_idx = {'left': 15, 'right': 16}
#self.hip_idx = {'left': 23, 'right': 24} #self.hip_idx = {'left': 23, 'right': 24}
@@ -14,6 +14,9 @@ class ArmController:
self.r_elbow_id = 13 #14 self.r_elbow_id = 13 #14
self.r_shoulder_id = 11 # 12 self.r_shoulder_id = 11 # 12
self.error = 23
self.bag = 27
self.debug = config.get('debug', False) 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) 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]
print(f"Angle between shoulder and elbow is {angle_shoulder_elbow}") angle_elbow_wirst = self._get_angle(right_elbow_x, right_elbow_y, right_wirst_x, right_wirst_y)
if (angle_shoulder_elbow < five_deg_in_rad) and (angle_shoulder_elbow > -five_deg_in_rad): five_deg_in_rad = np.deg2rad(self.error)
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. 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 return linear, angular