제9부 매니퓰레이션

9.4 MoveIt Servo와 실시간 제어

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

9.2의 계획은 "목표를 정하고 → 계획하고 → 실행한다" 이다. 계획에 수백 ms가 걸리고, 실행 중에는 경로가 고정된다. 대부분의 작업에는 이것으로 충분하다.

그런데 어떤 작업은 다르다. 조이스틱으로 팔을 조종하거나, 카메라가 본 물체를 실시간으로 따라가거나, 힘 센서에 반응해야 한다. 매 순간 목표가 바뀌므로 계획할 시간이 없다.

MoveIt Servo가 그 답이다. 속도 명령을 받아 충돌·특이점·관절 한계를 고려하면서 즉시 관절 명령으로 바꾼다.

배우는 것:

• 계획과 서보의 차이 — 언제 무엇을 쓰는가.
• 동작 원리 — 속도 명령이 관절 명령이 되는 과정.
• 특이점 처리 — 왜 문제이고 어떻게 다루는가.
• 충돌 근접 감속 — 안전 장치.
• 관절 한계 처리 — 경계 근처에서.
• ros2_control 연결 — 어떤 인터페이스로.
• 지연과 주기 — 실시간성의 실제.
• 함정 — 드리프트·타임아웃·특이점.

선수 지식: 9.1(MoveIt 구조), 9.2(계획 — 대조), 9.3(충돌), 6.5(컨트롤러), 6.6(실시간 제약).

정직: Servo의 파라미터·API·토픽 이름은 배포판마다 변해 왔다(확인 필요). 특히 노드 방식과 라이브러리 방식, 파라미터 파일 형식이 다르다. 정확한 설정은 MoveIt 2 문서의 Jazzy 버전이 정본이다. 기구학·특이점 이론은 자매서 robotics(RB-48~RB-52)가 다룬다(BK-04). 이 환경에서 검증하지 않았다(목차 3).(→ 174차 확인: Jazzy의 moveit_servo(2.12.4)로 실제 servo_node를 헤드리스 기동해 토픽·서비스 이름, 속도→관절 명령 변환, pause_servo를 실측했다 — 아래 실행 확인(174차) 참고. 이 과정에서 책이 언급하지 않은 필수 초기화 단계(/servo_node/switch_command_type)와, 책이 주장한 서비스(start_servo/stop_servo)가 이 버전엔 없고 대신 pause_servo(SetBool) 하나로 통합돼 있음을 발견했다.)

9.4.2 계획과 서보의 차이

핵심 차이: 서보는 회피하지 않는다. 장애물이 가까워지면 느려지고 멈출 뿐, 돌아가지 않는다. 그것은 사람이나 상위 로직이 해야 한다.

견해: 둘을 섞어 쓰는 것이 정상이다. 작업 위치까지는 계획으로 가고, 정밀 조정은 서보로 한다. 모드 전환 시 컨트롤러 충돌에 주의해야 한다(9.4.6).

9.4.3 동작 원리

입력 두 종류:

# ① 손끝 속도 (Cartesian) — 조이스틱·시각 서보
ros2 topic pub /servo_node/delta_twist_cmds geometry_msgs/msg/TwistStamped "{
  header: {frame_id: base_link},
  twist: {linear: {x: 0.05}, angular: {z: 0.0}}
}"

# ② 관절 속도 (Joint) — 개별 관절 조작
ros2 topic pub /servo_node/delta_joint_cmds control_msgs/msg/JointJog "{
  joint_names: [joint1],
  velocities: [0.1]
}"

정직: 토픽 이름은 설정에 따라 다르다(확인 필요)(→ 174차 확인: 기본 토픽 이름 자체는 위 코드 그대로 정확했다 — 아래 실행 확인(174차) 참고. 다만 명령을 받기 전에 반드시 먼저 호출해야 하는 서비스가 하나 더 있다는 것이 책에 빠져 있었다).

실행 확인(174차) — 토픽 이름을 실측 확인했고, 책에 없는 필수 초기화 단계를 발견했다. moveit_servo(2.12.4) 공식 demo_ros_api.launch.py를 헤드리스로(RViz 제외) 실제 servo_node(패키지 moveit_servo, 실행 파일·노드 이름 모두 servo_node)로 실행하고 ros2 topic list로 확인하니, 위 두 토픽(/servo_node/delta_twist_cmds·/servo_node/delta_joint_cmds)이 정확히 그 이름으로 존재했다(/servo_node/status도 함께 확인 — 9.4.9 참고). 추가로 /servo_node/pose_target_cmds(손끝 목표 자세를 직접 주는 세 번째 입력 모드)도 있었는데, 이는 책이 다루지 않는 것이다. 다만 처음엔 두 토픽에 명령을 발행해도 아무 효과가 없었다 — servo_node 로그에 "Command type has not been set, cannot accept input"이 반복 출력됐다. 원인은 /servo_node/switch_command_type(moveit_msgs/srv/ServoCommandType, JOINT_JOG=0·TWIST=1·POSE=2) 서비스를 먼저 호출해 사용할 입력 종류를 선언해야 하는 필수 초기화 단계였다 — 이 절차는 책에 전혀 언급돼 있지 않다. 이 서비스를 호출한 뒤에는 두 입력 모두 즉시 정상 동작했다: JointJog(panda_joint1, 속도 0.1 rad/s)를 1초간 발행하니 /panda_arm_controller/joint_trajectory(command_out_topic)에 정확히 100개 메시지(=publish_period: 0.01s→100 Hz와 일치)가 나왔고, 마지막 메시지의 속도값이 0.0999...(요청한 0.1과 거의 정확히 일치)였다. TwistStamped(linear.x=0.05)로 전환(switch_command_type(TWIST))해 1초간 발행하니 역시 101개 메시지가 나왔는데, 이번엔 여러 관절의 위치·속도가 동시에 변했다 — 자코비안 역변환이 실제로 여러 관절에 분배됨을 직접 확인했다. 원본: tools/gap_harness/gap174/servo_test/(servo_probe.py·servo_probe_result.log·topic_list.log·switch_command_type_type.log).

자코비안 역변환이 핵심 계산이다.

$$\dot{\mathbf{q}} = J^{-1}(\mathbf{q}) \, \mathbf{v}$$

여기서 $J$는 자코비안, $\mathbf{v}$는 손끝 속도, $\dot{\mathbf{q}}$는 관절 속도.

문제: $J$가 특이(singular)해지면 $J^{-1}$이 발산한다. 이것이 다음 절의 주제다.

frame_id가 중요하다:

견해: 조이스틱 조종에서 프레임 선택이 사용성을 좌우한다. 사람이 로봇을 보고 조종하면 베이스 기준이 직관적이고, 카메라 영상을 보며 조종하면 손끝(또는 카메라) 기준이 자연스럽다. 전환 가능하게 만드는 것이 좋다.

9.4.4 특이점 처리

특이점은 매니퓰레이터의 근본적 제약이다.

전형적 특이점:

Servo의 대처: 조건수 기반 감속.

# 특이점 처리 파라미터 (확인 필요: 이름)(→ 174차 확인: 세 이름 전부 정확 — 아래 실행 확인(174차) 참고)
lower_singularity_threshold: 17.0     # 감속 시작
hard_stop_singularity_threshold: 30.0 # 정지
leaving_singularity_threshold_multiplier: 2.0

실행 확인(174차) — 특이점 파라미터 이름을 실측 확인했고, 감속 동작이 실제로 발동함을 확인했다. ros2 param list /servo_node로 moveit_servo.lower_singularity_threshold·moveit_servo.hard_stop_singularity_threshold·moveit_servo.leaving_singularity_threshold_multiplier 세 이름이 위 코드와 정확히 일치함을 확인했다(기본값은 lower=10.0으로 이 장의 예시 17.0과는 다르지만, 예시값 자체는 원래도 임의값이었다). 동작도 실측으로 확인했다 — moveit_servo의 공식 demo_twist 데모(내부적으로 스스로 손끝 속도 명령을 만들어 특이 자세로 접근하는 자기구동 데모)를 헤드리스로 실행하니, 특이 자세에 가까워지며 "Moving closer to a singularity, decelerating" 경고가 실제로 반복 출력됐다 — 감속 메커니즘이 조건으로만 존재하는 것이 아니라 실제로 발동함을 확인했다. 원본: tools/gap_harness/gap174/servo_test/(param_list.log·demo_twist_launch.log).

조건수는 자코비안의 최대·최소 특이값 비다:

$$\kappa(J) = \frac{\sigma_{\max}}{\sigma_{\min}}$$

$\sigma_{\min} \to 0$이면 $\kappa \to \infty$ — 특이점이다.

함정: "팔이 갑자기 느려지거나 멈춘다"의 흔한 원인이 특이점이다. 사용자는 아무것도 안 바뀌었다고 느끼는데 로봇이 안 움직인다. RViz2나 진단으로 조건수를 표시하면 사용자가 이해하고 다른 방향으로 조작할 수 있다.

견해: leaving_singularity_threshold_multiplier가 유용하다. 특이점에 들어갈 때는 엄격하게 막고, 빠져나오는 방향의 움직임은 허용한다. 이것이 없으면 특이점에 갇혀 어느 방향으로도 못 움직인다.

견해: 특이점은 근본적으로 작업 공간 설계 문제다(9.6.3). 작업 위치가 특이 자세 근처에 있으면 서보든 계획이든 어려워진다. 로봇 배치 단계에서 주요 작업점의 조건수를 확인하는 것이 예방책이다.

9.4.5 충돌 근접 감속

장애물이 가까워지면 감속·정지한다.

check_collisions: true
collision_check_rate: 10.0            # Hz
self_collision_proximity_threshold: 0.01     # m
scene_collision_proximity_threshold: 0.02
collision_distance_safety_factor: 1000.0      # (→ 174차 확인: 이 이름은 Jazzy 2.12.4 servo_node 파라미터 목록에 없음 — 아래 실행 확인(174차) 참고)
min_allowable_collision_distance: 0.01        # (→ 174차 확인: 이 이름도 Jazzy 2.12.4 servo_node 파라미터 목록에 없음 — 아래 실행 확인(174차) 참고)

실행 확인(174차) — 충돌 관련 파라미터 이름을 실측했다. 앞 네 개는 정확했지만 뒤 두 개는 이 버전에 없었다. ros2 param list /servo_node로 실제 moveit_servo.* 파라미터 전체를 확인한 결과, check_collisions·collision_check_rate·self_collision_proximity_threshold·scene_collision_proximity_threshold 네 개는 위 코드와 정확히 일치했다. 그러나 collision_distance_safety_factor와 min_allowable_collision_distance는 목록 전체(160여 개 파라미터)를 뒤져도 존재하지 않았다 — Jazzy 2.12.4의 moveit_servo엔 이 두 이름의 파라미터가 없다. 다른 배포판·버전의 이름이거나, 착오일 수 있다 — 정직하게 "이 버전엔 없다"로만 남긴다. 원본: tools/gap_harness/gap174/servo_test/param_list.log.

Planning Scene(9.3)을 사용한다 — 명시적 물체와 옥토맵이 모두 반영된다.

실행 확인(175차) — 실제 장애물로 감속·정지를 재현했고, 책에 없는 통합 함정을 발견했다. 9.3의 /apply_planning_scene 하네스를 재사용해, move_group과 servo_node를 함께 헤드리스로 실행하고(servo_node는 is_primary_planning_scene_monitor: false로 설정 — move_group이 주 모니터가 되도록 함) 팔이 뻗는 경로 위에 실제 충돌 물체를 놓고 TwistStamped로 계속 접근시켰다.

첫 시도는 실패했다 — 물체를 /apply_planning_scene으로 정확히 반영시켰는데도(/get_planning_scene으로 확인) 팔이 물체 위치를 그대로 통과했고, 충돌 관련 반응이 전혀 없었다(우연히 특이점에 가까워지며 code=1 경고만 출력됐다). 원인을 추적한 결과 — servo_node의 monitored_planning_scene_topic 기본값이 /planning_scene인데, move_group이 보조 모니터에게 실제로 장면을 방송하는 토픽은 /monitored_planning_scene으로 서로 다르다(ros2 topic list로 두 토픽이 별개로 존재함을 확인). 기본값 그대로면 servo_node의 내부 충돌 검사기는 move_group에 적용된 장애물을 영영 받아 보지 못한다 — 책에 전혀 없는 통합 함정이다. monitored_planning_scene_topic을 /monitored_planning_scene으로 명시적으로 재설정하니 즉시 해결됐다.

수정 후 결과가 이 절의 핵심 주장을 정확히 재현했다. 장애물 없이 1초간 발행한 기준 시험은 자유롭게 움직였다(96~100개 궤적 메시지, 속도 크기 ~0.31~0.32). 장애물을 손 앞 약 7.5 cm 지점에 두고 같은 명령을 4초간 보내니, 처음 약 0.5초는 정상적으로 가속하다가(속도 크기 최대 0.318) 그 직후 속도가 정확히 0.0으로 떨어져 나머지 약 3.4초 내내 0.0을 유지했다 — 부분적 감속이 아니라 완전한 정지였다. 실제로 손끝은 4초 동안 0.030 m만 이동했다(장애물이 없었다면 이론상 약 0.32 m 이동했어야 함). /servo_node/status가 code: 0("No warnings")에서 code: 4, message: "Close to a collision, decelerating"로 전환됨을 실측으로 확인했다 — 특이점 경우의 code: 1과 명확히 구분되는 실제 코드값이며, 9.4.9의 "충돌 근접" 상태 행을 구체적인 숫자로 확정한다. 원본: tools/gap_harness/gap175/servo_collision_test/(collision_probe.py·collision_probe_result.log(수정 전, 실패 재현)·collision_probe_result_fixed.log(수정 후, 성공)·servo_plus_movegroup_headless.launch.py).

함정: collision_check_rate가 서보 주기보다 낮다. 충돌 검사는 비용이 크므로(9.3.8) 서보 주기(예: 125 Hz)보다 낮게(예: 10 Hz) 실행된다. 즉 최대 100 ms의 지연이 있을 수 있다. 빠르게 움직이면 그 사이 접근할 수 있으므로, 속도와 안전 거리를 함께 고려해야 한다.

함정: 감속만으로는 회피가 안 된다(9.4.2). 장애물 앞에서 멈춘 뒤 사람이나 상위 로직이 다른 방향으로 조작해야 한다. 자동 회피를 기대하면 안 된다.

견해: 서보의 충돌 검사는 Nav2의 collision_monitor(8.7.3)와 같은 역할이다 — 최종 안전 계층. 다만 Planning Scene에 의존하므로 Scene이 부정확하면 무력하다. 옥토맵 노이즈나 미갱신 물체가 있으면 오정지하거나 놓친다(9.3.10).

9.4.6 관절 한계와 출력

joint_limit_margin: 0.1               # rad — 한계에서 이만큼 전에 감속 (→ 174차 확인·정정: 실제 이름은 복수형 `joint_limit_margins`이며 목록(float64[])이다 — 아래 실행 확인(174차) 참고)
low_latency_mode: false               # (→ 174차 확인: 이 이름은 Jazzy 2.12.4 servo_node 파라미터 목록에 없음)
publish_period: 0.008                 # 125 Hz

관절 한계 근처에서 감속한다 — 갑자기 멈추는 대신 부드럽게.

실행 확인(174차) — joint_limit_margin은 실제로 joint_limit_margins(복수형, 목록)임을 확인했고, low_latency_mode는 이 버전에 존재하지 않음을 확인했다. ros2 param list /servo_node의 전체 파라미터 목록에 moveit_servo.joint_limit_margins는 있었지만 단수형 joint_limit_margin은 없었다 — ros2 param get /servo_node moveit_servo.joint_limit_margins로 실제 값이 [0.12](길이 1의 목록)로 반환됨을 확인했다(장의 주석대로 "크기 1이면 전체 관절에 적용"). low_latency_mode는 전체 목록을 뒤져도 존재하지 않았다 — 다른 버전의 파라미터이거나 착오로 보이며, 정직하게 "이 버전엔 없다"로만 남긴다. 원본: tools/gap_harness/gap174/servo_test/(param_list.log·joint_limit_margins_value.log).

출력 인터페이스:

command_out_type: trajectory_msgs/JointTrajectory
# 또는 std_msgs/Float64MultiArray
command_out_topic: /joint_trajectory_controller/joint_trajectory

publish_joint_positions: true
publish_joint_velocities: false
publish_joint_accelerations: false

견해: forward_command_controller + Float64MultiArray가 지연이 낮다(6.5.4). joint_trajectory_controller는 궤적을 받아 보간하므로 한 단계가 더 있다. 서보의 목적이 저지연이라면 전자를 검토하되, 전달 컨트롤러의 위험(급격한 변화 — 6.5.4)을 서보가 관리한다는 전제가 필요하다.

함정: move_group과 Servo가 동시에 같은 컨트롤러를 쓰면 충돌한다. 계획 실행 중에 서보 명령이 들어오면 예측 불가능하다. 모드 전환 시 한쪽을 비활성화해야 하며, ros2_control의 컨트롤러 전환(6.3.5)을 쓰거나 서보를 pause/unpause 한다.

# 서보 시작·정지 (확인 필요: 서비스 이름)(→ 174차 확인·정정: 이 두 서비스는 Jazzy 2.12.4에 없다 — 아래 실행 확인(174차) 참고)
ros2 service call /servo_node/start_servo std_srvs/srv/Trigger
ros2 service call /servo_node/stop_servo std_srvs/srv/Trigger

실행 확인(174차) — start_servo·stop_servo는 이 버전에 없고, 대신 pause_servo 하나로 통합돼 있음을 확인했다. 또한 책에 없는 필수 선행 서비스도 발견했다. ros2 service list로 확인한 결과 /servo_node/start_servo·/servo_node/stop_servo는 존재하지 않았다. 대신 /servo_node/pause_servo(std_srvs/srv/SetBool) 하나가 시작·정지를 함께 담당한다 — data: true를 보내면 정지("Servoing disabled" 응답), data: false를 보내면 재개("Servoing enabled" 응답)한다. 실제로 켜진 상태에서 TwistStamped를 1초간 연속 발행하며 pause_servo(true)를 호출하니 그 1초 동안 출력 궤적 메시지가 정확히 0개였고, pause_servo(false) 후 같은 1초 시험에서는 다시 정확히 100개(발행 주기와 일치)가 나와 정지·재개가 실제로 정확히 작동함을 확인했다. 추가로 발견한 것 — 책에 전혀 없는 필수 선행 절차: 서보는 /servo_node/switch_command_type(moveit_msgs/srv/ServoCommandType)로 입력 종류(JOINT_JOG·TWIST·POSE)를 먼저 선언하지 않으면 어떤 입력 토픽도 무시한다(9.4.3의 실행 확인 참고). 원본: tools/gap_harness/gap174/servo_test/(pause_servo_type.log·servo_probe_result.log).

9.4.7 지연과 주기

서보의 가치는 저지연이므로, 지연 예산이 중요하다(7.7.8).

주기 설정:

견해: 서보 주기가 컨트롤러 주기보다 빠를 필요는 없다. 컨트롤러가 100 Hz면 서보가 250 Hz로 명령해도 소용없다. 컨트롤러 주기에 맞추거나 약간 낮게 두는 것이 자연스럽다.

함정: 서보 노드도 executor 위에서 실행된다(2.3). 다른 콜백이 부하가 크면 주기가 지연된다. 전용 executor나 프로세스를 검토하고(2.3.8), 주기 지터를 측정하라(2.5.6).

타임아웃:

incoming_command_timeout: 0.1         # 초

명령이 끊기면 정지한다 — 1.4.8의 워치독 원칙이다.

함정: 타임아웃이 너무 짧으면 명령 주기가 조금만 흔들려도 끊긴 것으로 판단해 로봇이 불연속적으로 움직인다. 입력 주기의 2~3배가 출발점이다.

9.4.8 시각 서보 통합

카메라가 본 물체를 따라가는 응용의 구조다.

핵심 고려사항:

견해: 검출 지연이 시각 서보의 지배적 제약이다. 검출에 100 ms가 걸리면 제어 대역폭이 그만큼 제한된다. 게인을 낮춰 안정성을 확보하거나, 검출을 빠르게 만들어야 한다(7.8).

함정: 검출이 실패하면 마지막 오차를 계속 쓰면 안 된다. 물체가 사라졌는데 계속 그 방향으로 가면 위험하다. 검출 실패 시 속도 0을 보내거나 서보를 정지시켜야 한다 — 5.2.8의 "데이터 나이 검사"와 같은 원칙이다.

9.4.9 진단

실행 확인(174차) — /servo_node/status 토픽 이름·타입을 실측 확인했다. ros2 topic type /servo_node/status로 타입이 moveit_msgs/msg/ServoStatus(필드 code(정수)·message(문자열))임을 확인했고, 실제로 ros2 topic echo로 구독하니 정상 상태에서 code: 0, message: "No warnings"가 발행됐다. 9.4.3·9.4.6의 실행 확인에서 서보를 실제로 동작·정지·재개시키는 동안에도 이 토픽은 계속 정상 발행됐다. 원본: tools/gap_harness/gap174/servo_test/(status_topic_type.log·status_echo_before.log).

# 서보 상태 (확인 필요: 토픽 이름)(→ 174차 확인: 토픽 이름·타입 정확 — moveit_msgs/msg/ServoStatus, code+message 필드)
ros2 topic echo /servo_node/status

# 출력 명령
ros2 topic echo /joint_trajectory_controller/joint_trajectory --no-arr

# 주기 확인
ros2 topic hz /servo_node/delta_twist_cmds
ros2 topic hz /joint_trajectory_controller/joint_trajectory

상태 코드가 감속·정지 이유를 알려준다:

실행 확인(175차) — 상태 코드 값을 실측으로 일부 확정했다. moveit_msgs/msg/ServoStatus의 code 필드가 정상일 땐 0("No warnings"), 특이점에 가까워질 땐 1("Moving closer to a singularity, decelerating", 174차 확인), 충돌(장애물)에 가까워질 땐 4("Close to a collision, decelerating", 175차 확인) — 실제로 서로 다른 숫자로 구분됨을 확인했다. 중간값(2·3)이 무엇인지는 이번 회차에서 시도하지 않아 미확인으로 남긴다.

| 관절 한계 | 경계 근처 |

견해: 이 상태를 사용자 인터페이스에 표시하라. 조이스틱 조종 중에 로봇이 안 움직이면 사용자는 고장으로 오해한다. "특이점 근접"·"장애물 근접"을 표시하면 사용자가 이해하고 다르게 조작한다. 진단으로도 내보내면 사후 분석에 유용하다(6.9).

진단 항목:

9.4.10 함정 카탈로그

9.4.11 체크포인트

• 계획과 서보의 차이 일곱 축을 말할 수 있는가. "서보는 회피하지 않는다"의 의미는?
• 서보가 적합한 상황 세 가지와 계획이 적합한 상황 두 가지를 들 수 있는가.
• 속도 명령이 관절 명령이 되는 과정 여섯 단계를 말할 수 있는가.
• 자코비안 역변환에서 특이점이 왜 문제인가. 조건수 정의를 쓸 수 있는가.
• 전형적 특이점 세 종류와, Servo의 대처 방식은?
• leaving_singularity_threshold_multiplier가 없으면 무슨 문제가 생기는가.
• 충돌 검사 주기가 서보 주기보다 낮은 이유와, 그로 인한 위험은?
• 출력 인터페이스 두 종류와 각각 대응하는 컨트롤러는? 저지연이 목표라면 어느 쪽인가.
• move_group과 Servo를 동시에 쓰면 안 되는 이유와 대처는?
• 서보 주기를 컨트롤러 주기보다 빠르게 할 필요가 없는 이유는?
• 시각 서보에서 검출 지연이 지배적 제약인 이유와, 검출 실패 시 반드시 해야 할 것은?
• 서보 상태를 사용자에게 표시해야 하는 이유는?
• 특이점 문제의 근본적 해결은 어디에 있는가.

다음 장 9.5은 이 모든 것을 하나의 작업으로 묶는다 — 픽앤플레이스 통합. 인지에서 파지, 이송, 배치까지의 전체 흐름.