관절 한계 확인 조건 노드 (Joint Limit Check Condition Node)

1. 개요

관절 한계 확인 조건 노드는 로봇 매니퓰레이터의 각 관절이 허용된 위치, 속도, 토크 범위 내에서 동작하고 있는지를 판정하는 조건 노드이다. 관절이 물리적 한계에 접근하면 기계적 손상, 제어 불안정, 특이점(singularity) 문제 등이 발생할 수 있으므로, 관절 상태를 실시간으로 감시하여 안전 범위 이탈을 사전에 방지하는 것이 필수적이다.

2. 관절 한계의 유형

2.1 위치 한계

회전 관절(revolute joint)의 위치 한계는 최소 각도 q_{\min}과 최대 각도 q_{\max}로 정의된다.

q_{\min} \leq q_i \leq q_{\max}

직선 관절(prismatic joint)의 경우 최소 위치 d_{\min}과 최대 위치 d_{\max}로 정의된다.

속도 한계

각 관절의 최대 허용 속도 \dot{q}_{\max}는 모터의 정격 속도와 감속비에 의해 결정된다.

\lvert \dot{q}_i \rvert \leq \dot{q}_{\max}

2.2 토크 한계

각 관절에 작용하는 토크 \tau_i는 모터의 정격 토크와 열적 한계에 의해 제한된다.

\lvert \tau_i \rvert \leq \tau_{\max}

구현

전체 관절 위치 한계 확인

class AreJointsWithinLimits
    : public BT::RosTopicSubNode<sensor_msgs::msg::JointState>
{
public:
    AreJointsWithinLimits(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_limits",
                "관절 한계 (name,min,max;name,min,max;...)"),
            BT::InputPort<double>("margin", 0.1,
                "경계 여유 (rad)")
        });
    }

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

        std::string limits_str;
        double margin;
        getInput("joint_limits", limits_str);
        getInput("margin", margin);

        auto limits = parseLimits(limits_str);

        for (const auto& [joint_name, min_val, max_val] : limits)
        {
            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_val + margin) ||
                        pos > (max_val - margin))
                    {
                        return BT::NodeStatus::FAILURE;
                    }
                }
            }
        }
        return BT::NodeStatus::SUCCESS;
    }

private:
    struct JointLimit
    {
        std::string name;
        double min_val;
        double max_val;
    };

    std::vector<JointLimit> parseLimits(const std::string& str) const
    {
        std::vector<JointLimit> limits;
        std::istringstream stream(str);
        std::string segment;
        while (std::getline(stream, segment, ';'))
        {
            std::istringstream seg_stream(segment);
            std::string name, min_s, max_s;
            if (std::getline(seg_stream, name, ',') &&
                std::getline(seg_stream, min_s, ',') &&
                std::getline(seg_stream, max_s, ','))
            {
                limits.push_back({name, std::stod(min_s),
                                  std::stod(max_s)});
            }
        }
        return limits;
    }
};

margin 매개변수는 물리적 한계로부터의 안전 여유를 설정한다. 관절 위치가 한계로부터 margin 이내에 진입하면 FAILURE를 반환하여, 한계에 도달하기 전에 안전 행동을 유발한다.

단일 관절 한계 접근 감지

class IsJointNearLimit
    : public BT::RosTopicSubNode<sensor_msgs::msg::JointState>
{
public:
    IsJointNearLimit(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_limit", "하한 (rad)"),
            BT::InputPort<double>("max_limit", "상한 (rad)"),
            BT::InputPort<double>("warning_margin", 0.15,
                "경고 여유 (rad)")
        });
    }

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

        std::string joint_name;
        double min_limit, max_limit, margin;
        getInput("joint_name", joint_name);
        getInput("min_limit", min_limit);
        getInput("max_limit", max_limit);
        getInput("warning_margin", margin);

        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_limit + margin) ||
                    pos >= (max_limit - margin))
                {
                    return BT::NodeStatus::SUCCESS;  // 한계에 접근
                }
                return BT::NodeStatus::FAILURE;  // 안전 범위 내
            }
        }
        return BT::NodeStatus::FAILURE;
    }
};

이 조건 노드는 관절이 한계에 접근한 경우 SUCCESS를 반환한다. 즉, “관절이 한계에 가까운가?“라는 질문에 대한 답이다. Inverter 데코레이터를 적용하면 “관절이 안전한가?“로 변환할 수 있다.

URDF 기반 관절 한계 로딩

ROS2의 URDF(Unified Robot Description Format)에는 각 관절의 한계가 정의되어 있다. 이를 자동으로 로딩하여 조건 노드에 적용하면, 하드코딩 없이 로봇 모델에 맞는 한계를 사용할 수 있다.

<!-- URDF 관절 한계 정의 예시 -->
<joint name="shoulder_pan_joint" type="revolute">
    <limit lower="-3.14159" upper="3.14159"
           velocity="2.16" effort="330"/>
</joint>

XML 행동 트리에서의 활용

관절 안전 감시 하 궤적 실행

<BehaviorTree ID="SafeTrajectoryExecution">
    <ReactiveSequence>
        <Inverter>
            <Condition ID="IsJointNearLimit"
                       topic_name="/joint_states"
                       joint_name="shoulder_pan_joint"
                       min_limit="-3.14" max_limit="3.14"
                       warning_margin="0.2"/>
        </Inverter>
        <Action ID="ExecuteTrajectory"/>
    </ReactiveSequence>
</BehaviorTree>

설계 시 고려 사항

연속 회전 관절

일부 관절은 연속 회전(continuous rotation)이 가능하며, 위치 한계가 존재하지 않는다. 이러한 관절에 대해서는 위치 한계 검사를 수행하지 않아야 하며, URDF의 관절 타입(continuous)을 확인하여 적절히 처리하여야 한다.

특이점 근접 감지

매니퓰레이터가 특이점(singularity)에 접근하면, 야코비안(Jacobian) 행렬의 행렬식이 0에 가까워지며, 작은 말단장치 운동에 대해 큰 관절 속도가 요구된다. 이는 관절 위치 한계와는 다른 유형의 위험이며, 야코비안 행렬의 조건수(condition number)나 가조작성 지표(manipulability measure)를 평가하는 별도의 조건 노드로 처리하는 것이 적절하다.

다중 관절의 동시 한계 접근

복수의 관절이 동시에 한계에 접근하는 경우, 개별 관절은 아직 안전 범위 내에 있더라도 복합적으로 위험한 상태일 수 있다. 이러한 상황을 감지하기 위해서는 각 관절의 한계 여유를 종합적으로 평가하는 지표가 필요하다.

참고 문헌

• 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/