작업 공간 범위 확인 조건 노드 (Workspace Range Check Condition Node)

1. 개요

작업 공간 범위 확인 조건 노드는 로봇 매니퓰레이터의 목표 위치 또는 현재 말단장치(end-effector) 위치가 로봇의 도달 가능한 작업 공간(workspace) 내에 있는지를 판정하는 조건 노드이다. 작업 공간 외부의 위치에 대한 운동 명령은 역기구학(inverse kinematics)의 해가 존재하지 않거나, 관절 한계를 초과하는 결과를 초래하므로, 운동 계획 전에 목표 위치의 도달 가능성을 사전 검증하는 것이 필수적이다.

2. 작업 공간의 정의

2.1 도달 가능 작업 공간과 기민한 작업 공간

매니퓰레이터의 작업 공간은 두 가지로 구분된다.

• 도달 가능 작업 공간(reachable workspace): 말단장치가 적어도 하나의 자세로 도달할 수 있는 위치의 집합
• 기민한 작업 공간(dexterous workspace): 말단장치가 모든 방향의 자세로 도달할 수 있는 위치의 집합

기민한 작업 공간은 도달 가능 작업 공간의 부분 집합이다.

2.2 작업 공간의 기하학적 근사

다관절 매니퓰레이터의 정확한 작업 공간은 복잡한 형상을 가지나, 실용적 목적으로 단순 기하학적 형태로 근사할 수 있다.

구형 근사에서 작업 공간은 내경 r_{\min}과 외경 r_{\max}의 구각(spherical shell)으로 표현된다.

r_{\min} \leq \lVert \mathbf{p} - \mathbf{p}_{\text{base}} \rVert \leq r_{\max}

여기서 \mathbf{p}는 목표 위치, \mathbf{p}_{\text{base}}는 매니퓰레이터 기저부의 위치이다.

구현

구형 작업 공간 확인

class IsTargetInWorkspace : public BT::ConditionNode
{
public:
    IsTargetInWorkspace(const std::string& name,
                        const BT::NodeConfiguration& config,
                        rclcpp::Node::SharedPtr node)
        : BT::ConditionNode(name, config), node_(node)
    {}

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<geometry_msgs::msg::PoseStamped>(
                "target_pose", "목표 자세"),
            BT::InputPort<double>("base_x", 0.0, "기저부 X (m)"),
            BT::InputPort<double>("base_y", 0.0, "기저부 Y (m)"),
            BT::InputPort<double>("base_z", 0.0, "기저부 Z (m)"),
            BT::InputPort<double>("min_reach", 0.2,
                "최소 도달 거리 (m)"),
            BT::InputPort<double>("max_reach", 0.85,
                "최대 도달 거리 (m)")
        };
    }

    BT::NodeStatus tick() override
    {
        geometry_msgs::msg::PoseStamped target;
        if (!getInput("target_pose", target))
        {
            return BT::NodeStatus::FAILURE;
        }

        double bx, by, bz, min_r, max_r;
        getInput("base_x", bx);
        getInput("base_y", by);
        getInput("base_z", bz);
        getInput("min_reach", min_r);
        getInput("max_reach", max_r);

        double dx = target.pose.position.x - bx;
        double dy = target.pose.position.y - by;
        double dz = target.pose.position.z - bz;
        double distance = std::sqrt(dx * dx + dy * dy + dz * dz);

        if (distance >= min_r && distance <= max_r)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }

private:
    rclcpp::Node::SharedPtr node_;
};

직육면체 작업 공간 확인

직교 로봇이나 특정 작업 영역에 적합한 직육면체 형태의 작업 공간 확인이다.

class IsTargetInBoxWorkspace : public BT::ConditionNode
{
public:
    IsTargetInBoxWorkspace(const std::string& name,
                           const BT::NodeConfiguration& config,
                           rclcpp::Node::SharedPtr node)
        : BT::ConditionNode(name, config), node_(node)
    {}

    static BT::PortsList providedPorts()
    {
        return {
            BT::InputPort<geometry_msgs::msg::PoseStamped>(
                "target_pose", "목표 자세"),
            BT::InputPort<double>("x_min", "X 하한 (m)"),
            BT::InputPort<double>("x_max", "X 상한 (m)"),
            BT::InputPort<double>("y_min", "Y 하한 (m)"),
            BT::InputPort<double>("y_max", "Y 상한 (m)"),
            BT::InputPort<double>("z_min", "Z 하한 (m)"),
            BT::InputPort<double>("z_max", "Z 상한 (m)")
        };
    }

    BT::NodeStatus tick() override
    {
        geometry_msgs::msg::PoseStamped target;
        if (!getInput("target_pose", target))
        {
            return BT::NodeStatus::FAILURE;
        }

        double x_min, x_max, y_min, y_max, z_min, z_max;
        getInput("x_min", x_min); getInput("x_max", x_max);
        getInput("y_min", y_min); getInput("y_max", y_max);
        getInput("z_min", z_min); getInput("z_max", z_max);

        double x = target.pose.position.x;
        double y = target.pose.position.y;
        double z = target.pose.position.z;

        if (x >= x_min && x <= x_max &&
            y >= y_min && y <= y_max &&
            z >= z_min && z <= z_max)
        {
            return BT::NodeStatus::SUCCESS;
        }
        return BT::NodeStatus::FAILURE;
    }

private:
    rclcpp::Node::SharedPtr node_;
};

MoveIt2 기반 도달 가능성 확인

정확한 역기구학 기반 도달 가능성 확인은 MoveIt2의 서비스를 활용하여 수행할 수 있다. 이 방식은 기하학적 근사보다 정확하나 계산 비용이 높으므로 캐싱 전략과 함께 사용한다.

<BehaviorTree ID="ReachabilityCheck">
    <Sequence>
        <Condition ID="IsTargetInWorkspace"
                   target_pose="{grasp_target}"
                   max_reach="0.85" min_reach="0.15"/>
        <Action ID="ComputeIKSolution"
                target="{grasp_target}"
                result="{ik_solution}"/>
        <Condition ID="IsIKSolutionValid"
                   solution="{ik_solution}"/>
        <Action ID="ExecuteMotion"/>
    </Sequence>
</BehaviorTree>

기하학적 작업 공간 확인을 선행하여 명확히 도달 불가능한 목표를 빠르게 걸러낸 후, 역기구학 계산을 수행하여 정확한 도달 가능성을 확인하는 2단계 전략이다.

설계 시 고려 사항

자세 제약

목표 위치가 작업 공간 내에 있더라도, 특정 자세(orientation)로는 도달할 수 없는 경우가 있다. 위치만으로 도달 가능성을 판정하면 자세 제약에 의한 실패를 예측하지 못한다. 정밀한 판정이 필요한 경우 역기구학 솔버를 활용하여야 한다.

장착물에 의한 작업 공간 변화

그리퍼에 도구나 물체가 장착되면 유효 작업 공간이 변화한다. 장착물의 크기와 무게를 고려하여 작업 공간 매개변수를 동적으로 조정하여야 한다.

충돌 체적 고려

기하학적 작업 공간 내에 있더라도, 로봇 자체나 환경과의 충돌로 인해 실제로 도달할 수 없는 위치가 존재한다. 충돌 검사는 별도의 모듈(MoveIt2의 충돌 검사 서비스 등)에서 수행하고, 그 결과를 조건 노드가 참조하는 구조가 적절하다.

참고 문헌

• 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.
• MoveIt2 공식 문서. https://moveit.picknik.ai/
• BehaviorTree.CPP 공식 문서. https://www.behaviortree.dev/