二足歩行ロボット研修 二足歩行ロボット研修(kora編)

二足歩行ロボット研修(kora編)[22] CRANE X7のROSパッケージを使ってDYNAMIXELサーボを制御する

二足歩行ロボット研修(kora編)[19] DYNAMIXEL Workbenchでモーションを再生する 二足歩行ロボット研修

こんにちは。koraです。

今回はros_controlを使って、これまで製作してきたロボットを動かしたいと思います。

CRANE X7のROSパッケージ

ros_controlに則ったDYNAMIXELサーボ用のパッケージとして、アールティが製作・販売しているアームロボット CRANE X7のROSパッケージ を使うことにします。

crane_x7_controlの確認

ros_controlの実装はcrane_x7_controlパッケージにあります。コントローラを起動する crane_x7_control.launch ファイルを見てみましょう。

<launch>
  <!-- USB port name for dynamixel_port -->
  <arg name="port" default="/dev/ttyUSB0" />

  <param name="robot_description"
    command="$(find xacro)/xacro --inorder '$(find crane_x7_description)/urdf/crane_x7.urdf.xacro'"
    />
  <group ns="crane_x7">
    <rosparam file="$(find crane_x7_control)/config/crane_x7_control.yaml" command="load"/>
    <param name="dynamixel_port/port_name" value="$(arg port)"/>

    <node name="controller_manager"
        pkg="controller_manager"
        type="spawner" respawn="false"
        output="screen"
        args="joint_state_controller
              arm_controller
              gripper_controller"/>

    <node name="crane_x7_control" pkg="crane_x7_control" type="crane_x7_control" output="screen" respawn="false">
      <!-- <param name="config_ns" value="/crane_x7"/> -->
    </node>

    <node name="robot_state_publisher" pkg="robot_state_publisher" 
        type="robot_state_publisher" respawn="false" output="screen">
      <remap from="/joint_states" to="/crane_x7/joint_states" />
    </node>
  </group>
</launch>
  • 以下の行でサーボと通信するシリアルポートを設定しています。CRANE-X7はUSB-シリアル変換器のU2D2を使用することを想定しているので、デフォルトのシリアルポートのデバイス名は/dev/ttyUSB0になります。このシリアルポートはlaunchファイルを起動する際に変更可能です。
      <!-- USB port name for dynamixel_port -->
      <arg name="port" default="/dev/ttyUSB0" />
    
        <param name="dynamixel_port/port_name" value="$(arg port)"/>
    
  • こちらの行でロボットモデルを読み込んでいます。CRANE-X7のモデルはrobot_descriptionパッケージにあり、xacro形式で記述されています。
      <param name="robot_description"
        command="$(find xacro)/xacro --inorder '$(find crane_x7_description)/urdf/crane_x7.urdf.xacro'"
        />
    
  • 以下の行でControllerの設定ファイル crane_x7_control.yaml を読み込んでいます。このファイルには後でロードするControllerの名前やタイプ、制御対象のジョイント名が記述されています。
        <rosparam file="$(find crane_x7_control)/config/crane_x7_control.yaml" command="load"/>
    
  • これらの部分でControllerをロードさせるspawnerを起動しています。joint_state_controller、arm_controller、gripper_controllerはControllerの設定ファイル crane_x7_control.yaml に記述されています。
        <node name="controller_manager"
            pkg="controller_manager"
            type="spawner" respawn="false"
            output="screen"
            args="joint_state_controller
                  arm_controller
                  gripper_controller"/>
    
  • 最も重要な、制御ループを回すROSノードを起動します。ロボットのハードウェアを定義するクラスは dxlport_control.h に定義されている DXLPORT_CONTROL で、
    ROSノードは hardware.cpp に記述されています。

        <node name="crane_x7_control" pkg="crane_x7_control" type="crane_x7_control" output="screen" respawn="false">
          <!-- <param name="config_ns" value="/crane_x7"/> -->
        </node>
    
  • 最後に robot_state_publisher ノードを起動します。これはURDFとjoint_statesからtfを出力するノードです。
        <node name="robot_state_publisher" pkg="robot_state_publisher" 
            type="robot_state_publisher" respawn="false" output="screen">
          <remap from="/joint_states" to="/crane_x7/joint_states" />
        </node>
    

crane_x7_controlの利用

crane_x7_controlはCRANE-X7用に作られたパッケージですが、設定を変更することでDYNAMIXELサーボを使用したロボットにも利用できます。例として二足歩行ロボットの首だけ動かしてみます。

まず、crane_x7_ros リポジトリをGitHubからクローンします。次に、crane_x7_control/config ディレクトリにControllerの設定ファイルを作成します。Controllerの設定と、制御対象のサーボの設定を記述します。

joint_state_controller:
  type: "joint_state_controller/JointStateController"
  publish_rate: 250

neck_controller:
  type: "position_controllers/JointTrajectoryController"
  publish_rate: 250
  joints:
    - neck_yaw
    - neck_pitch
  constraints:
    goal_time: 0.0
    stopped_velocity_tolerance: 1.0

dynamixel_port:
  baud_rate: 57600
  joints:
    - neck_yaw
    - neck_pitch
  neck_yaw: {id: 21, center: 2048, home: 2048, effort_const: 1.00, mode: 3 }
  neck_pitch: {id: 22, center: 2048, home: 2048, effort_const: 1.00, mode: 3 }

そして、launchファイルを修正します。以前設定したようにシリアルポートを/dev/ttyAMA1に変更し、ロボットのモデルには前回の記事で作成したxacroファイルを指定、Controllerの設定として先程作成したyamlファイルを指定します。

<launch>
  <!-- USB port name for dynamixel_port -->
  <arg name="port" default="/dev/ttyAMA1" />

  <param name="robot_description"
    command="$(find xacro)/xacro --inorder '$(find nisoku_kenshu_xyz_description)/urdf/nisoku_kenshu_xyz.xacro'"
    />
  <group ns="crane_x7">
    <rosparam file="$(find crane_x7_control)/config/crane_x7_control.yaml" command="load"/>
    <param name="dynamixel_port/port_name" value="$(arg port)"/>

    <node name="controller_manager"
        pkg="controller_manager"
        type="spawner" respawn="false"
        output="screen"
        args="joint_state_controller
              neck_controller"/>

    <node name="crane_x7_control" pkg="crane_x7_control" type="crane_x7_control" output="screen" respawn="false">
      <!-- <param name="config_ns" value="/crane_x7"/> -->
    </node>

    <node name="robot_state_publisher" pkg="robot_state_publisher" 
        type="robot_state_publisher" respawn="false" output="screen">
      <remap from="/joint_states" to="/crane_x7/joint_states" />
    </node>
  </group>
</launch>

動作確認

以上の設定を適用してlaunchファイルを起動すればControllerを立ち上げることができます。neck_controllerにJointTrajectoryControllerを使用しているので、”/crane_x7/neck_controller/command”トピックにJointTrajectoryメッセージを送ることでサーボを動かすことができます。

それでは動作確認のため、DYNAMIXEL Workbenchでモーションを再生したときと同様に、dynamixel_workbench_operatorsを使ってJointTrajectoryを送ってみることにします。ただし、トピック名が変わっているので dynamixel_workbench_operatorsのソースコード の一部を次のように修正する必要があります。

  // joint_trajectory_pub_ = node_handle_.advertise<trajectory_msgs::JointTrajectory>("joint_trajectory", 100);
  joint_trajectory_pub_ = node_handle_.advertise<trajectory_msgs::JointTrajectory>("/crane_x7/neck_controller/command", 100);

修正したコードのビルドができたら、次のコマンドで動作させます。

# 1つ目のターミナルで
$ roslaunch crane_x7_control crane_x7_control.launch

# 2つ目のターミナルで
$ roslaunch dynamixel_workbench_operators joint_operator.launch

# 3つ目のターミナルで
$ rosservice call /dynamixel_workbench/execution "{}"

実物のロボットの動きがrobot_state_publisherから出力されているので、RViz上のロボットモデルで再現することもできます。

crane_x7_controlを使ったDYNAMIXELサーボの制御

ただし、まだ首の設定しかされていないので、腕や脚は上手く表示されていません。首以外の関節は、次回以降適用させていきたいと思います。

タイトルとURLをコピーしました