- Introduction
- myCobotの基本
- ファームウェアの書き込み
- 関節のキャリブレーション
- Transponderの起動
| $ sudo chmod 666 /dev/ttyUSB* |
myCobotの各関節には図5のように原点を表す溝が付いているので手動で溝同士が重なるようにします。
まず、pymycobotをインストールします。こちらはpipで簡単にインストールできます。
| $ pip install pymycobot –upgrade |
# mycobot_control_test.py
#!/usr/bin/env python
import time
from pymycobot.mycobot import MyCobot
from pymycobot.genre import Angle, Coord
if __name__ == "__main__":
port = "/dev/ttyUSB0"
mycobot = MyCobot(port, baudrate="115200", timeout=0.1, debug=False) # baudrate以降はデフォルト値
# 関節角の取得・操作
print("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)
# 座標と先端の向きの取得・操作
print("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_list, 70, 0)
time.sleep(3.0)
coord_list = [50, 50, 300, 90, 90, 0]
mycobot.send_coords(coord_list, 70, 0)
time.sleep(1.5)
# グリッパーが付いている場合
# print("open gripper")
# mycobot.set_gripper_state(0, 80)
# 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()
| $ python3 mycobot_control_test.py |
mycobot = MyCobot(port, baudrate="115200", timeout=0.1, debug=False)で4つの引数を設定します。baudrate以降はデフォルト値を入れています。portはUSBシリアルの通信ポートです。ターミナルで ls /dev/を実行するとPCに接続されているデバイスの一覧を見ることができます。Linuxだと他にシリアル通信をしているUSBポートがなければ /dev/ttyUSB0になっています。MacやWindowsの場合は違うと思うので、適宜確認してください。 ・getterについて 関節角を度数法・弧度法で取得する関数は get_angles(), get_radians()で、返り値は6つの関節角の値が入ったリストになります。 またベースの底面の中心を原点とした座標系※でアームの先の座標を取得することができる get_coords()関数も用意されています。返り値は6次元リストで、先端のx, y, z座標（mm）と向きrx, ry, rz（angle）になっています。MoveItを使わなくてもすでに逆運動学が実装されていて便利です。 ※座標系: Basicのパネルを背にそれぞれの正方向は、x: 前、y: 左、z: 上 です。後述するMoveItでは若干ベクトルが異なるため注意が必要です。 ・setterについて 関節角を度数法・弧度法で送信する関数は send_angles(), send_radians()で、引数の設定は二種類あります。 まず、6つの関節すべてを指定して送信する場合は、第一引数にgetterと同様にリスト型に6つのfloat値を入れ、第二引数に関節動作の速さ※（int: 0 ~ 100）を入れます。 例： mycobot.send_angles([0, 0, 0, 0, 0, 0], 80) 次に、指定の関節の角度だけを指定して送信する場合は、第一引数に関節角を指定するコード、第二引数に角度の値、第三引数に速さを入れます。 例： mycobot.send_angle(Angle.J4.value, -90, 80) またgetterと同様に座標指定で動作させることもできます。その場合、第一引数にcoordinationを示す6要素リスト[x, y, z, rx, ry, rz]、第二引数に速さ、第三引数にモードを入れます。モードは0:angular, 1:linearの2種類があり、これらの意味は完全には理解できておらず、モードを変えていくつか動作させてみましたが違いは見られませんでした。 例： mycobot.send_coords([[80, 50, 300, 0, 90, 0], 70, 0) ※ 速さの単位はソースコードにも詳しく書いていなかったので、必要な場合は変化させながら測定してみるしかないと思います。 ・その他のAPI 一度アームを動作させると、モーターがトルクをかけ続けて現在の状態を維持しようとするため、そのまま放置してしまうとモーターに負荷がかかります。そのため、動作が終了したら関数 release_all_servos()でモーターのトルクを開放しておきましょう。ただし、実行するとすぐに力が抜けてしまうため、アームが高いところから落ちて他のものに当たったりしないように注意してください。 アームが動作中に他の動作を指示するとエラーとなり次の動作に進むようになっています。そこでサンプルスクリプトでは、それぞれの動作が完了するのを待つために間にpython組み込み関数の time.sleep()を使っていますが、whileなどでループできるように関数 is_moving()でモーターが動作中かどうかを取得できます（と思っていたのですがこの関数はバグがあり常に動いている状態を返してしまうようです） 。
この他にも電源のon, offをしたり、LEDの色を変えたり、モーターの状態を確認したりするAPIがありますが今回はアームを動かしてみることを趣旨としているので省略しました。調べたい方は、READMEもしくはGitHubのソースコードに丁寧にそれぞれの関数についてコメントが書かれています。 次にROSからMoveItを使ってmyCobotを操作します。 MyCobotのMoveItはElephant Robotics公式の実装（mycobot_ros）がありインストールできますが、試した時点ではロボットモデルの可視化にバグが残っていたり、ROSと実機との接続が上手くいかなかったりと、なかなかインストールすれば誰でも簡単に使える状態ではありませんでした。
そこで、有志でMoveItを実装（mycobot_moveit）した方がいらっしゃったのでそちらを利用させていただくことにしました。 インストール方法は上記GitHubのREADMEのPrepararion通りです。
| $ 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 # setup workspace $ source devel/setup.bash |
| $ roslaunch mycobot_moveit mycobot_moveit_control.launch |
mycobot_urdf_gazebo.urdf ~~arm1 ~ arm6_jointで同じように記述されています。回転軸の方向を設定しているのが <axis xyz="0 0 1"></axis>で逆向きに回転している関節を1 → -1にすると反転させられます。 私の環境では第3関節以外は全て反転していたようです。これがロボットのサーボの個体差なのか別の原因なのかはわかっていないので、わかる方がいらっしゃれば教えていただきたいです。 以上がmyCobotの設定から操作の基本までです。~~
- RealSense D455の基本
| # サーバーの公開鍵登録 $ sudo apt-key adv –keyserver keyserver.ubuntu.com –recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv –keyserver hkp://keyserver.ubuntu.com:80 –recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE # リポジトリの追加 $ sudo add-apt-repository “deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main” -u # ライブラリのインストール $ sudo apt install librealsense2-dkms librealsense2-utils # 開発者パッケージもインストール $ sudo apt install librealsense2-dev librealsense2-dbg |
| $ sudo apt install ros-$ROS_DISTRO-realsense2-camera |
| $ roslaunch realsense2_camera rs_camera.launch filters:=pointcloud |
| $ rviz |
まず、左にある Display パネルの Global Option の Fixed Frame が map になっているのでクリックして camera_link に変えます。
次に Display パネル下部の add ボタンを押すと新しいウィンドウが立ち上がり、Rvizで表示できるROSメッセージの型が列挙されます。その中から rvis グループの中盤にある PointCloud2 を選択してOKを押すと Display パネルに PointCloud2 が追加されます。更にそのグループの中にあるTopic欄右側の空白をクリックすると出てくる /camera/depth/color/points を選択するとRviz上に点群が表示されます。デフォルトでは点群サイズが0.01 mとかなり大きめに設定してあるため点群同士が重なって表示されますが、0.001 mくらいにしてあげることでかなり細かく点群が取得できているのを確認できます。
また、add から今度は TF を選択して追加するとカメラの位置と姿勢（軸方向）を表示できます。デフォルトでRGBカメラの原点、ステレオカメラの原点、がそれぞれ世界座標系と光学座標系で表示されます。（camera_linkはステレオカメラの原点（世界座標系）と同じものになっているようです）
ワーキングスペースに設定保存用のディレクトリconfigを作成して、RvizのfileでSave Config Asを選択、作ったconfigディレクトリに名前を付けて保存します。以降は、
| $ rviz -d .rviz |
前節で実行したrs_camera.launch filters:=pointcloudを実行した状態で、別のターミナルで配信されている/camera/depth/color/pointsトピックの生データを見てみます。
| $ rostopic echo /camera/depth/color/points |
なのでそもそもこのメッセージがどういう型なのかを調べます。
| $ rostopic type /camera/depth/color/points sensor_msgs/PointCloud2 |
Header header # ヘッダー uint32 height # データ行数 uint32 width # データ列数 PointField[] fields # 1点のデータ構造 bool is_bigendian # ビッグエンディアンかどうか uint32 point_step # 1点のバイト数（8bit数値の数） uint32 row_step # 1行のデータ数（= width * point_step） uint8[] data # 生データ(8bitのrow_step * height個のデータ配列) bool is_dense # Trueなら無効なデータはない点群なので2次元画像と違ってデータの並び順に意味はあるのか？という疑問の通りheight, widthには何の意味もなさそうです（height×widthが点群の数になっていればOK）。他にも別の値から計算できる上に使わなさそうな値まで変数として保持する意味は分かりませんが、このような構造になっています。それぞれの実際の値を見たい場合は、
| $ rostopic echo /camera/depth/color/points/<変数名> |
| $ rostopic echo /camera/depth/color/points/point_step – 20 — $ rostopic echo /camera/depth/color/points/fields – name: “x” offset: 0 datatype: 7 count: 1 – name: “y” offset: 4 datatype: 7 count: 1 – name: “z” offset: 8 datatype: 7 count: 1 – name: “rgb” offset: 16 datatype: 7 count: 1 — |
offset: 何番目のByteが対応しているか
datatype: 表しているデータの型コード
count: 何個のデータが入っているか であるとわかります。例えば、x座標に対応するデータは0~3番目で型コードは7（float32に対応）、1つのデータを表します。y, zも同様に処理すればいいことになりますが、RGBも同じ様に表現されています。RGBに対応する16~19番目の値を確認すると、GBRAの順番で16進数の値が入っていました。float32に変換されてしまうと処理するのが厳しそうなのでxyz座標とRGBは分けて処理する必要がありそうです。
データの表現方法がわかったので実際に処理してみます。（次に出てくるスクリプトはもっと賢く速い処理の仕方があると思いますのでアドバイスがあればお願いします）
mycobot_testパッケージの中に、scriptsディレクトリを追加してそこにpythonのスクリプトを追加していきます。
| $ cd /src/mycobot_test $ mkdir scripts $ cd scripts $ touch red_point_detection.py $ touch object_position_search.py |
# CMakeLists.txt
find_package(catkin REQUIRED COMPONENTS
# rospyとsensor_msgsモジュールがimportできるように追加
rospy sensor_msgs
)
catkin_package(
# ビルドパッケージの追加
sensor_msgs
)
# 実行ファイルのディレクトリにscriptsを追加
catkin_install_python(PROGRAMS
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts
)
<build>rospy</build> <build>sensor_msgs</build>以下のred_point_detection.py は点群の中から赤のみを取り出して新しいメッセージを作るスクリプトです。
# 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
subscriberでcamera/depth/color/pointsのデータを読み込み、publisherでそのデータの中から赤い点だけを取り出して配信するという構成です。赤い点は元の点群からRGB値で
R < 180, G > 150, B > 150の点を除去する処理をしています。本来は照明条件に依存しやすいRGBを使うよりHSVを使うのがベターですが、今回はPointCloud2のデータを処理しながらHSVに変換するのが大変だったので保留しています（C++ならPCLを使って簡単に変換できたようですが、他の処理を書く大変さに甘えてpythonを使ったのが徒になりました）。書き終わったらワークスペースに移動して catkin_makeして実行してみます。
| $ roslaunch realsense2_camera rs_camera.launch filters:=pointcloud |
| $ rosrun mycobot_test red_point_detection.py |
| $ rviz -d .rviz |
# 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 |
- ROSを使ってmyCobotとRealSense D455の連携
| $ roslaunch realsense2_camera rs_camera.launch filters:=pointcloud |
| $ roslaunch mycobot_moveit mycobot_moveit_control.launch |
| $ cd <your_ros_ws_for_MyCobot>/src $ catkin_create_pkg tf_broadcaster roscpp $ cd tf_broadcaster $ touch src/tf_broadcaster.cpp |
// 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"; // 子link
// 平行移動
transformStamped.transform.translation.x = -0.3;
transformStamped.transform.translation.y = -0.3;
transformStamped.transform.translation.z = -0.3;
// 回転
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.txt
cmake_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="root@todo.todo">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もあるので気になる方は調べてみてください。
ここではまだ位置関係が分かっていないので数値は仮の値になっていますが、これで表示まではできるのでやってみます。また親Linkをcamera_linkではなくcamera_color_frameにしているのは、後者の原点が点群座標系と一致するためです。 catkin_makeしたら、先程と同じようにrs_camera.launchとmycobot_moveit_control.launchを実行して、もう一つのターミナルでtf_broadcasterを実行します。
| $ rosrun tf_broadcaster tf_broadcaster |
カメラを特定位置に固定することで位置関係を明確にする方法（特にロボットアームの特定位置に固定する場合はかなり簡単）もあると思いますが、今回はマーカーを3点に貼ってロボット座標の単位ベクトルを求め、カメラ座標系との関係を計算する方法で位置関係をキャリブレーションしました。
今までの写真でもすでにマーカーを貼ってある状態で撮影していたので気付いている人もいると思いますが、図13のようにマーカーを配置しています。
まず、青マーカーの点群を取得するためにred_point_detection.pyを書き換えてblue_point_detection.pyを作ります。抽出するRGBの範囲を書き換えただけなのでここではスクリプトは省略します。 以降、そこから座標変換を求める方法はいろいろあると思いますが、今回は以下の手順で求めました。
- マーカー座標の決定
- マーカー点群のサブスクライブ
- 点群を3つにクラスタリング（by k-means method）
- 原点と単位ベクトルの決定
- マーカー2, 3の中点を原点V_robot
- X, Yの単位ベクトルV_X, V_YをそれぞれV_robot→マーカー1, V_robot→マーカー2
- Zの単位ベクトルは外積でマーカーのある平面の法線ベクトル
- カメラ姿勢からロボット姿勢への回転をEuler角で作成
- theta_1 = カメラの水平面に対する仰角
- theta_2 = カメラの正面方向の回転角
- theta_3 = カメラとロボットのそれぞれの正面方向とのなす角
- 原点をmyCobotの固定台の分(2.7cm)、V_Z方向に平行移動
以下のスクリプトを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の位置ベクトル
a_1, a_2, a_3 = marker_points
# カメラからロボットへのベクトル
V_robot = self.cal_robot_position_vector(a_2, a_3)
# ロボットのXYZ単位ベクトル
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)
# カメラの水平面に対する仰角
theta_1 = - (np.pi/2 - self.cal_subtended_angle(-V_robot, V_Z))
# カメラの正面方向の回転角
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)
# カメラとロボットのそれぞれの正面方向とのなす角
_, 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方向に平行移動
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: 射影したいベクトル
# normal_vec: 射影したい平面の法線ベクトル
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
今回はカメラとロボットの位置関係から、近いマーカーから順にmarker_1,2,3としていますがカメラを置く場所によって調整する必要があるので注意が必要です。 ビルドが完了したら、3つのターミナルでカメラ、色探索、キャリブレーションのノードをそれぞれ起動します。
| $ roslaunch realsense2_camera rs_camera.launch filters:=pointcloud |
| $ rosrun mycobot_test blue_point_detection.py |
| $ rosrun mycobot_test mycobot_position_calibration.py |
値が取れたら、tf_broadcaster.cppの仮の値だった箇所を書き換えます。
// tf_broadcaster.cpp
~~
# それぞれ t_x,y,z と theta_1,_2,_3 に得られた値を入れる
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 filters:=pointcloud |
| $ roslaunch mycobot_moveit mycobot_moveit_control.launch |
| $ rosrun tf_broadcaster tf_broadcaster |
点群とロボットのモデルを重ねて動かしてみると、myCobotの関節には少し遊びがあり精細な動作をさせるのは難しいことがわかります。
RealSenseから取得した点群座標系をｍyCobotの座標系に変換することができたので、物体の位置に向かってｍyCobotの先端をリーチングさせてみます。（MoveItを使ってリーチングさせようと思ったのですがC++で沼ってしまったので簡単にpymycobotで動作させるスクリプトにしました）
object_position_serch.pyで求めた赤い物体の座標を表すobject_position topicをサブスクライブして、その座標にmyCobotの先端をリーチングさせます。座標はカメラの光学座標系になっているので、先程求めた平行移動と回転でmyCobot座標系に変換します。以下をscriptsディレクトリに追加しました。
# 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 インスタンス
port = "/dev/ttyUSB0"
self.mycobot = MyCobot(port, baudrate="115200", timeout=0.1, debug=False)
# 平行移動と回転
# 光学座標系→mycoboto座標系なので4.2節とXYZ軸が違うことに注意
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座標系への変換
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
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つのターミナルでそれぞれ実行してもいいのですが、毎回ターミナルを開いてsetupして……とやっているとデバッグも大変になってしまうので、いい加減自前のlaunchファイルを作成します。
自分のワークスペースに移動してlaunch用のディレクトリとファイルを作成、CMakeLists.txtにlaunchのパスを追加します。
| $ 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
~~
# launchディレクトリの追加
install(FILES
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
~~
launchファイルの構成は簡単でlaunchタグ内で同時に動かしたいnodeとlaunchのファイルを並べてやるだけです。nodeを実行する場合はnodeタグに任意のnode名と、rosrunするときと同じようにパッケージ名と実行ファイル名を書きます。出力をコマンドライン上に表示する場合はoutput=”screen”を追加します。launchファイルの中でlaunchを実行する場合はincludeタグにファイルパスを入れます。catkin_makeをして実行してみます。
| $ roslaunch mycobot_test mycobot_reaching.launch |
- まとめ
今回のmyCobotにシミュレーターで学習した強化学習モデルを適用してピッキングの実験も行ったので後編としてまたブログを書くかもしれません。
ALBERTでは、様々な専門的バックグラウンドを持つリサーチャー・アナリストを募集しています。詳しくは採用ページをご覧ください。
The post RealSense D455による空間認識でmyCobotを操作 first appeared on ALBERT Official Blog.