힘/토크 임계값 확인 조건 노드 (Force/Torque Threshold Check Condition Node)

1. 개요

힘/토크 임계값 확인 조건 노드는 로봇 말단장치(end-effector) 또는 관절에 작용하는 힘과 토크가 허용 범위 내에 있는지를 판정하는 조건 노드이다. 힘/토크 센서(Force/Torque sensor, F/T sensor)의 측정값을 임계값과 비교하여, 과도한 외력에 의한 충돌 감지, 조립 작업 중 접촉 확인, 힘 제어 기반 작업의 조건 판정 등에 활용된다.

2. ROS2 힘/토크 메시지

2.1 geometry_msgs::msg::WrenchStamped

힘 벡터의 크기 \lVert \mathbf{f} \rVert와 토크 벡터의 크기 \lVert \boldsymbol{\tau} \rVert는 다음과 같이 계산된다.

\lVert \mathbf{f} \rVert = \sqrt{f_x^2 + f_y^2 + f_z^2}

\lVert \boldsymbol{\tau} \rVert = \sqrt{\tau_x^2 + \tau_y^2 + \tau_z^2}

3. 구현

3.1 힘 크기 임계값 확인

class IsForceWithinLimits
    : public BT::RosTopicSubNode<geometry_msgs::msg::WrenchStamped>
{
public:
    IsForceWithinLimits(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>("max_force", 50.0,
                "최대 허용 힘 (N)"),
            BT::InputPort<double>("max_torque", 10.0,
                "최대 허용 토크 (N·m)")
        });
    }

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

        double max_force, max_torque;
        getInput("max_force", max_force);
        getInput("max_torque", max_torque);

        double force_mag = 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);

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

        if (force_mag <= max_force && torque_mag <= max_torque)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

3.2 충돌 감지 조건 노드

예상치 못한 외력이 감지되면 충돌이 발생한 것으로 판정한다.

class IsCollisionDetected
    : public BT::RosTopicSubNode<geometry_msgs::msg::WrenchStamped>
{
public:
    IsCollisionDetected(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>("collision_force_threshold", 20.0,
                "충돌 판정 힘 임계값 (N)")
        });
    }

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

        double threshold;
        getInput("collision_force_threshold", threshold);

        double force_mag = 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_mag > threshold)
        {
            return BT::NodeStatus::SUCCESS;  // 충돌 감지됨
        }
        return BT::NodeStatus::FAILURE;  // 충돌 미감지
    }
};

3.3 축별 힘 확인

특정 축 방향의 힘만을 개별적으로 확인하는 조건 노드이다. 조립 작업에서 삽입 방향의 힘을 감시하는 데 활용된다.

class IsAxialForceInRange
    : public BT::RosTopicSubNode<geometry_msgs::msg::WrenchStamped>
{
public:
    IsAxialForceInRange(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>("axis", "z",
                "검사할 축 (x, y, z)"),
            BT::InputPort<double>("min_force", -100.0,
                "최소 허용 힘 (N)"),
            BT::InputPort<double>("max_force", 100.0,
                "최대 허용 힘 (N)")
        });
    }

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

        std::string axis;
        double min_f, max_f;
        getInput("axis", axis);
        getInput("min_force", min_f);
        getInput("max_force", max_f);

        double force = 0.0;
        if (axis == "x") force = msg->wrench.force.x;
        else if (axis == "y") force = msg->wrench.force.y;
        else if (axis == "z") force = msg->wrench.force.z;

        if (force >= min_f && force <= max_f)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }
};

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

4.1 충돌 감지 기반 안전 정지

<BehaviorTree ID="CollisionSafeOperation">
    <ReactiveSequence>
        <Inverter>
            <Condition ID="IsCollisionDetected"
                       topic_name="/ft_sensor/wrench"
                       collision_force_threshold="30.0"/>
        </Inverter>
        <Action ID="ExecuteTrajectory"/>
    </ReactiveSequence>
</BehaviorTree>

4.2 힘 제어 기반 삽입 작업

<BehaviorTree ID="ForceControlledInsertion">
    <ReactiveSequence>
        <Condition ID="IsAxialForceInRange"
                   topic_name="/ft_sensor/wrench"
                   axis="z"
                   min_force="-50.0"
                   max_force="5.0"/>
        <Action ID="InsertPart"/>
    </ReactiveSequence>
</BehaviorTree>

5. 설계 시 고려 사항

5.1 센서 바이어스 보상

F/T 센서는 온도 변화, 중력에 의한 도구 무게 등으로 인해 바이어스(offset)를 가진다. 조건 판정 전에 바이어스를 보상(taring)하여야 정확한 외력 측정이 가능하다.

5.2 동적 힘과 정적 힘의 구분

매니퓰레이터의 가감속 운동 중에는 관성력이 발생하여 F/T 센서 측정값에 영향을 미친다. 충돌 감지 시 이러한 동적 힘을 잔여 오차로 인식하지 않도록, 운동 상태에 따른 임계값 조정이나 역동역학(inverse dynamics) 기반 관성력 보상이 필요하다.

5.3 저역 통과 필터

F/T 센서의 측정값에는 고주파 잡음이 포함되어 있다. 저역 통과 필터(low-pass filter)를 적용하여 잡음을 제거하되, 충돌과 같은 급격한 힘 변화를 감지하기 위해서는 필터의 차단 주파수(cutoff frequency)를 적절히 설정하여야 한다.

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/