MoveIt!机械臂控制（三）

一、配置文件

使用Setup Assistant工具生成的全部配置文件都放在了所生成功能包的config文件夹下

1.1SRDF文件

Setup Assistant配置的机械臂参数、夹具参数、规划组、自定义位姿等需要以文件的形式存储，URDF文件中没有存出这些参数的地方，所以产生了一种语义机器人描述格式（Semantic Robot Description Format, SRDF）来存储这些配置参数

1.1.1规划组

    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="arm">
        <chain base_link="base_link" tip_link="grasping_frame" />
    </group>
    <group name="gripper">
        <link name="gripper_finger_link1" />
        <link name="gripper_finger_link2" />
        <joint name="finger_joint1" />
        <joint name="finger_joint2" />
    </group>

1.1.2自定义位姿

    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="home" group="arm">
        <joint name="joint1" value="0" />
        <joint name="joint2" value="0" />
        <joint name="joint3" value="0" />
        <joint name="joint4" value="0" />
        <joint name="joint5" value="0" />
        <joint name="joint6" value="0" />
    </group_state>
    <group_state name="forward" group="arm">
        <joint name="joint1" value="0" />
        <joint name="joint2" value="0.4285" />
        <joint name="joint3" value="0.7672" />
        <joint name="joint4" value="0" />
        <joint name="joint5" value="-0.4487" />
        <joint name="joint6" value="0" />
    </group_state>

1.1.3机械臂终端

    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
    <end_effector name="robot_gripper" parent_link="grasping_frame" group="gripper" />

1.1.4碰撞矩阵

    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="bottom_link" reason="Adjacent" />
    <disable_collisions link1="base_link" link2="link1" reason="Adjacent" />
    <disable_collisions link1="bottom_link" link2="link1" reason="Never" />
    <disable_collisions link1="bottom_link" link2="link2" reason="Never" />
    <disable_collisions link1="gripper_finger_link1" link2="link2" reason="Never" />
    <disable_collisions link1="gripper_finger_link1" link2="link6" reason="Adjacent" />
    <disable_collisions link1="gripper_finger_link2" link2="link2" reason="Never" />
    <disable_collisions link1="gripper_finger_link2" link2="link4" reason="Never" />
    <disable_collisions link1="gripper_finger_link2" link2="link5" reason="Default" />
    <disable_collisions link1="gripper_finger_link2" link2="link6" reason="Adjacent" />
    <disable_collisions link1="link1" link2="link2" reason="Adjacent" />
    <disable_collisions link1="link1" link2="link4" reason="Never" />
    <disable_collisions link1="link1" link2="link5" reason="Never" />
    <disable_collisions link1="link2" link2="link3" reason="Adjacent" />
    <disable_collisions link1="link2" link2="link4" reason="Never" />
    <disable_collisions link1="link2" link2="link5" reason="Never" />
    <disable_collisions link1="link2" link2="link6" reason="Never" />
    <disable_collisions link1="link3" link2="link4" reason="Adjacent" />
    <disable_collisions link1="link4" link2="link5" reason="Adjacent" />
    <disable_collisions link1="link5" link2="link6" reason="Adjacent" />

1.2fake_controllers.yaml

机械臂可以按照MoveIt!规划出来的轨迹运动。从轨迹到模型，中间需要一个控制其进行连接，这些虚拟的控制器参数在fake_controllers.yaml文件中配置如下：

controller_list:
  - name: fake_arm_controller
    type: $(arg execution_type)
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
  - name: fake_gripper_controller
    type: $(arg execution_type)
    joints:
      - finger_joint1
initial:  # Define initial robot poses.
  - group: arm
    pose: home

1.3joint_limits.yaml

joint_limits.yaml文件用于设置机器人运动关节的速度限制，可以配置每个关节是否有速度、加速度限制，以及最大速度和最大加速度的数值

# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
  finger_joint1:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint1:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint2:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint3:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint4:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint5:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint6:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0

如果在运动规划过程中机器人运动的速度较慢，可以修改该文件中的速度限位值，但要重新启动move_group节点来完成

1.4kinematics.yaml

MoveIt!中可以提供多种运动学求解器，相应的配置参数存储在kinematics.yaml文件中，该配置文件在启动move_group.launch文件时由包含的planning_context.launch文件加载

arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05

• kinematics_solver：设置运动学插件，MoveIt!默认使用KDL插件
• kinematics_solver_search_resolution：设置存在冗余维度逆向运动学求解时的分辨率
• kinematics_solver_timeout：逆向运动学求解超市设置，单位为秒
• kinematics_solver_attempts：逆向运动学求解尝试次数

1.5ompl_planning.yaml

OMPL是MoveIt!中默认使用的运动规划库，ompl_planning.yaml文件中存储了运动规划的相关配置

二、添加ArbotiX关节控制器

MoveIt!默认生成的demo中所使用的控制器功能有限，可以使用其他控制器插件实现驱动机器人模型的功能

ArbotiX功能包提供了Joint Trajectory Action Controllers插件，可以用来驱动真实机器人的每个舵机关节，实现旋转运动

由于ArbotiX提供离线模式的支持，所以也可以使用该插件实现对仿真机器人的控制

2.1添加配置文件

在marm_description/config/arm.yaml文件中配置以下条目：

joints: {
    joint1: {id: 1, neutral: 205, max_angle: 169.6, min_angle: -169.6, max_speed: 90},
    joint2: {id: 2, max_angle: 134.6, min_angle: -134.6, max_speed: 90},
    joint3: {id: 3, max_angle: 150.1, min_angle: -150.1, max_speed: 90},
    joint4: {id: 4, max_angle: 150.1, min_angle: -150.1, max_speed: 90},
    joint5: {id: 5, max_angle: 150.1, min_angle: -150.1, max_speed: 90},
    joint6: {id: 6, max_angle: 360, min_angle: -360, max_speed: 90},
    finger_joint1: {id: 7, max_speed: 90},
}
controllers: {
    arm_controller: {type: follow_controller, joints: [joint1, joint2, joint3, joint4, joint5, joint6], action_name: arm_controller/follow_joint_trajectory, onboard: False }
}

arm.yaml文件中主要包含以下两个部分：

• 机器人关节属性的设置：包括每个驱动关节的最大最小角度、最大速度等等
• 控制器插件的设置：包含机器人六轴本体的控制类型、关节，以及所接收的action消息名称等等

2.2运行ArbotiX节点

运行ArbotiX节点，分别控制机器人的六轴本体和终端夹爪。机器人的启动使用fake_arm.launch文件描述，ArbotiX节点部分的代码如下：

    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find marm_description)/config/arm.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

    <node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller">
        <rosparam>
            model: singlesided
            invert: false
            center: 0.0
            pad_width: 0.004
            finger_length: 0.08
            min_opening: 0.0
            max_opening: 0.06
            joint: finger_joint1
        </rosparam>
    </node>

• 第一个ArbotiX节点会加载上一步中创建的配置文件，并且启动一个控制机器人六轴本体的控制器
• 第二个ArbotiX节点启动了控制终端夹爪的gripper_controller，同时需要配置一些相关参数

2.3测试历程

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import actionlib

from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

class TrajectoryDemo():
    def __init__(self):
        rospy.init_node('trajectory_demo')
        
        # 是否需要回到初始化的位置
        reset = rospy.get_param('~reset', False)
        
        # 机械臂中joint的命名
        arm_joints = ['joint1',
                      'joint2',
                      'joint3', 
                      'joint4',
                      'joint5',
                      'joint6']
        
        if reset:
            # 如果需要回到初始化位置，需要将目标位置设置为初始化位置的六轴角度
            arm_goal  = [0, 0, 0, 0, 0, 0]

        else:
            # 如果不需要回初始化位置，则设置目标位置的六轴角度
            arm_goal  = [-0.3, -1.0, 0.5, 0.8, 1.0, -0.7]
    
        # 连接机械臂轨迹规划的trajectory action server
        rospy.loginfo('Waiting for arm trajectory controller...')       
        arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        arm_client.wait_for_server()        
        rospy.loginfo('...connected.')  
    
        # 使用设置的目标位置创建一条轨迹数据
        arm_trajectory = JointTrajectory()
        arm_trajectory.joint_names = arm_joints
        arm_trajectory.points.append(JointTrajectoryPoint())
        arm_trajectory.points[0].positions = arm_goal
        arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
        arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
        arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
    
        rospy.loginfo('Moving the arm to goal position...')
        
        # 创建一个轨迹目标的空对象
        arm_goal = FollowJointTrajectoryGoal()
        
        # 将之前创建好的轨迹数据加入轨迹目标对象中
        arm_goal.trajectory = arm_trajectory
        
        # 设置执行时间的允许误差值
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    
        # 将轨迹目标发送到action server进行处理，实现机械臂的运动控制
        arm_client.send_goal(arm_goal)

        # 等待机械臂运动结束
        arm_client.wait_for_result(rospy.Duration(5.0))
        
        rospy.loginfo('...done')
        
if __name__ == '__main__':
    try:
        TrajectoryDemo()
    except rospy.ROSInterruptException:
        pass

2.4运行效果

首先运行marm_description/launch/fake_arm.launch文件，启动机器人模型、ArbotiX控制器以及RViz

$ roslaunch marm_description fake_arm.launch

启动成功后可以在界面中看到处于初始状态下的机械臂，在终端中可以看到ArbotiX启动成功的提示

之后运行历程

$ rosrun marm_planning trajectory_demo.py _reset:=False

机器人开始平滑运动，到达指定位姿后停止，如下所示

如果想让机器人回到初始位姿，可以使用如下命令：

$ rosrun marm_planning trajectory_demo.py _reset:=True

如下所示：

三、配置MoveIt!关节

配置MoveIt!关节的作用就是让系统能够通过MoveIt!实现运动规划，并且将规划结果通过FollowJointTrajectoryAction发送给机器人的ArbotiX关节控制器，整个系统才能称得上完整

所以在MoveIt!上层规划和ArbotiX关节控制器之间需要一个将两者结合的接口，这个接口在MoveIt!中同样以插件的形式提供，称为moveit_simple_controller_manager

除此之外，该机械臂还配备了一个两指夹爪，通过ArbotiX的gripper_controller驱动，使用的action类型是GripperCommandAction，在moveit_simple_controller_manager中也有同样的接口

3.1添加配置文件

MoveIt!所有的配置文件都放在了config功能包的config文件夹下，控制器的配置文件卸载了该文件夹下的controllers.yaml文件中

controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
      
  - name: gripper_controller
    action_ns: gripper_action
    type: GripperCommand
    default: true
    joints:
      - finger_joint1

controllers.yaml中列出了机器人每个规划组所需要的控制器插件以及这些插件具体的配置参数：

• name：控制器插件的名字
• action_ns：控制器发布action信息的命名空间
• type：实现action的类型
• default：是否为该规划组的默认控制器插件
• joint：该规划组所包含的关节

注意：name和action_ns组成的控制器action接口的信息名必须要与ArbotiX控制器插件一致，整个通信才能形成通路

3.2启动插件

启动moveit_simple_controller_manager插件，并同时加载controllers.yaml文件

在使用Setup Assistant生成的配置文件中，已经生成启动MoveIt!控制器插件的launch文件，在配置功能包的launch文件下命名为RobotName_moveit_controller_manager.launch.xml

<launch>

  <!-- loads moveit_controller_manager on the parameter server which is taken as argument
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

  <!--  load controller_list  -->
  <!--  Arbotix  -->
  <rosparam file="$(find marm_moveit_config)/config/controllers.yaml"/>
  <!--  Gazebo  -->
  <!--  <rosparam file="$(find marm_moveit_config)/config/controllers_gazebo.yaml"/>  -->
  <!-- loads ros_controllers to the param server -->
  <!-- <rosparam file="$(find marm_moveit_config)/config/ros_controllers.yaml"/> -->
</launch>

到目前为止，已经自底向上创建了机械臂模型，完成了模型在MoveIt!中的配置，添加了控制器插件，为下一步move_group的添加做好了准备