通過ROS控制真實機械臂(14) --- 笛卡爾運動規劃

笛卡爾運動規劃Python接口https://blog.csdn.net/qq_32618327/article/details/99966978

笛卡爾運動規劃C++接口https://blog.csdn.net/flyfish1986/article/details/81189737

Moveit!官網API介紹：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/motion_planning_api/motion_planning_api_tutorial.html

自從機械臂通過三次插補完成路徑規劃運動，發現和傳統的工業機械手運動總是有點差別，給我的感覺就是視頻裏面的機械手都是一段一段運動的，並不總是簡單的以時間最小原則規劃一條從起點運動到終點的路徑，也就是並沒有對機器人終端軌跡有任何約束
但是在很多應用場景中，不僅關心機械臂的起始、終止位姿，對運動過程中的位姿也有要求。如下例程來自於上述參考的鏈接，但是我自己實現起來有點報錯和小問題，進行了簡單的修改。實現的功能是讓機器人終端能夠走出一條直線軌跡。

$ roslaunch redwall_arm_moveit_config demo.launch
$ rosrun redwall_arm_control moveit_cartesian_demo.py

#!/usr/bin/env python
#coding=utf-8

# ROS包中使用python需要加上上面的東西，修改本文件爲可執行文件
# 添加rospy依賴

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy

class MoveItCartesianDemo:
    def __init__(self):

        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化moveit_cartesian_demo節點
        rospy.init_node('moveit_cartesian_demo', anonymous=True)
        
        # 是否需要使用笛卡爾空間的運動規劃
        cartesian = rospy.get_param('~cartesian', True)
                        
        # 初始化需要使用move group控制的機械臂中的arm group
        arm = MoveGroupCommander('arm')
        
        # 當運動規劃失敗後，允許重新規劃
        arm.allow_replanning(True)
        
        # 設置目標位置所使用的參考座標系
        arm.set_pose_reference_frame('base_link')
                
        # 設置位置(單位：米)和姿態（單位：弧度）的允許誤差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.1)
        
        # 獲取終端link的名稱
        end_effector_link = arm.get_end_effector_link()
                                        
        # 控制機械臂運動到之前設置的“forward”姿態
        arm.set_named_target('forward')
        arm.go()
        
        # 獲取當前位姿數據最爲機械臂運動的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose
                
        # 初始化路點列表
        waypoints = []
                
        # 將初始位姿加入路點列表
        if cartesian:
            waypoints.append(start_pose)
            
        # 設置第二個路點數據，並加入路點列表
        # 第二個路點需要向後運動0.2米，向右運動0.2米
        wpose = deepcopy(start_pose)
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
         
        # 設置第三個路點數據，並加入路點列表
        wpose.position.x += 0.05
        wpose.position.y += 0.15
        wpose.position.z -= 0.15
          
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
        
        # 設置第四個路點數據，回到初始位置，並加入路點列表
        if cartesian:
            waypoints.append(start_pose)
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0   #路徑規劃覆蓋率
            maxtries = 100   #最大嘗試規劃次數
            attempts = 0     #已經嘗試規劃次數
            
            # 設置機器臂當前的狀態作爲運動初始狀態
            arm.set_start_state_to_current_state()
     
            # 嘗試規劃一條笛卡爾空間下的路徑，依次通過所有路點
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses，路點列表
                                        0.01,        # eef_step，終端步進值
                                        0.0,         # jump_threshold，跳躍閾值
                                        True)        # avoid_collisions，避障規劃
                
                # 嘗試次數累加
                attempts += 1
                
                # 打印運動規劃進程
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
            # 如果路徑規劃成功（覆蓋率100%）,則開始控制機械臂運動
            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
                arm.execute(plan)
                rospy.loginfo("Path execution complete.")
            # 如果路徑規劃失敗，則打印失敗信息
            else:
                rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        # 控制機械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
        
        # 關閉並退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItCartesianDemo()
    except rospy.ROSInterruptException:
        pass

如果規劃成功，fraction的值爲1此時就可以使用execute控制機器人執行規劃成功的路徑軌跡.這個例程的關鍵是瞭解compute_cartesian_path 這個API的使用方法可以實現一系列路點之間的笛卡爾直線運動規劃。如果希望機器人的終端走出圓弧軌跡
也可以將圓弧分解爲多段直線後，使用compute_cartesian_path控制機器人運動。

但是無論使用上面的python接口還是使用C++的笛卡爾運動規劃接口，總是提示如下，機械臂也沒有真正的運動起來，

[INFO] [WallTime: 1577173016.537051] Path planning failed with only 0.278846153846 success after 100 attempts.

很顯然，接口並沒有報錯，只是無法規劃出能夠百分之百覆蓋所有路點，並能讓末端以一條直線運動的路徑。可以看到例程中的座標從起始位置x和y方向運動，我所使用的UR機械臂初始狀態已經是豎直狀態了，可以想象這樣的路徑的確不存在。

當然也有可能路徑是存在的，但由於MoveIt!默認使用的運動學插件是KDL，該算法是基於迭代的數值解，求解速度慢，失敗率高，導致求解失敗，可以嘗試更換運動學求解的插件。（最後面提供修改運動學插件的方法）

---

---

對於笛卡爾運動規劃要求較低的機器人，不需要關注路徑的形式，只關注需要經過的路點的時候，只需要修改上述代碼中的參數爲false即

        # 是否需要使用笛卡爾空間的運動規劃
        cartesian = rospy.get_param('~cartesian', True)

可讓機器人以隨意的路徑依次運動到需要進過的路點。如果覺得上面座標設置的方法不太滿意，可以參考如下鏈接：https://joveh-h.blog.csdn.net/article/details/99957394#3__111 ，實現的效果就是視頻裏面看到的一段一段的運動。

#!/usr/bin/env python
#coding=utf-8

import rospy, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler

class MoveItIkDemo:
    def __init__(self):

        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化moveit_ik_demo節點
        rospy.init_node('moveit_ik_demo')
                
        # 初始化需要使用move group控制的機械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
                
        # 獲取終端link的名稱
        end_effector_link = arm.get_end_effector_link()
                        
        # 設置目標位置所使用的參考座標系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
                
        # 當運動規劃失敗後，允許重新規劃
        arm.allow_replanning(True)
        
        # 設置位置(單位：米)和姿態（單位：弧度）的允許誤差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # 設置機械臂的目標位置爲自定義位姿中的home
        arm.set_named_target('home')

        # 控制機械臂規劃和運動期望位置
        arm.go()

        # 保存一段時間的延時，確保機械臂已經完成運動
        rospy.sleep(2)
               
        # 設置機械臂工作空間中的目標位姿，位置使用x、y、z座標描述，
        # 姿態使用四元數描述，基於base_link座標系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.191995
        target_pose.pose.position.y = 0.213868
        target_pose.pose.position.z = 0.520436
        target_pose.pose.orientation.x = 0.911822
        target_pose.pose.orientation.y = -0.0269758
        target_pose.pose.orientation.z = 0.285694
        target_pose.pose.orientation.w = -0.293653
        
        # 設置機器臂當前的狀態作爲運動初始狀態
        arm.set_start_state_to_current_state()
        
        # 設置機械臂終端運動的目標位姿
        arm.set_pose_target(target_pose, end_effector_link)
        
        # 規劃運動路徑
        traj = arm.plan()
        
        # 按照規劃的運動路徑控制機械臂運動
        arm.execute(traj)
        rospy.sleep(1)
         
        # 控制機械臂終端向右移動5cm
        arm.shift_pose_target(1, -0.05, end_effector_link)
        arm.go()
        rospy.sleep(1)
  
        # 控制機械臂終端反向旋轉90度
        arm.shift_pose_target(3, -1.57, end_effector_link)
        arm.go()
        rospy.sleep(1)
           
        # 控制機械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 關閉並退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItIkDemo()
    except rospy.ROSInterruptException:
        pass

使用ROS中的PoseStamped消息數據描述機器人的目標位要首先需要設置位姿所在的參考座標系，基於base_link座標系然後創建時間戳接着設置目標位姿的x、y、z座標值和四元數姿態值。

除了使用PoseStamped數據描述目標位姿並進行運動規劃外也可以使用shift_pose_target實現單軸方向上的目標設置和規劃
shift_pose_target有以下三個參數：

• 第一個參數：描述機器人需要在哪個軸向上進行運動； 0、1、2、3、4、5 分別對應於x、y、z、 r、 p、y，即 xyz 方向上的平移和圍繞 xyz 三個軸的旋轉
• 第二個參數：描述運動尺度；如果是平移運動，則單位爲米；如果是旋轉運動，單位爲弧度第
• 三個參數：描述運動針對的終端link首先讓機器人終端在y軸的負方向上平移0.05米，然後圍繞z軸反向旋轉90度

---

 

很多時候我們在做運動規劃的時候，MoveIt!經常會提示規劃失敗、求解失敗等錯誤，很多都是因爲KDL這款運動學插件導致的，以下介紹兩個用的最多的運動學插件：TRAC-IK和IKFAST。參考鏈接https://www.ncnynl.com/archives/201807/2519.html

---

一、TRAC-IK

TRAC-IK和KDL類似，也是一種基於數值解的運動學插件，但是在算法層面上進行了很多改進，求解效率高了很多。很多情況下KDL無法求解的姿態點，但是在使用TRAC-IK是可以求解的。但是TRAC-IK也有問題，它是一種數值算法，每次求解得到的關節位置不一定相同。

• 那麼如何將KDL更換成TRAC-IK呢，方法很簡單，ROS的軟件源中已經集成了TRAC-IK的安裝包，可以直接使用以下命令安裝：

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

• 然後修改機械臂MoveIt!配置功能包下的kinematics.yaml文件就可以使用：

arm:

  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin

  kinematics_solver_attempts: 3

  kinematics_solver_search_resolution: 0.005

  kinematics_solver_timeout: 0.05

• 接下來再次運行demo.launch，默認加載的就是TRAC-IK運動學插件了，試試規劃求解的效率是不是高了很多！

二、IKFAST

• IKFAST是一種基於解析算法的運動學插件，可以保證每次求解的一致性。
• 相比KDL和TRAC-IK，IKFAST的安裝過程就比較複雜了，據說IKFAST的效果還是很推薦的，具體安裝配置過程參考鏈接，這裏就不贅述了。