그리퍼 컨트롤러 분리와 액션 클라이언트 제어

이전 글(평행 그리퍼 장착)에서 로봇 팔 끝단에 평행 그리퍼의 외형(URDF)을 모델링하고 조인트를 구성했다. 이번 글에서는 이 그리퍼를 실제로 구동하기 위해 전용 컨트롤러를 분리하고 액션 클라이언트로 제어하는 과정을 다룬다.

1. 그리퍼 전용 컨트롤러 분리

6축 팔과 그리퍼 조인트(finger1_joint)의 제어를 분리한다. 궤적 제어가 필요한 팔과 달리 그리퍼는 개폐 동작만 수행하므로 전용 컨트롤러를 사용하는 것이 구조적으로 명확하다.

최신 평행 그리퍼 플러그인인 parallel_gripper_action_controller/GripperActionController를 사용한다.

먼저 arm_controllers.yaml 파일을 수정한다. 기존 joint_trajectory_controller 목록에서 그리퍼 조인트를 삭제하고, 새 컨트롤러 설정을 추가한다.

controller_manager:
  ros__parameters:
    # 기존 컨트롤러들...
    gripper_controller:
      type: parallel_gripper_action_controller/GripperActionController

gripper_controller:
  ros__parameters:
    joint: finger1_joint
    allow_stalling: true       # 물체에 걸려 목표치에 도달하지 못해도(stall) 성공으로 처리
    stall_velocity_threshold: 0.001
    stall_timeout: 0.5
    goal_tolerance: 0.002      # 기본값 0.01은 전체 개방폭(0.025m) 대비 헐거우므로 축소

arm_study_bringup.launch.py에 그리퍼 컨트롤러 스포너(Spawner) 노드를 추가한다.

    gripper_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["gripper_controller", "--controller-manager", "/controller_manager"],
    )

    return LaunchDescription([
        # ... 기존 항목 ...
        joint_trajectory_controller_spawner,
        gripper_controller_spawner,
        gz_launch_description,
    ])

2. 시뮬레이션용 대상 물체 추가

그리퍼로 잡을 PCB 보드 모양의 가상 물체를 src/arm_bringup/models/board.sdf에 정의한다.

시뮬레이션에서 물체를 쥐려면 표면 마찰력(friction) 값이 필수다. 마찰력이 없으면 그리퍼가 닫혀도 물체가 미끄러져 떨어진다. SDFormat 스펙에 따라 <collision> 내부 <surface>에 마찰 속성을 추가해야 한다.

가제보 스폰(Spawn) 시 위치 설정에 문제가 있었다. ros_gz_sim create 노드는 파일(SDF) 내부에 기재된 <pose> 값을 읽지 않고 기본값인 원점(0, 0, 0)에 오브젝트를 생성한다. 이를 해결하기 위해 launch 파일에서 스폰 인자로 위치를 직접 명시하는 방식으로 변경하였다.

board_spawn_entity = Node(
    package="ros_gz_sim",
    executable="create",
    output="screen",
    arguments=[
        "-file", PathJoinSubstitution([FindPackageShare("arm_bringup"), "models", "board.sdf"]),
        "-name", "board",
        "-x", "0.4",
        "-y", "0.6",
        "-z", "0.0025",
    ],
)

3. TCP (Tool Center Point) 설정

물체를 파지하는 물리적 기준점이 될 가상 링크를 추가한다.

src/arm_description/urdf/arm_gripper.urdf.xacro 파일에 TCP 링크를 고정 조인트로 정의한다. tool0 링크를 기준으로 그리퍼 끝단(z축 방향 0.075m 지점)에 위치시킨다.

<link name="tcp_link"/>

<joint name="tcp_joint" type="fixed">
  <parent link="tool0"/>
  <child link="tcp_link"/>
  <origin xyz="0 0 0.075" rpy="0 0 0"/>
</joint>

4. 그리퍼 액션 클라이언트 연동

그리퍼 제어는 MoveGroupInterface가 아닌 액션 클라이언트를 통해 gripper_controller로 직접 전달된다. 타겟 메시지 타입은 control_msgs/action/ParallelGripperCommand다.
컨트롤러 이름이 GripperActionController라서 GripperCommand를 쓸 것 같지만 그렇지 않다. 컨트롤러 문서 첫 문장에 ParallelGripperCommand를 실행하는 컨트롤러라고 적혀 있고, 실행 중인 시스템에서는 ros2 action info /gripper_controller/gripper_cmd -t로 확인할 수 있다.

<depend>control_msgs</depend>
<depend>rclcpp_action</depend>

find_package(control_msgs REQUIRED)
find_package(rclcpp_action REQUIRED)
ament_target_dependencies(arm_control_app rclcpp moveit_ros_planning_interface control_msgs rclcpp_action)

ParallelGripperCommand의 목표와 결과는 sensor_msgs/JointState로 되어 있어서 관절 이름과 값을 배열로 넣는다. 배열의 몇 번째가 어떤 관절인지는 보장되지 않으므로 결과를 읽을 때는 이름으로 찾는다.

#include <control_msgs/action/parallel_gripper_command.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

using gripper_action_client_t = rclcpp_action::Client<control_msgs::action::ParallelGripperCommand>::SharedPtr;
gripper_action_client_t action_client =
  rclcpp_action::create_client<control_msgs::action::ParallelGripperCommand>(
      node, "gripper_controller/gripper_cmd");

control_msgs::action::ParallelGripperCommand::Goal goal;
goal.command.name = {"finger1_joint"};
goal.command.position = {0.0};     // 0.0 열림, 0.025 닫힘 

if (!action_client->wait_for_action_server(std::chrono::seconds(10))) {
  RCLCPP_ERROR(logger, "Cannot find gripper action server.");
  return -1;
}

rclcpp_action::Client<control_msgs::action::ParallelGripperCommand>::SendGoalOptions options;
using GoalHandle = rclcpp_action::ClientGoalHandle<control_msgs::action::ParallelGripperCommand>;

options.goal_response_callback = [=](const GoalHandle::SharedPtr & goal_handle) {
  if (!goal_handle) RCLCPP_ERROR(logger, "Goal was rejected by server");
  else RCLCPP_INFO(logger, "Goal accepted by server, waiting for result");
};

options.result_callback = [=](const GoalHandle::WrappedResult & result) {
  if (result.code != rclcpp_action::ResultCode::SUCCEEDED) {
    RCLCPP_ERROR(logger, "Goal failed with code: %d", static_cast<int>(result.code));
    return;
  }
  const auto & st = result.result->state;
  auto it = std::find(st.name.begin(), st.name.end(), "finger1_joint");
  if (it != st.name.end()) {
    size_t idx = std::distance(st.name.begin(), it);
    RCLCPP_INFO(logger, "Final result: position=%.3f", st.position[idx]);
  }
  RCLCPP_INFO(logger, "stalled=%d reached_goal=%d",
    result.result->stalled, result.result->reached_goal);
};

action_client->async_send_goal(goal, options);