MoveIt2 핵심 개념 정리: 파싱, 쿼터니언, 비동기, Cartesian Path

flowchart LR
    A["XML 텍스트<br/>(긴 문자열)"] -->|ET.fromstring| B["트리 구조"]
    B --> C["robot"]
    C --> D["group_state"]
    D --> E["joint1: 0.0"]
    D --> F["joint2: -1.57"]
    D --> G["joint3: 1.57"]

<!-- SRDF 파일 원본 (그냥 텍스트) -->
<robot name="my_robot">
  <group_state name="home" group="manipulator">
    <joint name="joint1" value="0.0"/>
    <joint name="joint2" value="-1.57"/>
    <joint name="joint3" value="1.57"/>
  </group_state>
</robot>

import xml.etree.ElementTree as ET
 
# 파싱: 텍스트 → 트리 구조로 변환
root = ET.fromstring(srdf_text)
 
# 파싱 후: 원하는 값을 바로 접근 가능
group_state = root.find('.//group_state[@name="home"]')
for joint in group_state.findall('joint'):
    name = joint.get('name')     # "joint2"
    value = joint.get('value')   # "-1.57"
    print(f"{name}: {value}")

flowchart TB
    subgraph URDF ["URDF (물리적 구조)"]
        U1["링크 크기/질량"]
        U2["관절 범위/타입"]
        U3["메시 모델"]
    end
    subgraph SRDF ["SRDF (사용 설정)"]
        S1["그룹 정의"]
        S2["이름 포즈"]
        S3["충돌 무시"]
        S4["엔드이펙터"]
    end
    URDF -->|"+의미 정보"| SRDF
    SRDF --> MoveIt["MoveIt2 동작"]

<group name="manipulator">
  <chain base_link="base_link" tip_link="gripper_base"/>
</group>
<group name="gripper">
  <joint name="left_finger_joint"/>
</group>

<group_state name="home" group="manipulator">
  <joint name="joint1" value="0.0"/>
  <!-- ... -->
</group_state>

<disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>

<end_effector name="gripper_ee" parent_link="gripper_base"
              group="gripper" parent_group="manipulator"/>

# SRDF XML 직접 파싱
tree = ET.fromstring(srdf_text)
for joint in group_state.findall('joint'):
    name = joint.get('name')
    value = joint.get('value')
 
# MoveIt 메시지 직접 구성
request = MotionPlanRequest()
constraint = JointConstraint()
constraint.joint_name = 'joint1'
constraint.position = 0.0
# ... 하나하나 다 만듦
 
# Action 직접 호출
goal = MoveGroup.Goal()
goal.request = request
self._action_client.send_goal_async(goal)

helper = MoveGroupHelper(node)
helper.go_to_named_target('home')          # 한 줄로 끝
helper.go_to_joint_goal([0, 0.5, 0, ...])  # 한 줄로 끝

flowchart LR
    subgraph before ["ex02: 매번 직접 작성"]
        A1["SRDF 파싱"] --> B1["메시지 구성"] --> C1["Action 호출"]
    end
    subgraph after ["ex03~: utils.py 활용"]
        U["utils.py<br/>MoveGroupHelper"] --> D1["go_to_named_target()"]
        U --> D2["go_to_joint_goal()"]
        U --> D3["go_to_pose_goal()"]
        U --> D4["execute_trajectory()"]
    end

# 자주 쓰는 쿼터니언 값
no_rotation      = (0, 0, 0, 1)       # 회전 없음
x_180            = (1, 0, 0, 0)       # X축 180도 회전
z_90             = (0, 0, 0.707, 0.707)  # Z축 90도 회전

stateDiagram-v2
    state "오일러 각 회전 순서" as euler {
        [*] --> Roll: 1단계: X축 회전
        Roll --> Pitch: 2단계: Y축 회전
        Pitch --> Yaw: 3단계: Z축 회전
    }
    state "Pitch = 90도일 때" as lock {
        [*] --> Problem: Roll과 Yaw가 같은 축!
        Problem --> Lost: 자유도 3 → 2로 감소
    }
    euler --> lock: 짐벌락 발생 조건

# 유효한 쿼터니언 (길이 = 1)
(0, 0, 0.707, 0.707)  # Z축 90도 회전 ✅
 
# 유효하지 않은 쿼터니언 (길이 ≠ 1)
(0, 0, 0, 2)          # 의미 없는 숫자 ❌

flowchart TB
    subgraph joint ["Joint Space (ex02/03)"]
        JI["joint1=0, joint2=45, ..."] --> JM["바로 모터 구동"]
    end
    subgraph cart ["Cartesian Space (ex04)"]
        CI["x=0.3, y=0.0, z=0.3 + 자세"] --> IK["IK 계산<br/>(역기구학)"]
        IK -->|성공| CM["모터 구동"]
        IK -->|실패| CF["NO_IK_SOLUTION"]
    end

sequenceDiagram
    participant Node as ex04 노드
    participant MG as move_group
    participant CB as 콜백 (joint_states, tf)
 
    Note over Node,CB: 비동기 방식 (async)
    Node->>MG: send_goal_async(goal)
    activate MG
    Note over Node: spin_until_future_complete()
    loop 대기 중에도 이벤트 처리
        CB-->>Node: /joint_states 콜백 ✅
        CB-->>Node: /tf 업데이트 ✅
    end
    MG-->>Node: 결과 반환
    deactivate MG

# 비동기 호출: 보내고 바로 리턴
future = action_client.send_goal_async(goal)
 
# 기다리면서도 콜백은 계속 처리됨
rclpy.spin_until_future_complete(node, future)

flowchart LR
    subgraph square ["YZ 평면 8cm 사각형"]
        P1["① 시작점"] -->|"Y+8cm"| P2["②"]
        P2 -->|"Z+8cm"| P3["③"]
        P3 -->|"Y-8cm"| P4["④"]
        P4 -->|"Z-8cm"| P1
    end

trajectory, fraction = helper.compute_cartesian_path(waypoints, max_step=0.02)

sequenceDiagram
    participant EX as ex05 노드
    participant MG as move_group 노드
    participant RC as ros2_control
    participant RSP as robot_state_publisher
 
    EX->>MG: /compute_cartesian_path (서비스 요청)
    Note over MG: 웨이포인트 쪼개기 → IK → 궤적 생성
    MG-->>EX: trajectory + fraction (서비스 응답)
    EX->>MG: /execute_trajectory (액션 요청)
    MG->>RC: /arm_controller/joint_trajectory (토픽)
    RC->>RC: 모터 구동
    RC-->>RSP: /joint_states
    RSP-->>MG: /tf