feat: Implement hybrid planning architecture for motion planning

src/cobots2024/book/week12/hybrid_planning.md

@@ -0,0 +1,133 @@
+# 하이브리드 플래닝 (Hybrid Planning)
+
+MoveIt의 모션 플래닝 아키텍처는 "센스-플랜-액트(Sense-Plan-Act)" 접근 방식을 따릅니다. 모션을 계획하고 실행하기 위해 먼저 환경과 로봇 상태를 인식하고("센스"), 그 다음 플래너가 로봇의 궤적을 계산하며("플랜"), 마지막으로 궤적 컨트롤러를 사용하여 단일 실행으로 실행합니다("액트").
+
+이 솔루션은 잘 알려진 정적 환경에서의 글로벌 모션 플래닝에 잘 작동하지만, 불안정하거나 동적인 환경에서는 적용할 수 없습니다. 예를 들어, 테이블 위의 사람에게 물 한 잔을 서빙하거나 고르지 않은 칠판에 글씨를 쓰는 작업은 예측할 수 없는 변화에 반응할 수 있는 보다 정교한 방법이 필요합니다. 로봇의 환경이 동적으로 변하거나, 작업 자체에 불확실성이 존재할 수 있습니다. 예를 들어, 분필로 글씨를 쓸 때는 보드에 가하는 압력을 조절해야 하며, 분필은 사용하면서 점점 짧아집니다.
+
+이러한 문제를 해결하려면, 실행된 모션을 즉각적인 조건에 맞게 조정하거나 환경의 예상치 못한 변화에 따라 다시 계획을 세울 수 있는 방법이 필요합니다. 하이브리드 플래닝 아키텍처는 재귀적인 글로벌 및 로컬 플래너 쌍을 결합하여 이 문제를 해결하려고 합니다.
+
+## 하이브리드 플래닝이란?
+
+하이브리드 플래닝은 이질적인 모션 플래너를 결합하여 더 강력하고 반응적인 솔루션을 제공하는 모션 플래닝 방법을 의미합니다. 이 접근 방식은 내비게이션 커뮤니티에서 이미 매우 확립되어 있으며, navigation2와 같은 인기 있는 프로젝트에서 성공적으로 구현되었습니다.
+
+MoveIt의 하이브리드 플래닝 아키텍처는 다른 속도와 문제 범위로 병렬 및 재귀적으로 실행되는 글로벌 및 로컬 플래너 쌍을 결합합니다.
+
+### 글로벌 플래너
+
+글로벌 플래너는 "센스-플랜-액트" 응용 프로그램에서 사용되는 플래너와 유사한 글로벌 모션 플래닝 문제를 해결하는 역할을 합니다. 사용된 플래너 알고리즘은 완전해야 하므로 계산 시간이 상대적으로 느릴 것으로 예상됩니다. 또한, 글로벌 플래너는 특정 기한 내에 솔루션을 찾을 수 있다는 보장이 없는 실시간 안전성을 요구하지 않습니다. 플래너 구현에 따라 글로벌 플래너는 하나의 초기 솔루션을 생성하거나 실행 중에 점진적으로 최적화된 솔루션을 생성할 수 있습니다.
+
+### 로컬 플래너
+
+로컬 플래너는 실행 중에 지속적으로 실행되며 글로벌 궤적을 따르기 위한 반복적인 로봇 명령을 생성합니다. 어느 정도 로컬 플래너는 컨트롤러와 유사하지만, 이 아키텍처는 더 복잡한 문제와 제약을 해결할 수 있도록 허용합니다. 로컬 플래너는 세계에 대해 논리적으로 판단하고 내부 상태를 가질 수 있습니다. 이는 다음과 같은 다양한 로컬 플래닝 문제를 해결하는 데 매우 유용합니다:
+
+- 후속 글로벌 참조 궤적의 풀기, 혼합 또는 접합
+- 글로벌 경로를 따르면서 근접 충돌을 동적으로 피하기
+- 로컬 제약 조건에 맞게 글로벌 궤적 조정 (예: 고르지 않은 표면에서 원하는 힘의 압력 조정, 시각적 피드백에 따라 도구 조정)
+- 로컬 궤적 최적화 및 시간 매개변수화 (로컬 환경에서 궤적을 최적화하는 것이 계산적으로 더 저렴하고 빠름)
+
+로컬 플래너는 빠르고, 센서 피드백에 반응할 수 있으며, 많은 경우 실시간 안전해야 합니다. 또한, 로컬 플래너는 불규칙하거나 예측할 수 없는 동작을 피하기 위해 결정적이어야 합니다.
+
+### 글로벌 플래너와 로컬 플래너 비교
+
+| 글로벌 플래너                 | 로컬 플래너                      |
+| ----------------------------- | -------------------------------- |
+| 글로벌 솔루션 궤적 해결       | 글로벌 참조 궤적 따르기          |
+| 궤적 경로 최적화 (지속적으로) | 로컬 문제 제약 해결              |
+|                               | 센서 입력 처리 가능              |
+|                               | 로컬 솔루션 최적화               |
+|                               | 컨트롤러 명령 계산               |
+| 완전한 솔루션 제공            | 로컬 최소값에 갇힐 수 있음       |
+| 제한 없는 계산 시간           | 낮은 계산 시간                   |
+| 실시간 안전성 없음            | 실시간 안전성 (솔버에 따라 다름) |
+| 반드시 결정적이지 않음        | 결정적                           |
+| OMPL 플래너                   | IK 솔버, Jacobian                |
+| STOMP                         | 잠재적 필드 플래너               |
+| TrajOpt                       | 궤적 최적화 알고리즘             |
+| Cartesian 모션 플래너         | 모델 예측 제어 (MPC)             |
+| Pilz 산업 모션 플래너         | 센서 기반 최적 제어              |
+| MTC                           |                                  |
+
+## 하이브리드 플래닝의 유용한 시나리오
+
+하이브리드 플래닝은 다양한 사용 사례에 유용할 수 있습니다. 대부분의 응용 프로그램은 다음 세 가지 시나리오로 그룹화될 수 있습니다:
+
+- **온라인 모션 플래닝**: 글로벌 플래너는 초기 글로벌 솔루션을 생성하고 지속적으로 최적화합니다. 동시에 로컬 플래너는 참조 궤적을 실행하고 업데이트된 궤적 세그먼트를 혼합합니다.
+- **반응 모션**: 글로벌 플래너는 유효하지 않은 솔루션을 수정하기 위해 사용됩니다(재계획), 로컬 플래너는 충돌 전에 속도를 늦추거나 중지합니다.
+- **적응형 모션**: 로컬 플래너는 고르지 않은 표면에서 일정한 도구 접촉을 유지하는 등 동적 조건에 맞게 글로벌 솔루션을 조정하는 데 사용됩니다.
+
+## 하이브리드 플래닝 아키텍처
+
+아래 다이어그램은 하이브리드 플래닝 아키텍처를 구성하는 기본 플러그인 유형과 ROS 인터페이스를 보여줍니다.
+
+![하이브리드 플래닝 아키텍처](./figs/hybrid_planning_architecture.png)
+
+아키텍처는 세 가지 ROS 구성 요소 노드로 구성됩니다:
+
+- **하이브리드 플래닝 매니저**
+  - 하이브리드 플래닝 요청을 위한 ROS 액션 제공
+  - 플래닝 로직 실행 및 플래너 조정
+- **글로벌 플래너**
+  - 글로벌 플래닝 문제 해결 및 솔루션 궤적 게시
+- **로컬 플래너**
+  - 들어오는 글로벌 궤적 업데이트 처리
+  - 로봇 상태, 세계 및 참조 궤적에 기반하여 로컬 플래닝 문제 해결
+  - 로봇 드라이버에 위치/속도 명령 전송
+
+아키텍처 구성 요소는 일반적이고 고도로 사용자 정의할 수 있도록 설계되었습니다. 구성 요소는 ROS 2 메시지 인터페이스를 통해서만 상호 작용하므로, 각 구성 요소나 플러그인의 구현을 쉽게 교체할 수 있습니다. 플러그인 인터페이스는 최소화되어 실제 알고리즘 구현과 최대한 추상화되도록 설계되었습니다. 이를 통해 개발자는 인프라의 어떤 부분도 구현할 필요 없이 고립된 로직이나 솔버에만 집중할 수 있습니다. 이는 다른 설정이나 플래닝 문제에 동일한 구성 요소를 재사용할 수 있게 합니다.
+
+### 하이브리드 플래닝 매니저
+
+![하이브리드 플래닝 매니저](./figs/hybrid_planner_manager_small.png)
+
+이 구성 요소는 아키텍처의 "브레인"입니다. 주요 목적은 하이브리드 플래닝 액션 요청을 처리하고 플래닝 로직 플러그인을 기반으로 모션 플래닝 및 실행 프로세스를 조정하는 것입니다. 플래닝 로직은 PlanningLogic 플러그인에 구현되어 있으며 이벤트 기반으로 설계되었습니다. 이벤트는 문자열 식별자로 정의되며, 글로벌 또는 로컬 플래너를 대상으로 하는 액션 호출이나 취소를 트리거할 수 있습니다. 간단한 플래닝 로직의 이벤트 로그 예시는 아래 다이어그램에 나와 있습니다:
+
+![하이브리드 플래닝 이벤트 로직](./figs/hybrid_planning_event_logic.png)
+
+이벤트는 하이브리드 플래닝 액션 요청 및 글로벌 및 로컬 플래너의 액션 피드백 메시지에 의해 트리거됩니다. 이 예에서는 하이브리드 플래닝 매니저가 하이브리드 플래닝 요청을 받은 후 글로벌 플래너를 시작합니다. 글로벌 궤적이 도착하면 로컬 플래너가 시작되고, 로컬 플래너가 완료되면 하이브리드 플래닝 매니저는 하이브리드 플래닝 응답을 반환합니다.
+
+Planning Logic 플러그인의 사용자 정의 구현은 "글로벌 플래닝 시작", "궤적 실행 중지" 또는 "로컬 플래너 제약 조건 x로 전환"과 같은 아키텍처에서 제공하는 사용 가능한 액션에 일반적인 이벤트를 매핑하는 것을 지원합니다. 이를 통해 모션 플래닝 동작이 매우 사용자 정의 가능하고 적응 가능해집니다.
+
+### 글로벌 플래너
+
+![글로벌 플래너](./figs/global_planner_small.png)
+
+글로벌 플래너는 아키텍처에서 가장 간단한 구성 요소입니다. GlobalPlanner 요청을 처리하는 액션 서버를 제공하며, 이는 글로벌 플래너 플러그인에 의해 처리되는 일반적인 MotionPlanRequests를 포함합니다. 기본적으로 이는 단순히 MoveIt의 플래닝 파이프라인이지만, 이론적으로는 어떤 종류의 플래너나 심지어 MTC도 여기에서 사용될 수 있습니다. 플래닝 결과는 액션 피드백을 통해 보고되며, 솔루션 궤적은 로컬 플래너로 게시됩니다.
+
+### 로컬 플래너
+
+로컬 플래너는 하이브리드 플래닝 매니저의 요청을 처리하는 액션 서버를 실행합니다. 이 액션은 실행을 시작하고 중지하며, 제약 조건이나 솔버 유형과 같은 런타임 매개변수를 구성할 수도 있습니다.
+
+![로컬 플래너](./figs/local_planner_small.png)
+
+로컬 플래너 구현은 두 가지 플러그인에 기반합니다:
+
+- **Trajectory Operator**: 이 플러그인은 글로벌 참조 궤적을 유지 관리하고 글로벌 플래너의 궤적 업데이트를 처리하며 현재 로봇 상태의 과정을 모니터링합니다.
+- **Local Constraint Solver**: 이 플러그인은 참조 궤적과 로컬 제약 조건을 기반으로 로봇 명령을 생성하는 반복 솔버 알고리즘을 구현합니다. 또한 센서 입력이나 이벤트 업데이트를 동적으로 처리하기 위한 추가 인터페이스를 포함할 수 있습니다.
+
+아래 다이어그램은 하이브리드 플래닝 매니저의 액션 요청에 따른 로컬 플래너의 예시 루프 사이클을 보여줍니다:
+
+![로컬 플래너 루프](./figs/local_planner_loop.png)
+
+각 반복에서 로컬 플래너는 현재 플래닝 장면을 요청하고 현재 로봇 상태를 참조 궤적 내에서 일치시킵니다. 목표에 도달하면 로컬 플래닝 액션이 성공적으로 완료됩니다. 그렇지 않으면 현재 로컬 플래닝 문제가 로봇 상태를 기반으로 식별되고 이후 해결됩니다. 마지막으로, 결과 제어 명령이 로봇 컨트롤러에 게시됩니다.
+
+### 작동 방식
+
+하이브리드 플래너의 런타임 동작은 다양한 구성 요소의 통신 채널과 이벤트를 시각화하는 워크플로 다이어그램을 그려서 가장 잘 이해할 수 있습니다.
+
+아래 다이어그램은 성공적인 궤적 실행의 런타임 로직을 보여줍니다.
+
+![하이브리드 플래너 로직](./figs/hybrid_planner_logic.png)
+
+플래너는 하이브리드 플래닝 요청에 의해 호출되며, 이는 하이브리드 플래닝 매니저가 반응하는 첫 번째 이벤트입니다. 이 예에서는 플래너 로직이 단순히 두 플래너를 순차적으로 실행합니다. 초기 하이브리드 플래닝 요청 후, 하이브리드 플래닝 매니저는 글로벌 플래너를 호출합니다. 글로벌 플래너는 궤적을 계산하고 게시하며, 이는 하이브리드 플래닝 매니저와 로컬 플래너 구성 요소에 의해 수신됩니다. 중요한 점은 로컬 플래너 구성 요소가 새로운 궤적을 처리할 뿐 실행을 시작하지 않는다는 것입니다. 실행은 하이브리드 플래닝 매니저에 의해 호출될 때까지 시작되지 않습니다. 하이브리드 플래닝 매니저가 요청하면 로컬 플래너 구성 요소는 참조 궤적을 풀기 시작하고 최종 상태에 도달하면 액션 응답을 성공적으로 반환합니다. 그 후, 하이브리드 플래닝 매니저는 성공적인 하이브리드 플래닝 응답을 반환합니다.
+
+이제 실행 중에 재계획하여 장애물을 피하는 하이브리드 플래너의 더 어려운 시나리오를 고려해 보겠습니다. 아래 애니메이션은 충돌 객체가 변경되어 실행 시 수정되는 간단한 모션을 보여줍니다.
+
+![재계획 예시](./figs/replanning_example.gif)
+
+여기서, 글로벌 플래닝 프로세스 동안 존재했던 충돌 객체는 글로벌 궤적이 계산된 후 사라집니다. 대신 두 개의 새로운 충돌 객체가 나타나 초기 글로벌 궤적을 무효화합니다. 로컬 플래너는 임박한 충돌을 감지하고 글로벌 플래너가 업데이트된 충돌 없는 궤적을 제공할 때까지 실행을 일시 중지합니다.
+
+아래는 설명된 동작의 워크플로를 보여줍니다.
+
+![하이브리드 플래너 로직 2](./figs/hybrid_planner_logic_2.png)
+
+시작은 첫 번째 예와 동일하지만, 참조 궤적을 풀면서 로컬 플래너가 충돌을 감지합니다. 여기서 플래너 로직은 글로벌 플래너를 다시 호출하여 반응합니다. 새로운 글로벌 솔루션을 계산하는 동안 로컬 플래너는 충돌 객체와의 충돌을 방지하기 위해 현재 위치를 유지해야 합니다. 글로벌 플래너가 계산을 완료하면 새로운 글로벌 솔루션이 로컬 플래너에 게시되고 로컬 플래너의 Trajectory Operator 플러그인이 참조 궤적에 업데이트를 혼합합니다. 이후 로컬 플래너 구성 요소는 업데이트된 솔루션이 충돌 객체를 피할 수 있도록 참조 궤적을 계속 따릅니다.