IK(역기구학)란 무엇인가

flowchart LR
    subgraph FK["정기구학 (FK)"]
        A["관절 각도<br/>θ₁, θ₂, ..., θₙ"] -->|변환 행렬 곱| B["엔드 이펙터<br/>위치/자세 (x,y,z,R)"]
    end
    subgraph IK["역기구학 (IK)"]
        C["엔드 이펙터<br/>목표 위치/자세"] -->|역계산| D["관절 각도<br/>θ₁, θ₂, ..., θₙ"]
    end
    B -.->|"역방향 문제"| C

flowchart TD
    TARGET["목표 좌표 (x, y)"] --> COSQ2["cos θ₂ 계산"]
    COSQ2 --> CHECK{"|cos θ₂| ≤ 1?"}
    CHECK -->|No| NOANSWER["해 없음<br/>(도달 불가)"]
    CHECK -->|Yes| THETA2["θ₂ = atan2(±√(1-cos²θ₂), cos θ₂)"]
    THETA2 --> ELBOW_UP["해 1: Elbow-Up (+)"]
    THETA2 --> ELBOW_DOWN["해 2: Elbow-Down (-)"]
    ELBOW_UP --> THETA1_1["θ₁ 계산"]
    ELBOW_DOWN --> THETA1_2["θ₁ 계산"]

flowchart TD
    START["IK 풀이 방법 선택"] --> Q1{"로봇 구조가<br/>단순한가?"}
    Q1 -->|Yes| ANALYTICAL["해석적 방법<br/>(Analytical)"]
    Q1 -->|No| Q2{"실시간 성능<br/>필요?"}
    Q2 -->|Yes| CCD_FABRIK["CCD / FABRIK"]
    Q2 -->|No| JACOBIAN["Jacobian 기반<br/>(Pseudo-Inverse, DLS)"]
    ANALYTICAL --> RESULT["정확한 다중 해"]
    CCD_FABRIK --> RESULT2["빠른 근사 해"]
    JACOBIAN --> RESULT3["수렴된 수치 해"]

import numpy as np
import matplotlib.pyplot as plt
 
def inverse_kinematics_2link(x: float, y: float, l1: float, l2: float):
    """2-Link 평면 로봇 팔의 역기구학 계산"""
    # 목표 거리 계산
    dist = x**2 + y**2
 
    # cos(θ₂) 계산 — 코사인 법칙 활용
    cos_theta2 = (dist - l1**2 - l2**2) / (2 * l1 * l2)
 
    # 도달 가능 여부 확인
    if abs(cos_theta2) > 1.0:
        raise ValueError(f"목표 ({x}, {y})은 도달 불가능합니다.")
 
    # θ₂: elbow-up, elbow-down 두 가지 해
    sin_theta2_pos = np.sqrt(1 - cos_theta2**2)   # elbow-up
    sin_theta2_neg = -sin_theta2_pos                # elbow-down
 
    solutions = []
    for sin_theta2 in [sin_theta2_pos, sin_theta2_neg]:
        theta2 = np.arctan2(sin_theta2, cos_theta2)
        theta1 = np.arctan2(y, x) - np.arctan2(
            l2 * sin_theta2, l1 + l2 * cos_theta2
        )
        solutions.append((np.degrees(theta1), np.degrees(theta2)))
 
    return solutions
 
# 사용 예시
l1, l2 = 1.0, 0.8          # 링크 길이
target_x, target_y = 1.2, 0.6  # 목표 좌표
 
solutions = inverse_kinematics_2link(target_x, target_y, l1, l2)
for i, (t1, t2) in enumerate(solutions):
    label = "Elbow-Up" if i == 0 else "Elbow-Down"
    print(f"{label}: θ₁ = {t1:.2f}°, θ₂ = {t2:.2f}°")

# MoveIt 2에서 IK를 사용하는 간단한 예시 (개념 코드)
from moveit.core.robot_state import RobotState
 
# 목표 위치/자세 설정
target_pose = Pose()
target_pose.position.x = 0.5
target_pose.position.y = 0.2
target_pose.position.z = 0.3
 
# IK 계산 — MoveIt이 내부적으로 설정된 IK 솔버를 호출
robot_state = RobotState(robot_model)
success = robot_state.set_from_ik(
    joint_model_group,  # 사용할 관절 그룹 (예: "arm")
    target_pose,        # 목표 위치/자세
    timeout=0.1         # 타임아웃 (초)
)
 
if success:
    joint_values = robot_state.get_joint_group_positions(joint_model_group)
    print(f"IK 성공! 관절 각도: {joint_values}")
else:
    print("IK 실패: 도달 불가능하거나 타임아웃")