在ROS仿真中使用Python程序控制UR机械臂运动

本文参考：手写ROS程序控制ur5机械臂运动（Python）_python控制ur5_孟德尔的猫的博客-CSDN博客

本文使用的环境：

Linux版本：Ubantu20.04 

ros版本：ros-noetic-desktop-full，安装此版本ros无需再安装moveit运动规划库

编译软件：vscode

1  创建工作空间并安装ros包

mkdir -p ~/ur_ws/src
cd ~/ur_ws/src
git clone https://github.com/ros-industrial/universal_robot
cd ..
catkin_make

2  运行相关launch文件

在下载的包中，在universal_robot/ur_gazebo/launch路径下有一个ur5_bringup.launch文件

roslaunch ur_gazebo ur5_bringup.launch

可以在Gazebo中看到有一个ur机械臂

 新打开一个终端输入rostopic list查看话题

rostopic list

 其中 /eff_joint_traj_controller/follow_joint_trajectory 是控制 ur 运动的话题，很明显，该话题使用的是action通信，有关 action 通信的知识可以看一下赵虚左老师讲的 ros 教程【Autolabor初级教程】ROS机器人入门_哔哩哔哩_bilibili

终端输入命令

rostopic type /eff_joint_traj_controller/follow_joint_trajectory/goal 

可以看到该话题的类型是 control_msgs/FollowJointTrajectoryActionGoal

再使用命令

rosmsg show control_msgs/FollowJointTrajectoryActionGoal

 可以看到红框中的就是我们需要关心的

3  编写Python文件

进入工作空间打开vscode

cd ur_ws/
code .

新建功能包，并添加依赖项：rospy  std_msgs actionlib

在新建功能包下添加问价夹 scripts，并在该文件夹添加demo01.py文件，可以参考赵虚左老师的ros教程，Python代码如下

#! /usr/bin/env python
from trajectory_msgs.msg import *
from control_msgs.msg import *
import rospy
import actionlib
from sensor_msgs.msg import JointState
 
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
               'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
 
def move():
          #goal就是我们向发送的关节运动数据，实例化为FollowJointTrajectoryGoal()类
          goal = FollowJointTrajectoryGoal()
 
          #goal当中的trajectory就是我们要操作的，其余的Header之类的不用管
          goal.trajectory = JointTrajectory()
          #goal.trajectory底下一共还有两个成员，分别是joint_names和points，先给joint_names赋值
          goal.trajectory.joint_names = JOINT_NAMES
 
          #从joint_state话题上获取当前的关节角度值，因为后续要移动关节时第一个值要为当前的角度值
          joint_states = rospy.wait_for_message("joint_states",JointState)
          joints_pos = joint_states.position
 
          #给trajectory中的第二个成员points赋值
          #points中有四个变量，positions,velocities,accelerations,effort，我们给前三个中的全部或者其中一两个赋值就行了
          goal.trajectory.points=[0]*4
          goal.trajectory.points[0]=JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6,time_from_start=rospy.Duration(0.0))
          goal.trajectory.points[1]=JointTrajectoryPoint(positions=[0.1,0,-0.2,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(1.0))
          goal.trajectory.points[2]=JointTrajectoryPoint(positions=[2,0,-1,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(2.0))
          goal.trajectory.points[3]=JointTrajectoryPoint(positions=[2.57,0,-1.57,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(3.0))
          
          #发布goal，注意这里的client还没有实例化，ros节点也没有初始化，我们在后面的程序中进行如上操作
          client.send_goal(goal)
          client.wait_for_result()
 
def pub_test():
          global client
 
          #初始化ros节点
          rospy.init_node("pub_action_test")
 
          #实例化一个action的类，命名为client，与上述client对应，话题为/eff_joint_traj_controller/follow_joint_trajectory，消息类型为FollowJointTrajectoryAction
          client = actionlib.SimpleActionClient('/eff_joint_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
          print("Waiting for server...")
          #等待server
          client.wait_for_server()
          print("Connect to server......")
          #执行move函数，发布action
          move()
 
if __name__ == "__main__":
    count = 0
    while not rospy.is_shutdown():
        count += 1
        pub_test()
        rospy.loginfo("发布次数:%d",count)

给demo01.py 文件添加可执行权限

 在新建功能包下的 CMakeList.txt 中配置 demo01.py文件

 配置完后编译文件

4   终端测试

编译成功后运行demo01.py文件

source ./devel/setup.bash
rosrun control_p demo01.py

这时候就可以看到Gazebo中的ur机械臂动起来了

 运行rviz

roslaunch ur5_moveit_config moveit_rviz.launch config:=true

勾选末端轨迹 Show Trail ，就可以看到rviz中的ur机械臂运动轨迹了