通过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的效果还是很推荐的，具体安装配置过程参考链接，这里就不赘述了。