1294.70 매니퓰레이터의 접근-파지-이동-배치 Sequence

1294.70 매니퓰레이터의 접근-파지-이동-배치 Sequence

1. 픽 앤 플레이스의 순차적 구조

로봇 매니퓰레이터의 픽 앤 플레이스(pick-and-place) 작업은 접근(approach), 파지(grasp), 이동(transport), 배치(place)의 4단계로 구성되는 전형적인 순차적 작업이다. 각 단계는 이전 단계의 물리적 완료에 의존하며, 단계의 순서를 바꾸거나 건너뛸 수 없다. Sequence 노드는 이러한 엄격한 순차적 의존 관계를 행동 트리 수준에서 표현한다(Colledanchise & Ogren, 2018).

2. 기본 접근-파지-이동-배치 시퀀스

2.1 최소 구조

<SequenceWithMemory>
    <Action ID="MoveToPreGraspPose" pose="{pre_grasp_pose}"/>
    <Action ID="OpenGripper"/>
    <Action ID="MoveToGraspPose" pose="{grasp_pose}"/>
    <Action ID="CloseGripper"/>
    <Action ID="MoveToPrePlacePose" pose="{pre_place_pose}"/>
    <Action ID="MoveToPlacePose" pose="{place_pose}"/>
    <Action ID="OpenGripper"/>
    <Action ID="Retreat" pose="{retreat_pose}"/>
</SequenceWithMemory>

이 시퀀스는 8개의 액션으로 구성되며, 각 단계의 역할은 다음과 같다:

  1. 접근(Approach): 파지 전 자세(pre-grasp pose)로 이동한다. 이 자세는 대상 물체의 위쪽 또는 측면에 위치하며, 충돌 없이 파지 자세에 진입할 수 있는 준비 자세이다.
  2. 그리퍼 개방: 파지 전에 그리퍼를 완전히 개방한다.
  3. 파지 자세 진입: 대상 물체를 파지할 수 있는 정확한 자세로 이동한다.
  4. 파지(Grasp): 그리퍼를 폐쇄하여 물체를 파지한다.
  5. 배치 전 자세 이동: 배치 위치 위의 준비 자세로 이동한다.
  6. 배치 자세 진입: 물체를 놓을 정확한 자세로 이동한다.
  7. 배치(Place): 그리퍼를 개방하여 물체를 놓는다.
  8. 후퇴(Retreat): 배치된 물체와의 충돌을 피해 후퇴한다.

2.2 Tick별 실행 흐름

Tick 1:  MoveToPreGraspPose→R         (접근 이동 중)
Tick 2:  MoveToPreGraspPose→R         (이동 중)
Tick 3:  MoveToPreGraspPose→S, OpenGripper→R (도달, 그리퍼 개방 중)
Tick 4:  OpenGripper→S, MoveToGraspPose→R    (파지 자세 진입 중)
Tick 5:  MoveToGraspPose→R            (미세 접근 중)
Tick 6:  MoveToGraspPose→S, CloseGripper→R   (파지 시작)
Tick 7:  CloseGripper→S, MoveToPrePlacePose→R (파지 완료, 이동 시작)
...
Tick N:  Retreat→S                    → SUCCESS (작업 완료)

SequenceWithMemory에 의해 완료된 단계는 반복되지 않고, 현재 활성 단계만 매 Tick 실행된다.

3. 단계 서브트리 분해

3.1 단계별 서브트리 구성

<SequenceWithMemory>
    <SubTree ID="ApproachPhase"/>
    <SubTree ID="GraspPhase"/>
    <SubTree ID="TransportPhase"/>
    <SubTree ID="PlacePhase"/>
</SequenceWithMemory>

각 서브트리의 내부 구조:

<!-- ApproachPhase -->
<BehaviorTree ID="ApproachPhase">
    <Sequence>
        <Action ID="PerceiveObject" object_id="{target_object}"/>
        <Action ID="ComputeGraspPose" object_pose="{object_pose}"
                grasp_pose="{grasp_pose}"/>
        <Action ID="PlanMotion" target="{pre_grasp_pose}"/>
        <Action ID="ExecuteMotion"/>
    </Sequence>
</BehaviorTree>

<!-- GraspPhase -->
<BehaviorTree ID="GraspPhase">
    <Sequence>
        <Action ID="OpenGripper"/>
        <Action ID="PlanMotion" target="{grasp_pose}"/>
        <Action ID="ExecuteMotion"/>
        <Action ID="CloseGripper"/>
        <Action ID="VerifyGrasp"/>
    </Sequence>
</BehaviorTree>

<!-- TransportPhase -->
<BehaviorTree ID="TransportPhase">
    <Sequence>
        <Action ID="LiftObject" height="0.1"/>
        <Action ID="PlanMotion" target="{pre_place_pose}"/>
        <Action ID="ExecuteMotion"/>
    </Sequence>
</BehaviorTree>

<!-- PlacePhase -->
<BehaviorTree ID="PlacePhase">
    <Sequence>
        <Action ID="PlanMotion" target="{place_pose}"/>
        <Action ID="ExecuteMotion"/>
        <Action ID="OpenGripper"/>
        <Action ID="Retreat" distance="0.1"/>
    </Sequence>
</BehaviorTree>

4. 인식 통합 픽 앤 플레이스 시퀀스

4.1 시각 기반 물체 인식 포함

<SequenceWithMemory>
    <!-- 1단계: 물체 인식 -->
    <Action ID="MoveToScanPose"/>
    <Action ID="CapturePointCloud"/>
    <Action ID="DetectObject" object_id="{target_object}"
            result_pose="{object_pose}"/>
    <Action ID="EstimateGraspPose" object_pose="{object_pose}"
            grasp_pose="{grasp_pose}"/>
    <!-- 2단계: 접근 및 파지 -->
    <Action ID="PlanAndMoveToPreGrasp" pose="{grasp_pose}"/>
    <Action ID="OpenGripper"/>
    <Action ID="PlanAndMoveToGrasp" pose="{grasp_pose}"/>
    <Action ID="CloseGripper"/>
    <Action ID="VerifyGrasp"/>
    <!-- 3단계: 이동 및 배치 -->
    <Action ID="LiftObject" height="0.1"/>
    <Action ID="PlanAndMoveTo" pose="{place_pose}"/>
    <Action ID="OpenGripper"/>
    <Action ID="Retreat" distance="0.15"/>
</SequenceWithMemory>

물체 인식, 파지 자세 추정, 경로 계획이 모두 순차적으로 통합되어 있다. 블랙보드 포트를 통해 인식 결과({object_pose})가 파지 계획({grasp_pose})으로, 파지 계획이 운동 실행으로 전달된다.

5. 안전 감시 통합 시퀀스

5.1 ReactiveSequence를 이용한 조작 안전 감시

<ReactiveSequence>
    <!-- 지속 감시 조건 -->
    <Condition ID="IsJointTorqueNormal"/>
    <Condition ID="IsCollisionNotDetected"/>
    <Condition ID="IsWorkspaceNotViolated"/>
    <!-- 조작 임무 -->
    <SequenceWithMemory>
        <SubTree ID="ApproachPhase"/>
        <SubTree ID="GraspPhase"/>
        <SubTree ID="TransportPhase"/>
        <SubTree ID="PlacePhase"/>
    </SequenceWithMemory>
</ReactiveSequence>

매 Tick마다 관절 토크, 충돌, 작업 공간 위반을 검사하며, 어느 조건이든 위반되면 조작이 즉시 중단된다.

5.2 협동 로봇의 안전 감시 통합

<ReactiveSequence>
    <Condition ID="IsHumanDistanceSafe"/>
    <Condition ID="IsContactForceNormal"/>
    <Condition ID="IsSpeedWithinLimit"/>
    <SequenceWithMemory>
        <SubTree ID="ApproachPhase"/>
        <SubTree ID="GraspPhase"/>
        <SubTree ID="TransportPhase"/>
        <SubTree ID="PlacePhase"/>
    </SequenceWithMemory>
</ReactiveSequence>

협동 로봇(cobot)의 경우, 사람과의 거리, 접촉력, 속도를 지속적으로 감시하여 작업자의 안전을 보장한다.

6. 파지 검증과 오류 처리

6.1 파지 성공 확인 포함 시퀀스

<SequenceWithMemory>
    <Action ID="MoveToPreGraspPose"/>
    <Action ID="OpenGripper"/>
    <Action ID="MoveToGraspPose"/>
    <Action ID="CloseGripper"/>
    <!-- 파지 검증 -->
    <Condition ID="IsObjectGrasped"/>
    <!-- 파지 성공 후 이동 -->
    <Action ID="LiftObject" height="0.1"/>
    <Action ID="MoveToPlacePose"/>
    <Action ID="OpenGripper"/>
    <Action ID="Retreat"/>
</SequenceWithMemory>

IsObjectGrasped 조건 노드는 그리퍼의 폭, 힘 센서 값, 또는 촉각 센서를 통해 물체가 실제로 파지되었는지 확인한다. 파지가 실패한 경우(그리퍼가 완전히 닫혔으나 물체를 잡지 못한 경우) FAILURE를 반환하여 시퀀스 전체가 중단된다.

6.2 Fallback을 이용한 파지 재시도

<SequenceWithMemory>
    <Action ID="MoveToPreGraspPose"/>
    <!-- 파지 시도 (재시도 포함) -->
    <Fallback>
        <Sequence>
            <Action ID="OpenGripper"/>
            <Action ID="MoveToGraspPose"/>
            <Action ID="CloseGripper"/>
            <Condition ID="IsObjectGrasped"/>
        </Sequence>
        <Sequence>
            <Action ID="OpenGripper"/>
            <Action ID="AdjustGraspPose"/>
            <Action ID="MoveToGraspPose"/>
            <Action ID="CloseGripper"/>
            <Condition ID="IsObjectGrasped"/>
        </Sequence>
    </Fallback>
    <!-- 파지 성공 후 이동 및 배치 -->
    <Action ID="LiftObject"/>
    <Action ID="MoveToPlacePose"/>
    <Action ID="OpenGripper"/>
    <Action ID="Retreat"/>
</SequenceWithMemory>

첫 번째 파지가 실패하면 파지 자세를 조정한 후 재시도한다. Sequence 내부에 Fallback을 중첩하여 파지 단계에서의 대안 전략을 구현하였다.

7. 다중 물체 픽 앤 플레이스

7.1 순차적 다중 물체 처리

<SequenceWithMemory>
    <!-- 물체 1 -->
    <SubTree ID="PickAndPlace" object="{object_1}"
             pick_pose="{pick_1}" place_pose="{place_1}"/>
    <!-- 물체 2 -->
    <SubTree ID="PickAndPlace" object="{object_2}"
             pick_pose="{pick_2}" place_pose="{place_2}"/>
    <!-- 물체 3 -->
    <SubTree ID="PickAndPlace" object="{object_3}"
             pick_pose="{pick_3}" place_pose="{place_3}"/>
</SequenceWithMemory>

동일한 PickAndPlace 서브트리를 재사용하여 여러 물체를 순차적으로 처리한다. 블랙보드 포트를 통해 각 호출에 서로 다른 물체와 위치를 전달한다.

8. 설계 시 고려 사항

  1. 운동 계획의 통합: 각 이동 단계에서 경로 계획(motion planning)을 수행하여 충돌 없는 궤적을 생성해야 한다. 계획 실패 시 Sequence가 FAILURE를 반환하므로, 상위 트리에서 복구 전략을 제공해야 한다.

  2. 파지 검증의 필수성: 그리퍼 폐쇄 후 물체가 실제로 파지되었는지 반드시 검증해야 한다. 검증 없이 이동 단계로 진행하면, 물체가 파지되지 않은 상태에서 이동하여 작업이 실패한다.

  3. Halt 시 그리퍼 상태 관리: 물체를 파지한 상태에서 Halt가 발생하면, 그리퍼를 유지할지 해제할지를 onHalted() 메서드에서 결정해야 한다. 파지 해제 시 물체가 낙하할 수 있고, 유지 시 다음 시도에서 간섭이 발생할 수 있다.

  4. 블랙보드를 통한 자세 전달: 인식 결과, 파지 자세, 배치 자세 등의 기하학적 데이터는 블랙보드 포트를 통해 단계 간에 전달하여, 동적으로 계산된 자세를 런타임에 활용할 수 있도록 한다(Faconti, 2022).

참고 문헌

  • Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
  • Faconti, D. (2022). BehaviorTree.CPP documentation and API reference. https://www.behaviortree.dev/