IK(역기구학)를 구하는 방법

flowchart TD
    A["초기 추정값 q₀ (시드 상태)"] --> B["FK 계산: x = f(q)"]
    B --> C{"오차 Δx = x_d - f(q)\n충분히 작은가?"}
    C -->|"‖Δx‖ < ε"| D["수렴 완료\n관절 각도 q 반환"]
    C -->|"‖Δx‖ ≥ ε"| E["야코비안 J(q) 계산"]
    E --> F["q = q + J†·Δx"]
    F --> G{"반복 횟수 초과?"}
    G -->|"아니오"| B
    G -->|"예"| H["실패: 해를 찾지 못함"]

# kinematics.yaml - MoveIt IK 솔버 설정
manipulator:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005   # 탐색 해상도 (rad)
  kinematics_solver_timeout: 0.05              # 타임아웃 50ms
  kinematics_solver_attempts: 3                # 최대 3회 시도 (서로 다른 시드)

flowchart LR
    subgraph "방법 A: 간접 호출"
        A1["목표 Pose 설정"] --> A2["MoveGroup"]
        A2 --> A3["IK 자동 계산"]
        A3 --> A4["경로 계획"]
        A4 --> A5["실행"]
    end
 
    subgraph "방법 B: 직접 호출"
        B1["목표 Pose 설정"] --> B2["/compute_ik 서비스"]
        B2 --> B3["관절각 반환"]
        B3 --> B4["컨트롤러에 직접 발행"]
    end

# MoveIt을 통한 간접 IK 호출
# 목표 pose만 지정하면 IK + 경로 계획이 자동 수행됨
pose = make_pose(x, y, z, roll, pitch, yaw)
helper.plan_to_pose_goal(pose)  # IK → 경로 계획 → 실행까지 한 번에

# /compute_ik 서비스를 직접 호출하여 IK만 계산
request = GetPositionIK.Request()
request.ik_request.group_name = 'manipulator'
 
# 원하는 목표 위치/자세 설정
request.ik_request.pose_stamped.pose = target_pose
 
# 시드(seed) 상태: 현재 관절 각도를 초기값으로 전달
request.ik_request.robot_state = current_robot_state
 
# 타임아웃: 50ms 이내에 해를 찾아야 함
request.ik_request.timeout.nanosec = int(0.05 * 1e9)
 
# 서비스 호출 → 관절 각도 반환
result = ik_client.call(request)
joint_angles = result.solution.joint_state
# → {joint1: θ1, joint2: θ2, ..., joint6: θ6}

stateDiagram-v2
    state "현재 관절 상태 [θ1...θ6]" as Current
    state "IK 솔버 (KDL)" as Solver
    state "가장 가까운 해 선택" as Select
    state "부드러운 연속 동작" as Smooth
 
    Current --> Solver : 시드로 전달
    Solver --> Select : 현재 상태 근처에서 탐색
    Select --> Smooth : 관절 점프 없이 이동
 
    state "시드 없음" as NoSeed
    state "IK 솔버" as Solver2
    state "임의의 해 반환" as Random
    state "관절 점프/불연속 동작" as Jump
 
    NoSeed --> Solver2 : 초기값 불명확
    Solver2 --> Random : 매번 다른 해 수렴
    Random --> Jump : 위험한 동작 발생 가능

# 올바른 시드 상태 전달 예시
from sensor_msgs.msg import JointState
 
# 현재 관절 상태를 구독하여 시드로 사용
current_joints = get_current_joint_state()  # 현재 관절 각도 구독
request.ik_request.robot_state.joint_state = current_joints
# → 솔버가 현재 상태에서 가장 가까운 해를 반환

sequenceDiagram
    participant User as 사용자 입력
    participant Node as ROS 노드
    participant IK as /compute_ik
    participant Ctrl as /arm_controller
 
    loop 30Hz 실시간 루프
        User->>Node: 목표 좌표 (x, y, z)
        Node->>Node: 현재 관절 상태 조회 (시드)
        Node->>IK: IK 요청 (목표 + 시드)
        IK-->>Node: 관절 각도 [θ1...θ6]
        Node->>Ctrl: JointTrajectory 발행
        Ctrl-->>Node: 로봇 동작 실행
    end

from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration
 
# IK로 얻은 관절각을 trajectory 메시지로 변환
msg = JointTrajectory()
msg.joint_names = ['joint1', 'joint2', 'joint3',
                   'joint4', 'joint5', 'joint6']
 
point = JointTrajectoryPoint()
point.positions = [θ1, θ2, θ3, θ4, θ5, θ6]  # IK 결과
point.time_from_start = Duration(sec=0, nanosec=int(0.05 * 1e9))  # 50ms
 
msg.points = [point]
 
# 컨트롤러 토픽에 발행 → 로봇 동작
publisher.publish(msg)  # → /arm_controller/joint_trajectory

# TRAC-IK로 솔버 변경
manipulator:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 0.05