Python 的使用總是比 C++ 簡單許多，Move Group 的 Python 介面更為便捷，也為使用者提供了很多用于操縱機器人和機械臂的函式，能夠和 C++ 介面實作相同的功能：
- 設定機械臂的位姿
- 進行運動規劃
- 移動機器人本體
- 將物品添加到環境 / 從環境移除
- 將物體系結到機器人 / 從機器人解綁
1. 執行示例代碼
1.1 運行程序
cd ~/ARM/ws_moveit/
source devel/setup.bash
# 打開機器人模型結點
roslaunch panda_moveit_config demo.launch
新開一個終端，在相同目錄下執行
source devel/setup.bash
# 機器人python 介面結點
rosrun moveit_tutorials move_group_python_interface_tutorial.py
1.2 預期效果
在RViz 中，會看到以下效果：
- 規劃并運動到指定的 關節目標 joint goal
- 規劃并運動到指定的 姿態目標 pose goal
- 規劃并顯示一個笛卡爾軌跡
- 重演一遍 3 中的笛卡爾軌跡（執行一個保存/規劃過的軌跡）
- 在 Panda 的末端執行器 處添加一個盒子
- 改變盒子的顏色，表示已經被機械臂抓到了
- 機械臂抓著這個盒子執行一段笛卡爾軌跡
- 再次改變盒子顏色，表示從機械臂處解綁
- 移除盒子，
2. 代碼分析
2.1 初始化
2.1.1 引入包
使用python介面時，我們通常需要加載moveit_commander ， 這是一個namespace package，里面會加載MoveGroupCommander類, PlanningSceneInterface
以及RobotCommander類；以及其他的一些類和訊息msg，
后續學習到細節時，可能會講解這些類，但目前沒有必要，因為我們學習的就是這個介面，
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
2.1.2 初始結點
初始化moveit_commander，創建一個node結點：
# 初始化moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
# rospy node初始化
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
2.1.3 創建物件
創建/實體化 一個RobotCommander的物件，RobotCommander是針對整個機器人的控制，會提供機器人的運動學模型和機器人的當前關節狀態：
robot = moveit_commander.RobotCommander()
創建一個PlanningSceneInterface的物件，PlanningSceneInterface是用于和機器人環境的互動（如添加物體時會用到）：
scene = moveit_commander.PlanningSceneInterface()
創建 MoveGroupCommander 的物件， MoveGroupCommander 是針對一個規劃組進行控制，這里通過設定 group_name = panda_arm 控制 Panda 機器人的手臂，
規劃組的概念在C++部分以及基本概念中都提到過，是我們控制規劃和執行運動的物件，
在 panda 機器人中，panda_arm 是主要手臂關節，
創建一個Publisher，發布的型別是DisplayTrajectory，用于 rivz 中顯示軌跡
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
2.2 獲取基本資訊
# 獲取機器人的參考坐標系
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
# 獲取規劃組的末端執行器
eef_link = move_group.get_end_effector_link()
print "============ End effector link: %s" % eef_link
# 獲取機器的所有規劃組
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names()
# 獲取機器人的當前狀態
print "============ Printing robot state"
print robot.get_current_state()
print ""
2.3 Planning to a Joint Goal
規劃到一個關節目標，與C++ 不同的是需要使用弧度制，
因為 panda 機器人的初始位置是一個奇點，先移動到一個較好的位置，這個動作使用 joint goal 規劃：
# 賦值調整關節姿態
joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi/4
joint_goal[2] = 0
joint_goal[3] = -pi/2
joint_goal[4] = 0
joint_goal[5] = pi/3
joint_goal[6] = 0
# go()命令規劃并執行動作，如果設定好了，可以不需要第一個引數
move_group.go(joint_goal, wait=True)
# 呼叫 stop() 確保沒有多余的移動
move_group.stop()
2.4 Planning to a Pose Goal
同樣的，也可以為末端執行器設定目標姿態來進行機械臂規劃：
位置xyz，角度w，合起來是位姿
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 0.4
pose_goal.position.y = 0.1
pose_goal.position.z = 0.4
# 這個函式也與 C++ 中相同
move_group.set_pose_target(pose_goal)
呼叫規劃器計算并執行，
plan = move_group.go(wait=True)
move_group.stop()
#完成規劃后清除目標
move_group.clear_pose_targets()
注意只有pose goal這里可以有清除目標的函式
clear_pose_targets()，joint value goal 沒有這個對應的函式，
2.5 Cartesian Paths
笛卡爾路徑，
通過指定 末端執行器 要通過的途徑點串列，直接規劃笛卡爾路徑，
如果在 Python shell 中互動執行，需要設定scale=1.0，
waypoints = []
## 第一段軌跡
wpose = move_group.get_current_pose().pose
wpose.position.z -= scale * 0.1 # First move up (z)
wpose.position.y += scale * 0.2 # and sideways (y)
# 將該段的終點加入途徑點的串列中，
waypoints.append(copy.deepcopy(wpose))
## 第二段軌跡
wpose.position.x += scale * 0.1 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
wpose.position.y -= scale * 0.1 # Third move sideways (y)
waypoints.append(copy.deepcopy(wpose))
# 以1cm的解析度進行插值，所以設定eef為0.01
# jump_threshold跳轉閾值為0.0
(plan, fraction) = move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
# 注意這里只是規劃一個笛卡爾路徑，但機械臂還沒有執行，
return plan, fraction
2.6 Displaying a Trajectory
在 RViz 中顯示路徑，呼叫 group.plan() 規劃路徑的時候會自動在 rviz 中顯示，
也可以手動進行顯示，創建一個 DisplayTrajectory 的訊息物件，然后發布到 '/move_group/display_planned_path' 話題，見下面代碼：
#創建顯示路徑的物件
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
# 用當前狀態作為起始點
display_trajectory.trajectory_start = robot.get_current_state()
# plan 是傳入函式的引數，是已知的
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory);
DisplayTrajectory msg 主要有兩個屬性：
- 起始點 trajectory_start
- 路徑 trajectory
2.7 Executing a Plan
規劃完畢 plan 之后，要讓機器人實際執行這個規劃，就使用 move_group.execute()，
move_group.execute(plan, wait=True)
這個函式要求起始點在機器人軌跡的可允許的誤差內（即符合機器人的動力運動學）
2.8 物體操作
2.8.1 添加物體
在planning scence 中 機器人左指的位置添加一個 box，
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_leftfinger"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.z = 0.07 # slightly above the end effector
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))
注意，這里使用的坐標系是 左指坐標系，這樣就很容易確定 box 與 左指 的位置，
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_leftfinger"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.z = 0.07 # slightly above the end effector
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))
2.8.2 檢查是否發布成功
在添加/移除物體后，會發布一個更新碰撞物體(collision object) 的訊息，這個訊息在發布出去的時候可能會丟失，
為了確保物體成功添加/移除，可以通過get_known_object_names()獲取當前已知的物體來檢查是否成功，
start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
# 檢查盒子是否是捆綁物體
attached_objects = scene.get_attached_objects([box_name])
is_attached = len(attached_objects.keys()) > 0
# 檢查盒子是否被添加
# 注意：如果將盒子捆綁在機械臂上，則不會出現在 known_object_names 中
is_known = box_name in scene.get_known_object_names()
# 檢查我們是否在預期的狀態
if (box_is_attached == is_attached) and (box_is_known == is_known):
return True
# Sleep so that we give other threads time on the processor
rospy.sleep(0.1)
seconds = rospy.get_time()
# If we exited the while loop without returning then we timed out
return False
2.8.3 將物體捆綁在機械臂上
接下來，我們將把 box 系結到 panda 上，
系結有一個條件，就是movelt 不能將機械臂的某些部位與物體的這種正常接觸當作碰撞而報錯，所以需要將這些 部位/link 加入 touch_links，告訴規劃場景 planning scene 忽略這些 links 與 物體 之間的碰撞
對于 panda 機械臂，設定 grasping_group = 'hand'，如果使用不同的機器人，則應將此值更改為 末端執行器 規劃組的名稱，
grasping_group = 'hand'
touch_links = robot.get_link_names(group=grasping_group)
scene.attach_box(eef_link, box_name, touch_links=touch_links)
2.8.4 解綁物體
scene.remove_attached_object(eef_link, name=box_name)
將物體從機械臂上解除，
2.8.5 移除物體
物體必須要先解綁，然后才能移除，
scene.remove_world_object(box_name)
當然，這些操作都可以使用2.8.2 中的檢驗函式來確保規劃器執行了我們的命令，原始碼中都使用了:
# 如移除物體操作的檢驗函式的引數應當為：
return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout)
3. 總結
就語言角度來講，python 更親切一點，只需要明白 類的相關概念 就能很好的閱讀原始碼，
原始碼呈現了很好的封裝性，不同操作的代碼寫在同一類的不同的函式下，等待 main 函式來依次呼叫，而不用考慮彼此之間的一些關聯，
3.1 規劃方式
同樣，python介面也在強調三種規劃方式：
我個人認為是兩種，笛卡爾路徑應當是pose goal規劃的延伸
-
joint value goal planning：
- 使用move_group.get_current_joint_values()函式得到當前關節狀態的陣列
- 修改陣列的數值
- 注意此處數值是弧度制，帶 pi
- 用move_group.go() 來進行規劃
- 用move_group.stop()來停止運動
-
pose goal planning：
- 用geometry_msgs.msg.Pose()得到當前位置姿態；
- 修改角度w，坐標x、y、z
- 使用move_group.set_pose_target(pose_goal)來設定目標
- 同樣使用 go() 和 stop() 規劃和停止
- 這種規劃多了一個可以清除目標的函式
物體的相關操作我就不再此列舉了，上面的函式也挺清楚，
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/501183.html
標籤：其他
上一篇：Computational Protein Design with Deep Learning Neural Networks
下一篇：語言模型大串燒之變形金剛