(x, y)
           ●
          ╱
         ╱ L₂
        ╱
       ● θ₂
      ╱
     ╱ L₁
    ╱
   ● θ₁
   │
───┴───  (база)

import math

def forward_kinematics(L1, L2, theta1, theta2):
    """
    Прямая кинематика для 2-звенного манипулятора
    L1, L2 - длины звеньев (см)
    theta1, theta2 - углы (радианы)
    """
    x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2)
    y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2)
    return x, y

# Пример
L1, L2 = 10, 8  # см
theta1 = math.radians(30)
theta2 = math.radians(45)

x, y = forward_kinematics(L1, L2, theta1, theta2)
print(f"Позиция: ({x:.2f}, {y:.2f}) см")

Сустав 1: a=0, α=90°, d=0, θ=θ₁
Сустав 2: a=L1, α=0°, d=0, θ=θ₂
...

Точка недостижима:        Два решения:
                          
    ●                         ●
    (слишком далеко)       ╱   ╲
                          ╱     ╲
    ●───●                ╱       ╲
    (робот)             (локоть вверх/вниз)

         ●(x, y)
        ╱╲
    L₂ ╱  ╲ ?
      ╱    ╲
     ●      ╲
    ╱        ╲
L₁ ╱          ╲
  ╱            ╲
 ●──────────────
      d = √(x² + y²)

def inverse_kinematics(L1, L2, x, y, elbow_up=True):
    """
    Обратная кинематика для 2-звенного манипулятора
    elbow_up: True = локоть вверх, False = локоть вниз
    """
    d_sq = x**2 + y**2
    d = math.sqrt(d_sq)
    
    # Проверка достижимости
    if d > L1 + L2 or d < abs(L1 - L2):
        raise ValueError("Точка недостижима!")
    
    # Угол локтя
    cos_theta2 = (d_sq - L1**2 - L2**2) / (2 * L1 * L2)
    theta2 = math.acos(cos_theta2)
    if not elbow_up:
        theta2 = -theta2
    
    # Угол плеча
    alpha = math.atan2(y, x)
    beta = math.atan2(L2 * math.sin(theta2), L1 + L2 * math.cos(theta2))
    theta1 = alpha - beta
    
    return theta1, theta2

# Тест
x_target, y_target = 12, 8
theta1, theta2 = inverse_kinematics(10, 8, x_target, y_target)
x_result, y_result = forward_kinematics(10, 8, theta1, theta2)

print(f"Цель: ({x_target}, {y_target})")
print(f"Результат: ({x_result:.4f}, {y_result:.4f})")
# Должны совпадать!

       Камера видит объект          Рука должна схватить
       в своих координатах          в своих координатах
             │                            │
             ▼                            ▼
        ┌─────────┐                 ┌─────────┐
        │ Камера  │ ─── TF ───────► │  Рука   │
        │ (x,y,z) │   transform     │ (x,y,z) │
        └─────────┘                 └─────────┘

                    map
                     │
                     ▼
                   odom
                     │
                     ▼
                 base_link
                ╱    │    ╲
               ▼     ▼     ▼
           camera  lidar  arm_base
                            │
                            ▼
                        arm_link1
                            │
                            ▼
                        arm_link2
                            │
                            ▼
                         gripper

# Визуализация с matplotlib
import matplotlib.pyplot as plt
import numpy as np

def draw_arm(L1, L2, theta1, theta2):
    # Позиции суставов
    x0, y0 = 0, 0
    x1 = L1 * np.cos(theta1)
    y1 = L1 * np.sin(theta1)
    x2, y2 = forward_kinematics(L1, L2, theta1, theta2)
    
    plt.figure(figsize=(8, 8))
    plt.plot([x0, x1, x2], [y0, y1, y2], 'o-', linewidth=3)
    plt.xlim(-20, 20)
    plt.ylim(-5, 20)
    plt.grid(True)
    plt.axis('equal')
    plt.title(f'θ1={np.degrees(theta1):.1f}°, θ2={np.degrees(theta2):.1f}°')
    plt.show()

# Визуализация рабочей зоны
theta1_range = np.linspace(0, np.pi, 50)
theta2_range = np.linspace(-np.pi, np.pi, 50)

points = []
for t1 in theta1_range:
    for t2 in theta2_range:
        x, y = forward_kinematics(10, 8, t1, t2)
        points.append((x, y))

xs, ys = zip(*points)
plt.scatter(xs, ys, s=1, alpha=0.5)
plt.title('Рабочая зона манипулятора')
plt.axis('equal')
plt.show()