매니퓰레이션 관련 조건 노드 설계 (Manipulation-Related Condition Node Design)

1. 개요

매니퓰레이션 관련 조건 노드는 로봇 매니퓰레이터(manipulator)의 상태를 평가하여 파지(grasping), 배치(placing), 조립(assembly) 등의 작업 수행 가능 여부를 판정하는 조건 노드 군이다. 매니퓰레이터 시스템은 관절 상태, 그리퍼 상태, 작용 힘/토크, 작업 공간 범위 등 다양한 상태 변수를 가지며, 이들의 안전 범위 내 적합성을 실시간으로 감시하는 것이 안전한 매니퓰레이션의 기본 요건이다.

2. 매니퓰레이션 상태의 평가 범주

2.1 주요 조건 분류

2.2 ROS2 매니퓰레이터 관련 메시지

3. sensor_msgs::msg::JointState 메시지 구조

4. 공통 설계 원칙

4.1 관절 인덱스 매핑

JointState 메시지에서 특정 관절의 데이터를 추출하려면, name 배열에서 관절 이름을 검색하여 해당 인덱스를 찾아야 한다.

int findJointIndex(
    const sensor_msgs::msg::JointState::SharedPtr& msg,
    const std::string& joint_name)
{
    for (size_t i = 0; i < msg->name.size(); ++i)
    {
        if (msg->name[i] == joint_name)
        {
            return static_cast<int>(i);
        }
    }
    return -1;  // 관절을 찾지 못함
}

4.2 안전 한계의 계층 구조

매니퓰레이터의 안전 한계는 다음 계층으로 구성된다.

조건 노드에서는 일반적으로 소프트 한계 또는 작업 한계를 기준으로 평가하며, 하드웨어 한계는 제어기 수준에서 강제한다.

5. 기본 관절 상태 확인 조건 노드

class IsJointInRange
    : public BT::RosTopicSubNode<sensor_msgs::msg::JointState>
{
public:
    IsJointInRange(const std::string& name,
                   const BT::NodeConfiguration& config,
                   const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<std::string>("joint_name", "관절 이름"),
            BT::InputPort<double>("min_position", "최소 위치 (rad)"),
            BT::InputPort<double>("max_position", "최대 위치 (rad)")
        });
    }

    BT::NodeStatus onTick(
        const sensor_msgs::msg::JointState::SharedPtr& msg) override
    {
        if (!msg || msg->name.empty())
        {
            return BT::NodeStatus::FAILURE;
        }

        std::string joint_name;
        double min_pos, max_pos;
        getInput("joint_name", joint_name);
        getInput("min_position", min_pos);
        getInput("max_position", max_pos);

        for (size_t i = 0; i < msg->name.size(); ++i)
        {
            if (msg->name[i] == joint_name &&
                i < msg->position.size())
            {
                double pos = msg->position[i];
                if (pos >= min_pos && pos <= max_pos)
                {
                    return BT::NodeStatus::SUCCESS;
                }
                return BT::NodeStatus::FAILURE;
            }
        }
        return BT::NodeStatus::FAILURE;
    }
};

6. XML 행동 트리에서의 활용 패턴

6.1 파지 작업 전 조건 확인

<BehaviorTree ID="GraspSequence">
    <Sequence>
        <!-- 관절 상태 확인 -->
        <Condition ID="AreAllJointsInRange"
                   topic_name="/joint_states"/>
        <!-- 작업 공간 확인 -->
        <Condition ID="IsTargetReachable"
                   target="{grasp_pose}"/>
        <!-- 그리퍼 준비 확인 -->
        <Condition ID="IsGripperOpen"
                   topic_name="/gripper/state"/>
        <!-- 파지 실행 -->
        <Action ID="MoveToGraspPose"/>
        <Action ID="CloseGripper"/>
        <Condition ID="IsObjectGrasped"
                   topic_name="/ft_sensor/wrench"/>
    </Sequence>
</BehaviorTree>

6.2 안전 감시 하 매니퓰레이션

<BehaviorTree ID="SafeManipulation">
    <ReactiveSequence>
        <Condition ID="AreJointsWithinLimits"
                   topic_name="/joint_states"/>
        <Condition ID="IsForceWithinLimits"
                   topic_name="/ft_sensor/wrench"
                   max_force="50.0"/>
        <Action ID="ExecuteTrajectory"/>
    </ReactiveSequence>
</BehaviorTree>

7. 설계 시 고려 사항

7.1 관절 메시지의 순서 비일관성

JointState 메시지에서 관절의 순서는 발행자에 따라 상이할 수 있으며, 동일 발행자에서도 순서가 변경될 수 있다. 따라서 인덱스를 하드코딩하지 않고, 항상 name 필드를 검색하여 관절을 식별하여야 한다.

7.2 센서 잡음과 힘 임계값

힘/토크 센서의 측정값에는 잡음이 포함되어 있으므로, 파지 여부 판정이나 충돌 감지 시 적절한 필터링과 임계값 설정이 필요하다. 낮은 임계값은 잡음에 의한 오탐을, 높은 임계값은 실제 상황의 미탐을 초래한다.

7.3 실시간 제어와의 조율

매니퓰레이터의 제어 루프(일반적으로 1kHz)는 행동 트리의 tick 주기(일반적으로 10~100Hz)보다 빠르게 동작한다. 조건 노드의 평가 주기와 제어기의 응답 시간 사이의 차이를 고려하여, 안전 한계에 충분한 여유를 설정하여야 한다.

7.4 MoveIt2와의 연동

ROS2의 MoveIt2 프레임워크는 매니퓰레이터의 운동 계획, 충돌 검사, 역기구학 계산 등을 제공한다. 조건 노드에서 MoveIt2의 서비스를 호출하여 도달 가능성 검사나 충돌 검사를 수행할 수 있으나, 서비스 호출의 지연을 고려하여 캐싱 전략을 적용하여야 한다.

8. 참고 문헌

• Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
• Siciliano, B., et al. (2010). Robotics: Modelling, Planning and Control. Springer.
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/
• MoveIt2 공식 문서. https://moveit.picknik.ai/