last dimas

gesture_control/arm_control.py

@@ -92,6 +92,7 @@ class ArmController:
 
         disp = shoulder[1] - wrist[1]
         return disp / torso_height
+
     '''
     def compute_speeds(self, landmarks):
         if (landmarks[11][3] < 0.5 or landmarks[12][3] < 0.5 or
@@ -116,7 +117,7 @@ class ArmController:
             linear = min(lin_rel, 1.0) * self.config['max_speed_linear']
 
         # Угловая скорость
-        if abs(ang_rel) < self.dead_zone:norm_lin * self.config['max_speed_linear']
+        if abs(ang_rel) < self.dead_zone:
             angular = 0.0
         else:
             ang_rel_clipped = np.clip(ang_rel, -1.0, 1.0)
@@ -124,87 +125,24 @@ class ArmController:
 
         return linear, angular
     '''
-
-    def _get_angle(self, landmark1, landmark2):
-        x1 = landmark1[0]
-        y1 = landmark1[1]
-        x2 = landmark2[0]
-        y2 = landmark2[1]
-        angle = np.arctan2(y2 - y1, x2 - x1)
-        return angle
-
-    def compute_speeds(self, landmarks):
-        if (landmarks[11][3] < 0.5 or landmarks[12][3] < 0.5 or
-            landmarks[15][3] < 0.5 or landmarks[16][3] < 0.5):
-            if self.debug:
-                print("Руки не видны")
-            return 0.0, 0.0
-
-        #right_shoulder_y = landmarks[12][1]
-        #right_hand_y = landmarks[16][1]
-        #right_distance = right_shoulder_y - right_hand_y
-
-        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_right < np.deg2rad(5):
-            linear = 0
-
-        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_right:
-            linear = min(1, angle_elbow_wirst_right / -np.deg2rad(90))
-
-        #print(f"Test: {linear} {angle_elbow_wirst_left}")
-
-        error = 15
-
-        #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))
-
-        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))
-
-        error = 35
-        if -np.deg2rad(error) < angle_elbow_wirst_left < np.deg2rad(error):
-            linear = 0
+    def compute_speeds(self, w, h, landmarks):
+        linear = 0
+        angular = 0
+        if 3*w/5>landmarks[19][0]>2*w/5:
             angular = 0
-
-        #angular = angular / 2
+        if 3*w/5<landmarks[19][0]:
+            angular = (landmarks[19][0]*5)/(2*w)-1
+        if landmarks[15][0]<2*w/5:
+            angular = (landmarks[19][0]-3*w/5)/(2*w/5)
+        if 3*h/5>landmarks[19][1]>2*h/5:
+            linear = 0
+        if 3*h/5<landmarks[19][1]:
+            linear = -((landmarks[19][1]*5)/(2*h)-1)
+        if landmarks[19][1]<2*h/5:
+            linear = -(landmarks[19][1]-3*h/5)/(2*h/5)
 
 
-        #left_shoulder_y = landmarks[11][1]
-        #left_hand_y = landmarks[15][1]
-        #left_distance = left_shoulder_y - left_hand_y
 
-        #torso_height = self._get_torso_height(landmarks)
-        #max_distance = torso_height * 0.5
-        #norm_lin = np.clip((right_distance/max_distance), a_min = 0, a_max = 1)
+        print(f"{linear},{angular}")
+        return linear, angular
 
-        #torso_height = self._get_torso_height(landmarks)
-        #max_distance = torso_height * 0.5
-        #norm_ang = np.clip((left_distance/max_distance), a_min = -1, a_max = 1)
-
-        #linear = norm_lin * self.config['max_speed_linear']
-        #angular = norm_ang * self.config['max_speed_linear']
-
-        return linear, -angular
-        #return angular, linear