제9부 매니퓰레이션

9.3 Planning Scene과 충돌 처리

9.3.1 이 장에서 배우는 것 / 선수 지식

플래너(9.2)는 "충돌 없는 경로"를 찾는다. 그런데 무엇이 충돌인지는 어떻게 아는가? 그 답이 Planning Scene이다 — 로봇의 현재 상태와 주변 환경을 담은 세계 모델이다.

이 층이 중요한 이유는 명확하다. Scene이 틀리면 계획이 틀린다. 없는 장애물이 있으면 계획이 실패하고, 있는 장애물이 없으면 충돌한다. 그리고 충돌 검사는 계획 시간의 대부분을 차지하므로, Scene의 복잡도가 곧 성능이다.

배우는 것:

• Planning Scene의 구성 — 로봇 + 세계.
• 충돌 물체 — 추가·제거·부착.
• Octomap — 깊이 센서에서 환경으로.
• 허용 충돌 행렬(ACM) — 무엇을 무시할 것인가.
• 패딩과 스케일링 — 안전 여유.
• 충돌 검사 비용 — 무엇이 비용이 큰가.
• 동기화 — Scene이 어떻게 갱신되는가.
• 진단 — Scene이 틀렸을 때.

선수 지식: 9.1(SRDF·disable_collisions), 9.2(계획), 5.3.4(충돌 기하), 7.4(포인트클라우드), 5.1(tf).

정직: Planning Scene API와 Octomap 설정은 배포판마다 세부가 다르다(확인 필요). 특히 PlanningSceneInterface·PlanningSceneMonitor의 관계와 갱신 방식이 변해 왔다. 이 환경에서 검증하지 않았다(목차 3).(→ 173차 확인: Jazzy의 MoveIt 2.12.4로 move_group을 실제로 헤드리스 기동해 /apply_planning_scene·/get_planning_scene·/clear_octomap 서비스와 동기·비동기 반영, components 비트를 실측했다 — 아래 실행 확인(173차) 참고. 다만 Octomap 자체(깊이 센서 기반 갱신)와 ACM의 getAllowedCollisionMatrixNonConst() C++ API, 패딩 런타임 변경 API는 이번에도 검증하지 못했다.)

9.3.2 구성

충돌 검사는 세 종류다:

견해: 부착 물체 충돌이 자주 간과된다. 물체를 든 채 움직이면 그 물체도 장애물과 부딪힐 수 있는데, 부착을 선언하지 않으면 MoveIt이 모른다. 파지 후 반드시 부착해야 한다(9.3.4).

9.3.3 충돌 물체

#include <moveit/planning_scene_interface/planning_scene_interface.hpp>

moveit::planning_interface::PlanningSceneInterface psi;

moveit_msgs::msg::CollisionObject obj;
obj.header.frame_id = move_group.getPlanningFrame();    // ★ 계획 프레임
obj.id = "table";

shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = {1.2, 0.8, 0.05};      // x, y, z

geometry_msgs::msg::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.5;
box_pose.position.y = 0.0;
box_pose.position.z = 0.4;

obj.primitives.push_back(box);
obj.primitive_poses.push_back(box_pose);
obj.operation = obj.ADD;

psi.applyCollisionObjects({obj});       // 동기 적용

형상 종류:

견해: 원시 도형으로 근사하는 것이 원칙이다(5.3.4의 반복). 테이블은 박스, 기둥은 실린더로 충분하다. 메시는 정말 형상이 중요할 때만 쓰고, 그때도 볼록 근사를 검토한다.

함정: frame_id를 계획 프레임과 맞추라. 다른 프레임을 쓰면 tf 변환이 필요하고, tf가 없거나 시각이 안 맞으면 물체가 잘못된 곳에 놓인다(5.2).

함정: applyCollisionObjects는 동기, addCollisionObjects는 비동기다(확인 필요: API 이름·의미론)(→ 173차 확인: 동기 쪽은 실측으로 확정, 비동기 쪽은 부분 확인 — 아래 실행 확인(173차) 참고). 비동기 버전을 쓰고 바로 계획하면 물체가 아직 반영되지 않은 채로 계획될 수 있다. 동기 버전을 쓰거나 반영을 확인한다.

// 반영 확인
auto objects = psi.getKnownObjectNames();
// "table"이 목록에 있는지 확인

실행 확인(173차) — applyCollisionObjects가 실제로 동기임을 확인했다. moveit_resources_panda_moveit_config의 데모(RViz·MongoDB 제외, 헤드리스)로 실제 move_group을 실행하고, /apply_planning_scene 서비스(applyCollisionObjects가 내부적으로 호출하는 바로 그 서비스)를 직접 호출했다. 호출 직전엔 /get_planning_scene이 물체 목록을 []로 반환했고, 서비스 호출이 반환된 직후 곧바로(추가 대기 없이) 다시 조회하니 ['sync_box']로 이미 반영돼 있었다 — "서비스가 반환되면 이미 적용된 상태"라는 동기 의미론을 그대로 확인했다. 반면 /collision_object 토픽(비동기 경로)으로 물체를 발행한 뒤 지연 없이 곧바로 조회했을 때도 이미 반영돼 있었는데, 이는 그사이에 낀 /get_planning_scene 서비스 왕복 자체가 토픽 콜백이 처리될 시간을 준 것으로 보인다 — 토픽 경로가 실제 비동기임을 부정하는 것은 아니지만, "발행 직후 조회"만으로는 이 회차의 시험 조건에서 미반영 경합을 재현하지 못했다는 것을 정직하게 남긴다. 원본: tools/gap_harness/gap173/scene_test/(scene_probe.py·scene_probe_result.log).

9.3.4 부착 물체

그리퍼가 물체를 들면 그 물체는 로봇의 일부가 된다.

// 파지 후 부착
move_group.attachObject("cup", "tool0",
                        {"left_finger", "right_finger"});   // 접촉 허용 링크

// 배치 후 분리
move_group.detachObject("cup");

부착의 효과:

함정: touch_links를 지정하지 않으면 그리퍼와 물체가 충돌 상태가 되어 계획이 실패한다. 물체를 파지하면 손가락이 닿는 것이 당연한데, MoveIt은 그것을 충돌로 본다. 접촉하는 링크를 전부 나열해야 한다.

함정: 부착을 잊으면 든 물체가 무시된다. 컵을 들고 좁은 곳을 지나가는데 컵이 벽에 부딪힌다 — MoveIt은 컵의 존재를 모르기 때문이다. 파지 성공 직후 부착이 필수 절차다(9.5.5).

함정: 분리를 잊으면 물체가 계속 따라다닌다. 놓았는데도 로봇에 붙어 있는 것으로 계산되어, 이후 계획이 불필요하게 제약된다.

실행 확인(173차) — touch_links가 Scene에 정확히 반영됨을 확인했다. /apply_planning_scene으로 물체를 세계에서 제거하며 동시에 attached_collision_objects에 추가하는 diff를 보내니, /get_planning_scene(ROBOT_STATE_ATTACHED_OBJECTS 컴포넌트)로 조회한 결과 touch_links가 요청한 세 링크(panda_hand·panda_leftfinger·panda_rightfinger) 그대로 반영돼 있었다. 다만 정직하게 남길 점 — 이 raw diff 호출은 서비스 응답 success=False를 반환했는데도 효과(세계에서 제거·부착 목록에 추가·touch_links 반영)는 정확히 일어났다. 원인은 조사하지 않았다 — 책의 move_group.attachObject()(고급 C++ API)는 이 raw diff 방식과 내부 구현이 달라 같은 문제가 없을 가능성이 있지만, 그것까지 확인하지는 못했다. 반대로 분리(detach — 물체를 다시 세계로 되돌리는 diff)는 success=True로 정상적으로 반환됐고, 세계 목록에 물체가 정확히 복귀하고 부착 목록은 비었다. 원본: tools/gap_harness/gap173/scene_test/scene_probe_result.log(TEST 3).

9.3.5 Octomap — 센서 기반 환경

깊이 센서로 본 것을 점유 격자로 만든다.

# sensors_3d.yaml
sensors:
  - kinect_pointcloud
kinect_pointcloud:
  sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
  point_cloud_topic: /camera/depth/points
  max_range: 2.0
  point_subsample: 1
  padding_offset: 0.1
  padding_scale: 1.0
  max_update_rate: 1.0
  filtered_cloud_topic: /filtered_cloud

# 또는 깊이 이미지 기반
# sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater

# move_group 파라미터
octomap_frame: map
octomap_resolution: 0.02
max_range: 2.0

핵심 기능: 로봇 자기 필터링(self-filter). 센서가 로봇의 팔을 보면 그것이 장애물로 들어가 계획이 불가능해진다. MoveIt은 로봇 모델을 이용해 자기 몸을 지운다.

함정: padding이 부족하면 로봇이 자기 팔을 장애물로 본다. 캘리브레이션 오차·모델 오차 때문에 실제 팔과 모델이 조금 어긋나면, 그 차이가 옥토맵에 남는다. 패딩을 늘려 여유 있게 지우되, 너무 크면 팔 주변의 실제 장애물도 지워진다.

함정: 옥토맵은 "지워지지 않는" 문제가 있다. 센서가 못 보는 곳의 옛 데이터가 남는다 — Nav2 코스트맵의 유령 장애물(8.4.7)과 같은 성질이다. 주기적으로 옥토맵을 비우는 것이 실무적 대처다.

# 옥토맵 초기화 (확인 필요: 서비스 이름)(→ 173차 확인: 서비스 이름·타입 정확)
ros2 service call /clear_octomap std_srvs/srv/Empty

실행 확인(173차) — 서비스 이름 확정. 실제로 실행한 move_group의 ros2 service list에 /clear_octomap이 그대로 있었고(std_srvs/srv/Empty), 직접 호출하니 빈 성공 응답을 정상적으로 반환했다. 위 명령을 그대로 확정한다. 원본: tools/gap_harness/gap173/scene_test/(service_list.log·clear_octomap_result.log).

견해: 옥토맵은 강력하지만 다루기 까다롭다. 노이즈·자기 필터링·잔존 데이터·성능이 모두 문제가 된다. 환경이 알려져 있다면 명시적 충돌 물체(9.3.3)가 훨씬 안정적이며, 옥토맵은 예측 불가능한 환경에서 값을 한다. 산업 셀처럼 고정된 환경이라면 옥토맵 없이 가는 것도 합리적 선택이다.

함정: 깊이 센서 노이즈가 계획을 방해한다. 반사면·유리·검은 물체에서 노이즈 점이 생기고, 그것이 옥토맵에 들어가면 없는 장애물이 된다(7.4·7.5.5와 같은 문제). 센서 데이터를 필터링해 넣는 것이 중요하며, max_range 제한과 이상점 제거(7.4.6)가 기본이다.

9.3.6 허용 충돌 행렬(ACM)

"이 쌍은 충돌해도 괜찮다"를 기록한다.

// 런타임 ACM 수정 (개념 — 확인 필요: 정확한 API)
planning_scene->getAllowedCollisionMatrixNonConst()
    .setEntry("gripper_link", "table", true);

언제 필요한가:

견해: 파지·배치 순간에 ACM 조정이 필요한 경우가 많다. 물체를 테이블에서 집으려면 그리퍼가 테이블에 매우 가까워지는데, 패딩(9.3.7) 때문에 충돌로 판정될 수 있다. 그 순간만 허용하고 이후 되돌리는 것이 실무 패턴이다(9.5.4).

함정: ACM을 되돌리는 것을 잊으면 이후 실제 충돌을 놓친다. 임시 허용은 반드시 짝을 맞춰 해제해야 한다.

9.3.7 패딩과 스케일링

충돌 기하에 여유를 준다.

# 로봇 링크 패딩
robot_link_padding: 0.01      # m
robot_link_scale: 1.0

# 물체 패딩
default_object_padding: 0.01
default_attached_padding: 0.01

왜 필요한가:

견해: 패딩이 매니퓰레이터의 안전 여유다(Nav2의 팽창 — 8.4.6과 같은 역할). 너무 작으면 실제로 충돌하고, 너무 크면 좁은 곳에 접근할 수 없다. 파지처럼 정밀 접근이 필요한 단계에서는 일시적으로 줄이는 방법도 쓰인다(확인 필요: 런타임 변경 방법).

함정: 패딩이 파지를 방해하는 것이 흔하다. 물체를 파지하려면 그리퍼가 물체에 닿아야 하는데, 패딩 때문에 접근이 충돌로 판정된다. ACM 조정(9.3.6)이나 패딩 축소로 대처한다.

9.3.8 충돌 검사 비용

계획 시간의 대부분이 충돌 검사다.

$$t_{\text{계획}} \approx N_{\text{표본}} \times t_{\text{충돌검사}}$$

충돌 검사 비용의 지배 요인:

개선 방법:

견해: "계획이 느리다"의 첫 번째 확인은 충돌 기하다. 시각 메시를 충돌에도 쓰고 있으면(5.3.4의 흔한 실수) 계획이 자릿수로 느려진다. 5.7.6의 충돌 복잡도 리포트가 여기서 값을 한다.

견해: 옥토맵 해상도가 성능에 크게 영향을 준다. 0.01 m와 0.05 m는 셀 수가 125배 차이다($5^3$). 필요한 정밀도를 정하고 그보다 작게 하지 말라.

9.3.9 Scene 동기화

Scene은 여러 곳에서 갱신되며, 그 흐름을 이해해야 문제를 진단할 수 있다.

diff vs 전체:

함정: 토픽으로 보낸 물체는 비동기로 반영된다. 보내자마자 계획하면 반영 전일 수 있다(9.3.3). 서비스(applyCollisionObjects)를 쓰면 동기이므로 안전하다.

함정: 여러 노드가 Scene을 수정하면 경쟁한다. 인지 노드가 물체를 갱신하고 작업 노드가 부착을 하면 충돌할 수 있다. Scene 수정 주체를 명확히 하는 것이 좋으며, 가능하면 한 노드가 담당하게 한다.

RViz2에서 확인:

9.3.10 진단

"계획이 실패한다"에서 Scene 원인을 가려내는 법이다.

# 알려진 물체 목록
ros2 service call /get_planning_scene moveit_msgs/srv/GetPlanningScene \
  "{components: {components: 1}}"    # (확인 필요: components 비트)(→ 173차 확인·정정: 1(SCENE_SETTINGS)은 물체 목록을 채우지 못한다 — "알려진 물체 목록"이라는 목적에는 8(WORLD_OBJECT_NAMES)을 써야 한다. 아래 실행 확인(173차) 참고)

# 옥토맵 초기화 후 재시도 — 잔존 데이터 배제
ros2 service call /clear_octomap std_srvs/srv/Empty

실행 확인(173차) — components 비트값을 실측으로 확정했고, 원래 예시(1)가 틀렸음을 발견했다. ros2 interface show moveit_msgs/msg/PlanningSceneComponents로 정확한 비트 정의를 확인했다: SCENE_SETTINGS=1·ROBOT_STATE=2·ROBOT_STATE_ATTACHED_OBJECTS=4·WORLD_OBJECT_NAMES=8·WORLD_OBJECT_GEOMETRY=16·OCTOMAP=32·TRANSFORMS=64·ALLOWED_COLLISION_MATRIX=128·LINK_PADDING_AND_SCALING=256·OBJECT_COLORS=512. 실제로 move_group에 세 값을 각각 호출해 비교하니 — components: 1(SCENE_SETTINGS)은 scene.name·robot_model_name만 채우고 world.collision_objects는 빈 목록이었다("알려진 물체 목록"이라는 원래 주석의 목적을 달성하지 못한다). components: 8(WORLD_OBJECT_NAMES)이라야 실제 물체 id 목록(['async_box', 'sync_box'])이 채워졌다. components: 0은 메시지 정의의 주석("지정하지 않으면 전체가 반환된다")대로 전체 필드가 채워짐을 실측으로 확인했다. 이 장의 원래 예시는 "알려진 물체 목록"이라 적어 놓고 실제로는 그 목적을 이루지 못하는 비트값(1)을 쓰고 있었다 — 위 코드 블록을 8로 정정한다. 원본: tools/gap_harness/gap173/scene_test/(interface_defs.log·components_bitmask_result.log).

진단 절차:

증상별 원인:

견해: "옥토맵 초기화 후 재시도"가 빠른 감별법이다. 초기화 후 계획이 되면 옥토맵 문제이고, 여전히 안 되면 다른 원인이다. Nav2의 코스트맵 초기화(8.4.7)와 같은 진단 기법이다.

9.3.11 함정 카탈로그

9.3.12 체크포인트

• Planning Scene의 구성 요소 여섯 가지와 충돌 검사 세 종류를 말할 수 있는가.
• 충돌 물체의 네 가지 연산과 형상 종류별 비용을 말할 수 있는가. 원칙은?
• 부착이 하는 일 세 가지와, touch_links를 지정하지 않으면 무슨 일이 생기는가.
• 부착·분리를 잊었을 때의 각 증상은?
• Octomap의 자기 필터링이 무엇이며, 패딩이 부족하면 어떤 문제가 생기는가.
• 옥토맵의 "지워지지 않는" 문제와, Nav2의 어떤 문제와 같은 성질인가.
• 알려진 환경에서 옥토맵보다 명시적 물체를 권하는 이유는?
• ACM의 세 가지 출처와, 파지·배치 순간에 조정이 필요한 이유는?
• 패딩이 필요한 이유 네 가지와, Nav2의 무엇과 같은 역할인가.
• 충돌 검사 비용의 지배 요인 다섯 가지와 개선 방법 여섯 가지를 말할 수 있는가.
• 옥토맵 해상도를 0.01에서 0.05로 바꾸면 셀 수가 몇 배 달라지는가.
• Scene 갱신의 동기·비동기 차이와, 여러 노드가 수정할 때의 문제는?
• "옥토맵 초기화 후 재시도"가 왜 빠른 감별법인가.

다음 장 9.4은 계획과 다른 접근 — MoveIt Servo를 다룬다. 계획 없이 실시간으로 팔을 움직이는 방법과 그 제약.