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

fix angle

Browse Source
This commit is contained in:
gestures5
2026-07-10 14:36:49 +03:00
parent 35b185e3f5
commit e5e9354a51
1 changed files with 34 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
gesture_control/arm_control.py
+31 -18
View File
@@ -144,39 +144,51 @@ class ArmController:
#right_hand_y = landmarks[16][1]
#right_distance = right_shoulder_y - right_hand_y
angle_elbow_wirst_right = self._get_angle(landmarks[14], landmarks[16])
angle_elbow_wirst_left = self._get_angle(landmarks[13], landmarks[15])
angle_elbow_wirst_left = self._get_angle(landmarks[14], landmarks[16])
angle_elbow_wirst_right = self._get_angle(landmarks[13], landmarks[15])
linear, angular = 0, 0
if np.deg2rad(-5) < angle_elbow_wirst_left < np.deg2rad(5):
if np.deg2rad(-5) < angle_elbow_wirst_right < np.deg2rad(5):
linear = 0
elif np.deg2rad(-5) > angle_elbow_wirst_left:
linear = min(1, angle_elbow_wirst_left / -np.deg2rad(90))
elif np.deg2rad(-5) > angle_elbow_wirst_right:
linear = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
elif np.deg2rad(5) < angle_elbow_wirst_left:
linear = min(1, angle_elbow_wirst_left / -np.deg2rad(90))
elif np.deg2rad(5) < angle_elbow_wirst_right:
linear = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
#print(f"Test: {linear} {angle_elbow_wirst_left}")
error = 35
error = 15
if np.deg2rad(180 - error) < angle_elbow_wirst_right < np.deg2rad(180 + error):
#print(f"Test: {angle_elbow_wirst_right} {np.rad2deg(angle_elbow_wirst_right)}")
#if np.deg2rad(180 - error) < angle_elbow_wirst_left < np.deg2rad(180 + error):
max_angle = np.deg2rad(180 - error)
min_angle = np.deg2rad(-180 + error)
if angle_elbow_wirst_left < 0:
if angle_elbow_wirst_left < min_angle:# or angle_elbow_wirst_left > max_angle:
angular = 0
elif angle_elbow_wirst_left > min_angle:
#angular = min(1, angle_elbow_wirst_left / min_angle)
angular = -(min_angle - angle_elbow_wirst_left) / (min_angle - np.deg2rad(-90))
elif np.deg2rad(180 - error) > angle_elbow_wirst_right:
angular = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
else:
if angle_elbow_wirst_left > max_angle:
angular = 0
elif angle_elbow_wirst_left < max_angle:
#angular = -min(1, angle_elbow_wirst_left / max_angle)
angular = (max_angle - angle_elbow_wirst_left) / (max_angle - np.deg2rad(90))
elif np.deg2rad(180 + error) < angle_elbow_wirst_right:
angular = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
if -np.deg2rad(error) < angle_elbow_wirst_right < np.deg2rad(error):
error = 35
if -np.deg2rad(error) < angle_elbow_wirst_left < np.deg2rad(error):
linear = 0
angular = 0
angular = angular / 2
#print(f"Test: {angular} {angle_elbow_wirst_right}")
#angular = angular / 2
#left_shoulder_y = landmarks[11][1]
@@ -195,3 +207,4 @@ class ArmController:
#angular = norm_ang * self.config['max_speed_linear']
return linear, angular
#return angular, linear