添加arbotix关节控制器

添加arbotix关节控制器

以下内容，注意文件的路径

1.新建一个文件arm.yaml。arm_moveit_config/config/arm.yaml

joints: {
    joint1: {id: 1, max_angle: 180, min_angle: -180, max_speed: 90},
    joint2: {id: 2, max_angle: 134.6, min_angle: -108.9, max_speed: 90},
    joint3: {id: 3, max_angle: 86, min_angle: -166, max_speed: 90},
    joint4: {id: 4, max_angle: 180, min_angle: -180, max_speed: 90},
    joint5: {id: 5, max_angle: 237.2, min_angle: -68.8, max_speed: 90},
    joint6: {id: 6, max_angle: 180, min_angle: -180, 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 }
}

2.继续新建一个文件fake_arm.launch。
arm_moveit_config/launch/fake_arm.launch

<!-- 配置moveit关节控制器  p322    1   -->
<launch>
    <!-- 不使用仿真时间 -->
    <param name="/use_sim_time" value="false" />


    <param name="robot_description" textfile="$(find arm)/urdf/arm.urdf" />
    <!-- 启动arbotix driver-->
    <arg name="sim" default="true" />
    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find  arm_moveit_config)/config/arm.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />
    <!-- 启动rviz -->
    <!-- 第一次打开rviz使用，需要配置，保存一下就可以用下面这个 -->
    <node name="rviz" pkg="rviz" type="rviz" args="rviz"  />
    <!-- 配置保存后，把上面的注释，下面这个注释打开 -->
    <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find arm_moveit_config)/urdf/fake_arm_urdf.rviz" required="true" /> -->

</launch>

3.继续新建一个文件夹urdf。
arm_moveit_config/urdf
一会打开rviz配置信息，保存在这里。

4.继续新建一个文件夹scripts。
arm_moveit_config/scripts 。
在scripts文件夹下面新建文件 trajectory_demo.py
arm_moveit_config/scripts/trajectory_demo.py

#!/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.5, 0.8, 1, 1.2,-2,-0.9] 
        # 连接机械臂轨迹规划的trajectory action server
        rospy.loginfo('等待手臂轨迹控制器...')       
        arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        arm_client.wait_for_server()        
        rospy.loginfo('联系中...')  
    
        # 使用设置的目标位置创建一条轨迹数据
        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('将手臂移动到目标位置 ....')
        
        # 创建一个轨迹目标的空对象
        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('....完成')
        
if __name__ == '__main__':
    try:
        TrajectoryDemo()
    except rospy.ROSInterruptException:
        pass
    

5. 运行
   

机器人在RVIZ中显示

roslaunch arm_moveit_config fake_arm.launch 

启动rviz没有显示，需要配置一下


模型就能正常显示了按“ctrl+shift+s”保存一下我们刚才的设置，选择刚才新建的urdf文件下

fake_arm_urdf.rviz

把原来的rviz注释，打开下面的注释。

机器人开始移动到目标位置

rosrun arm_moveit_config trajectory_demo.py  _reset:=False

机器人从目标位置回到原始位置

rosrun arm_moveit_config trajectory_demo.py  _reset:=True

[rosrun] Couldn't find executable named trajectory_demo.py below /home/hh/ws_moveit/src/mechanical_arm/arm_moveit_config
[rosrun] Found the following, but they're either not files,
[rosrun] or not executable:
[rosrun]   /home/hh/ws_moveit/src/mechanical_arm/arm_moveit_config/scripts/trajectory_demo.py

在文件的上一级目录下打开终端

 chmod +x trajectory_demo.py