こんにちはshotaです。
このシリーズでは、アールティのアームロボットCRANE+ V2（クラインプラスブイツー）をROS 2で動かしていきます。
CRANE+ V2のROS 2パッケージの詳しい使い方を説明します。
前回のおさらい
前回の記事ではLinux PC（Ubuntu 20.04）にMoveIt 2をインストールし、デモパッケージを実行しました。
その後、CRANE+ V2のROS 2パッケージをインストールしました。
また、こちらの記事ではCRANE+ V2とはどんなロボットなのか、ROS 2パッケージでどんな動きをするのかを紹介しました。ぜひご覧ください。
CRANE+ V2のセットアップ
CRANE+ V2 のROS 2パッケージはrt-net/crane_plusという名前でGitHubに公開されています。
前回の記事ではQuick Startの項目を実行しました。
今回はサンプルコード（crane_plus_examples）を実行します。
サンプルコードの実行手順は、crane_plus/crane_plus_examplesのREADMEに書かれています。
READMEの手順通りにセットアップし、サンプルを実行します。
CRANE+ V2本体をPCに接続する
まずはCRANE+ V2本体をPCに接続します。接続方法は製品マニュアルを参照してください。
また、こちらの記事でも接続方法を説明しています。
USBポートのアクセス権限を変更する
CRANE+ V2をPCに接続すると/dev/ttyUSB0のようなデバイスファイルが作られます。
これが、CRANE+ V2との通信に使われるUSBポートです
次のコマンドを実行すると、ポート名を確認できます。
$ ls /dev/ttyUSB* /dev/ttyUSB0
このUSBポートを使用するためにアクセス権限を変更します。
詳しい説明は、CRANE+ V2の制御パッケージであるcrane_plus_controlのREADMEに書かれているので参照してください。
次のコマンドを実行してアクセス権限を変更します。
$ sudo chmod 666 /dev/ttyUSB0
/dev/ttyUSB0以外を使用する場合（例：/dev/ttyUSB1、/dev/ttyUSB2）、アクセス権限の変更に加えて、パッケージのパラメータ設定が必要です。
パラメータはcrane_plus_control/config/crane_plus_controllers.yamlにセットされています。
このファイルのパラメータport_nameを変更します。
control_param_node:
ros__parameters:
port_name: /dev/ttyUSB0
baudrate: 1000000
joint_name_list:
- crane_plus_joint1
- crane_plus_joint2
- crane_plus_joint3
- crane_plus_joint4
- crane_plus_joint_hand
dxl_id_list: [0x01, 0x02, 0x03, 0x04, 0x05]
timeout_seconds: 5.0
100 Hzで制御するための設定
crane_plus_controlはデフォルト100 Hz周期で制御するように設定されていますが、
100 Hz周期の制御を実現するためにはUSBポートのlatency_timerの変更と、サーボモータ AX-12AのReturn Delay Timeの変更が必要です。
まず、USBポートのlatency_timerを変更します。
ターミナルを開き、次のコマンドを実行します。
$ sudo su
パスワードを入力してroot権限でログインします。
その後、次のコマンドを実行します。
# echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer # exit
つづいて、サーボモータAX-12AのReturn Delay Timeを設定します。
設定にはROBOTISのDynamixel Wizard 2というアプリケーションを使用します。
Dynamixel Wizard 2のインストール方法は下記のe-manualを参照してください。
AX-12AのReturn Delay Timeはアドレス0x05にセットされています。
この値を0にします。
Return Delay Timeの設定
これで準備完了です。
ROS 2ノードを起動する
次のコマンドを実行して、demo.launch.pyを起動します。
$ ros2 launch crane_plus_examples demo.launch.py
これにより、MoveIt2 のノード（move_group）と、制御ノード（crane_plus_control）、RVizが起動するので、
RVizを操作してCRANE+ V2を動かすことが出来ます。
RVizでCRANE+ V2を動かす方法は前回の記事でも紹介しています。
demo.launch.pyの起動後
サンプルを実行する
準備が整ったのでサンプルを実行します。
グリッパの開閉（gripper_control）
グリッパを開閉させるサンプルです。
ターミナルをもう一つ開き、次のコマンドを実行します。
$ ros2 launch crane_plus_examples example.launch.py example:='gripper_control'
実行するとこのように動きます。
ソースファイルgripper_control.cppを見ると、
グリッパのジョイント（サーボモータ）の角度を±30度で変化させていることがわかります。
gripper_joint_values[0] = to_radians(30); // 目標角度を代入 move_group_gripper.setJointValueTarget(gripper_joint_values); // 目標角度をセット move_group_gripper.move(); // 動かす gripper_joint_values[0] = to_radians(-30); // 目標角度を代入 move_group_gripper.setJointValueTarget(gripper_joint_values); // 目標角度をセット move_group_gripper.move(); // 動かす
設定済みの姿勢に動かす（pose_groupstate）
設定済みの姿勢に動かすサンプルです。
次のコマンドを実行します。
$ ros2 launch crane_plus_examples example.launch.py example:='pose_groupstate'
実行するとこのように動きます。
ソースファイルpose_groupstate.cppを見ると、
homeとverticalという目標姿勢をセットして動かしていることがわかります。
move_group_arm.setNamedTarget("home"); // 目標姿勢の名前をセット
move_group_arm.move(); // 動かす
move_group_arm.setNamedTarget("vertical"); // 目標姿勢の名前をセット
move_group_arm.move(); // 動かす
これらの目標姿勢はcrane_plus_moveit_config/config/crane_plus.srdfで設定されています。
<group_state name="vertical" group="arm">
<joint name="crane_plus_joint1" value="0" />
<joint name="crane_plus_joint2" value="0" />
<joint name="crane_plus_joint3" value="0" />
<joint name="crane_plus_joint4" value="0" />
</group_state>
<group_state name="home" group="arm">
<joint name="crane_plus_joint1" value="0.0" />
<joint name="crane_plus_joint2" value="-1.16" />
<joint name="crane_plus_joint3" value="-2.01" />
<joint name="crane_plus_joint4" value="-0.73" />
</group_state>
ジョイント角度を一つずつ変更する（joint_values）
ジョイント角度を１つずつ変更するサンプルです。
次のコマンドを実行します。
$ ros2 launch crane_plus_examples example.launch.py example:='joint_values'
実行するとこのように動きます。
ソースファイルjoint_values.cppを見ると、
ジョイント角度をそれぞれ45度動かしていることがわかります。
auto joint_values = move_group_arm.getCurrentJointValues();
double target_joint_value = to_radians(45.0);
for (size_t i = 0; i < joint_values.size(); i++) {
joint_values[i] = target_joint_value;
move_group_arm.setJointValueTarget(joint_values);
move_group_arm.move();
}
ピックアンドプレース（pick_and_place）
モノを掴む・持ち上げる・運ぶ・置くサンプルです。
次のコマンドを実行します。
$ ros2 launch crane_plus_examples example.launch.py example:='pick_and_place'
実行するとこのように動きます。
ソースファイルpick_and_place.cppを見ると、
手先の目標位置座標（x, y, z）と、目標姿勢（ロール、ピッチ、ヨー）をセットして動かしていることがわかります。
target_pose.position.x = 0.15; // 手先の目標座標 x=0.15 m target_pose.position.y = 0.0; target_pose.position.z = 0.06; q.setRPY(to_radians(0), to_radians(90), to_radians(0)); // 手先の目標姿勢ピッチ＝90度 target_pose.orientation = tf2::toMsg(q); move_group_arm.setPoseTarget(target_pose); // 目標位置・姿勢をセット move_group_arm.move(); // 動かす
まとめ
今回は、
- CRANE+ V2をROS 2で動かすためにセットアップしました。100 Hz周期で動かすためには、latency_timerとReturn Delay Timeの設定が必要です。
- CRANE+ V2のサンプルを実行しました。ソースファイルを書き換えることで、様々な動きを試すことが出来ます。
次回は、ROS/ROS2 Advent Calendar 2020にCRANE+ V2の記事を投稿する予定です。
