4관절 로봇팔 제어 코드

import numpy as np

def inverse_kinematics(x, y, z, L1, L2, L3, L4):
    """
    4자유도 로봇팔의 역기구학 계산
    x, y, z: 목표 위치
    L1, L2, L3, L4: 각 링크(팔)의 길이
    반환: (θ1, θ2, θ3, θ4)
    """
    # 1. 베이스 회전각 θ1
    theta1 = np.arctan2(y, x) * (180 / np.pi)

    # 2. 목표점까지의 수평 거리 (r) 및 높이 (h)
    r = np.sqrt(x**2 + y**2)
    h = z - L1  # L1은 베이스 높이

    # 3. 목표점까지의 직선 거리 d
    d = np.sqrt(r**2 + h**2)

    # 4. θ3 계산 (Law of Cosines)
    cos_theta3 = (L2**2 + L3**2 - d**2) / (2 * L2 * L3)
    theta3 = np.arccos(np.clip(cos_theta3, -1.0, 1.0)) * (180 / np.pi)

    # 5. θ2 계산
    alpha = np.arctan2(h, r) * (180 / np.pi)
    beta = np.arccos(np.clip((L2**2 + d**2 - L3**2) / (2 * L2 * d), -1.0, 1.0)) * (180 / np.pi)
    theta2 = alpha + beta

    # 6. 손목 회전각 θ4
    theta4 = -(theta2 + theta3)  # 기본적으로 손목을 목표 방향과 정렬

    return round(theta1, 2), round(theta2, 2), round(theta3, 2), round(theta4, 2)

# 테스트
L1, L2, L3, L4 = 10, 15, 15, 5  # 링크 길이 예제
x, y, z = 20, 10, 15  # 목표 위치 예제

theta1, theta2, theta3, theta4 = inverse_kinematics(x, y, z, L1, L2, L3, L4)
print(f"θ1: {theta1}°, θ2: {theta2}°, θ3: {theta3}°, θ4: {theta4}°")