Обратная кинематика для планарного робота с 2 степенями свободы ⇐ Python

[Гость]

У меня есть простой планарный робот с 2 степенями свободы такой конфигурации:

L1 и L2 — длины моих двух рук. L1 составляет 245,5 мм, а L2 — 160 мм. Альфа — это первый угол (основной), а бета — второй угол, прикрепленный к первому рычагу.
Когда я программировал Arduino и пробовал выполнять математические расчеты, я просто получил противоречивые результаты. . Очки были просто полностью потеряны. Даже линейное движение вдоль одной оси не работало. Поэтому я написал простой скрипт Python с точно такой же математикой, в котором я сохраняю константу x или y и увеличиваю другую с небольшим шагом. Затем я построил график рассчитанных углов альфа и бета. Я ожидаю, что они будут увеличиваться или уменьшаться линейно (примерно как пунктирные линии). Но вместо этого я получаю следующие результаты:

Я использую стандартные формулы, которые можно найти по всему Интернету (Источник оригинала), но они явно не работают. Вот они:
$\beta = cos^{-1}(\frac{x^2+y^2-L_{1}^2-L_{2}^2 }{2*L_{1}*L_2})$
$\alpha = tan^{-1}(\frac{y}{x})-tan^{-1 }(\frac{L_2sin(\beta)}{L_1+L_2cos(\beta)})$
Также вот мой код Python, с помощью которого Я делаю расчеты:

Код: Выделить всё

import math

L1 = 245.5
L2 = 160

x = 100
y = 70

def calc_IK_beta(x, y):
beta = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
beta = math.acos(beta)

beta_d = beta * 180 / math.pi

if x < 0 and y > 0:
beta_d = -beta_d
elif x < 0 and y < 0:
beta_d = -180 - beta_d
elif x > 0 and y < 0:
beta_d = -beta_d

return beta #in radians

def calc_IK_alpha(x, y):
beta = calc_IK_beta(x, y)

alpha = math.atan2(y, x) - math.atan2(L2 * math.sin(beta), L1 + L2 * math.cos(beta))

alpha_d = alpha * 180 / math.pi

if x < 0 and y > 0:
alpha_d = -alpha_d
elif x < 0 and y < 0:
alpha_d = -180 - alpha_d
elif x > 0 and y < 0:
alpha_d = 180 - alpha_d

return alpha #in radians

Когда я программировал Arduino и пробовал выполнять математические расчеты, я получил противоречивые результаты. Очки были просто полностью потеряны. Даже линейное движение вдоль одной оси не работало. Поэтому я написал простой скрипт Python с точно такой же математикой, в котором я сохраняю константу x или y и увеличиваю другую с небольшим шагом. Затем я построил график рассчитанных углов альфа и бета. Я ожидаю, что они будут увеличиваться или уменьшаться линейно (примерно как пунктирные линии).

Подробнее здесь: https://stackoverflow.com/questions/781 ... anar-robot