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.
198 lines
7.0 KiB
Python
198 lines
7.0 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.wrist_idx = {'left': 15, 'right': 16}
|
|
self.hip_idx = {'left': 23, 'right': 24}
|
|
self.dead_zone = config.get('dead_zone', 0.2)
|
|
self.debug = config.get('debug', False)
|
|
|
|
def _get_side_indices(self, side):
|
|
"""
|
|
Возвращает (shoulder_idx, wrist_idx) для заданной стороны (left/right).
|
|
При mirror=True интерпретируем сторону как в реальности: левая/правая рука.
|
|
"""
|
|
if self.mirror:
|
|
if side == 'left':
|
|
s_idx = 12
|
|
w_idx = 16
|
|
else:
|
|
s_idx = 11
|
|
w_idx = 15
|
|
else:
|
|
if side == 'left':
|
|
s_idx = 11
|
|
w_idx = 15
|
|
else:
|
|
s_idx = 12
|
|
w_idx = 16
|
|
return s_idx, w_idx
|
|
|
|
def _get_shoulder_width(self, landmarks):
|
|
"""Ширина плеч для нормировки горизонтальных смещений."""
|
|
left = landmarks[11][:2]
|
|
right = landmarks[12][:2]
|
|
width = np.linalg.norm(right - left)
|
|
if width < 50 or width > 300:
|
|
return None
|
|
return width
|
|
|
|
def _get_torso_height(self, landmarks):
|
|
"""Высота торса для нормировки вертикальных смещений."""
|
|
left_shoulder = landmarks[11][:2]
|
|
right_shoulder = landmarks[12][:2]
|
|
left_hip = landmarks[23][:2]
|
|
right_hip = landmarks[24][:2]
|
|
shoulder_center = (left_shoulder + right_shoulder) / 2
|
|
hip_center = (left_hip + right_hip) / 2
|
|
height = np.linalg.norm(shoulder_center - hip_center)
|
|
if height < 50:
|
|
return None
|
|
return height
|
|
|
|
def _horizontal_displacement_rel(self, landmarks, side):
|
|
"""
|
|
Нормированное горизонтальное смещение запястья относительно плеча.
|
|
Сторона `side` — это реальная сторона руки
|
|
"""
|
|
s_idx, w_idx = self._get_side_indices(side)
|
|
|
|
if landmarks[s_idx][3] < 0.5 or landmarks[w_idx][3] < 0.5:
|
|
return 0.0
|
|
|
|
shoulder = landmarks[s_idx][:2]
|
|
wrist = landmarks[w_idx][:2]
|
|
|
|
shoulder_width = self._get_shoulder_width(landmarks)
|
|
if shoulder_width is None:
|
|
return 0.0
|
|
|
|
disp = wrist[0] - shoulder[0]
|
|
return disp / shoulder_width
|
|
|
|
def _vertical_displacement_rel(self, landmarks, side):
|
|
"""
|
|
Вертикальное смещение: верх/низ запястья относительно плеча.
|
|
Сторона `side` — реальная сторона руки.
|
|
"""
|
|
s_idx, w_idx = self._get_side_indices(side)
|
|
|
|
if landmarks[s_idx][3] < 0.5 or landmarks[w_idx][3] < 0.5:
|
|
return 0.0
|
|
|
|
shoulder = landmarks[s_idx][:2]
|
|
wrist = landmarks[w_idx][:2]
|
|
|
|
torso_height = self._get_torso_height(landmarks)
|
|
if torso_height is None:
|
|
return 0.0
|
|
|
|
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
|
|
landmarks[15][3] < 0.5 or landmarks[16][3] < 0.5):
|
|
if self.debug:
|
|
print("Руки не видны")
|
|
return 0.0, 0.0
|
|
|
|
linear_side = self.config['linear_arm']
|
|
angular_side = self.config['angular_arm']
|
|
|
|
lin_rel = self._horizontal_displacement_rel(landmarks, linear_side)
|
|
ang_rel = self._vertical_displacement_rel(landmarks, angular_side)
|
|
|
|
if self.debug:
|
|
print(f"lin_rel={lin_rel:.3f}, ang_rel={ang_rel:.3f}")
|
|
|
|
# Линейная скорость (только вперёд)
|
|
if lin_rel < self.dead_zone:
|
|
linear = 0.0
|
|
else:
|
|
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']
|
|
angular = 0.0
|
|
else:
|
|
ang_rel_clipped = np.clip(ang_rel, -1.0, 1.0)
|
|
angular = ang_rel_clipped * self.config['max_speed_angular']
|
|
|
|
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_right = self._get_angle(landmarks[14], landmarks[16])
|
|
angle_elbow_wirst_left = self._get_angle(landmarks[13], landmarks[15])
|
|
|
|
linear, angular = 0, 0
|
|
|
|
if np.deg2rad(-5) < angle_elbow_wirst_left < 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_left:
|
|
linear = min(1, angle_elbow_wirst_left / -np.deg2rad(90))
|
|
|
|
#print(f"Test: {linear} {angle_elbow_wirst_left}")
|
|
|
|
error = 35
|
|
|
|
if np.deg2rad(180 - error) < angle_elbow_wirst_right < np.deg2rad(180 + error):
|
|
angular = 0
|
|
|
|
elif np.deg2rad(180 - error) > angle_elbow_wirst_right:
|
|
angular = min(1, angle_elbow_wirst_right / -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):
|
|
linear = 0
|
|
angular = 0
|
|
|
|
angular = angular / 2
|
|
#print(f"Test: {angular} {angle_elbow_wirst_right}")
|
|
|
|
|
|
#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)
|
|
|
|
#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
|