首页 > 其他分享> > ROS笔记（31） ArbotiX关节控制器

ROS笔记（31） ArbotiX关节控制器

2019-08-24 10:08:05 作者：互联网

ROS笔记（31） ArbotiX关节控制器

1. 关节控制器
2. 添加配置文件
3. 运行ArbotiX节点
4. action
5. 添加测试文件
6. 启动测试

1. 关节控制器

Movelt!默认生成的demo中所使用的 fake_controllers.yaml 控制器功能有限
一般需要使用其他控制器插件实现驱动机器人模型的功能

Arbotix功能包中提供了Joint Trajectory Action Controllers插件
可以用来驱动真实机器人的每个舵机关节，实现旋转运动
由于ArbotiX提供离线模式的支持
所以也可以使用该插件实现驱动仿真机器人的每个舵机关节，实现旋转运动

2. 添加配置文件

首先要创建一个 hharm_description/config/arm.yaml 配置文件

joints: {
    joint1: {id: 1, neutral: 205, max_angle: 175.1, min_angle: -175.1, max_speed: 90},
    joint2: {id: 2, max_angle: 135.1, min_angle: -135.1, 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 }
}

配置文件主要分成以下两个部分：

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

3. 运行ArbotiX节点

创建一个 hharm_description/launch/fake_hharm.launch 启动文件
启动Arbotix中的节点，分别控制机器人的六轴本体和终端夹爪

<launch>

    <!-- 不使用仿真时间 -->
    <param name="/use_sim_time" value="false" />

    <!-- 启动arbotix driver-->
    <arg name="sim" default="true" />
    
    <!-- 模型地址 -->
    <arg name="model" default="$(find xacro)/xacro --inorder '$(find hharm_description)/urdf/hharm.xacro'" />

    <!-- 加载模型 -->
    <param name="robot_description" command="$(arg model)" />

    <!-- 运行arm_controller节点 -->
    <node name="arm_controller" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find hharm_description)/config/arm.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

    <!-- 运行gripper_controller节点 -->
    <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>

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 
    <!-- 运行rviz可视化界面 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hharm_description)/rviz/urdf.rviz" required="true" />

</launch>

第一个ArbotiX节点会加载上一步中创建的配置文件
并且启动一个控制机器六轴本体的arm_controller 控制器
Arbotix还提供了一个gripper_controller 夹爪控制器，可以支持控制一个或多个舵机组成的终端夹爪

HHArm夹爪只有一个可动关节，虽然是直线运动
但依然可以使用ArbotixX中的gripper_controller进行控制
且在输入数据上需要将直线运动的长度近似转换成角度旋转
因此，第二个Arbotix节点需要配置一些相关参数

4. action

暂时不理会Movelt!的那些上层规划，先直接给一个特定的目标弧度arm_goal 测试控制器是否正常

在之前的配置中可以看到，无论是机器人的控制还是夹爪的控制，使用的通信机制都是 action

action 用于定义任务，本质是消息
任务定义包括目标(Goal)、任务执行过程状态反馈 (Feedback)和结果(Result)等

action 是一种与平台、编程语言无关的接口定义语言

这里的“任务”指的就是用户自定义的机器人任务
如智能机器人自主识别及抓物体、机器人依据规划路径自主行走等，具体任务的程序由用户来实现

actionlib接口只是提供了相应的调度接口
在actionlib中，客户端发送请求到服务器端，服务器端根据相应的任务调度策略选行任务调度，被选择的任务将会抢占当前任务的执行权
同时，客户端还可以发送取请求，服务器端依照一定的原则选择取消一个或多个任务的执行

actionlib接口中提供了Simple调度策略，具体策略为：
服务器端一次只选择个任务进行调度执行，后来的任务总是抢占当前任务的执行，不管当前任务是否执完毕

5. 添加测试文件

所以可以在hharm_planning功能包编写一个trajectory_demo.py文件
通过发布需要的action请求来测试以上配置是否成功

action需要客户端向服务端发起请求，两者建立连接之后才能发送具体数据
因此，必须使用actionlib.SimpleActionClient创建一个客户端 arm_client
声明action的消息名为arm_controller/follow_joint_trajectory，类型为FollowJointTrajectoryAction

然后通过arm_client.wait_for_server 等待与服务端创建连接

连接成功后就可以通过 arm_client.send_goal 将action的运动目标 arm_goal 发送到服务端action server进行处理

import rospy
import actionlib

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

class TrajectoryDemo():
    def __init__(self):

        # 声明节点为trajectory_demo
        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  = [-1.57, -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

运动目标arm_goal使用了一个预先设置好的自定义位姿
如果命令行输人的参数reset为 true，则目标位姿是六轴全为0的初始位姿
如果参数reset为 false，则目标位姿的六轴弧度是 [-1.57, -1.0, 0.5, 0.8, -1.0, -0.7]
将目标点发送出去后，就可以等待服务端的控制结果，这里设置超时等待时间为5s

6. 启动测试

现在可以测试ArbotiX控制器的效果了
首先运行 fake_hharm.launch 启动文件启动机器模型、ArbotixX控制器以及rviz

$ roslaunch hharm_description fake_hharm.launch

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

在这里插入图片描述
在终端中可以看到ArbotiX启动成功的提示

在这里插入图片描述
运行测试例程

$ rosrun hharm_planning trajectory_demo.py _reset:=False

机械臂已经开始平滑运动，到达指定位姿后停止

在这里插入图片描述

如果想让机械臂回到初始位姿

$ rosrun hharm_planning trajectory_demo.py _reset:=Ture

机械臂已经开始平滑运动，到达初始位姿后停止

在这里插入图片描述
在控制器接收到了对应关节的目标弧度后，ArbotiX经过一系列的处理，让关节旋转对应的弧度

在驱动机械臂时，为了方便理解，在新的终端使用rqt_graph查看当前系统运行情况的动态图形

$ rqt_graph

在这里插入图片描述
在控制过程中
trajectory_demo节点通过发送目标位姿/arm_controller/follow_joint_trajectory/goal到arm_controller节点
arm_controller节点通过发送反馈位姿/arm_controller/follow_joint_trajectory/feedback到trajectory_demo节点

可见这种方法类似于在rviz中搭建了一个机械臂的仿真环境，通过代码实现对机械模型的控制

参考：

ROS官方wiki
古月居