1 はじめに
先端技術ユニットでは、カメラ画像や触覚センサーなどを含むマルチモーダルな強化学習に取り組んでいます。中でも、シミュレータでの強化学習の結果を実機でも実行する、いわゆるSim2Realを実現するには、実機のロボットアームとカメラを協調動作させる必要があります。そこで今回は、6軸ロボットアームmyCobot（Elephant Robotics社製）とデプスカメラRealSense D455（Intel社製）をROSを用いて連携テストしました。その過程と結果を以下に詳しく説明します。
動作環境：
PC：Ubuntu 20.04、ROS Noetic、Python 3.8.10
ロボットアーム: myCobot280 M5Stack
深度カメラ: RealSense D455
このブログ投稿では、簡単なプログラムを作成して実行する方法を説明していますが、ROS と Python の環境がすでにセットアップされていることを前提としています。
2. 私の協働ロボットの基本
まずはmyCobotを用意しますが、ファームウェアのアップデートなどで使用する部分が変わっているので少し戸惑います。この作業は、myStudio のバージョンが 1.3.<> だった 2021 年 3 月頃に行われました。大きな変更はないと思いますが、違いがある場合は公式の説明を参照してください。
myCobot を実行するには、次の 3 つの準備が必要です。
● ファームウェアの更新 (ベーシック、アトミック)
●ロボットアーム関節の校正
●ロボットアームとPC間のシリアル通信
ファームウェアをアップデートする
myCobot 280 M5 には、ベースに M5Stack Basic があり、これら 2 つのモジュールにファームウェアを書き込むための ATOM (Basic、ATOM) がロボット アームの端にあります。
まず、USB とchmodを使用して PC に接続し、デバイスの読み書きを可能にします。また、Windows または Mac での USB シリアル通信には、特別なドライバーをインストールする必要があります。
$ sudo chmod 666 /dev/ttyUSB*コピー
次に、更新されたファームウェアアプリケーション myStudioをダウンロードします(この記事の執筆時点での最新バージョンは 3.1.4 ですが、Windows のみ、Linux では 3.1.3 が利用可能です)。
ソースコードを抽出してAppImageを実行すると、それぞれBasicとATOMが起動します。図 1 と図 2 に示すように。
myStudio 3.1.3 では、図 3 と図 4 の画面が表示されます。Basic の Minirobot と ATOM の AtomMain の最新バージョンをダウンロードし、Flash を選択してファームウェアを書き込みます。Basic で書き込みが完了すると、ミニロボットの出力がパネルに表示されます。（Basic、ATOMは最新版で書かないとロボットアームが正常に動作しない可能性がありますのでご注意ください）。
ファームウェアを更新したら、次のステップは関節角度を校正することです。
関節角度の校正
まず、[基本] パネルで [調整] を選択し、[OK] を押します。
図 5 に示すように、myCobot の各ジョイントには原点を表すノッチがあるため、ノッチは手動で互いに位置合わせされます。
この状態で「Basic」パネルの「Calibrate Servo」を選択し「Next」を押せばキャリブレーションは完了です。テストサーボを実行すると、モーターがノッチの周りを少し回転して、適切なキャリブレーションが行われていることを確認できます。
ロボットアームとPC間のシリアル通信
最後に、トランスポンダーを起動すると、PC からシリアル通信経由で myCobot を操作できるようになります。これは簡単で、[基本] パネルでトランスポンダを選択し、[OK] を押すだけです。次に、図 6 に示すような画面が表示されます。基本ボタンを押します。
Basic パネルの他のメニューのうち、Basic の Master Control は ATOM を制御し、Info は各ジョイントが正しく接続されているかどうかをチェックします。myCobot が PC 上で正しく動作しない場合は、myCobot 自体に問題があるかどうかを確認できます。
3. Pythonインターフェース
準備が完了したら、PC 上で myCobot を操作できるようになります。今回はpymycobotを使ってPythonスクリプトから操作する方法と、mycobot_moveitライブラリを使ってROSからMoveItを操作する方法を試してみました。
まず、pymycobotをインストールします。
$ pip install pymycobot --upgradeコピー
Githubからソース コードをダウンロードしてインストールすることもできます。
サンプル スクリプトは、ソースからダウンロードすると test ディレクトリに含まれます。ただし、そのままでは機能しませんので注意してください。書き直す代わりに、以下の例を作成しました。
# mycobot_control_test.py#!/usr/bin/env pythonimport time from pymycobot.mycobot import MyCobotfrom pymycobot.genre import Angle, Coord if __name__ == "__main__":port = "/dev/ttyUSB0"mycobot = MyCobot(port, baudrate="115200", timeout=0.1, debug=False) #After the baud rate are the default values #Get the operation of the joint angleprint("get angles")print(" degrees: {}\n".format(mycobot.get_angles()))time.sleep(0.5) print("get radians")print(" radians: {}\n".format(mycobot.get_radians()))time.sleep(0.5) print("send angles: [0, 0, 0, 0, -90, 0]")mycobot.send_angles([0, 0, 0, 0, 0, 0], 80)time.sleep(1.5)mycobot.send_angles([0, 0, 0, 0, -90, 0], 80)print(" Is moving: {}\n".format(mycobot.is_moving()))time.sleep(1.5) print("send radians: [0, 0, 0, 0, 1.57, 0]\n")mycobot.send_radians([0, 0, 0, 0, 1.57, 0], 80)time.sleep(1.5) print("send angle to joint 4: -90\n")mycobot.send_angle(Angle.J4.value, -90, 80)time.sleep(1.5) # Operations to get coordinatesprint("get coordination")print(" coordination {}\n".format(mycobot.get_coords()))time.sleep(0.5) print("send coordination: [50, 50, 300, 0, 0, 0] → [50, 50, 300, 90, 90, 0]\n")coord_list = [50, 50, 300, 0, 0, 0]mycobot.send_coords(coord_lis, 70, 0)time.sleep(3.0)coord_list = [50, 50, 300, 90, 90, 0]mycobot.send_coords(coord_lis ,70, 0)time.sleep(1.5) # Scenarios using grippers# print("open gripper")# mycobot.set_gripper_state(0,0)# time.sleep(1.0)# print("close gripper")# mycobot.set_gripper_state(1,80)# time.sleep(1.0) time.sleep(1.5)print("release all servos")mycobot.release_all_servos()コピー
mycobot_control_test.pyファイルを実行します。
$ python3 mycobot_control_test.pyコピー
mycobot モジュールで MyCobot クラスのインスタンスを作成し、ゲッターとセッターを使用して状態を検査および変更します。
インスタンスの作成
mycobot = MyCobot(port, baudrate="115200", timeout=0.1, debug=False)コピー
4 つのパラメータを設定します。Baud Rate の後にデフォルト値を入力します。ポートはUSBシリアル通信ポートです。ターミナル でls /dev/を実行すると、 PC に接続されているデバイスのリストを表示できます。Linux では、シリアル通信用の他の USB ポートがない場合は、/ dev/ttyUSB0になります。MacとWindowsでは違うと思うので確認してください。
ゲッターについて
get_angles( ) およびget_radians( ) は、関節角度を度およびラジアンで取得する関数です。戻り値は 6 つの関節角度値のリストです。
また、ベースの中心を中心とした座標系*でアームの先端の座標を取得するget_coords()関数もあります。戻り値は、先端のx、y、z座標 (mm) と方向rx、ry、rz (角度) を含む 6 次元のリストです。MoveIt を使わずにインバースキネマティクスを実装できれば素晴らしいでしょう。
※座標系：「基本」パネルを奥にして、x:前方、y:左方、z:上方を正の方向とします。MoveIt ではベクトルが若干異なることに注意してください。これについては後で説明します。
セッターについて
関節角度を度およびラジアンで送信する関数はsend_angles()とsend_radians()であり、2種類のパラメータ設定があります。
まず、6つの関節をすべて指定して送信する場合、第一引数はゲッターと同じで、6つの浮動小数点値をリスト型に入れ、第二引数*に関節の動作速度を入力します(int:0) ~ 100 ) 。
mycobot .send_angles ( [ 0 , 0 , 0 , 0 , 0 , 0 ] , 80 )コピー
次に、関節の角度を取得して送信します。第一パラメータに関節角度コード、第二パラメータに角度値、第三パラメータに速度を入力してください。
mycobot .send_angle ( Angle.J4.value , -90,80 )コピー
ゲッターのように座標によって操作することもできます。この場合、6 つの要素[x, y, z, rx, ry, rz]のリストが配置され、最初のパラメーターは座標、2 番目のパラメーターは速度、3 番目のパラメーターはモードです。モードには0: Angular と 1: Linearの 2 種類があります。
mycobot .send_coords ( [ [ 80 , 50 , 300 , 0 , 90 , 0 ] , 70 , 0 )コピー
ソースコードには速度単位の指定がないので、必要に応じて計測することになると思います。
他のインターフェース
アームが動くとモーターは現状を維持しようとしてトルクを加え続けるため、放っておくとモーターに過負荷がかかります。そこで、関数release_all_servos(y)を使用して、操作が完了した後にモーターのトルクを解放しましょう。
ロボットアームの動作中に別の動作を指示すると、エラーとなり次の動作に移ります。サンプル スクリプトでは、Python の組み込み関数time.sleep() を使用して各動作が完了するのを待機していますが、関数is_moving()を使用してモーターが動作しているかどうかを取得できるため、while などをループすることができます。 。(この関数にはバグがあり、常に移動する状態を返すと思います。
他にも電源のオンオフやLEDの色の変更、モーターの状態の確認などのAPIがありますが、今回はアームを動かすことが目的なので割愛しました。
4. 読み取り専用規格
次に、ROS で MoveIt を使用して、myCobot を操作します。
ゾウロボットはmycobotの動作を実現、設置可能、設置可能
MoveIt のボランティア実装があるので、それを使用することにしました。インストールは上記のGitHub Readmeに従って記述します。
$ cd /src$ git clone https://github.com/Tiryoh/mycobot_ros$ git clone https://github.com/nisshan-x/mycobot_moveit$ rosdep update$ rosdep install -i –from-paths mycobot_moveit$ cd .$ catkin_make #Set up work area$ source devel/setup.bashコピー
インストールとワークスペースの設定が完了したら
$ roslaunch mycobot_moveit mycobot_moveit_control.launchコピー
図 7 に示すように、MoveIt と Rviz GUI が起動します。Rvizは緑色のボールをドラッグ＆ドロップしてロボットのアームの終端位置のポーズを決め、左下の計画・実行ボタンを押すことで実際のロボットと一緒に動きます。
※機種と実機が連携できない場合
これはすべての環境で発生するバグであり、モデルと実機が常に連動するとは限りません。これはモーターの回転方向が逆の場合に発生する誤差なので少し難しいですが、模型と実機を見比べて関節の回転方向が逆になっていないかを探してください。
逆方向に回転する関節が何個あるかを確認したら、ロボットモデルを記述したURDFファイルを書き換えます。
/ src / mycobot_moveit / urdf / mycobot_urdf_gazebo.urdf
定義された関節情報の説明は以下の通りです。
mycobot_urdf_gazebo.urdf ~~ <joint name="arm1_joint" type="revolute"> <parent link="body_link" /> <child link="arm1_link" /> <origin xyz="0 0 0.0706" rpy="0 0 0" /> <axis xyz="0 0 1" /> <limit effort="30" lower="-2.88" upper="2.88" velocity="1.5" /> </joint> ~~コピー
Arm1 ～ arm6_joint も同様の手順です。回転軸の方向は<axis xyz="0 0 1"></axis> に設定されており、 1→-1 に設定するとジョイントの回転が逆方向に反転します。
私の環境では第三関節以外が反転しているようです。ロボットのサーボの個体差によるものなのか、その他の理由によるものなのかわかりませんが、ご存知の方、教えてください。
myCobot のセットアップから基本的な操作まで、これがすべてです。
5. リアルなD455の基礎知識
今回はD455でテストしましたが、基本的にはD400シリーズでも同様に使えます。D435i と D455 のみ IMU センサーが内蔵されていますが、この記事では使用しません (固定状態で IMU を使用する利点よりもエラーが蓄積する欠点の方が明白であるため)。IRステレオカメラを搭載したD400シリーズに加えて、LiDARを搭載したL515もありますが、目的は同じです。あと、新品の性能が一番良いと思うので、D455を購入します。部分的な部分もあるので、購入前に使用したい環境を問い合わせた方が良いと思います（旧モデルは基本的に下位互換なので、価格と性能のトレードオフになります）。
ソフトウェアのインストールとビューアの基本操作
RealSense を実行するには、ライブラリ librealsense をインストールします。これがないと後述する realsense_ros が動作しません。Linux にインストールする方法に関するドキュメントがあります。Windows でのインストール方法も、必要に応じて同じドキュメントライブラリにあります。
# Server Public Key Registration$ sudo apt-key adv –keyserver keyserver.ubuntu.com –recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE|| Adding a Repository$ sudo add-apt-repository “deb https://librealsense.intel.com/Debian/apt- repo $(lsb_release -cs) main” -u# install lib$ sudo apt install librealsense2-dkms librealsense2- utils# install dev$ sudo apt install librealsense2-dev librealsense2-dbgコピー
インストールが完了したら、ターミナルで RealSense Viewer を実行して表示します。動作しない場合は、RealSense USB 接続を抜き差しし、PC を再起動してみてください。起動に成功すると、図 8 に示すようにビューアが表示されます。
右上隅にある 2D | 3D ボタンを使用して、ビューアを 2D と 3D の間で切り替えることができます。さらに、左側のステレオモジュールとRGBカメラをオンにすると、深度情報とRGB情報を表示できます。2D では、RGB および深度情報を 2D で表示できます。3D では、深度推定赤外線ステレオ カメラによって推定された点群は、深度カラー マップと RGB カメラ情報で色付けされ、あらゆる角度から見ることができます。さらに、IMU センサーを内蔵した D435i および D455 は、モーション モジュールを通じて姿勢情報を取得することもできます。次に、ROS パッケージ realsense_ros を使用して Rviz 点群を見てみましょう。これはaptを使ってインストールできます。
$ sudo apt install ros-$ROS_DISTRO-realsense2-cameraコピー
RealSense カメラは最初の端末で起動され、各カメラの情報に加えて色付きの点群が提供され、視覚化のために 2 番目の端末で Rviz が起動されます。
$ roslaunch realsense2_camera rs_camera.launch filter：=pointcloud$rvizコピー
図 9 に示すように点群を表示するには、GUI を使用して設定を調整します。
まず、左側の表示パネルのGlobal Optionsの固定ボックスがMap になっているので、それをクリックしてCamera_link に変更します。
次に、表示パネルの下部にある [追加] ボタンを押すと、新しいウィンドウがポップアップ表示され、Rviz で表示できる ROS メッセージ タイプがリストされます。rviz グループの中央にある PointCloud2 を選択し、OK を押して、PointCloud2 を表示パネルに追加します。
また、グループ内の「被写体」列の右側の空白をクリックして表示される/カメラ/深さ/色/点を選択すると、Rviz上に点群が表示されます。デフォルトでは点群サイズが0.01mと大きな値に設定されているため、点群が重なって表示されますが、0.001m程度に設定すると非常に細かく点群が取得されていることがわかります。
また、「追加」から「TF」を選択して追加すると、カメラの位置と方向（軸）を表示できます。デフォルトでは、RGB カメラの原点とステレオ カメラの原点はそれぞれワールド座標と光学座標で表示されます。( camera_linkはステレオカメラと起源が同じようです)
毎回このボタンを押すのは大変なので、設定を保存してください。
ワークスペースで設定を保存するためのディレクトリ構成を作成し、Rviz のファイルに名前を付けて構成を、作成した構成ディレクトリに名前を付けて保存します。
$ rviz -d .rvizコピー
何かを変更した場合は、構成を保存することで、毎回同じファイルを更新できます。点群とカメラの位置を確認するだけの場合は、 realsense_viewer を使用して簡単に実行できますが、生データには ROS を使用できます。ここから始めることは、ROS メッセージ データの意味を理解するための演習のようなものです。
（ただし、個人的にはこれが大きなネックです）
rs_camera.launch フィルター := 点群を実行した後。
別の端末から送信された/ camera/ Depth/color/pointsトピックの生データを見てみましょう。
$ rostopic echo /camera/depth/color/pointsコピー
もちろん、この値の配列は点群データであり、xyz 座標と位置の RGB 値で表されるはずですが、直接読み取るのは困難です。数字を見ると、それぞれの値が 0 ～ 255 の範囲にあり、同様の値が規則的なパターンで規則的に出現していることがわかります。ただし、ここから x がどこを表し、赤がどこを表しているかを推測するのは無理があります。
したがって、まず、これがどのような種類のメッセージであるかを調べてください。
$ rostopic type /camera/depth/color/pointssensor_msgs/PointCloud2コピー
これで、メッセージ タイプがsensor_msgs/PointCloud2 であることがわかりました。
また、ROSのドキュメントを見ると、次のようなことがわかります。
Header header # header uint32 height # number of rows of datauint32 width # Number of columns of dataPointField[] fields # 1 point data structurebool is_bigendian # whether the big enduint32 point_step # number of bytes in a point (number of 8-bit numbers)uint32 row_step # number of data in a row (= width * point_step)uint8[] data # raw data (8bit row_step * height data array)bool is_dense # if true, then no invalid dataコピー
点群なので、2D 画像とは異なり、データの順序はどうなるのでしょうか? 質問が示すように、高さと幅は意味がないようです。
他の値から計算できて使用される可能性が低い変数を保持するという意味は不明ですが、構造はこんな感じです。それぞれの実際の値を確認したい場合は、
$ rostopic echo /camera/depth/color/points/<Variable Name>コピー
こうすることで、データ内の変数を 1 つだけ出力できます。たとえば、バックポイントステップとフィールドを表示します。
$ rostopic echo /camera/depth/color/points/point_step–20—$ rostopic echo /camera/depth/color/points/fields–name: “x”offset: 0datatype: 7count: 1–name: “y”offset: 4datatype: 7count: 1–name: “z”offset: 8datatype: 7count: 1–name: “rgb”offset: 16datatype: 7count: 1—コピー
出力はpoint_step = 20によるもので、ポイントのデータが 20 バイトで表されていることがわかり、これらの 20 バイトがフィールドの内容からどのように構成されているかがわかります。
それぞれの変数の意味は、
name:表すデータの名前
オフセット:対応するバイト
データ型:表現されたデータの型コード
count:含まれるデータ項目の数
たとえば、x 座標は 0 から 3 までのデータに対応し、型コード 7 (float32 に対応) がそのデータを表します。yとz は同じ方法で処理される必要がありますが、RGB も同じ方法で表現されます。RGBに相当する16～19番目の値を見ると、GBRA順に16進数の値が並んでいます。float32 への変換は扱いが難しいようですので、xyz座標と RGB は別々に処理する必要があるようです。
データを表現する方法がわかったので、実際にデータを操作してみましょう。
(次のスクリプトを処理するための、よりスマートで高速な方法があると思いますので、何か提案があればお願いします)
scripts ディレクトリを mycobot_test パッケージに追加し、そこに Python スクリプトを追加します。
$ cd /src/mycobot_test$ mkdir scripts$ cd scripts$ touch red_point_detection.py$ touch object_position_search.pyコピー
CMakeLists.txt と package.xml に依存関係を追加します。
# CMakeLists.txtfind_package(catkin REQUIRED COMPONENTS # Add rospy and sensor_msgs modules to be able to import rospy sensor_msgs ) catkin_package( # Add build package sensor_msgs ) # Add scripts to the executable directory catkin_install_python(PROGRAMS DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts )Python<!-- package.xml --><!-- add --> <build>rospy</build> <build>sensor_msgs</build> <!-- package.xml --><!-- add --> <build>rospy</build> <build>sensor_msgs</build>コピー
以下のred_point_detection.py は点群から赤色のみを抽出して新規メッセージを作成するスクリプトです。
#!/usr/bin/env python3 import time, os, sys, signal, threading import numpy as np import rospy from sensor_msgs.msg import PointCloud2 class RedPointsDetectionNode(object): def __init__(self): super(RedPointsDetectionNode, self).__init__() self.data = PointCloud2() self.point_step = 20 self.R_COL = 18 self.G_COL = 17 self.B_COL = 16 rospy.init_node("red_points_detection_node") rospy.loginfo("red points detection start") # Subscriber def sub_pointcloud(self): def callback(data): self.sub_data = data sub = rospy.Subscriber("camera/depth/color/points", PointCloud2, callback = callback) rospy.spin() # Publisher def pub_red_pointcloud(self): pub = rospy.Publisher("red_point_cloud", PointCloud2, queue_size = 1) while not rospy.is_shutdown(): pub_data = self.red_point_detection(self.sub_data) pub.publish(pub_data) def red_point_detection(sub_data): red_point_data = sub_data red_pointcloud = np.array([d for d in sub_data.data]).reshape(-1, self.point_step) r_flags = red_pointcloud < 180 g_flags = red_pointcloud > 150 b_flags = red_pointcloud > 150 R_under_threshold_row = np.where(r_flags)[0][np.where(np.where(r_flags)[1]==self.R_COL)] G_over_threshold_row = np.where(g_flags)[0][np.where(np.where(g_flags)[1]==self.G_COL)] B_over_threshold_row = np.where(b_flags)[0][np.where(np.where(b_flags)[1]==self.B_COL)] not_red_row = np.unique(np.concatenate([R_under_threshold_row, G_over_threshold_row, B_over_threshold_row])) red_pointcloud = np.delete(red_pointcloud, not_red_row, axis=0).ravel().tolist() red_point_data.width = int(len(red_pointcloud) / self.point_step) red_point_data.height = 1 red_point_data.row_step = pub_pc2_data.width * self.point_step red_point_data.data = red_pointcloud rospy.loginfo("red pointcloud {}".format(int(len(red_pointcloud) / self.point_step))) return red_point_data # node start to subscribe and publish def start_node(self): sub_pointcloud = threading.Thread(target = self.sub_pointcloud) pub_red_pointcloud = threading.Thread(target = self.pub_red_pointcloud) sub_pointcloud.setDaemon(True) sub_pointcloud.start() pub_red_pointcloud.setDaemon(True) pub_red_pointcloud.start() sub_pointcloud.join() pub_red_pointcloud.join() if __name__ == "__main__": color_points_detector = ColorPointsDetectionNode() color_points_detector.start_node() passコピー
サブスクライバーはカメラ/深度/カラー/ポイント データを読み取り、パブリッシャーはデータから赤いポイントを抽出して配布するだけです。赤い点は、生の点群からRGB 値f R < 180、G > 150、B > 150 を持つ点を削除するために処理されます。光の影響を受けやすいRGBよりもHSVを使用した方が良いのですが、今回はPointCloud2のデータを処理する際にHSVへの変換が困難だったので見送られました。C++ならPCLを使えば簡単にPython変換できそうですが、Pythonでは他の手続きを書くのが大変なので役に立ちません）。
書いたら、ワークスペース catkin_make に移動して実行します。
$ roslaunch realsense2_camera rs_camera.launch filters:=pointcloud$ rosrun mycobot_test red_point_detection.py$ rviz -d .rvizコピー
Rviz のPointCloud2メッセージに追加された/red_point_cloudを選択すると、図 10 に示すように、抽出された点群が表示されます。
object_position_search.py は抽出した赤い点の座標の平均を求めるスクリプトです。
# object_position_search.py#!/usr/bin/env python3 import time, os, sys, signal, threading import numpy as np import rospy from sensor_msgs.msg import PointCloud2 import sensor_msgs.point_cloud2 as pc2 from geometry_msgs.msg import Point class ObjectPositionSearchNode(object): def __init__(self): super(ObjectPositionSearchNode, self).__init__() rospy.init_node("object_position_search_node") rospy.loginfo("object position search start") self.object_position = Point() # Subscriber def sub_pointcloud(self): def callback(data): rospy.loginfo("subscribed pointcloud") xyz_generator = pc2.read_points(data, field_names = ("x", "y", "z"), skip_nans=True) xyz_list = [gen for gen in xyz_generator] list_num = len(xyz_list) x = np.array([xyz_list[i][0] for i in range(list_num) if (xyz_list[i][2] < 1.0 and xyz_list[i][2] > -1.0)]) y = np.array([xyz_list[i][1] for i in range(list_num) if (xyz_list[i][2] < 1.0 and xyz_list[i][2] > -1.0)]) z = np.array([xyz_list[i][2] for i in range(list_num) if (xyz_list[i][2] < 1.0 and xyz_list[i][2] > -1.0)]) self.object_position.x = np.average(x) self.object_position.y = np.average(y) self.object_position.z = np.average(z) sub = rospy.Subscriber("red_point_cloud", PointCloud2, callback = callback) rospy.spin() # Publisher def pub_target_position(self): pub = rospy.Publisher("object_position", Point, queue_size=1) while not rospy.is_shutdown(): rospy.loginfo("published object position: {:.5f}, {:.5f}, {:.5f}\n".format(self.object_position.x, self.object_position.y, self.object_position.z)) pub.publish(self.object_position) def start_node(self): sub_pointcloud = threading.Thread(target = self.sub_pointcloud) pub_target_position = threading.Thread(target = self.pub_target_position) sub_pointcloud.setDaemon(True) sub_pointcloud.start() pub_target_position.setDaemon(True) pub_target_position.start() sub_pointcloud.join() pub_target_position.join() if __name__ == "__main__": object_position_searcher = ObjectPositionSearchNode() object_position_searcher.start_node() passコピー
red_point_search.py を前に実行した状態で、新しいターミナルを起動して実行します。
$ rosrun mycobot_test object_position_search.pyコピー
red_point_cloud 座標値の平均を取得して分散し、これらの値を記録します。座標はメートル単位であり、このスクリプトは 1.0 メートル以内のポイントを取得します。rospy.loginfoで取得した座標をログに記録するので、端末に座標が出力されます。
sensor_msgsライブラリにはpoint_cloud2モジュールがあり、 PointCloud2データの処理と読み取り、座標値の 4Byte から float32 への変換に使用されます。内容は簡単なのですが、このモジュールの存在がわかりにくかったです。他に複雑なメッセージ データがある場合は、付随する処理モジュールがあるかどうかを確認することをお勧めします。これが実際のセットアップとデータ処理です。データ処理に関しては、アプリケーションに応じて、より高速に処理する方法（またはアルゴリズムを改善する方法）を見つける必要があると思います。
6. ROS を使用して myCobot を RealSense D455 に接続します
RealSense データ処理が完了したので、それを myCobot で使用したいと思います。したがって、最も重要なことは、RealSense カメラ座標系から myCobot 座標系への変換です。Rviz にはカメラ軸を表示する TF (トランスフォーム) がありますが、それを理解する必要があります。名前が示すように、これは、ある座標系と別の座標系の間の関係を記述する座標変換を記述します。まず、この TF が設定されていない場合に何が起こるかを見てみましょう。
$ roslaunch realsense2_camera rs_camera.launch filter：=pointcloud$ roslaunch mycobot_moveit mycobot_moveit_control.launchコピー
図 11 に示すように、Rviz 表示パネルに警告が表示されます。メッセージは、カメラのすべての座標系がBase_link (myCobot の原点) に変換されないということです。
それでは、カメラから myCobot に TF (座標変換) をブロードキャストするパッケージを作成しましょう。C++ と Python を行ったり来たりで申し訳ありませんが、今回は roscpp を使用します。まずパッケージを作成します。
$ cd <your_ros_ws_for_MyCobot>/src$ catkin_create_pkg tf_broadcaster roscpp$ cd tf_broadcaster$ touch src/tf_broadcaster.cppコピー
生成されたtf_broadcaster.cpp CMakeLists.txtとpackage.xml を次のように書き換えます。C++ ファイルはクラス内でノードを作成する練習をするために作成されたため、クラスですが、より簡単に作成できることに注意してください。
// tf_broadcaster.cpp#include <ros/ros.h> #include <geometry_msgs/TransformStamped.h> #include <tf2_ros/static_transform_broadcaster.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> class TfBroadcaster { public: TfBroadcaster(); ~TfBroadcaster(); // Broadcast void BroadcastStaticTfFromCameraToMyCobot(); private: ros::NodeHandle nh_; // TF Broadcaster tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_; // constant double PI_ = 3.1419265; }; TfBroadcaster::TfBroadcaster(){} TfBroadcaster::~TfBroadcaster(){} void TfMyCobotBroadcaster::BroadcastStaticTfFromCameraToMyCobot() { geometry_msgs::TransformStamped transformStamped; transformStamped.header.stamp = ros::Time::now(); transformStamped.header.frame_id = "camera_color_frame"; //link transformStamped.child_frame_id = "base_link"; // son link // Parallel movement transformStamped.transform.translation.x = -0.3; transformStamped.transform.translation.y = -0.3; transformStamped.transform.translation.z = -0.3; // Turning back tf2::Quaternion q; q.setEuler(0, 0, 0); transformStamped.transform.rotation.x = q.x(); transformStamped.transform.rotation.y = q.y(); transformStamped.transform.rotation.z = q.z(); transformStamped.transform.rotation.w = q.w(); static_tf_broadcaster_.sendTransform(transformStamped); } int main(int argc, char** argv) { ros::init(argc, argv, "tf_mycobot_broadcaster"); TfMyCobotBroadcaster tf_mycobot_broadcaster; tf_mycobot_broadcaster.BroadcastStaticTfFromCameraToMyCobot(); ros::Rate loop_rate(10); while(ros::ok()){ ros::spinOnce(); loop_rate.sleep(); } return 0; }# CMakeLists.txtcmake_minimum_required(VERSION 3.0.2) project(tf_broadcaster) find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs tf2_geometry_msgs) catkin_package( CATKIN_DEPENDS geometry_msgs tf2_geometry_msgs) include_directories(${catkin_INCLUDE_DIRS}) add_executable(tf_mycobot_broadcaster src/tf_mycobot_broadcaster.cpp) target_link_libraries(tf_mycobot_broadcaster ${catkin_LIBRARIES})<!-- package.xml --><?xml version="1.0"?> <package format="2"> <name>tf_broadcaster</name> <version>0.0.0</version> <description>The transform broadcaster package</description> <maintainer email="[email protected]">root</maintainer> <license>TODO</license> <buildtool_depend>catkin</buildtool_depend> <depend>tf2_geometry_msgs</depend> <depend>geometry_msgs</depend> </package>コピー
ご覧のとおり、transformStamped クラスのインスタンス変数を設定し、インスタンスをブロードキャストします。ブロードキャスト変数の型は tf2_ros::StaticTransformBroadcaster ですが、今回はカメラとロボットの位置が固定されているため静的 TF を使用します。興味がある方のために、時間の経過とともに位置関係が変化する場合にも動的 TF を使用できます。
ここではまだ位置関係が分からないので仮の値ですが、表示を組み立てることはできるので試してみましょう。また、親リンクの原点は点群座標系と一致するため、親リンクはCamera_linkではなくCamera_color_frameになります。catkin_makeの後、前と同じようにrs_camera.launchとmycobot_moveit_control.launchを実行し、別のターミナルで tf_broadcaster を実行します。
$ rosrun tf_broadcaster tf_broadcasterコピー
これにより、イメージャと myCobot Base_link の間の位置関係がブロードキャストされるため、 TF 警告は表示されなくなります。この状態で点群を追加すると図12のようになります。もちろん、カメラとmyCobotの位置関係は一時的なものなので、カメラから見たmyCobotの位置とRvizで表示されるモデルの位置は重なりません。
次に、カメラから見たロボットの相対的な姿勢と位置を調整します。
カメラを特定の位置に固定して位置関係を指定する方法もあると思いますが、今回はロボットの座標を決定するために3点をマークし、単位ベクトルを求めて位置を校正し、それに対して計算を行います。カメラ座標系の関係。
お気づきの方もいるかもしれませんが、前の写真はマーカーがすでに取り付けられている状態で撮影されていますが、図 13 に示すように配置されています。
配給マーク
myCobot の原点 (中央下) はマーカー 2 と 3 の中点にあり、マーカー 1 はセグメント 2 と 3 の原点に対して垂直です。
1. まず、red_point_detection.pyを書き換えてblue_point_detection.pyを作成し、青でマークされた点群を取得します。このスクリプトは抽出する RGB 範囲をオーバーライドするだけなので、ここでは省略します。その後、そこから座標変換を取得する方法は色々あると思いますが、今回は以下の手順で行います。マーカー座標を決定する マーカー点群をサブスクライブする
1つ。点群を 3 つにクラスタリングします (K 平均法による)
2. 単位ベクトル マーカー 2 と 3 の原点と中点を決定します。これが原点 V_robot です。
1つ。X、Yの単位ベクトルV_X、V_YはそれぞれV_robot→mark 1、V_robot→mark 2となります。
bZ の単位ベクトルは、ラベル付き平面と法線ベクトルの外積です。
3. オイラー角を使用して、カメラのポーズからロボットのポーズまでの回転を作成します。 theta_1 = 水平面に対するカメラの仰角
a.theta_2 = カメラの正面に向かう回転角度
b.theta_3 = カメラとロボットの正面方向との間の角度
4. VZ 方向の原点を myCobot の固定ベース (2.7 cm) に変換します。
※オイラー回転を開始する軸は任意のため、使用するライブラリによって結果が異なります。(誰もがこれをわざわざ行うとは思わないので、単位ベクトルと 2 つの座標系の原点を渡して、それを TF に変換するライブラリがあるかどうか疑問に思います。
次のスクリプトが scripts ディレクトリに追加されます。
/*
* 提示：该行代码过长，系统自动注释不进行高亮。一键复制会移除系统注释
* # mycobot_position_calibration.py#!/usr/bin/env python3 import time, os, sys, signal, threading import numpy as np import rospy from sensor_msgs.msg import PointCloud2 from geometry_msgs.msg import Point import sensor_msgs.point_cloud2 as pc2 class MyCobotPositionCalibrationNode(object): def __init__(self): super(MyCobotPositionCalibrationNode, self).__init__() rospy.init_node("mycobot_position_calibration_node") rospy.loginfo("mycobot position calibration start") # Subscriber def sub_pointcloud(self): def callback(data): self.data = data rospy.loginfo("subscribed pointcloud") xyz_generator = pc2.read_points(self.data, field_names = ("x", "y", "z"), skip_nans=True) xyz_list = [xyz for xyz in xyz_generator if (abs(xyz[0]) < 0.8 and abs(xyz[1]) < 0.8 and abs(xyz[2]) < 0.8)] xyz_array = np.array(xyz_list) if len(xyz_list) > 3: marker_centroids = self.kmeans(3, xyz_array) rospy.loginfo("\n marker positions\n{}".format(marker_centroids)) translation = self.cal_translation(marker_centroids) rospy.loginfo("\n translation\n{}".format(translation)) sub = rospy.Subscriber("blue_point_cloud", PointCloud2, callback = callback) rospy.spin() # node start def start_node(self): sub_pointcloud = threading.Thread(target = self.sub_pointcloud) sub_pointcloud.setDaemon(True) sub_pointcloud.start() sub_pointcloud.join() # clustering method def kmeans(self, k, X, max_iter=30): # k: cluster num, X: numpy array data_size, n_features = X.shape centroids = X[np.random.choice(data_size, k)] new_centroids = np.zeros((k, n_features)) cluster = np.zeros(data_size) for epoch in range(max_iter): for i in range(data_size): distances = np.sum((centroids - X[i]) ** 2, axis=1) cluster[i] = np.argsort(distances)[0] for j in range(k): new_centroids[j] = X[cluster==j].mean(axis=0) if np.sum(new_centroids == centroids) == k: break centroids = new_centroids max_norm = 0 min_norm = 0 sorted_centroids = [] for centroid in centroids: norm = centroid[2] if norm > max_norm: sorted_centroids.append(centroid) max_norm = norm if min_norm == 0: min_norm = sorted_centroids[0][2] else: if norm > min_norm and min_norm != 0: sorted_centroids.insert(1, centroid) else: sorted_centroids.insert(0, centroid) min_norm = norm sorted_centroids = np.array(sorted_centroids) return sorted_centroids # translation angles calculation ## calculation def cal_translation(self, marker_points): # マーカー1, 2, 3の位置ベクトル #Position vector of marker 1, 2, 3 a_1, a_2, a_3 = marker_points # カメラからロボットへのベクトル #Vector from camera to robot V_robot = self.cal_robot_position_vector(a_2, a_3) # ロボットのXYZ単位ベクトル #XYZ unit vector of the robot V_X = (a_2 - V_robot) / (np.linalg.norm(a_2 - V_robot)) V_Y = (a_1 - V_robot) / (np.linalg.norm(a_1 - V_robot)) V_Z = self.cal_normal_vector(marker_points) # カメラの水平面に対する仰角 # Elevation angle of the camera relative to the horizontal plane theta_1 = - (np.pi/2 - self.cal_subtended_angle(-V_robot, V_Z)) # カメラの正面方向の回転角 #Rotation angle of the camera in the frontal direction V_Y_camera = np.array([0, 1, 0]) V_Y_camera_rotated = self.cal_rotate_vector_xaxis(V_Y_camera, -theta_1) theta_2 = - self.cal_subtended_angle(V_Z, -V_Y_camera_rotated) # カメラとロボットのそれぞれの正面方向とのなす角 #Angle between the camera and the respective frontal direction of the robot _, V_robot_projected_to_plane = self.cal_vector_projection(V_robot, V_Z) theta_3 = self.cal_subtended_angle(V_Y, V_robot_projected_to_plane) # mycobotの位置を土台の高さ0.027 m, V_Z方向に平行移動 # mycobot position at foundation height 0.027 m, parallel to V_Z direction V_robot = V_robot + 0.027*V_Z return V_robot, theta_1, theta_2, theta_3 ## vector and angle caluculation def cal_robot_position_vector(self, a_2, a_3): return (a_2 + a_3) / 2 def cal_normal_vector(self, marker_points): a_1 = marker_points[0] a_2 = marker_points[1] a_3 = marker_points[2] A_12 = a_2 - a_1 A_13 = a_3 - a_1 cross = np.cross(A_13, A_12) return cross / np.linalg.norm(cross) def cal_subtended_angle(self, vec_1, vec_2): dot = np.dot(vec_1, vec_2) norm_1 = np.linalg.norm(vec_1) norm_2 = np.linalg.norm(vec_2) return np.arccos( dot / (norm_1 * norm_2) ) def cal_vector_projection(self, org_vec, normal_vec): # org_vec: 射影したいベクトル # org_vec: the vector you want to project # normal_vec: 射影したい平面の法線ベクトル # normal_vec: Normal vector of the plane to be projected projected_to_vertical = np.dot(org_vec, normal_vec) * normal_vec projected_to_horizontal = org_vec + projected_to_vertical return projected_to_vertical, projected_to_horizontal def cal_rotate_vector_xaxis(self, vec, angle): rotate_mat = np.array([[1, 0, 0], [0, np.cos(angle), np.sin(angle)], [0, -np.sin(angle), np.cos(angle)]]) return vec.dot(rotate_mat) def cal_rotate_vector_yaxis(self, vec, angle): rotate_mat = np.array([[np.cos(angle), 0, -np.sin(angle)], [0, 1, 0], [np.sin(angle), 0, np.cos(angle)]]) return vec.dot(rotate_mat) def cal_rotate_vector_zaxis(self, vec, angle): rotate_mat = np.array([[np.cos(angle), np.sin(angle), 0], [-np.sin(angle), np.cos(angle), 0], [0, 0, 1]]) return vec.dot(rotate_mat) if __name__ == "__main__": mycobot_position_calibrator = MyCobotPositionCalibrationNode() mycobot_position_calibrator.start_node() pass
*/コピー
今回はカメラとロボットの位置関係に合わせて、近いマーカー_1 、マーカー_2、マーカー_3から設定しましたが、カメラの位置に合わせて調整する必要があることに注意してください。
ビルドが完了すると、カメラ、カラー検索、キャリブレーション ノードが 3 つの端末でそれぞれ起動されます。
$ roslaunch realsense2_camera rs_camera.launch filter：=pointcloud$ rosrun mycobot_test blue_point_detection.py$ rosrun mycobot_test mycobot_position_calibration.pyコピー
これにより、変換t_x、y、z (m ) とオイラー角theta_1、2、3 (ラジアン) がmycobot_position_calibration.pyの端末に記録されます。これらの値は点群データの収集とクラスタリングにより変動するため、これらの値は複数回平均されます。(この設定では平行移動とオイラー角の両方が約 1% 変動します)
値を取得したら、tf_broadcaster.cpp に一時的な値を上書きします。
// tf_broadcaster.cpp~~ # それぞれ t_x,y,z と theta_1,_2,_3 に得られた値を入れる # Put the obtained values in t_x,y,z and theta_1,_2,_3 respectively transformStamped.transform.translation.x = t_z; transformStamped.transform.translation.y = -t_x; transformStamped.transform.translation.z = -t_y; tf2::Quaternion q; q.setEuler(theta_1, theta_2, theta_3); ~~コピー
Camera_color_frameからBase_linkまでのTF を探しています。ポイント マーシャリング座標系 Camera_ Depth_optical_frame が見つかりました。計算後にxyz方向が異なるため (違いに気付かず、混乱しています)、x=t_z,y= -t_x,z= -t_y。
書き換えてcatkin_makeしたらノードを起動します。
$ roslaunch realsense2_camera rs_camera.launch filter：=pointcloud$ roslaunch mycobot_moveit mycobot_moveit_control.launch$ rosrun tf_broadcaster tf_broadcasterコピー
そして、カメラとロボットの位置関係を反映させると、図14のように点群上のロボットとRvizで示したロボットモデルを重ね合わせることができます。
図 14: キャリブレーションから得られた値を TF に設定し、それをブロードキャストすることは完璧ではありませんでしたが、単に物体に到達して拾う場合には偏差を許容範囲内に保つことができました。
これは、点群と重ね合わされたロボット モデルを移動すると確認できます。
myCobot は関節に少し遊びがあるため、正確な動作が困難です。
RealSense から取得した点群座標系を myCobot の座標系に変換するので、myCobot の先端をオブジェクトとして位置決めしようとします。(MoveItでアクセスしようとしているのですがC++で止まってしまうのでpymycobotで簡単に操作できるスクリプトを作りました)
座標はカメラの光学座標系にあるため、前に取得した移動と回転を使用して myCobot 座標系に変換します。以下を script ディレクトリに追加します。
# mycobot_reaching.py#!/usr/bin/env python3 import time, os, sys import numpy as np import quaternion from pymycobot.mycobot import MyCobot import rospy from geometry_msgs.msg import Point from tf.transformations import quaternion_from_euler class MyCobotReachingNode(object): def __init__(self): super(MyCobotReachingNode, self).__init__() rospy.init_node("mycobot_reaching_node") rospy.loginfo("mycobot reaching start") # メンバ変数 # mycobot インスタンス # member variable # mycobot instance port = "/dev/ttyUSB0" self.mycobot = MyCobot(port, baudrate="115200", timeout=0.1, debug=False) # 平行移動と回転 # 光学座標系→mycoboto座標系なので4.2節とXYZ軸が違うことに注意 # Translation and rotation # Note that the XYZ axes are different from those in section 4.2 because the optical coordinate system is a mycoboto coordinate system self.translation_from_camera_to_mycobot = np.array([t_x, t_y, t_z]) q = quaternion_from_euler(-theta_2, -theta_3, theta_1) self.rotation_from_camera_to_mycobot = quaternion.as_quat_array(q) # subscriber self.sub_point() # Subscriber def sub_point(self): def callback(data): rospy.loginfo("subscribed target point") self.mycobot_move(data) sub = rospy.Subscriber("object_position", Point, callback = callback) rospy.spin() # move mycobot def mycobot_move(self, point_data): # mycobot座標系への変換 # Convert to mycobot coordinate system target_point = np.array([point_data.x, point_data.y, point_data.z]) target_point -= self.translation_from_camera_to_mycobot target_point = quaternion.rotate_vectors(self.rotation_from_camera_to_mycobot, target_point) # mm単位のため*1000、手前中心付近にリーチングするためx-20mm, z+40mm # *1000 for mm increments, x-20mm, z+40mm for reaching around the center of the front coord_list = [target_point[0]*1000-20, -target_point[2]*1000, target_point[1]*1000+40, 0, 90, 0] self.mycobot.send_coords(coord_list, 70, 0) time.sleep(1.5) if __name__ == "__main__": reaching = MyCobotReachingNode() passコピー
今回は今まで使ったライブラリや作ったスクリプトを総動員します。
rs_camera.launch、mycobot_moveit_control.launch、tf_broadcaster、red_point_detection.py、object_position_search.py 、mycobot_reaching.pyを 6 つの端末で実行できますが、端末を開くたびに設定する必要があります... デバッグは難しい場合があるため、独自の起動ファイル。
ワークスペースに移動し、起動用のディレクトリとファイルを作成し、起動パスを CMakeLists.txt に追加します。
$ cd <your_ros_ws_for_MyCobot>/src/mycobot_test$ mkdir launch$ cd launch$ touch mycobot_reaching.launch<!-- mycobot_reaching.launch --><launch> <include file="$(find mycobot_moveit)/launch/mycobot_moveit_control.launch" /> <include file="$(find realsense2_camera)/launch/rs_camera.launch"> <arg name="filters" value="pointcloud"/> </include> <node name="tf_broadcaster" pkg="tf_broadcaster" type="tf_broadcaster" /> <node name="color_points_detectior" pkg="mycobot_test" type="red_points_detection.py" output="screen" /> <node name="object_position_searcher" pkg="mycobot_test" type="object_position_search.py" output="screen" /> <node name="mycobot_reaching_node" pkg="mycobot_test" type="mycobot_reaching.py" /> </launch># CMakeLists.txt~~# Add launch fileinstall(FILES DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ) ~~コピー
スタートアップファイルの構造は非常にシンプルで、ノードのスタートアップタブにスタートアップファイルを配置し、同時に実行したいスタートアップファイルを配置するだけです。ノードを実行するには、nodeタグに任意のノード名を記述し、rosrunのようにパッケージ名と実行ファイル名を記述します。出力をコマンドラインに表示したい場合は、output="screen" を追加します。
ブートストラップ ファイルで起動を行う場合は、include タグにファイル パスを含めます。catkin_make を試して実行してください。
$ roslaunch mycobot_test mycobot_reaching.launchコピー
ムービー 1 と同じように、RealSense によってキャプチャされた赤いオブジェクトを追跡するように myCobot を動かすことができました。
プロジェクトに従うまでにかなりの遅れがあり、あらゆる面で十分に学習していないことを常に感じます。
7. まとめ
この記事では、ROSを使って6軸ロボットアームmyCobotとデプスカメラRealSense D455を連携させる方法をまとめました。ROSを含むロボット開発の経験はありません。私が今まで試したことを一からまとめたブログはなかなかないと思うので、ロボットアームやデプスカメラを買ったけど使い方が分からない、どうやって使えばいいのか迷っている方の参考になれば幸いです。それを使用します。また、ボット開発の経験がある人、またはデータの処理に優れている人が、このようなことを行うための最良の方法を指摘していただければ幸いです。
今回はシミュレータで学習した強化学習モデルをmyCobotに適用してピッキング実験を行ったので、その2としてまたブログ記事を書くかもしれません。
参考
1. 松林达志， 2022.12.21， 实感D455による空間認識でmyCobotを操作. データ ＆ アナリティクス | アクセンチュア
2. 阿尔伯特公司 データ ＆ アナリティクス | アクセンチュア