最近想用ros来搭建ur机械臂的双臂平台，想先在gazebo中测试一下如何用自己手写的程序来控制ur机械臂运动，但网上的教程都局限在用moveit控制gazebo仿真环境下的ur运动，考虑到后期肯定会自己进行轨迹算法规划，用moveit的话想要更换算法太麻烦，于是干脆自己手写程序，不通过moveit来控制机械臂。

Let's do it!!!

首先，下载好ur机械臂仿真所需要的包

依次运行如下命令：

mkdir -p ~/你的工作空间名/src
cd ~/你的工作空间名/src
git clone https://github.com/ros-industrial/universal_robot
cd ..
catkin_make

将ur官方提供的包放入工作空间中。

打开ur_gazebo/launch发现里面的ur5.launch文件就是ur5的gazebo仿真环境。

新开一个终端，运行这个launch文件：

roslaunch ur_gazebo ur5.launch

运行后，会出现gazebo仿真界面，ur5机械臂就在其中了

使用rostopic list命令查看话题：

其中，arm_controller/follow_joint_trajectory就是控制ur运动的话题，显然，这是action通信。

使用rostopic type arm_controller/follow_joint_trajectory/goal 命令找出该话题的消息类型为control_msgs/FollowJointTrajectoryActiongoal

使用rosmsg show control_msgs/FollowJointTrajectoryActiongoal命令找出该消息具体包含的数据类型如下：

中间如下几个就是我们要关心的

 

正式开干！！！

新建一个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.5,0,-0.5,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(1.0))
          goal.trajectory.points[2]=JointTrajectoryPoint(positions=[1,0,-1,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(2.0))
          goal.trajectory.points[3]=JointTrajectoryPoint(positions=[1.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对应，话题为arm_controller/follow_joint_trajectory，消息类型为FollowJointTrajectoryAction
          client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
          print("Waiting for server...")
          #等待server
          client.wait_for_server()
          print("Connect to server")
          #执行move函数，发布action
          move()

if __name__ == "__main__":
          pub_test()

然后直接使用python+文件名运行该脚本就搞定了，可以看到gazebo中的机械臂运动起来了。