1. 簡介

先進技術部門正在研究多模態強化學習，包括相機圖像和觸覺傳感器。除其他外，要實現所謂的Sim2Real，其中模擬器中的強化學習結果也在實際機器上運行，必須協作操作真實機器的機械臂和相機。因此，在這種情況下，我們使用ROS測試了鏈接的6軸機械臂myCobot（由大象機器人制造）和深度攝像頭RealSense D455（由英特爾制造），流程和結果將在下面詳細描述。

操作環境：

PC：Ubuntu 20.04， ROS Noetic， Python 3.8.10

機械臂：myCobot280 M5Stack

深度攝像頭：實感D455

這篇博客描述了如何創建和運行一個簡單的程序，但我假設 ROS 和 Python 環境已經設置好了。

2. 我的協作機器人基礎知識

首先，準備myCobot，但我有點困惑，因為由于固件更新等原因，某些部件在使用中發生了變化。這項工作是在 2021 年 3 月左右完成的，當時 myStudio 版本是 1.3.<>。我認為不會有任何重大變化，但如果您看到不同之處，請參閱官方說明。

運行myCobot需要以下三個準備工作。

● 更新固件（基本，原子）

● 機械臂關節校準

● 機械臂與PC機之間的串行通信

更新固件

myCobot 280 M5在底座上有一個M5Stack Basic，在機械臂的末端有一個ATOM（Basic，ATOM），用于將固件寫入這兩個模塊。

首先，使用 USB 連接到 PC，并使用 chmod 允許讀取和寫入設備。此外，對于Windows或Mac上的USB串行通信，需要安裝特殊的驅動程序。

$ sudo chmod 666 /dev/ttyUSB*

接下來，下載更新的固件應用程序-myStudio（在撰寫本文時，最新版本是3.1.4，但僅適用于Windows，3.1.3可用于Linux）。

提取源代碼并執行 AppImage 后，它將分別啟動 Basic 和 ATOM。如圖 1 和圖 2 所示。

在myStudio 3.1.3中，將出現如圖3和圖4所示的屏幕，然后下載最新版本的Minirobot for Basic和AtomMain for ATOM，選擇Flash，然后寫入固件。使用Basic完成寫入后，迷你機器人的輸出將顯示在面板上。（請注意，如果您不使用Basic和ATOM編寫最新版本，則機器人手臂可能無法正常工作）。

更新固件后，下一步是校準接頭角度。

接頭角度校準

首先，在“基本”面板中選擇“校準”，然后按“確定”。

myCobot 的每個接頭都有一個代表原點的凹口，如圖 5 所示，因此凹口是手動相互對齊的。

在此狀態下，在“基本”面板中選擇“校準伺服”，然后按“下一步”完成校準。運行測試伺服將允許電機圍繞凹口稍微旋轉以確認正確校準。

機械臂和PC之間的串行通信

最后，當您啟動應答器時，您可以通過串行通信從PC操作myCobot。這很容易做到，只需在“基本”面板中選擇應答器，然后按“確定”。然后顯示屏將顯示如圖6所示，按基本按鈕。

在“基本”面板的其他菜單中，“基本”中的“主控制”控制 ATOM，“信息”檢查每個關節是否正確連接。當myCobot在PC上無法正常工作時，您可以檢查myCobot本身是否存在問題。

3. 蟒蛇接口

準備完成后，我們可以在PC上操作myCobot。這次，我嘗試了使用 pymycobot 從 python 腳本進行操作的方法，以及使用 mycobot_moveit 庫從 ROS 操作 MoveIt 的方法 。

首先，安裝pymycobot。

$ pip install pymycobot --upgrade

您也可以從 Github 下載安裝源代碼。

從源代碼下載時，示例腳本包含在測試目錄中。但是，它不會按原樣工作，因此請小心。我做了下面的例子，而不是重寫。

# 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: -90n")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 類的實例，并使用 getter 和 setter 來檢查和更改狀態。

創建實例

mycobot = MyCobot(port, baudrate="115200", timeout=0.1, debug=False)

設置四個參數。在波特率之后輸入默認值。端口是 USB 串行通信端口。您可以通過在終端中運行ls / dev /來查看連接到PC的設備列表。在Linux中，如果沒有其他用于串行通信的USB端口，它將是/ dev / ttyUSB0。我認為Mac和Windows是不同的，所以相應地檢查。

關于吸氣劑

get_angles （ ） 和 get_radians （） 是獲取關節角度（以度和弧度為單位）的函數。返回值是六個關節角度值的列表。

還有一個get_coords（ ）函數，它獲取以底底中心為原點的坐標系*中手臂尖端的坐標。返回值是一個 6 維列表，其中包含尖端的 x、y、z 坐標 （mm） 和方向 rx、ry、rz（角度）。在沒有 MoveIt 的情況下實現反向運動學真是太好了。

*坐標系：以“基本”面板為背面，每個正方向分別為 x：向前、y：向左和 z：向上。請注意，MoveIt 中的向量略有不同，稍后將對此進行介紹。

關于二傳手

以度和弧度為單位發送關節角的函數是send_angles （ ） 和 send_radians （ ），并且具有兩種類型的參數設置。

首先，在指定并發送所有 6 個關節時，第一個參數與 getter 相同，在列表類型中放入 6 個浮點值，第二個參數中輸入關節運動的速度 *（int： 0 ~ 100）。

mycobot .send_angles ( [ 0 , 0 , 0 , 0 , 0 , 0 ] , 80 )

接下來，您取關節的角度并發送它。請將關節角度的代碼放在第一個參數中，角度值放在第二個參數中，速度放在第三個參數中。

mycobot .send_angle ( Angle.J4.value , -90,80 )

也可以通過像getter這樣的坐標來操作。在這種情況下，放置了 6 個元素的列表 [x， y， z， rx， ry， rz]，第一個參數是協調的，第二個參數是速度，第三個參數是模式。模式有兩種類型，0：角度和1：線性

mycobot .send_coords ( [ [ 80 , 50 , 300 , 0 , 90 , 0 ] , 70 , 0 )

由于源代碼中沒有詳細說明速度單位，因此我認為您在必要時對其進行測量。

其他接口

一旦機械臂移動，電機將繼續施加扭矩以試圖保持當前狀態，因此如果保持原樣，電機將過載。因此，讓我們在操作完成后使用函數release_all_servos（y）釋放電機扭矩。

如果機器人手臂正在運行，并且您指示它執行另一個操作，則會發生錯誤，它將繼續執行下一個操作。在示例腳本中，python內置函數time.sleep（）用于等待每個動作完成，但您可以使用函數is_moving（）來獲取電機是否正在運行，以便您可以循環while等。（我認為這個函數是錯誤，并返回一個不斷移動的狀態。

還有一些其他 API 可用于打開和關閉電源、更改 LED 的顏色以及檢查電機狀態，但這次我省略了它們，因為目標是移動手臂。

4. 只讀標準

接下來，在ROS中使用MoveIt來操縱myCobot。

大象機器人實現mycobot的動作它可以安裝并可以安裝

有一個MoveIt的志愿者實現，我決定使用它。安裝是根據上面的 GitHub 自述文件編寫的。

$ 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

MoveIt 和 Rviz GUI 將啟動，如圖 7 所示。通過拖放綠色球，計算出機器人手臂末端位置的姿勢，然后按下左下角的計劃和執行按鈕，Rviz 與實際機器人一起移動。

* 當模型和實際機器不能一起工作時

我不知道這是否是所有環境中都會發生的錯誤，并且模型和真實機器并不總是協同工作。這是電機旋轉方向反轉時發生的錯誤，所以有點棘手，但請將模型與真機進行比較，尋找相反方向的關節旋轉。

確認有多少關節沿相反方向旋轉后，重寫描述機器人模型的 URDF 文件。

/ src / mycobot_moveit / urdf / mycobot_urdf_gazebo.urdf

定義的聯合信息的說明如下。

mycobot_urdf_gazebo.urdf ~~                                                  ~~

Arm1 ~ arm6_joint 相同的說明。旋轉軸的方向設置為<軸xyz = “0 0 1”> ，并且設置1→-1反轉關節在相反方向上的旋轉。

在我的環境中，似乎除了第三個關節之外的所有關節都是反轉的。我不知道這是因為機器人舵機的個體差異還是其他原因，如果您知道，請告訴我。

這就是它的全部內容，從myCobot設置到操作基礎。

5. 現實D455基礎知識

這次我用D455進行了測試，但基本上D400系列可以以相同的方式使用。只有D435i和D455內置了IMU傳感器，但本文沒有用到（因為誤差累積的缺點比在固定狀態下使用IMU的優點更明顯）。除了帶有紅外立體攝像頭的D400系列外，還有帶有LiDAR的L515，但用途相同。另外，我認為新的性能最好，所以我會買D455。我認為最好在購買前咨詢一下您想使用的環境，因為有一些零件（以前的型號基本上是向后兼容的，所以在價格和性能之間有一個權衡）。

查看器的軟件安裝和基本操作

安裝庫 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

安裝完成后，在終端中運行實感查看器進行查看。如果它不起作用，請嘗試拔下并插入實感 USB 連接并重新啟動 PC。如果啟動成功，將顯示查看器，如圖 8 所示。

您可以使用右上角的 2D | 3D 按鈕在 2D 和 3D 之間切換查看器。此外，您可以通過打開左側的立體模塊和RGB攝像頭來查看深度信息和RGB信息。在 2D 中，您可以在 2D 中查看 RGB 和深度信息。在3D中，由深度估計紅外立體相機估計的點云用深度彩色圖和RGB相機信息著色，可以從各個角度查看。此外，內置 IMU 傳感器的 D435i 和 D455 還可以通過運動模塊獲取姿態信息。接下來，我們來看看使用 ROS 包realsense_ros的 Rviz 點云。這可以使用 apt 安裝。

$ sudo apt install ros-$ROS_DISTRO-realsense2-camera

實感攝像頭在第一個終端啟動，除了每個攝像頭的信息外，還提供彩色點云，在第二個終端上啟動 Rviz 進行可視化。

$ roslaunch realsense2_camera rs_camera.launch filter：=pointcloud$rviz

為了如圖 9 所示查看點云，我們將使用 GUI 來調整設置。

首先，由于左側顯示面板中的全局選項的 固定框是地圖，請單擊它并將其更改為camera_link。

然后按顯示面板底部的添加按鈕，將彈出一個新窗口，列出可以在 Rviz 中顯示的 ROS 消息類型。選擇 rviz 組中間的 PointCloud2，然后按 OK 將 PointCloud2 添加到顯示面板。

此外，如果您單擊組中“主題”列右側的空白并選擇出現的 /camera/depth/color/points，則點云將顯示在 Rviz 上。默認情況下，點云大小設置為 0.01 m，這是一個很大的值，因此點云相互重疊顯示，但如果將其設置為 0.001 m 左右，您可以看到點云的獲取非常精細。

此外，如果從“添加”中選擇“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/

通過執行此操作，您只能在數據中輸出一個變量。例如，顯示回point_step和字段給出。

$ 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 個字節是如何組成的。

每個變量的含義是

名稱：要表示的數據的名稱

偏移量：對應的字節

數據類型：所表示數據的類型代碼

計數：包含的數據項數

例如，x 坐標對應于從 0 到 3 的數據，類型代碼為 7（對應于 float32），表示數據。 y 和 z 應該以相同的方式處理，但 RGB 也以相同的方式表示。當我查看與 RGB 對應的第 16 個到第 19 個值時，有 GBRA 順序的十六進制值。到 float32 的轉換似乎很難處理，因此似乎需要單獨處理 xyz 坐標和 RGB。

現在我們知道了如何表示數據，讓我們實際處理它。

（我認為有一個更智能，更快捷的方法來處理下一個腳本，所以如果你有任何建議，請）

將腳本目錄添加到mycobot_test包中，并在其中添加 python 腳本。

$ cd /src/mycobot_test$ mkdir scripts$ cd scripts$ touch red_point_detection.py$ touch object_position_search.py

將依賴項添加到 CMakeLists.txt 并打包.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    rospy   sensor_msgs     rospy   sensor_msgs

以下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 的點。使用HSV比RGB更好，RGB容易受到照明條件的影響，但是這次在處理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 連接到實感 D455

現在實感數據處理已經完成，我想將其與myCobot結合使用。因此，最重要的是從實感相機坐標系到myCobot坐標系的轉換。Rviz 上有一個 TF（變換）顯示相機軸，但您需要了解這一點。顧名思義，它描述了描述一個坐標系與另一個坐標系之間關系的坐標變換。首先，讓我們看看如果未設置此 TF 會發生什么情況。

$ roslaunch realsense2_camera rs_camera.launch filter：=pointcloud$ roslaunch mycobot_moveit mycobot_moveit_control.launch

然后，您將在 Rviz 顯示面板中看到一條警告，如圖 11 所示。消息是相機的每個坐標系都沒有變換到base_link（myCobot的起源）。

因此，讓我們創建一個包，將 TF（坐標變換）從相機廣播到 myCobot。很抱歉在C++和Python之間來回，但這次我將使用roscpp。首先創建一個包。

$ cd /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  #include  #include  #include   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})      tf_broadcaster    0.0.0   The transform broadcaster package   root   TODO    catkin   tf2_geometry_msgs   geometry_msgs 

如您所見，我們設置 transformStamped 類的實例變量并廣播該實例。廣播變量類型是tf2_ros：：StaticTransformBroadcaster，但這次我們使用靜態TF，因為攝像機和機器人的位置是固定的。對于那些感興趣的人，當職位關系隨時間變化時，也可以使用動態 TF。

在這里，位置關系還不知道，所以值是暫時的，但我們可以組成顯示，所以讓我們嘗試一下。此外，父鏈接camera_color_frame而不是camera_link，因為父鏈接的原點與點云坐標系匹配。 catkin_make后，像以前一樣運行 rs_camera.launch 和 mycobot_moveit_control.launch，并在另一個終端中運行tf_broadcaster。

$ rosrun tf_broadcaster tf_broadcaster

這將廣播成像儀和myCobot base_link之間的位置關系，因此TF警告消失。如果在此狀態下添加點云，它將如圖 12 所示。當然，相機和myCobot之間的位置關系是暫時的，因此相機看到的myCobot位置與Rviz中顯示的模型位置不重疊。

因此，接下來我們將校準從相機中看到的機器人的相對姿勢和位置。

我認為還有另一種通過將相機固定在特定位置來指定位置關系的方法，但是這次我們標記三個點以確定機器人坐標，我們通過找到單位向量并相對于相機坐標系進行計算來校準位置關系。

你們中的一些人可能已經注意到，之前的照片是在已經連接標記的情況下拍攝的，但標記的放置方式如圖 13 所示。

口糧標記

myCobot 的原點（底部中心）位于標記 2 和 3 的中點，標記 1 垂直于該原點的線段 2 和 3。

1.首先，重寫red_point_detection.py并創建blue_point_detection.py以獲得藍色標記的點云。此處省略了該腳本，因為它只是重寫了要提取的 RGB 范圍。之后，我認為有多種方法可以從那里獲得坐標轉換，但這次是通過以下過程獲得的。確定標記坐標 訂閱標記點云

一個。將點云聚類為三個（通過 k 均值方法）

2. 確定原點和單位向量 標記 2 和 3 的中點是原點V_robot

一個。X 和 Y 的單位向量V_X和V_Y分別V_robot →標記 1、V_robot →標記 2。

b.Z 的單位向量是帶有標記的平面和法線向量的叉積

3. 使用歐拉角創建從相機姿勢到機器人姿勢的旋轉 theta_1 = 相機相對于水平面的仰角

a.theta_2 = 朝向相機前方的旋轉角度

b.theta_3 = 相機與機器人前方方向之間的角度

4. 將 V-Z 方向的原點轉換為 myCobot 的固定底座（2.7 厘米）

* 歐拉旋轉開始的軸是任意的，因此結果會因所使用的庫而異。（我認為不是每個人都費心這樣做，所以我想知道您是否傳遞單位向量和兩個坐標系的原點，如果有一些庫將其轉換為 TF。

以下腳本將添加到腳本目錄中。

# 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 positionsn{}".format(marker_centroids))                 translation = self.cal_translation(marker_centroids)                 rospy.loginfo("n translationn{}".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

這次我們從最近的marker_1設置相機和機器人，根據它們的位置關系marker_2和marker_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

然后反射攝像頭和機器人的位置關系，點云上的機器人和Rviz所示的機器人模型可以疊加，如圖14所示。

圖 14：將從校準中獲得的值設置為 TF 并廣播它并不完美，但我能夠將偏差保持在可接受的范圍內，以便簡單地到達并拾取物體。

當移動點云和疊加的機器人模型時，您可以看到這一點。

myCobot的關節有一點間隙，這使得很難做出精確的動作。

由于從實感獲得的點云坐標系被轉換為myCobot的坐標系，因此我們嘗試將myCobot的尖端定位為對象。（我試圖使用 MoveIt 訪問它，但它被困在C++，所以我制作了一個可以使用 pymycobot 輕松操作的腳本）

由于坐標位于相機的光學坐標系中，因此請使用先前獲得的平移和旋轉將它們轉換為myCobot坐標系。將以下內容添加到腳本目錄。

# 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

這一次，我將調動我使用過的所有庫和我制作的腳本。

您可以在 6 個終端中運行 rs_camera.launch、mycobot_moveit_control.launch、tf_broadcaster、red_point_detection.py、object_position_search.py、mycobot_reaching.py，但每次打開終端時，您都必須對其進行設置......調試會很困難，因此請創建自己的啟動文件。

轉到工作區，創建用于啟動的目錄和文件，并將啟動路徑添加到 CMakeLists.txt。

$ cd /src/mycobot_test$ mkdir launch$ cd launch$ touch mycobot_reaching.launch                                                # CMakeLists.txt~~# Add launch fileinstall(FILES   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ) ~~

啟動文件的結構很簡單，只需將要同時運行的節點和啟動文件中的啟動文件排列在啟動選項卡中即可。要執行節點，請在節點標記中寫入任何節點名稱，并像 rosrun 一樣寫入包名稱和可執行文件名稱。添加輸出=“屏幕”，如果您希望輸出顯示在命令行上。

在引導文件中執行啟動時，在包含標記中包含文件路徑。嘗試catkin_make并運行它。

$ roslaunch mycobot_test mycobot_reaching.launch

我能夠移動myCobot來跟蹤RealSense捕獲的紅色物體，就像在電影1中一樣。

跟著項目跟著做之前有很多滯后，總覺得自己在各方面都學得還不夠。

7. 總結

在本文中，我總結了如何使用 ROS 來協調 6 軸機械臂 myCobot 和深度攝像頭實感 D455。我沒有機器人開發的經驗，包括ROS。我認為沒有一個博客總結了我到目前為止從頭開始嘗試的內容，所以如果你買了一個機械臂或深度相機但不知道如何使用它，我希望它對那些想知道如何使用它的人有所幫助。另外，如果具有機器人開發經驗或擅長數據處理的人可以指出最好做這樣的事情，我將不勝感激。

這次我將我在模擬器中學到的強化學習模型應用于myCobot，并進行了拾取實驗，所以我可能會寫另一篇博客作為第2部分。

參考

1. 松林達志， 2022.12.21， 實感D455による空間認識でmyCobotを操作.

2. 阿爾伯特公司

審核編輯黃宇