ROS基入门十 MoveIt!编程中
前情回顾
笛卡尔路径规划
代码部分
前情回顾
上一篇讲了MoveIt！的接口部分，主要介绍了如何使用MoveIt!的接口进行关节空间规划及工作空间规划上篇链接
关节空间规划和工作空间规划都存在着相同的问题，末端执行器的姿态是不可预知的，无法让机器人走出只想或者圆弧，也就是无法让机器人走出你想要的运动。下面就来使用笛卡尔路径规划来解决这个问题。

笛卡尔路径规划
运行roslaunch marm_planning arm_planning_with_trail.launch打开机器人模型，再运行rosrun marm_planning moveit_cartesian_demo.py_cartesian:=True 也就是robot_marm\marm_planning\scripts\moveit_cartesian_demo.py这个路径下的文件，通过代码来使机器人运动起来。

这里启动了另外一个launch文件marm_planning/launch/arm_planning_with_trail.launch 内容与arm_planning.launch的几乎一致只是添加了终端轨迹的可视化显示设置可以更方便地对比运动效果。moveit_cartesian_demo还增加了_cartesian参数设置方便看到不同类型规划的运动效果。cartesian:=True是走直线规划的，false是曲线规划。
从运动的效果可以看出，因为第一条线并没有进行笛卡尔规划，所以是一条曲线，其他的都是直线。
运行rosrun marm_planning moveit_cartesian_demo.py_cartesian:=false看一下不进行笛卡尔规划的现象，两者做对比。

从结果可以看出不带路径约束的运动规划，机器人终端的运动轨迹不再以直线轨迹运动。

如果你教程的运行命令是roslaunch marm_moveit_config demo.launch这个的话，需要在
Rviz里进行轨迹显示设置。
打开rviz添加插件

添加RobotMode插件，点击ok

插件下点开Links，选择show trail，打上勾。

机器人运行时就会显示末端执行器的运动轨迹了。

代码部分

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)

        # 初始化ROS节点
        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(deepcopy(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

• waypoints.append(start_pose)这里需要了解“waypoints”的概念也就是路点。waypoints是一个路点列表意味着笛卡儿路径中需要经过的每个位姿点相邻两个路点之间使用直线轨迹运动。将运动需要经过的路点都加入路点列表中但此时并没有开始运动规划。

(plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses，路点列表
                                        0.01,        # eef_step，终端步进值
                                        0.0,         # jump_threshold，跳跃阈值
                                        True)        # avoid_collisions，避障规划

这是整个例程的核心部分使用了笛卡儿路径规划的API——compute_cartesian_path()它共有四个参数第一个参数是之前创建的路点列表第二个参数是终端步进值第三个参数是跳跃阈值第四个参数用于设置运动过程中是否考虑避障。
compute_cartesian_path()执行后会返回两个值plan是规划出来的运动轨迹fraction用于描述规划成功的轨迹在给定路点列表中的覆盖率从0到1。如果fraction小于1说明给定的路点列表没办法完整规划这种情况下可以重新进行规划但需要人为设置规划次数。

if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
                arm.execute(plan)
                rospy.loginfo("Path execution complete.")

如果规划成功fraction的值为1此时就可以使用execute()控制机器人执行规划成功的路径轨迹了。
cartesian = rospy.get_param(’~cartesian’, True)如果这个判断是否，则运行的是空间轨迹规划。
如果想走出圆的效果，需要将圆微分成一段段小直线，再进行笛卡尔规划，用走直线的方法模拟着走出圆的形状。分的越小，走出来的轨迹越接近于圆。