파지 상태 확인 조건 노드 (Grasp State Check Condition Node)

1. 개요

파지 상태 확인 조건 노드는 로봇 그리퍼(gripper)가 물체를 파지(grasp)하고 있는지를 판정하는 조건 노드이다. 파지 상태는 그리퍼의 개폐 위치, 그리퍼에 가해진 힘, 힘/토크 센서의 측정값 등을 통해 판정되며, 물체의 파지 성공 여부 확인, 파지 유지 감시, 배치 작업 전 조건 확인 등에 활용된다.

2. 파지 상태 판정 방법

2.1 그리퍼 위치 기반 판정

그리퍼의 개폐 위치를 기반으로 파지 상태를 유추한다. 그리퍼가 완전히 닫힌 위치보다 약간 열려 있으면 물체를 잡고 있는 것으로 판정할 수 있다.

2.2 힘 기반 판정

그리퍼 모터의 전류 또는 힘/토크 센서를 통해 파지 힘을 측정하여, 임계값 이상의 힘이 감지되면 물체를 잡고 있는 것으로 판정한다.

2.3 복합 판정

위치와 힘을 동시에 평가하는 것이 가장 신뢰성이 높다. 그리퍼가 부분적으로 닫혀 있고 동시에 일정 수준 이상의 파지 힘이 감지되면 파지 성공으로 판정한다.

3. 구현

3.1 관절 위치 기반 파지 확인

class IsObjectGrasped
    : public BT::RosTopicSubNode<sensor_msgs::msg::JointState>
{
public:
    IsObjectGrasped(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>("gripper_joint",
                "finger_joint", "그리퍼 관절 이름"),
            BT::InputPort<double>("closed_position", 0.0,
                "완전 닫힘 위치"),
            BT::InputPort<double>("min_grasp_position", 0.005,
                "파지 판정 최소 위치 (물체가 있으면 이 이상)")
        });
    }

    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 closed_pos, min_grasp_pos;
        getInput("gripper_joint", joint_name);
        getInput("closed_position", closed_pos);
        getInput("min_grasp_position", min_grasp_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 > (closed_pos + min_grasp_pos))
                {
                    return BT::NodeStatus::SUCCESS;
                }
                return BT::NodeStatus::FAILURE;
            }
        }
        return BT::NodeStatus::FAILURE;
    }
};

3.2 힘/토크 센서 기반 파지 확인

class IsGraspForceDetected
    : public BT::RosTopicSubNode<geometry_msgs::msg::WrenchStamped>
{
public:
    IsGraspForceDetected(const std::string& name,
                         const BT::NodeConfiguration& config,
                         const BT::RosNodeParams& params)
        : RosTopicSubNode(name, config, params)
    {}

    static BT::PortsList providedPorts()
    {
        return providedBasicPorts({
            BT::InputPort<double>("min_grasp_force", 2.0,
                "파지 판정 최소 힘 (N)")
        });
    }

    BT::NodeStatus onTick(
        const geometry_msgs::msg::WrenchStamped::SharedPtr& msg) override
    {
        if (!msg)
        {
            return BT::NodeStatus::FAILURE;
        }

        double min_force;
        getInput("min_grasp_force", min_force);

        double force_magnitude = std::sqrt(
            msg->wrench.force.x * msg->wrench.force.x +
            msg->wrench.force.y * msg->wrench.force.y +
            msg->wrench.force.z * msg->wrench.force.z);

        if (force_magnitude >= min_force)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

3.3 그리퍼 개폐 상태 확인

class IsGripperOpen
    : public BT::RosTopicSubNode<sensor_msgs::msg::JointState>
{
public:
    IsGripperOpen(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>("gripper_joint",
                "finger_joint", "그리퍼 관절 이름"),
            BT::InputPort<double>("open_threshold", 0.035,
                "개방 판정 위치 임계값")
        });
    }

    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 open_threshold;
        getInput("gripper_joint", joint_name);
        getInput("open_threshold", open_threshold);

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

4. XML 행동 트리에서의 활용

4.1 파지-운반-배치 시퀀스

<BehaviorTree ID="PickAndPlace">
    <Sequence>
        <!-- 파지 단계 -->
        <Condition ID="IsGripperOpen"
                   topic_name="/joint_states"
                   gripper_joint="finger_joint"/>
        <Action ID="MoveToGraspPose"/>
        <Action ID="CloseGripper"/>
        <Condition ID="IsObjectGrasped"
                   topic_name="/joint_states"
                   gripper_joint="finger_joint"/>
        <!-- 운반 단계 -->
        <ReactiveSequence>
            <Condition ID="IsGraspForceDetected"
                       topic_name="/ft_sensor/wrench"
                       min_grasp_force="1.5"/>
            <Action ID="MoveToPlacePose"/>
        </ReactiveSequence>
        <!-- 배치 단계 -->
        <Action ID="OpenGripper"/>
    </Sequence>
</BehaviorTree>

운반 단계에서 ReactiveSequence를 사용하여 파지 힘을 지속적으로 감시한다. 물체가 미끄러져 파지 힘이 감소하면 운반 동작이 중단된다.

5. 설계 시 고려 사항

5.1 그리퍼 타입에 따른 판정 기준

5.2 센서 잡음과 파지 판정 안정성

그리퍼 관절의 인코더 해상도가 낮거나, 힘 센서의 잡음이 큰 경우 파지 판정이 불안정할 수 있다. 이동 평균 필터나 히스테리시스를 적용하여 안정성을 확보한다.

5.3 물체 미끄러짐 감지

파지 중 물체가 서서히 미끄러지는 경우, 파지 힘은 유지되나 그리퍼 위치가 점진적으로 변화한다. 그리퍼 위치의 시간 변화율을 감시하여 미끄러짐을 감지할 수 있다.

6. 참고 문헌

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