????????R2(Robonaut 2)是NASA美國宇航局與GM通用聯(lián)合推出的宇航人形機器人，能在國際空間站使用，可想而知其價格是非常昂貴，幾百萬美刀吧，還好NASA發(fā)布了一個R2機器人的Gazebo模型，使用模型就不需要花錢了，由于我們的機器人軟件通常是不依賴于具體機器人的，所以在R2學(xué)到的東西也可以應(yīng)用到其他的機器人身上，所以我們在仿真平臺上來操作它。

1、安裝R2機器人

1.1、OS版本

使用的是虛擬機上的一個Ubuntu18的版本系統(tǒng)來安裝，我們先來查看下版本：cat /etc/os-release?

NAME="Ubuntu"
VERSION="18.04.6 LTS (Bionic Beaver)"
ID=ubuntu
ID_LIKE=debian
PRETTY_NAME="Ubuntu 18.04.6 LTS"
VERSION_ID="18.04"
HOME_URL="https://www.ubuntu.com/"
SUPPORT_URL="https://help.ubuntu.com/"
BUG_REPORT_URL="https://bugs.launchpad.net/ubuntu/"
PRIVACY_POLICY_URL="https://www.ubuntu.com/legal/terms-and-policies/privacy-policy"
VERSION_CODENAME=bionic
UBUNTU_CODENAME=bionic

?1.2、安裝ROS

在前面的 Ubuntu18.04版本安裝ROS及出現(xiàn)錯誤的處理方法
有介紹不同OS版本對應(yīng)著不同的ROS版本安裝，所以這里我的是melodic版本
分別輸入命令安裝，也可以使用空格隔開寫在一條指令上進行安裝：

sudo apt-get install ros-melodic-ros-control
sudo apt-get install ros-melodic-gazebo-ros-control
sudo apt-get install ros-melodic-joint-state-controller
sudo apt-get install ros-melodic-effort-controllers
sudo apt-get install ros-melodic-joint-trajectory-controller
sudo apt-get install ros-melodic-moveit*
sudo apt-get install ros-melodic-object-recognition-*

從名稱也可以知道，除了ROS之外，還有一些關(guān)節(jié)狀態(tài)控制器，路徑控制器以及后面重點介紹的MoveIt

1.3、創(chuàng)建工作空間?

創(chuàng)建一個名為chessbot的工作空間，這里我想用R2來做下棋的機器人。?

mkdir -p ~/chessbot/src
cd ~/chessbot/src
catkin_init_workspace

?1.4、下載編譯

網(wǎng)上好像也只有下面這個貌似要被棄用的包，哈哈。所以也會在后面帶來一個我沒有解決的問題或許是這個原因。

git clone https://bitbucket.org/nasa_ros_pkg/deprecated_nasa_r2_simulator.git
git clone https://bitbucket.org/nasa_ros_pkg/deprecated_nasa_r2_common.git
其余包來源：https://bitbucket.org/nasa_ros_pkg/workspace/repositories/

下載好了之后，進行編譯：

cd ..
catkin_make

1.5、顯示機器人

編譯好了之后，就可以顯示R2雙臂機器人了

cd ~/chessbot
source devel/setup.bash
roslaunch r2_gazebo r2_display.launch

如下圖：

運行robot_state_publisher節(jié)點，會使用R2的幾何描述和它的關(guān)節(jié)狀態(tài)向量，持續(xù)地計算并更新機器人上的所有坐標(biāo)系(前向運動學(xué)計算)，這種操作的是標(biāo)準(zhǔn)的ROS實現(xiàn)，不依賴機器人，所以我們只需要直接啟動它，它就能為R2機器人做正確的事情。

cd ~/chessbot
source devel/setup.bash
rosrun robot_state_publisher robot_state_publisher

?然后我們試著讓機器人的雙臂揮動起來，代碼如下：

cd ~/chessbot/src
gedit r2_move.py

#!/usr/bin/env python
import sys,rospy,tf,moveit_commander,random
from geometry_msgs.msg import Pose,Point,Quaternion
from math import pi

Q=Quaternion(*tf.transformations.quaternion_from_euler(pi,-pi/2,-pi/2))
orient=[Q,Q]
pose=[Pose(Point(0.5,-0.5,1.3),orient[0]),Pose(Point(-0.5,-0.5,1.3),orient[1])]
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('r2_wave_arm',anonymous=True)
group=[moveit_commander.MoveGroupCommander("left_arm"),moveit_commander.MoveGroupCommander("right_arm")]
while not rospy.is_shutdown():
    pose[0].position.x = 0.5+random.uniform(-0.1,0.1)
    pose[1].position.x = -0.5+random.uniform(-0.1,0.1)
    for side in [0,1]:
        pose[side].position.z = 1.5+random.uniform(-0.1,0.1)
        group[side].set_pose_target(pose[side])
        group[side].go(True)

moveit_commander.roscpp_shutdown()

代碼的意思比較簡單，首先將歐拉角轉(zhuǎn)成四元數(shù)(tf.transformations.quaternion_from_euler(pi,-pi/2,-pi/2))，因為四元數(shù)在計算幾何軟件包中更為常用，因為它的數(shù)值穩(wěn)定性較好，然后我們設(shè)置左右臂的位姿，通過MoveIt里面的moveit_commander的Python接口來驅(qū)動雙臂的隨機揮動。

加個可執(zhí)行權(quán)限：chmod u+x r2_move.py

./r2_move.py

不出意外的話就會出現(xiàn)意外，報錯如下：

[ INFO] [1702101228.432735750]: Loading robot model 'r2'...
[ INFO] [1702101228.434537951]: No root/virtual joint specified in SRDF. Assuming fixed joint
[FATAL] [1702101228.847908597]: Group 'left_arm' was not found.
Traceback (most recent call last):
? File "./r2_move.py", line 11, in <module>
? ? group=[moveit_commander.MoveGroupCommander("left_arm"),moveit_commander.MoveGroupCommander("right_arm")]
? File "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_commander/move_group.py", line 66, in __init__
? ? name, robot_description, ns, wait_for_servers
RuntimeError: Group 'left_arm' was not found.

SRDF中沒有指定根/虛擬關(guān)節(jié)。找不到這個'left_arm' 組，也就是沒有定義它，這個錯誤涉及到了MoveIt軟件包，由于比較復(fù)雜，放在后面進行單獨說明。

2、R2關(guān)節(jié)

R2機器人由于完全模擬了人體的各個關(guān)節(jié)，所以會出現(xiàn)很多的關(guān)節(jié)（手臂、手指、手掌等），那么對于這些關(guān)節(jié)的英文名稱還是需要非常熟悉的：

backpack：背包、行囊
baseplate：底座
bodycover：機身罩（類似衣服）
chest base：胸部區(qū)域
shoulder：肩膀
arm：手臂
elbow：肘部
distal：末端的（比如手指的指尖）
medial：中間的
proximal：近端的（比如手指靠近手掌的關(guān)節(jié)部分）
tip：末梢 尖端
lower arm：前臂（手垂直向下的下面部分）
upper arm：上臂（手垂直向下的上面部分）
palm：手掌
wrist：手腕
neck：脖子
waist：腰部
五根手指的名稱：
thumb：大拇指；index：食指；middle：中指； ring：無名指；little：小指

3、MoveIt

我們可以看到在上面想要讓機器人運動的時候，會報錯：Group 'left_arm' was not found.

就是這個left_arm和right_arm左右兩個手臂的組，找不到，這里就需要MoveIt這個軟件包，功能強大但也比較復(fù)雜，一起來看下是如何通過MoveIt來制作機器人運動所需要的相關(guān)操作：

3.1、創(chuàng)建SRDF文件

打開MoveIt輔助安裝工具：

cd ~/chessbot
source devel/setup.bash
rosrun moveit_setup_assistant moveit_setup_assistant

這里我們選擇創(chuàng)建新的MoveIt包，選擇的路徑在deprecated_nasa_r2_common包里面的
r2_description/robots/r2.display.urdf.xacro文件，然后點擊右下角?Load Files?加載文件
根據(jù)機器人的URDF(Unified Robot Description Format統(tǒng)一機器人描述格式)文件生成SRDF(Semantic Robot Description Format語義機器人描述格式)文件。

如下圖：
?

3.2、優(yōu)化自碰撞檢查

對碰撞的檢查，禁用碰撞的部件對，可以減少運動規(guī)劃的時間，如下圖：

默認(rèn)10000次檢查即可，點擊?Generate Collision Matrix?生成碰撞矩陣，紅線有兩種展示方式。?

3.3、創(chuàng)建虛擬關(guān)節(jié)?

虛擬關(guān)節(jié)是將機器人連接到世界（World）的。主要作用是把機器人固定在地面上別亂跑，如下圖：

3.4、規(guī)劃組

這里是最關(guān)鍵的一個地方了，將機器人的子集規(guī)劃成一個一個的組，如下圖：

所以按照我們的雙臂機器人，控制雙臂運動，所以分別創(chuàng)建left_arm和right_arm

3.5、機器人位姿

這個可以定義機器人的位姿，對位姿進行分組，在經(jīng)常去到一個點(比如返航充電的位姿)，將這個位姿設(shè)置一個名稱就比較方便，圖略
類似下面這樣，用home代替坐標(biāo)的意思，這樣更直觀

arm.set_named_target('home')
arm.go()

其余可選的還有，末端執(zhí)行器：如果有夾具，吸盤之類的末端執(zhí)行器就進行配置，可選
被動關(guān)節(jié)：就是不能主動控制的關(guān)節(jié)，也叫從動關(guān)節(jié)，可選
模擬文件：生成URDF文件，可選
3D感知傳感器：這里可以選擇點云或者深度圖像，可選

3.6、ROS控制器

可以直接點擊 Auto Add FollowJointsTrajectory Controllers For Each Planning Group 自動為每個規(guī)劃組添加控制器，用來規(guī)劃路徑的用途

3.7、作者信息

指定作者的名字和郵箱地址，這個可以隨意填寫，圖略

3.8、配置文件

最后一步就是對上述這些操作，進行打包操作，將會生成兩個文件以及兩個目錄(配置文件的目錄和launch目錄)?

全部配置好了之后，先來看個demo，這里以右臂為例

cd ~/chessbot
source devel/setup.bash
roslaunch mybot demo.launch

如下圖(實際是動態(tài)，這里就截圖了)：

代碼運行規(guī)劃組：

cd ~/chessbot
source devel/setup.bash
roslaunch mybot move_group.launch

3.9、錯誤處理

重新開一個終端，我們運行上面那個代碼文件：

cd ~/chessbot
source devel/setup.bash
cd ~/chessbot/src
./r2_move.py

報錯如下：

[ERROR] [1702121793.117659791]: Could not initialize chain object
[ERROR] [1702121793.117758850]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'lef_arm'
[ERROR] [1702121793.117789067]: Kinematics solver could not be instantiated for joint group lef_arm.
[ERROR] [1702121793.118336454]: Could not initialize chain object
[ERROR] [1702121793.118389993]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'right_arm'
[ERROR] [1702121793.118409474]: Kinematics solver could not be instantiated for joint group right_arm.
[FATAL] [1702121793.119064673]: Group 'left_arm' was not found.

可能遇到這樣的錯誤：

[ INFO] [1702174557.779660012]: Ready to take commands for planning group left_arm.
[ INFO] [1702174558.017795107]: Ready to take commands for planning group right_arm.
[ INFO] [1702174558.072099968]: ABORTED: Catastrophic failure
[ INFO] [1702174558.172742129]: ABORTED: Catastrophic failure?

也可能會遇到這樣的錯誤：ABORTED: Solution found but controller failed during execution

主要原因是運動學(xué)插件和關(guān)節(jié)類型的匹配問題。

4、話題解釋

我們來查看下出現(xiàn)了哪些話題：rostopic list

/attached_collision_object
/clicked_point
/collision_object
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/head_mount_kinect/depth_registered/camera_info
/head_mount_kinect/depth_registered/image_raw
/head_mount_kinect/depth_registered/points
/initialpose
/joint_states
/move_base_simple/goal
/move_group/camera_info
/move_group/cancel
/move_group/display_contacts
/move_group/display_planned_path
/move_group/feedback
/move_group/filtered_cloud
/move_group/filtered_cloud/compressed
/move_group/filtered_cloud/compressed/parameter_descriptions
/move_group/filtered_cloud/compressed/parameter_updates
/move_group/filtered_cloud/compressedDepth
/move_group/filtered_cloud/compressedDepth/parameter_descriptions
/move_group/filtered_cloud/compressedDepth/parameter_updates
/move_group/filtered_cloud/theora
/move_group/filtered_cloud/theora/parameter_descriptions
/move_group/filtered_cloud/theora/parameter_updates
/move_group/filtered_label
/move_group/filtered_label/compressed
/move_group/filtered_label/compressed/parameter_descriptions
/move_group/filtered_label/compressed/parameter_updates
/move_group/filtered_label/compressedDepth
/move_group/filtered_label/compressedDepth/parameter_descriptions
/move_group/filtered_label/compressedDepth/parameter_updates
/move_group/filtered_label/theora
/move_group/filtered_label/theora/parameter_descriptions
/move_group/filtered_label/theora/parameter_updates
/move_group/goal
/move_group/model_depth
/move_group/model_depth/compressed
/move_group/model_depth/compressed/parameter_descriptions
/move_group/model_depth/compressed/parameter_updates
/move_group/model_depth/compressedDepth
/move_group/model_depth/compressedDepth/parameter_descriptions
/move_group/model_depth/compressedDepth/parameter_updates
/move_group/model_depth/theora
/move_group/model_depth/theora/parameter_descriptions
/move_group/model_depth/theora/parameter_updates
/move_group/monitored_planning_scene
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/rosout
/rosout_agg
/tf
/tf_static
/trajectory_execution_event

常見的話題有必要去熟悉它：

4.1、點擊的坐標(biāo)點

rostopic info /clicked_point
rosmsg show geometry_msgs/PointStamped

表示用戶在圖形界面上點擊的坐標(biāo)點，也可以表示從點云數(shù)據(jù)中提取的某個點的坐標(biāo)信息。其中獲取的消息類型是 geometry_msgs/PointStamped 表示一個帶有時間戳的2D或3D點。它包含一個Point 類型（表示2D或3D坐標(biāo)）和一個Header 類型（表示消息的時間戳和frame ID）

4.2、顯示機器人狀態(tài)

rostopic info /display_robot_state
rosmsg show moveit_msgs/DisplayRobotState

讀取機器人的狀態(tài)信息（比如位姿、關(guān)節(jié)角度、角速度等），并將其顯示在ROS圖形界面上，需要加載MoveIt軟件包

4.3、初始位姿

rostopic info /initialpose
rosmsg show geometry_msgs/PoseWithCovarianceStamped

對機器人進行定位，當(dāng)從這個話題得到消息的時候，它會重置所有候選位姿(candidate pose)，并生成一組服從正態(tài)分布的，以消息中提供的位姿為中心的候選位姿。比如，使用rviz設(shè)置初始位姿就是在這個話題上發(fā)布一條消息。
好的導(dǎo)航結(jié)果依賴好的機器人定位，改進初始位姿估計的一個辦法是在rviz中查看傳感器數(shù)據(jù)并確保它與地圖相符，尤其是在有激光測距儀的時候，這個辦法工作得很好，因為激光傳感器的數(shù)據(jù)就像是一張本地的地圖，不斷改進初始位姿估計直到激光的數(shù)據(jù)與地圖符合起來，你就得到了很好的位姿估計。想要讓位姿估計更好一些，可以駕駛機器人到處走走，這將使候選集合很好地分布在實際位置周圍，從而給出一個更好的估計。
隨著機器人到處移動并用傳感器測量環(huán)境數(shù)據(jù)，這些候選位姿逐漸接近機器人的真實位姿，在任意時刻，用于路徑規(guī)劃的機器人的真實位姿就是候選位姿中可信度最高的那個。需要注意的是，當(dāng)使用導(dǎo)航系統(tǒng)驅(qū)動機器人前往某個特定位置的時候，最終很有可能靠得很近，但是不太可能完全重合，不過對于路徑規(guī)劃以及基于傳感器的循跡算法來說，這通常足夠精確了。

4.4、附加碰撞物體

該話題會偵聽提供的AttachedCollisionObject消息，這些消息會決定將碰撞物體附加到機器人狀態(tài)中的鏈接上，或者從鏈接上將物體分離開。

4.5、執(zhí)行軌跡

execute_trajectory有五個話題服務(wù)（Action通信機制），對軌跡規(guī)劃的執(zhí)行操作，類似的話題還有pickup抓取和place放置操作。關(guān)于Action動作想要了解的可以查閱：ROS通信機制之動作(Action)服務(wù)的實踐

4.6、move_group

這個是MoveIt的核心組件話題，為用戶提供一系列的動作指令和服務(wù)，通過參數(shù)服務(wù)器提供三種信息，URDF\SRDF\Config一些配置和關(guān)節(jié)限位和運動學(xué)插件之類

5、更換運動學(xué)插件

上面默認(rèn)的是KDL插件，速度慢，求解很容易失敗，所以可以自己更換插件，比如trac-ik-kinematics-plugin運動插件：

sudo apt-get install ros-melodic-trac-ik-kinematics-plugin
sudo apt-get install ros-melodic-trac-ik

將config的kinematics.yaml運動學(xué)的插件修改下

left_arm:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
right_arm:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005

因為已經(jīng)安裝好了，所以也可以在上面MoveIt的Moveit_setup_assistant可視化界面中的規(guī)劃路徑去選擇。

最后想要查看整個機器人的結(jié)構(gòu)，可以使用樹形結(jié)構(gòu)命令查看：rosrun rqt_tf_tree rqt_tf_tree

6、小結(jié)

總的來說這個R2機器人還是非常復(fù)雜，遇到問題網(wǎng)上資料都很少，相關(guān)問題也可能是其他機器人，最終的路徑規(guī)劃和執(zhí)行是表面上沒有出錯了，查看軌跡也有信息，不是空，但是沒有速度，也就是代碼沒有驅(qū)動機器人手臂運動（GUI界面可以運動），出現(xiàn)坐標(biāo)變換不被識別的問題，修改代碼如下：

cd ~/chessbot
source devel/setup.bash

?

import sys,rospy,tf,moveit_commander,random
from geometry_msgs.msg import Pose,Point,Quaternion
from math import pi

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('r2_wave_arm',anonymous=True)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander("left_arm")
group.set_goal_position_tolerance(0.005)
group.set_goal_orientation_tolerance(0.005)
pose_target = Pose(Point(0.5,-0.5,1.3),Quaternion(*tf.transformations.quaternion_from_euler(pi,-pi/2,-pi/2)))
group.set_joint_value_target(pose_target,group.get_end_effector_link(),True)
plan1 = group.plan()
group.execute(plan1)

#group.go()#相當(dāng)于Plan and Execute

?代碼2：

import sys,rospy,tf,moveit_commander,random
import geometry_msgs.msg
from geometry_msgs.msg import Pose,Point,Quaternion
from math import pi

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('r2_wave_arm',anonymous=True)
#機器人接口
robot = moveit_commander.RobotCommander()
#所在環(huán)境接口
scene = moveit_commander.PlanningSceneInterface() 
#控制對象接口
group = moveit_commander.MoveGroupCommander("left_arm")

#允許最大的誤差、速度、加速度
group.set_goal_position_tolerance(0.005)
group.set_goal_orientation_tolerance(0.005)
group.set_max_velocity_scaling_factor(0.5) 
group.set_max_acceleration_scaling_factor(0.5)

#pose_target = geometry_msgs.msg.Pose()
pose_target = geometry_msgs.msg.PoseStamped() 
pose_target .header.frame_id = robot.get_planning_frame()
pose_target.pose.position.x = group.get_current_pose().pose.position.x+0.5
pose_target.pose.position.y = group.get_current_pose().pose.position.y
pose_target.pose.position.z = group.get_current_pose().pose.position.z-0.1
pose_target.pose.orientation = group.get_current_pose().pose.orientation
group.set_joint_value_target(pose_target,group.get_end_effector_link(),True)
#group.go(wait=True) 

plan1= group.plan()
group.execute(plan1)

這個問題也找遍了全網(wǎng)，估計是時間久，版本的問題了，另外資料很少沒有解決，只能等后期會了再來處理這個問題，如果有大神遇到且能解決，還請大神留下解決方案，感謝。?文章來源地址http://www.zghlxwxcb.cn/news/detail-770174.html

到了這里，關(guān)于ROS仿真R2機器人之安裝運行及MoveIt的介紹的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！