1296.63 MoveToJointPositions 액션 노드 구현

1296.63 MoveToJointPositions 액션 노드 구현

1. MoveToJointPositions의 정의와 역할

MoveToJointPositions는 로봇 매니퓰레이터의 각 관절을 지정된 목표 각도(joint positions)로 이동시키는 행동 트리 액션 노드이다. 이 노드는 관절 공간(joint space)에서 직접 목표를 지정하며, MoveIt2의 MoveGroup 액션 서버를 통해 관절 공간 경로 계획과 궤적 실행을 수행한다.

MoveToPose가 작업 공간에서 말단 장치의 자세를 목표로 지정하는 것과 달리, MoveToJointPositions는 관절 공간에서 직접 목표를 지정한다. 이 방식은 역기구학의 다해(multiple solutions) 문제를 회피하며, 홈 위치 복귀, 사전 정의된 자세로의 이동, 관절 한계 회피 등의 상황에서 활용된다.

2. 관절 공간 제어의 특성

관절 공간 경로 계획은 작업 공간 경로 계획과 비교하여 다음의 특성을 가진다.

구분관절 공간 계획작업 공간 계획
목표 지정관절 각도 벡터말단 장치 자세 (6-DoF)
역기구학불필요필요
해의 유일성유일다수의 해 존재 가능
직선 경로관절 공간 직선작업 공간 직선 불보장
적용 사례홈 위치, 사전 정의 자세물체 파지, 정밀 위치 제어

3. 클래스 구조와 구현

#include "behaviortree_cpp/action_node.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "moveit_msgs/action/move_group.hpp"

class MoveToJointPositions : public BT::StatefulActionNode
{
public:
  MoveToJointPositions(
    const std::string & xml_tag_name,
    const BT::NodeConfiguration & conf)
  : BT::StatefulActionNode(xml_tag_name, conf)
  {
    node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
    action_client_ =
      rclcpp_action::create_client<moveit_msgs::action::MoveGroup>(
        node_, "move_action");
  }

  static BT::PortsList providedPorts()
  {
    return {
      BT::InputPort<std::vector<double>>(
        "joint_positions", "Target joint positions [rad]"),
      BT::InputPort<std::vector<std::string>>(
        "joint_names", "Names of joints to control"),
      BT::InputPort<std::string>(
        "planning_group", "manipulator", "MoveIt2 planning group"),
      BT::InputPort<double>(
        "planning_time", 5.0, "Allowed planning time [s]"),
      BT::InputPort<double>(
        "joint_tolerance", 0.01,
        "Joint position tolerance [rad]"),
      BT::InputPort<double>(
        "max_velocity_scaling", 1.0,
        "Maximum velocity scaling factor [0.0-1.0]"),
      BT::InputPort<double>(
        "max_acceleration_scaling", 1.0,
        "Maximum acceleration scaling factor [0.0-1.0]"),
      BT::OutputPort<int>(
        "error_code", "MoveIt2 error code")
    };
  }

  BT::NodeStatus onStart() override;
  BT::NodeStatus onRunning() override;
  void onHalted() override;

private:
  rclcpp::Node::SharedPtr node_;
  rclcpp_action::Client<moveit_msgs::action::MoveGroup>::SharedPtr
    action_client_;
  rclcpp_action::ClientGoalHandle<
    moveit_msgs::action::MoveGroup>::SharedPtr goal_handle_;
  bool result_received_{false};
  moveit_msgs::action::MoveGroup::Result::SharedPtr result_;
};

4. onStart() 구현

BT::NodeStatus MoveToJointPositions::onStart()
{
  std::vector<double> joint_positions;
  std::vector<std::string> joint_names;
  std::string planning_group;
  double planning_time, joint_tolerance;
  double vel_scaling, accel_scaling;

  getInput("joint_positions", joint_positions);
  getInput("joint_names", joint_names);
  getInput("planning_group", planning_group);
  getInput("planning_time", planning_time);
  getInput("joint_tolerance", joint_tolerance);
  getInput("max_velocity_scaling", vel_scaling);
  getInput("max_acceleration_scaling", accel_scaling);

  if (joint_positions.size() != joint_names.size()) {
    RCLCPP_ERROR(node_->get_logger(),
      "Joint positions and names size mismatch: %zu vs %zu",
      joint_positions.size(), joint_names.size());
    return BT::NodeStatus::FAILURE;
  }

  if (!action_client_->wait_for_action_server(std::chrono::seconds(5))) {
    return BT::NodeStatus::FAILURE;
  }

  // 골 메시지 구성
  auto goal = moveit_msgs::action::MoveGroup::Goal();
  goal.request.group_name = planning_group;
  goal.request.num_planning_attempts = 10;
  goal.request.allowed_planning_time = planning_time;
  goal.request.max_velocity_scaling_factor = vel_scaling;
  goal.request.max_acceleration_scaling_factor = accel_scaling;

  // 관절 제약 조건 설정
  moveit_msgs::msg::Constraints goal_constraints;
  goal_constraints.name = "joint_position_constraints";

  for (size_t i = 0; i < joint_names.size(); ++i) {
    moveit_msgs::msg::JointConstraint jc;
    jc.joint_name = joint_names[i];
    jc.position = joint_positions[i];
    jc.tolerance_above = joint_tolerance;
    jc.tolerance_below = joint_tolerance;
    jc.weight = 1.0;
    goal_constraints.joint_constraints.push_back(jc);
  }

  goal.request.goal_constraints.push_back(goal_constraints);
  goal.planning_options.plan_only = false;

  // 비동기 골 전송
  auto options =
    rclcpp_action::Client<moveit_msgs::action::MoveGroup>::SendGoalOptions();

  options.result_callback =
    [this](auto%20result) {
      result_ = std::make_shared<
        moveit_msgs::action::MoveGroup::Result>(result.result);
      result_received_ = true;
    };

  result_received_ = false;
  action_client_->async_send_goal(goal, options);

  return BT::NodeStatus::RUNNING;
}

5. onRunning() 구현

BT::NodeStatus MoveToJointPositions::onRunning()
{
  if (!result_received_) {
    return BT::NodeStatus::RUNNING;
  }

  if (result_->error_code.val ==
      moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
  {
    return BT::NodeStatus::SUCCESS;
  }

  setOutput("error_code", static_cast<int>(result_->error_code.val));
  return BT::NodeStatus::FAILURE;
}

6. onHalted() 구현

void MoveToJointPositions::onHalted()
{
  if (goal_handle_) {
    action_client_->async_cancel_goal(goal_handle_);
  }
  goal_handle_.reset();
  result_received_ = false;
}

7. XML 행동 트리에서의 사용

7.1 홈 위치 복귀

<MoveToJointPositions
  joint_positions="0.0;0.0;0.0;0.0;0.0;0.0"
  joint_names="joint1;joint2;joint3;joint4;joint5;joint6"
  planning_group="manipulator"
  max_velocity_scaling="0.5"/>

7.2 사전 정의 자세로의 이동

매니퓰레이션 작업에서 빈번하게 사용되는 자세를 사전에 정의하고 블랙보드를 통해 전달한다.

<Sequence>
  <!-- 수직 직립 자세 -->
  <MoveToJointPositions
    joint_positions="{upright_positions}"
    joint_names="{joint_name_list}"
    planning_group="manipulator"/>
  
  <!-- 작업 수행 -->
  <MoveToPose target_pose="{task_pose}"
              planning_group="manipulator"/>
  
  <!-- 수납 자세로 복귀 -->
  <MoveToJointPositions
    joint_positions="{stow_positions}"
    joint_names="{joint_name_list}"
    planning_group="manipulator"/>
</Sequence>

8. 속도 및 가속도 스케일링

max_velocity_scaling과 max_acceleration_scaling 포트를 통해 궤적의 최대 속도와 가속도를 URDF에 정의된 한계값의 비율로 제한한다.

  • 1.0: 최대 속도/가속도 사용 (기본값)
  • 0.5: 최대 속도/가속도의 50% 사용
  • 0.1: 최대 속도/가속도의 10% 사용 (저속 정밀 이동)

안전 영역에서는 높은 스케일링을 사용하여 작업 효율을 높이고, 장애물 인접 영역이나 정밀 작업에서는 낮은 스케일링을 사용하여 안전성을 확보한다.

<!-- 빠른 이동 (개방 공간) -->
<MoveToJointPositions
  joint_positions="{target}"
  joint_names="{names}"
  max_velocity_scaling="1.0"
  max_acceleration_scaling="1.0"/>

<!-- 느린 이동 (정밀 작업) -->
<MoveToJointPositions
  joint_positions="{target}"
  joint_names="{names}"
  max_velocity_scaling="0.1"
  max_acceleration_scaling="0.1"/>

9. 관절 한계 검증

MoveIt2는 URDF에 정의된 관절 한계를 자동으로 검증하여, 목표 관절 각도가 허용 범위를 벗어나면 계획을 거부한다. 행동 트리 노드에서는 입력 포트의 값이 관절 한계 내에 있는지를 사전에 검증하여, 불필요한 계획 시도를 방지할 수 있다.

10. 플러그인 등록

#include "behaviortree_cpp/bt_factory.h"

BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<MoveToJointPositions>("MoveToJointPositions");
}

참고 문헌 및 출처

  • Coleman, D., Sucan, I., Chitta, S., & Correll, N. (2014). “Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study.” Journal of Software Engineering for Robotics, 5(1), 3-16.
  • Colledanchise, M., & Ogren, P. (2018). Behavior Trees in Robotics and AI: An Introduction. CRC Press.
  • MoveIt2 공식 문서: https://moveit.picknik.ai/main/index.html
  • BehaviorTree.CPP 공식 문서: https://www.behaviortree.dev/

버전 정보: MoveIt2 Humble (ROS2 Humble) 기준, BehaviorTree.CPP v3.8 이상.