1295.32 매니퓰레이터 동작과 힘 감시의 동시 실행 패턴

1. 패턴의 정의와 필요성

매니퓰레이터 동작과 힘 감시의 동시 실행 패턴은 로봇 팔(manipulator)이 궤적 추종, 파지(grasp), 삽입(insertion), 연마(polishing) 등의 동작을 수행하는 동안, 엔드 이펙터(end effector) 또는 관절에 가해지는 힘과 토크를 지속적으로 감시하는 행동 트리 설계 패턴이다. Parallel 노드를 활용하여 동작 제어 행동과 힘 감시 행동이 매 Tick마다 동시에 실행되도록 구성한다.

산업용 매니퓰레이터와 협동 로봇(collaborative robot)에서 힘 감시는 안전성과 작업 품질 모두에 직결되는 핵심 기능이다. 과도한 접촉력은 작업 대상물의 파손, 공구의 마모, 로봇 관절의 기계적 손상을 유발할 수 있으며, 협동 로봇의 경우 인간 작업자의 신체적 안전에 직접적인 위협이 된다. ISO 10218 및 ISO/TS 15066 규격에서는 협동 로봇의 접촉력에 대한 명시적 한계값을 규정하고 있으며, 이러한 규격 준수를 행동 트리 수준에서 보장하기 위해 본 패턴이 필수적이다.

2. 기본 구조

Parallel (success_policy: SUCCESS_ALL, failure_policy: FAILURE_ONE)
├── ExecuteMotion
│   input: "target_pose", "velocity_profile"
│   output: "motion_status"
└── MonitorForce
    input: "force_limit", "torque_limit"
    output: "measured_force"

ExecuteMotion 노드는 목표 자세(target pose)까지의 궤적을 추종하며, 목표에 도달하면 SUCCESS를 반환한다. MonitorForce 노드는 매 Tick마다 힘/토크 센서(Force/Torque sensor, F/T sensor)의 측정값을 읽어 설정된 한계값과 비교한다.

FAILURE_ONE 정책에 의해 힘 한계 초과가 감지되면 즉시 Parallel 노드 전체가 실패로 종료되고, 동작 제어 노드에 Halt가 전파되어 매니퓰레이터의 동작이 즉시 중단된다.

3. 힘 감시 노드의 상세 설계

3.1 힘 크기 기반 감시

가장 기본적인 힘 감시 방법은 측정된 힘 벡터의 크기(magnitude)가 임계값을 초과하는지 확인하는 것이다.

function MonitorForce.tick():
    wrench ← readFTSensor()
    force_magnitude ← sqrt(wrench.fx² + wrench.fy² + wrench.fz²)
    torque_magnitude ← sqrt(wrench.tx² + wrench.ty² + wrench.tz²)
    
    if force_magnitude > force_limit or torque_magnitude > torque_limit:
        blackboard.set("failure_reason", "FORCE_EXCEEDED")
        blackboard.set("measured_force", force_magnitude)
        return FAILURE
    else:
        blackboard.set("measured_force", force_magnitude)
        return RUNNING

힘 벡터 \mathbf{f} = [f_x, f_y, f_z]^T에 대해 크기 \|\mathbf{f}\| = \sqrt{f_x^2 + f_y^2 + f_z^2}를 산출하고, 이를 허용 한계 f_{max}와 비교한다. 토크 벡터 \boldsymbol{\tau} = [\tau_x, \tau_y, \tau_z]^T에 대해서도 동일한 방식으로 크기를 산출하여 \tau_{max}와 비교한다.

3.2 방향별 힘 성분 감시

특정 작업에서는 특정 방향의 힘 성분만을 감시하는 것이 적합할 수 있다. 예를 들어, 수직 삽입(vertical insertion) 작업에서는 삽입 방향(z축)의 힘은 허용하되, 수평 방향(x, y축)의 과도한 힘은 정렬 불량(misalignment)을 의미하므로 감시 대상이 된다.

Parallel (success_policy: SUCCESS_ALL, failure_policy: FAILURE_ONE)
├── ExecuteInsertion
│   input: "insertion_depth", "insertion_speed"
└── MonitorLateralForce
    input: "max_lateral_force"

MonitorLateralForce 노드는 \sqrt{f_x^2 + f_y^2}로 산출되는 수평 방향 힘 성분만을 감시한다. 수평 힘이 임계값을 초과하면 삽입 핀과 구멍의 정렬이 불량한 것으로 판정하여 동작을 중단시킨다.

3.3 힘 변화율 감시

접촉력의 절대값뿐 아니라 시간에 따른 변화율(rate of change)도 중요한 감시 대상이다. 급격한 힘 증가는 충돌이나 구속(jamming)을 나타내는 지표이다.

function MonitorForceRate.tick():
    current_force ← readForceMagnitude()
    force_rate ← (current_force - previous_force) / dt
    previous_force ← current_force
    
    if force_rate > max_force_rate:
        return FAILURE
    else:
        return RUNNING

힘 변화율 \dot{f} = \frac{df}{dt}가 허용 한계 \dot{f}_{max}를 초과하면 비정상적 접촉 상태로 판정한다. 이 감시 방법은 점진적인 힘 증가는 허용하되 급격한 충격을 감지하는 데 효과적이다.

4. 작업별 적용 사례

4.1 파지 작업

Parallel (success_policy: SUCCESS_ALL, failure_policy: FAILURE_ONE)
├── ExecuteGrasp
│   input: "grasp_pose", "grasp_width"
│   output: "grasp_result"
├── MonitorGripForce
│   input: "max_grip_force", "min_grip_force"
└── MonitorSlippage
    input: "slip_threshold"

파지 작업에서는 과도한 파지력과 부족한 파지력 모두를 감시하여야 한다. MonitorGripForce 노드는 파지력이 최대 허용값을 초과하면 물체 파손 방지를 위해 FAILURE를 반환하고, MonitorSlippage 노드는 미끄러짐(slippage)이 감지되면 파지력이 부족한 것으로 판정하여 FAILURE를 반환한다. 상위 행동 트리에서 실패 원인에 따라 파지력을 조정하는 복구 전략을 구현할 수 있다.

4.2 연마/연삭 작업

Parallel (success_policy: SUCCESS_ALL, failure_policy: FAILURE_ONE)
├── ExecutePolishing
│   input: "polishing_trajectory", "target_force"
│   output: "polishing_status"
├── MonitorNormalForce
│   input: "force_tolerance_band"
└── MonitorVibration
    input: "max_vibration_amplitude"

연마 작업에서는 공구가 작업 표면에 가하는 법선 방향 힘(normal force)이 목표값 주위의 허용 대역(tolerance band) 내에 유지되어야 한다. 법선력이 대역을 이탈하면 연마 품질이 저하되므로 동작을 중단시킨다. MonitorVibration 노드는 이상 진동(chatter vibration)을 감지하여 공구 마모나 공정 불안정을 조기에 발견한다.

4.3 조립 작업 (Peg-in-Hole)

Parallel (success_policy: SUCCESS_ALL, failure_policy: FAILURE_ONE)
├── ExecutePegInsertion
│   input: "hole_position", "insertion_force_limit"
│   output: "insertion_depth"
├── MonitorLateralForce
│   input: "max_lateral_force"
├── MonitorInsertionForce
│   input: "max_insertion_force"
└── MonitorTorque
    input: "max_insertion_torque"

핀-홀(peg-in-hole) 조립에서는 삽입 방향 힘, 수평 방향 힘, 토크를 각각 독립적으로 감시한다. 수평 힘 또는 토크가 과도하면 핀과 홀의 정렬이 불량한 상태에서 무리하게 삽입을 시도하는 것이므로, 동작을 중단하고 정렬 보정 루틴을 실행하여야 한다.

5. 다중 감시 조건의 체계적 구성

복잡한 매니퓰레이션 작업에서는 힘 이외에도 관절 한계, 모터 전류, 온도 등 다양한 조건을 동시에 감시하여야 한다.

Parallel (success_policy: SUCCESS_ALL, failure_policy: FAILURE_ONE)
├── ExecuteMotion
├── MonitorForce
├── MonitorJointLimits
├── MonitorMotorCurrent
└── MonitorTemperature

6. XML 정의 예시

<Parallel success_count="3" failure_count="1">
    <ExecuteCartesianMotion target="{target_pose}" 
                            velocity="{max_velocity}" />
    <MonitorForce force_limit="50.0" 
                  torque_limit="10.0" 
                  output_force="{current_force}" />
    <MonitorJointLimits margin="0.05" />
</Parallel>

7. Halt 시 매니퓰레이터 안전 처리

힘 한계 초과로 인해 Parallel 노드가 실패하고 동작 제어 노드에 Halt가 전파되면, 매니퓰레이터의 안전한 정지가 보장되어야 한다. Halt 처리 시 다음 사항을 준수하여야 한다.

1. 즉시 정지: 현재 궤적 추종을 즉시 중단하고 매니퓰레이터를 현재 위치에서 정지시킨다. 급격한 감속으로 인한 관성력이 추가적인 힘 초과를 유발하지 않도록 감속 프로파일을 적용하여야 한다.

2. 파지 해제 판단: 파지 작업 중 과도한 힘이 감지된 경우, 즉시 파지를 해제할 것인지 현재 파지 상태를 유지할 것인지를 작업 맥락에 따라 판단하여야 한다. 물체를 높은 위치에서 파지하고 있는 경우 즉시 해제하면 낙하 사고가 발생할 수 있다.

3. 상태 기록: Halt 시점의 관절 각도, 접촉력, 동작 진행률 등을 블랙보드에 기록하여, 상위 행동 트리에서 복구 전략을 수립하는 데 활용할 수 있도록 한다.

4. ROS2 액션 취소: 동작 제어가 ROS2 액션 서버를 통해 실행되는 경우, Halt 호출 시 액션 목표 취소(cancel goal) 요청을 전송하여 액션 서버 측에서도 안전한 정지가 이루어지도록 하여야 한다.