ROS入门九 MoveIt！编程 （上）

接口介绍

在这里插入图片描述

由move_group的框架图可以看出，前面讲的是关于GUI可视化接口的部分，通过可视化的形式完成了机械臂的运动规划的控制，主要是去熟悉了关于MoveIt！提供的GUI接口。
接下来的是C++/Python用户接口的部分，这里的整个用户接口都是通过action或者service或者是其他信息跟Move_group来做数据交互的，最终Move_group完成了路径规划之后，通过action跟实际机器人或者虚拟机器人去做数据交互。
在工作空间src中marm_moveit_config文件夹中有一个fake_controllers配置文件，里面配置了关于六轴arm和gripper的控制器，这两个控制器就是机器人控制器，通过action可以完成move_group和机器人控制器之间的信息交互。

基础编程
编程接口
在这里插入图片描述

MoveIt主要给用户提供了两种编程接口，一种Python的结构，一种是C++的接口，这两种接口的使用非常类似，API基本上保持一致。
如果要控制一个规划组做运动，首先要选择我们之前配置好的规划组，针对arm做控制就选择arm，要对前端的夹手做控制就选择gripper，图片中是针对left_arm规划组做控制。
首先都是要先选择控制组，接下来要去给控制设置目标位姿，运动都会有一个起始状态和目标状态，通过ROS当中的pose这样一个消息类型去定义目标姿态的，数据类型定义之后就是使用setPoseTarget来把目标姿态设置到机器人的控制组当中，接下来就可以使用plan的API来完成运动路径的规划，如果这个路径规划是成功的，则返回success，否则fail。
Python返回一个plan1规划的路径，C++返回的路径放到my_plan，可以通过printf把这个路径打印出来。里面包含起点到终点里的一些关键点位，还有路径点的位置、速度、加速度、时间，有了这些东西之后，我们就可以在机器人的控制端完成机器人的实际控制。这就是关于编程接口的大致介绍。

关节空间规划
关节空间介绍
机械臂可以通过关节空间来描述它的位姿状态。
在这里插入图片描述

图上机器人的姿态是由六个轴上的电机的位置决定的，知道各个电机的位置就可以将这个机械臂的位姿求出来。根据机器人的每个电机的位置，最终决定了整个机器人的姿态。
在关节空间中做规划就是已知起始位姿中各个关节的位置，让机器人运动到目标位置，目标位置也是使用这种六个轴的位置信息来描述的。也就是知道目标位置六个轴分别对应的角度信息，要让机器人从起始位置运动到目标位置。
关节空间规划中，六个轴都是一起运动的，虽然各个轴到达目标点的时间是一样的，但是各个轴都是只考虑各自的轨迹，相互之间是没有联系的，所以会导致终端夹手的轨迹是任意的轨迹，这种规划是保证不了终端规划的。

代码部分
运行roslaunch marm_moveit_config demo.launch打开机器人模型，再运行rosrun marm_planning moveit_fk_demo.py 也就是robot_marm\marm_planning\scripts\moveit_fk_demo.py这个路径下的文件，通过代码来使机器人运动起来。
在这里插入图片描述

可以看到机器人从初始位姿运动到目标位姿，这个目标姿态就是程序中指定的在关节空间下的一个目标姿态。

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

class MoveItFkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)#此处出现了roscpp，这是因为moveit的底层都是c++实现对的

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)
 
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
        
        # 初始化需要使用move group控制的机械臂中的gripper group
        gripper = moveit_commander.MoveGroupCommander('gripper')
        
        # 设置机械臂和夹爪的允许误差值
        arm.set_goal_joint_tolerance(0.001)
        gripper.set_goal_joint_tolerance(0.001)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
         
        # 设置夹爪的目标位置，并控制夹爪运动
        gripper.set_joint_value_target([0.01])
        gripper.go()
        rospy.sleep(1)
         
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

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

古月提供的源码都是有注释的，很容易看到，我主要总结一下流程。

着重点：

使用moveit_commander.MoveGroupCommander(),设置规划组，告诉Move_group需要控制机器人的哪个部分。
设置机器人的目标位姿，如果是关节空间就是用set_joint_value_target这个方法，里面的参数是各个关节的电机角度。
arm.go()相当于GUI里的Plan and Execute，也可以将arm.go()拆分开来。
工作空间规划
工作空间的介绍
机器人工作空间的描述对象主要是对机械臂终端的姿态，在工作空间下，描述机器人的姿态，使用的是机器人终端的姿态，数据类型是由六个数据组成，分别是XYZ位置数据和绕XYZ旋转的RPY数据，通过这六个数据可以描述机器人终端的姿态。根据机器人学理论，知道了终端的姿态可以反解出各个轴的位置，也有可能存在多解或者无解的情况。
因为反解的值不一定唯一，所以在工作空间下就存在了一个选解的问题，选解中涉及到最优化的原则，要去选择是用工作最优、效率最优等等限制条件。这些原则可以根据实际情况来做选择。

代码部分
运行roslaunch marm_moveit_config demo.launch打开机器人模型，再运行rosrun marm_planning moveit_ik_demo.py 也就是robot_marm\marm_planning\scripts\moveit_ik_demo.py这个路径下的文件，通过代码来使机器人运动起来。
在这里插入图片描述

可能会碰到规划失败的情况，可以关闭程序重新启动。

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)
        
        # 初始化ROS节点
        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)
        
        # 控制机械臂先回到初始化位置
        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__":
    MoveItIkDemo()

要点：

set_pose_target(target_pose, end_effector_link)设置哪个link要到达目标姿态
将arm.go()分解为arm.plan()和arm.execute()
程序中并没有使用RPY表示旋转，而是选用属于四元数来描述的，这两种都是姿态的的描述方式，两者是可以相互做转化的
arm.shift_pose_target(1, -0.05, end_effector_link) 0,1,2代表XYZ，第二个参数代表尺度
arm.shift_pose_target(3, -1.57, end_effector_link) 3,4,5代表RPY，第二个参数代表尺度
无论是空间规划还是关节规划都是无法保证终端的姿态是我们想要的一种姿态，无法做到沿着直线运动或者圆弧运动。