You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
56 lines
1.6 KiB
Python
56 lines
1.6 KiB
Python
import numpy as np
|
|
|
|
class ArmController:
|
|
def __init__(self, config, mirror=False):
|
|
#self.config = config
|
|
#self.mirror = mirror
|
|
|
|
#self.shoulder_idx = {'left': 11, 'right': 12}
|
|
#self.elbow_idx = {'left': 13, 'right': 14}
|
|
#self.wrist_idx = {'left': 15, 'right': 16}
|
|
#self.hip_idx = {'left': 23, 'right': 24}
|
|
|
|
self.r_wirst_id = 15 #16
|
|
self.r_elbow_id = 13 #14
|
|
self.r_shoulder_id = 11 # 12
|
|
|
|
self.debug = config.get('debug', False)
|
|
|
|
|
|
def _get_angle(self, x1, y1, x2, y2):
|
|
angle = np.arctan2(y2 - y1, x2 - x1)
|
|
return angle
|
|
|
|
|
|
|
|
def compute_speeds(self, landmarks):
|
|
|
|
linear = 0.
|
|
angular = 0.
|
|
|
|
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_point = landmarks[self.r_shoulder_id]
|
|
right_shoulder_x = right_shoulder_point[0]
|
|
right_shoulder_y = right_shoulder_point[1]
|
|
|
|
right_elbow_point = landmarks[self.r_elbow_id] # [x, y, z, v]
|
|
right_elbow_x = right_elbow_point[0]
|
|
right_elbow_y = right_elbow_point[1]
|
|
|
|
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)
|
|
|
|
print(f"Angle between shoulder and elbow is {angle_shoulder_elbow}")
|
|
|
|
if (angle_shoulder_elbow < five_deg_in_rad) and (angle_shoulder_elbow > -five_deg_in_rad):
|
|
linear = 1.
|
|
|
|
return linear, angular
|
|
|