제9부 매니퓰레이션

9.5 픽앤플레이스 통합

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

픽앤플레이스는 매니퓰레이션의 "Hello World"이자 실제로는 가장 어려운 통합 문제다.

데모는 쉽다 — 물체 위치를 알려주고 계획해서 집으면 된다. 그런데 실제 시스템에서는 인지가 틀리고, 파지가 미끄러지고, 접근 경로가 막히고, 물체를 놓쳤는데 로봇은 모른다. 각 단계가 실패할 수 있고, 실패를 감지하고 복구해야 한다.

이 장은 전체 흐름을 세우고, 각 단계의 실패 모드와 대처에 집중한다. 이 책의 여러 부가 여기서 만난다 — 인지(제7부), 제어(제6부), 계획(9.2), 충돌(9.3).

배우는 것:

• 전체 흐름 — 단계와 각 단계의 산출물.
• 파지 자세 생성 — MoveIt이 해 주지 않는 부분.
• 접근·이탈 — 왜 직선이어야 하는가.
• 부착·분리 타이밍 — 언제 무엇을.
• 그리퍼 제어 — 파지 성공 판정.
• MTC 활용 — 단계 간 의존을 다루기.
• 실패 모드와 복구 — 단계별.

선수 지식: 9.1~9.4(MoveIt 전반), 6.5.7(그리퍼 컨트롤러), 7.3(카메라 캘리브레이션), 7.4(포인트클라우드), 5.1(tf).

정직: 파지 자세 생성은 표준 해법이 없는 영역이다. moveit_grasps 같은 패키지가 있으나 성숙도·Jazzy 지원은 확인이 필요하고(확인 필요)(→ 176차 확인: moveit_grasps는 표준 apt엔 없음 — 아래 실행 확인(176차) 참고), 실무에서는 응용에 맞춰 직접 구현하는 경우가 많다. MTC의 상태도 확인이 필요하다. 이 환경에서 검증하지 않았다(목차 3).(→ 176차 확인: MTC로 실제 픽앤플레이스 과제를 헤드리스로 계획·실행까지 끝까지 실행했다 — 물체가 정확한 놓기 자세로 이동함을 실측했다. 아래 실행 확인(176차) 참고.)

9.5.2 전체 흐름

견해: 3단계에서 "후보 여럿"을 만드는 것이 핵심이다. 하나의 파지 자세만 시도하면 계획이 실패했을 때 끝이다. 여러 후보를 점수 순으로 시도하면 성공률이 크게 오른다 — 이것이 픽앤플레이스 견고성의 가장 큰 요인일 수 있다.

견해: 7단계 파지 확인이 자주 생략되고 가장 큰 비용으로 돌아온다. 물체를 파지하지 못했는데 로봇은 파지한 줄 알고 이송하면, 빈 그리퍼로 배치하고 다음 작업으로 넘어간다. 파지 확인 없이는 신뢰할 수 있는 시스템이 아니다(9.5.6).

9.5.3 파지 자세 생성

"이 물체를 어떻게 파지할 것인가"는 MoveIt이 답하지 않는다(9.1.8).

생성 방법:

견해: 알려진 물체를 다루는 산업 응용에서는 고정 파지가 가장 견고하다. 물체마다 "이 방향에서 이렇게 파지한다"를 정의해 두면 예측 가능하고 디버깅이 쉽다. 학습 기반은 미지 물체가 오는 경우에 값을 하며, 그만큼 검증이 어렵다.

파지 자세의 구성:

struct GraspCandidate {
  geometry_msgs::msg::PoseStamped grasp_pose;      // 파지 순간 손끝 자세
  geometry_msgs::msg::Vector3 approach_dir;        // 접근 방향(대개 손끝 z)
  double approach_dist;                            // 접근 거리
  geometry_msgs::msg::Vector3 retreat_dir;         // 이탈 방향(대개 위)
  double retreat_dist;
  double gripper_open_width;
  double gripper_close_width;
  double score;                                    // 정렬용
};

필터링 기준:

함정: IK만 확인하고 접근 경로를 확인하지 않으면 실행 단계에서 실패한다. 파지 자세는 도달 가능한데 그리로 가는 길이 막혀 있을 수 있다. 후보 필터링에 접근 경로 계획을 포함하는 것이 좋으며, 이것이 MTC(9.5.7)가 해결하는 문제다.

함정: 파지 자세의 프레임을 명확히 하라. 물체 기준인지 손끝 기준인지, 손끝의 어느 지점(TCP)인지가 정확해야 한다. tool0와 실제 파지점(손가락 사이) 사이의 오프셋을 URDF나 계산에 반영해야 한다.

9.5.4 접근과 이탈

직선 이동이 필수다(9.2.6).

// 접근: 직선으로 approach_dist 만큼 (→ 177차 확인: 이 정확한 4-인자 시그니처는 Jazzy 2.12.4에서
// 컴파일 경고와 함께 여전히 동작한다 — 아래 실행 확인(177차) 참고)
std::vector<geometry_msgs::msg::Pose> waypoints = {pre_grasp_pose, grasp_pose};
moveit_msgs::msg::RobotTrajectory traj;
double fraction = move_group.computeCartesianPath(waypoints, 0.005, 0.0, traj);

if (fraction < 0.99) {
  RCLCPP_WARN(logger, "접근 경로 %.0f%% — 이 파지 후보 포기", fraction * 100);
  return false;      // 다음 후보로(9.5.2의 3단계)
}
move_group.execute(traj);

실행 확인(177차) — 이 코드를 실제 MoveGroupInterface C++ 노드로 컴파일·실행해 두 시나리오로 검증했다. 이 투자 전체에서 처음으로 moveit_ros_planning_interface를 직접 링크한 독립 C++ 노드를 작성했다(그동안은 rclcpp/rclpy 서비스·토픽 프로브나 패키지 데모 실행 파일만 썼다). 컴파일 시점 발견: 책의 정확한 4-인자 시그니처(waypoints, eef_step, jump_threshold, traj)는 Jazzy 2.12.4에서 "Drop jump_threshold"라는 폐기 경고와 함께 컴파일된다 — 에러는 아니라 여전히 동작하지만, 이 인자 없는 새 시그니처로 옮겨가는 것이 권장된다.

시나리오 A(도달 가능 — 현재 자세에서 곧장 아래로 5cm): fraction = 1.000000(정확히 1.0), 궤적점 15개. fraction ≥ 0.99라 execute()를 호출했고, "Execute request accepted" → "Execute request success!"(결과 코드 1 = moveit_msgs::MoveItErrorCodes::SUCCESS, 9.2.12에서 확인한 "FAILURE=99999만 양수"와 교차 확인됨). 실행 전후 실제 자세를 측정하니 z가 0.590 → 정확히 0.540으로, 명령한 5cm 하강과 정확히 일치했다 — 로그가 아니라 물리적 이동으로 확인한 것이다.

시나리오 B(도달 불가 — 현재 자세에서 x로 +3m, panda 팔 길이 약 0.85m를 훨씬 초과): fraction = 0.570715(57%), 궤적점 101개(요청한 경로를 eef_step=0.005m 간격으로 보간했으나 57%까지만 IK로 도달 가능했음). 책의 fraction < 0.99 분기가 정확히 발동해 "접근 경로 57% — 이 파지 후보 포기"가 그대로 출력됐다 — 책의 경고 문구·조건·포기 로직 전부가 실측과 정확히 일치했다. 원본: tools/gap_harness/gap177/cartesian_test/(cartesian_test_node.cpp·cartesian_test_result.log·build_log.txt).

함정: 접근 fraction이 1.0 미만이면 중간에 멈춘다(9.2.6). 파지 접근에서 이것은 물체를 파지하지 못하거나 치는 것을 의미한다. 임계를 높게(0.99) 두고 미달이면 그 후보를 포기하는 것이 안전하다.

함정: 이탈 방향이 중요하다. 테이블 위 물체를 파지했으면 위로 들어야 한다. 옆으로 빼면 물체가 테이블에 끌리거나 넘어진다. retreat_dir을 물체·환경에 맞게 정한다.

ACM·패딩 조정(9.3.6·9.3.7):

// 파지 순간에는 그리퍼-물체, 그리퍼-지지면 충돌 허용
// (접근 직전 설정, 이탈 후 해제)

견해: 접근·파지 단계에서 충돌 검사를 완화하는 것이 실무적으로 필요하다. 그리퍼가 물체에 닿는 것은 목적이지 오류가 아니고, 테이블 위 물체를 집으려면 테이블에 매우 가까워진다. 다만 완화 범위를 최소로 하고 반드시 되돌려라(9.3.11의 12번).

9.5.5 부착과 분리 타이밍

9.3.4의 부착을 언제 하는가.

핵심: 부착과 세계 물체는 배타적이다.

// 파지 후
move_group.attachObject(object_id, "tool0", {"left_finger", "right_finger"});
// → MoveIt이 자동으로 세계에서 제거하고 부착으로 전환 (확인 필요: 동작)

// 배치 후
move_group.detachObject(object_id);
// → 현재 위치에 세계 물체로 복원

함정: 부착 시점이 이르면 그리퍼가 닫히기 전에 물체가 따라다니는 것으로 계산된다. 늦으면 이탈 계획에 물체가 반영되지 않아 부딪힌다. 그리퍼 닫힘 확인 직후가 맞는 시점이다.

함정: 배치 후 물체의 위치를 갱신해야 한다. 분리하면 현재 손끝 위치 기준으로 세계에 놓이는데, 실제로 놓인 위치와 다를 수 있다(그리퍼를 열면서 물체가 조금 떨어짐). 필요하면 인지로 재확인한다.

9.5.6 그리퍼 제어와 파지 확인

그리퍼 명령(6.5.7):

// GripperCommand 액션
auto goal = control_msgs::action::GripperCommand::Goal();
goal.command.position = 0.0;        // 닫기(폭)
goal.command.max_effort = 30.0;     // N
gripper_client->async_send_goal(goal, options);

allow_stalling이 핵심이다(6.5.7) — 물체를 파지하면 목표 위치에 도달하지 못한 채 멈추는데, 이것을 실패로 처리하면 파지가 항상 실패한다.

파지 확인 방법:

// 위치 기반 확인 — 가장 단순
bool grasp_succeeded(double gripper_pos, double expected_object_width) {
  const double tol = 0.005;
  // 완전히 닫혔으면(빈 손) 실패
  if (gripper_pos < closed_position_ + tol) return false;
  // 예상 물체 폭 근처면 성공
  return std::abs(gripper_pos - expected_object_width) < grasp_tolerance_;
}

견해: 위치 기반 확인이 가장 쉽고, 파지력 기반이 가장 신뢰할 만하다. 위치만 보면 물체가 예상보다 작거나 비스듬히 파지된 경우를 놓친다. 가능하면 두 가지 이상을 조합하라 — 위치 + 전류, 또는 위치 + 비전.

함정: 파지 확인 없이 진행하면 "빈 손 배치"가 발생한다. 시스템은 성공으로 기록하고 다음으로 넘어가며, 문제를 나중에 발견한다. 자동화 라인에서 이것이 누적되면 심각하다.

견해: 이송 중 재확인도 검토하라. 파지 직후에는 파지했는데 이송 중 미끄러져 떨어질 수 있다. 주기적으로 파지 상태를 확인하고, 놓쳤으면 즉시 정지·보고하는 것이 안전하다.

9.5.7 MTC 활용

9.1.7에서 소개한 MTC가 여기서 값을 한다.

문제: 단계를 순차 계획하면 앞 단계가 뒤 단계를 불가능하게 만들 수 있다.

MTC는 전체를 함께 탐색한다:

// 개념 구조 (확인 필요: 정확한 API)
Task task;
task.add(std::make_unique<stages::CurrentState>("current"));

auto grasp = std::make_unique<SerialContainer>("pick");
grasp->insert(std::make_unique<stages::MoveRelative>("approach"));
grasp->insert(std::make_unique<stages::GenerateGraspPose>("generate grasp"));
grasp->insert(std::make_unique<stages::ModifyPlanningScene>("attach"));
grasp->insert(std::make_unique<stages::MoveRelative>("retreat"));
task.add(std::move(grasp));

task.add(std::make_unique<stages::Connect>("move to place", planners));
task.add(/* place container */);

if (task.plan(10)) {           // 최대 10개 해 탐색
  task.execute(*task.solutions().front());
}

실행 확인(176차) — MTC로 실제 픽앤플레이스 과제를 처음부터 끝까지(계획+실행) 헤드리스로 완주시켰다. ros-jazzy-moveit-task-constructor-core·-demo가 Jazzy apt에 정식 패키징돼 있음을 확인했다(성숙도 관련 미확인 표시를 이 실측으로 닫는다). 패키지가 제공하는 pick_place_demo 실행 파일(원통 물체+테이블+파지·접근·이탈·이송·배치 전 단계를 포함한 완전한 MTC 태스크)을 move_group(move_group/ExecuteTaskSolutionCapability 탑재, RViz 제외 헤드리스) 위에서 실행했다.

1차: 기본 설정(실행 없음). 로그에 "Planning succeded"가 출력됐고(태스크 파이프라인 전체에서 76회의 ValidateSolution 호출 — 여러 후보 해를 실제로 평가함을 보여준다), 곧이어 "Execution disabled"가 출력됐다 — 패키지 기본 설정은 RViz 대화형 사용을 염두에 두고 계획만 한다.

2차: execute: true로 재설정 후 재실행 — 완전한 계획+실행 확인. "Planning succeded" → "Executing solution trajectory" → 약 41초의 실제 실행 시간 → "Execution complete"가 순서대로 출력됐다. 같은 시간대에 panda_hand_controller가 실제 GripperCommand 액션 목표를 여러 차례 수신·수락했다(그리퍼 열기·닫기가 실제로 컨트롤러에 명령됨을 확인).

결정적 확인 — 실행 후 /get_planning_scene으로 물체의 최종 위치를 직접 측정했다. 원통 물체는 원래 픽 위치(object_pose: [0.5, -0.25, 0.0])에서 시작해, 실행 완료 후 위치가 정확히 (0.600, -0.150, 0.125)였다 — 설정 파일의 place_pose: [0.6, -0.15, 0.0]와 정확히 일치한다(z=0.125는 물체 높이 0.25m의 절반, 테이블 위에 평평히 놓인 것과 부합). 로그 메시지만이 아니라 물리적으로 물체가 한 위치에서 다른 위치로 실제 이동했음을 수치로 확인했다 — 9.5.7의 "MTC는 전체를 함께 탐색한다"는 주장과 픽앤플레이스 전체 흐름(9.5.2)을 종단간으로 재현한 것이다. 원본: tools/gap_harness/gap176/mtc_test/(pick_place_demo_plan_only.log·pick_place_demo_execute.log·final_scene_check.log·mtc_demo_launch.log).

견해: MTC의 실제 가치는 "파지 후보를 자동으로 순회하며 전체가 되는 해를 찾는 것"이다. 이것을 직접 구현하면 중첩 루프와 백트래킹이 되어 복잡하다. 파지 후보가 많고 환경이 복잡하면 MTC가 값을 하고, 단순한 반복 작업이면 순차 계획이 명확하다.

9.5.8 실패 모드와 복구

각 단계의 실패와 대처다.

복구 설계 원칙:

견해: "안전 자세로 후퇴 후 재시도"가 기본 패턴이다. 실패한 자리에서 바로 재시도하면 같은 실패를 반복하거나 물체에 접촉한다. 알려진 안전 자세(home — 9.1.3)로 돌아간 뒤 다시 시작하는 것이 예측 가능하다.

견해: 재시도 횟수에 반드시 상한을 두라(8.3.8·8.7.6의 반복). 무한 재시도는 시간을 낭비하고 물체를 손상시킬 수 있다. N회 실패하면 사람에게 알리고 다음으로 넘어가는 것이 자동화 시스템의 정상적 동작이다.

진단 노출(6.9):

견해: 단계별 성공률을 기록하면 개선 지점이 즉시 보인다. 인지가 95%인데 파지가 70%면 파지 전략이 문제다. 숫자 없이는 "가끔 실패한다"에 머문다.

9.5.9 인지 통합

물체 위치를 어떻게 얻는가(제7부).

견해: 가능하면 문제를 쉽게 만들라. 지그로 위치를 고정하거나 마커를 붙이면 인지 문제가 거의 사라진다. "임의 위치의 임의 물체를 집는다"는 훨씬 어려운 문제이며, 정말 필요한지 먼저 확인할 가치가 있다.

정밀도 예산:

$$\varepsilon_{\text{총}} \approx \sqrt{\varepsilon_{\text{인지}}^2 + \varepsilon_{\text{캘리브}}^2 + \varepsilon_{\text{로봇}}^2}$$

이 총합이 그리퍼의 허용 오차보다 작아야 파지가 성공한다.

견해: 그리퍼 설계로 오차 허용을 늘리는 것이 효과적이다. 원뿔형 가이드·순응 구조·넓은 접촉면을 가진 그리퍼는 몇 mm의 오차를 흡수한다. 소프트웨어로 mm를 경합하기보다 기구로 여유를 만드는 편이 견고한 경우가 많다.

9.5.10 함정 카탈로그

9.5.11 체크포인트

• 픽앤플레이스 12단계와 각 단계의 산출물을 말할 수 있는가. 견고성의 가장 큰 요인은?
• 파지 자세 생성 방법 네 가지와, 산업 응용에서 고정 파지가 견고한 이유는?
• 파지 후보 필터링 기준 다섯 가지를 들 수 있는가. IK만 확인하면 안 되는 이유는?
• 각 구간에 어떤 계획 방법을 쓰는가. 접근에 직교 경로가 필수인 이유는?
• 접근 fraction 임계를 높게 두는 이유와, 이탈 방향이 왜 중요한가.
• 부착의 올바른 타이밍과, 이르거나 늦었을 때의 각 문제는?
• 파지 확인 방법 다섯 가지와, 확인을 생략하면 무슨 일이 생기는가.
• allow_stalling이 파지에서 핵심인 이유는?
• MTC가 순차 계획과 다른 점과, 실제 가치는 무엇인가.
• 실패 복구의 기본 패턴은? 재시도 횟수에 상한을 두어야 하는 이유는?
• 단계별 성공률을 기록하면 무엇을 알 수 있는가.
• 파지 정밀도 예산의 세 오차원과, 기구로 여유를 만드는 것이 효과적인 이유는?

다음 장 9.6은 제9부의 마지막 — 데모를 실제 배치로 만드는 실무다. 작업 공간 설계, 캘리브레이션, 사이클 타임, 안전, 유지보수.