六自由度机械臂ROS+Rviz+Arbotix控制器仿真

写在前面

在上一篇六自由度机械臂SolidWorks模型转化成urdf文件，添加简单gazebo属性并修改为xacro中，模型建立的顺序是正确的，也能够在rviz中显示，但是加载arbotix配置后出现如下错误，模型的tf树始终都是断的，网上提到的方法几乎尝试遍了也没有解决这个问题，纠结了好几天，最终决定从模型开始重新入手，自己建模丰衣足食。

1.Arbotix

Arbotix是一款控制电机、舵机的控制板，并提供相应的ros功能包，但是这个功能包的功能不仅可以驱动真实的Arbotix控制板，它还提供一个差速控制器，通过接受速度控制指令更新机器人的joint状态，从而帮助我们实现机器人在rviz中的运动。

这个差速控制器在arbotix_python程序包中，完整的arbotix程序包还包括多种控制器，分别对应dynamixel电机、多关节机械臂以及不同形状的夹持器。本文用到如下控制器：

• 关节轨迹控制器FollowController
• 单伺服电机并行夹持器parallel_single_servo_controller

2.Arbotix配置文件

arbotix配置文件格式为.yaml，通常放在模型包的config文件夹下。模拟仿真比较简单，一个launch文件+arbotix配置文件即可。该配置文件与控制器相关，定义控制器名称、作用的关节（关节一定要写全，不然会出现tf变换错误）以及位置速度参数等，具体可见本文代码。参考phantomx_pincher_arm的配置代码，arm部分的配置文件为myrobot_arm.yaml，如下：

source: myrobot_arm.yaml
baud: 1000000   #与实际有关，虚拟仿真时可以不用管
rate: 100	#与实际有关，虚拟仿真时可以不用管
port: /dev/ttyUSB0
read_rate: 15       #与实际有关，虚拟仿真时可以不用管
write_rate: 25      #与实际有关，虚拟仿真时可以不用管
joints: {
    joint1: {id: 1, max_angle: 135, min_angle: -135, max_speed: 90},
    joint2: {id: 2, max_angle: 135, min_angle: -135, max_speed: 90},
    joint3: {id: 3, max_angle: 135, min_angle: -135, max_speed: 90},
    joint4: {id: 4, max_angle: 135, min_angle: -135, max_speed: 90},
    joint5: {id: 5, max_angle: 135, min_angle: -135, max_speed: 90},
    finger_joint1: {id: 6, max_speed: 90},
}
controllers: {
    arm_controller: {type: follow_controller, joints: [joint1, joint2, joint3, joint4, joint5], action_name: arm_controller/follow_joint_trajectory, onboard: False }
}

简单说一下第一部分joints每个参数的含义：

• id 电机的硬件编号，按顺序写
• neutral（默认512）：上面代码没有，主要作用是当发布关节状态时，neutral值由arbotix驱动映射为0弧度。实际操作中怎么用我现在还不清楚，后面遇到了再解决
• max_angle、min_angle 关节电机转动角度的范围
• max_speed 电机的最大允许速度

第二部分controllers是配置文件的控制器部分，这里arm只需配置一个控制器：机械臂关节的轨迹运动控制器follow_controller，涉及参数如下：

• onboard（默认：true）在使用真正的arbotix控制板时改为true
• action_name 定义了关节轨迹控制器的命名空间，指定arbotix节点接受的消息名称
• joint 该控制器管理的一系列关节，顺序要与上一部分中相同
• type 控制器的种类

gripper部分的配置文件为myrobot_gripper.yaml，代码如下。

source: myrobot_gripper.yaml
model: parallel
invert: false
pad_width: 0.005
finger_length: 0.08
min_opening: 0.0
max_opening: 0.06
joint: finger_joint1

• model 夹持器的种类，总共有三种，单边单伺服模型（pi机器人）、双边双伺服模型（MaxWell）、单伺服并行模型（PhantomX），本文选择单伺服并行模型parallel
• pad_width、finger_length 单位为米，根据具体模型指定finger的宽度和长度
• min_opening、max_opening 单位为米，根据具体模型指定夹持器的最大、最小开合距离
• joint 关节名必须匹配urdf模型文件中的夹持器关节名
• invert 如果发现关节以一个跟urdf模型中规定方向的反方向运动，则可将invert参数设置为true

需要特别注意的是，yaml格式比较严格，不能解析tab，要用空格来代替。写好之后可以利用这个地址格式检查来检查一下yaml是否有格式错误。

3.launch启动文件

<?xml version="1.0"?>
<launch>
	<param name="/use_sim_time" value="false"/>
    <arg name="simulation" default="true"/>
	<arg name="port" default="dev/ttyUSB0"/>

  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find myrobot_description_ultimate)/urdf/myrobot.urdf.xacro'"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <param name="publish_frequency" type="double" value="20.0"/> 
  </node>
  
<!--启动arm控制器，arbotix_python程序包里的arbotix_driver节点 -->
  <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
	<!-- 加载mrobot_arm.yaml配置文件 -->
    <rosparam file="$(find myrobot_description_ultimate)/config/myrobot_arm.yaml" command="load"/>
    <param name="sim" value="$(arg simulation)"/>
    <param name="port" value="$(arg port)"/>
  </node>
<!-- 启动gripper控制器，arbotix-controllers程序包中包含了gripper_controller节点 -->
  <node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller" output="screen">
  	<!-- 加载mrobot_gripper.yaml配置文件 -->
    <rosparam file="$(find myrobot_description_ultimate)/config/myrobot_gripper.yaml" command="load" />
    <param name="port" value="$(arg port)"/>
	<param name="sim" value="$(arg simulation)"/>
  </node>

<!-- Show in Rviz -->
  <node name="rviz" pkg="rviz" type="rviz" />
</launch>

利用arbotix_gui测试

运行命令：

$ cd ~/catkin_ws && catkin_make
$ source ./devel/setup.bash
$ runlaunch myrobot_description fake_myrobot.launch

屏幕上会出现如下输出：

process[robot_state_publisher-1]: started with pid [25867]
process[arbotix-2]: started with pid [25868]
process[gripper_controller-3]: started with pid [25869]
process[rviz-4]: started with pid [25885]
shutdown request: new node registered with same name
[INFO] [1556967484.301683]: ArbotiX being simulated.
shutdown request: new node registered with same name
[INFO] [1556967484.404963]: Started FollowController (arm_controller). Joints: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5'] on C1

可以看到gripper_controller、虚拟模式下的arbotix控制器启动了，还启动了FollowController（arm_controller）。
输入命令arbotix_gui，调节各关节滚动条可以看到对应的机械臂关节在rviz中运动。

启动后，运行起来的节点图

利用trajectory_demo.py例程测试arbotix关节轨迹动作控制器

参考例程古月大神著作源码
创建这个例程的主要目的在于方便。前面提到arbotix中的follow_controller，这是机械臂关节的虚拟控制器，它能够实现一个joint trajectory action server，并且通过FollowJointTrajectoryGoal消息来响应动作目标，这种消息对于手工在命令行输入而言太长太复杂，因此利用trajectory_demo这个python脚本代替，代码如下：

#!/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的命名，要与urdf中描述的一致
        arm_joints = ['joint1',
                      'joint2',
                      'joint3', 
                      'joint4',
                      'joint5',
                      ]
        # 下面关节位置的顺序需要与上面定义的关节名一一对应，如果在命令行将reset参数设为true，目标位置就会回到空挡位置；如果为false，则会到设定的foward位姿
        if reset:
            # 如果需要回到初始化位置，需要将目标位置设置为初始化位置的六轴角度
            arm_goal  = [0, 0, 0, 0, 0]

        else:
            # 如果不需要回初始化位置，则设置目标位置的六轴角度
            # 设定位姿为forward
            arm_goal  = [-9.87189668696e-05, 0.804022496599, 0.88171765075, 0.700188077266, 9.77076938841e-05]
    
        # 连接机械臂轨迹规划的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

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

control_msgs包含用于控制机器人的基本消息（message）和动作（action），提供了控制器设定点、关节以及笛卡尔轨迹的表示；
trajectory_msgs定义了用于定义机器人轨迹的消息（message）。
首先需要导入一些和轨迹相关的message和action，比如FollowJointTrajectoryAction动作和FollowJointTrajectoryGoal消息。另外，这些轨迹信息是由关节位置、速度、加速度和受到的作用力进行定义的，因此还需要导入JointTrajectory和JointTrajectoryPoint消息。

 arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)

创建一个简单的动作客户端action client，将其连接到机械臂的joint trajectory action server上，声明action的消息名为arm_controller/follow_joint_trajectory，这是用前面提到arbotix配置文件的action_name参数，类型为FollowJointTrajectoryAction。

        arm_trajectory = JointTrajectory() #创建一个空的JointTrajectory()对象
        arm_trajectory.joint_names = arm_joints #将上面定义的arm_joints变量存入关节名称中
        # 追加一个空的JointTrajectoryPoint，序号为0，之后会在里面填入目标位姿
        arm_trajectory.points.append(JointTrajectoryPoint()) 
        arm_trajectory.points[0].positions = arm_goal #填入目标位置
        # 机械臂运动到目标位置后，各关节速度和加速度应均为0
        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) #规定整条轨迹在3s内运行完

之前arm_goal变量中保存了机械臂各关节的目标位姿，将该变量赋给arm_trajectory.point[0].positions。创建完单点目标轨迹arm_trajectory发送给动作服务器joint trajectory action server。

首先启动上文创建的fake_myrobot.launch，然后运行测试例程

$ roslaunch myrobot_description fake_myrobot.launch
$ rosrun myrobot_planning trajectory_demo.py _reset:=False

可以在rviz界面看到机械臂开始平滑运动，到达目标位置后停止。
可以查看话题列表如下