#!/usr/bin/env python
import rospy
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

首先，我们需要导入一些和轨迹相关的消息和动作类型。我们已知FollowJointTrajectoryAction动作和FollowJointTrajectoryGoal消息。这些对象的关键组成部分是用于关节位置、速度、加速度和受到的作用力来定义关节轨迹。因此我们还要导入JointTrajectory和JointTrajectoryPoint两种消息类型。

# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
        
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)

 

reset参数允许我们将机械臂或其他规划组移回到其起始位置。sync(synchronize)参数确定我们时同时运行多个规划组的轨迹还是先后运行。我这里只有一个规划组。

# Which joints define the arm?
arm_joints = ['shoulder_pan_joint',
              'shoulder_lift_joint',
              'elbow_joint',
              'wrist_1_joint',
              'wrist_2_joint',
              'wrist_3_joint']   

这里包含你将要规划的规划组中包含的关节名称。在之后指定轨迹目标时会用到。

if reset:
    # Set the arm back to the resting position
    arm_goal  = [0, 0, 0, 0, 0, 0]

else:
    # Set a goal configuration for the arm
    arm_goal  = [-0.3, -2.0, -1.0, 0.8, 1.0, -0.7]

我们使用关节位置来定义目标位置。关节位置的顺序需要和你之前定义的关节名一一对应。如果命令行里面参数reset设置为true，则目标位置就会被设置回空档位置。

# xxx_moveit_config controllers.yaml
arm_client = actionlib.SimpleActionClient('six_manipulator_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        
arm_client.wait_for_server()

 创建一个简单的动作客户端，它会连接到关节轨迹动作服务器。这个控制器的命名空间是用action_name参数定义在arbotix控制器的配置文件中。我的文件位置为（myur_dynamixels/config/arbotix/myur_arbotix.yaml），内容如下图所示。

# Create a single-point arm trajectory with the arm_goal as the end-point
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)

实际上，仅用一个点（关节配置）来创建机械臂的轨迹目标，也就是之前在arm_goal 变量里保存的目标位置。换言之，是用轨迹的结束点来定义这段轨迹。可以这么做的原因时，轨迹服务器会在开始配置和我们发送给它的目标配置之间进行插值，添加过度状态的关节配置。

上面创建了一个空的JointTrajectory对象。把机械臂关节名称一一列举并存放在arm_joints变量里，之后填入这条轨迹的关节名称内。由于一个关节轨迹包含一个轨迹点的序列，所以我们创建一个空的JointTrajectoryPoint，之后往里面太难如目标配置。

我们将目标配置设置为我们存放在轨迹点列表里序号为0的位置，设置此时每个关节的速度和加速度为0，因为这是轨迹的终点，机械臂会停下来。

time_from_start设置为3.0，意思是轨迹在开始动作后的0秒时经过这个点，由于这是轨迹的端点，意味着我们希望整条轨迹大约用3秒时间执行完。

# Create an empty trajectory goal
 arm_goal = FollowJointTrajectoryGoal()
        
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
        
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    
# Send the goal to the action server
arm_client.send_goal(arm_goal)
        
if not sync:
     # Wait for up to 5 seconds for the motion to complete 
     arm_client.wait_for_result(rospy.Duration(5.0))

我们创建一个FollowJointTrajectoryGoal消息。然后设置目标的轨迹为刚才创建的机械臂轨迹，如果声明要轨迹按时完成，则设置goal.time.tolerance为0，如果机械臂到达目标位置晚一些也可以接受，可以增加限度。

使用arm_client.send_goal()方法发送实际的轨迹目标到动作服务器。

 下面是完整的代码：

#!/usr/bin/env python

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')
        
        # Set to True to move back to the starting configurations
        reset = rospy.get_param('~reset', False)
        
        # Set to False to wait for arm to finish before moving head
        sync = rospy.get_param('~sync', True)
        
        # Which joints define the arm?
        arm_joints = ['shoulder_pan_joint',
                      'shoulder_lift_joint',
                      'elbow_joint',
                      'wrist_1_joint',
                      'wrist_2_joint',
                      'wrist_3_joint']
        
        if reset:
            # Set the arm back to the resting position
            arm_goal  = [0, 0, 0, 0, 0, 0]

        else:
            # Set a goal configuration for the arm
            arm_goal  = [-0.3, -2.0, -1.0, 0.8, 1.0, -0.7]
             
        # Connect to the right arm trajectory action server
        rospy.loginfo('Waiting for myur trajectory controller...')
        
        # xxx_moveit_config controllers.yaml
        arm_client = actionlib.SimpleActionClient('six_manipulator_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        
        arm_client.wait_for_server()
        
        rospy.loginfo('...connected.')
        
        # Connect to the head trajectory action server
        # rospy.loginfo('Waiting for head trajectory controller...'  
    
        # Create a single-point arm trajectory with the arm_goal as the end-point
        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)
    
        # Send the trajectory to the arm action server
        rospy.loginfo('Moving the arm to goal position...')
        
        # Create an empty trajectory goal
        arm_goal = FollowJointTrajectoryGoal()
        
        # Set the trajectory component to the goal trajectory created above
        arm_goal.trajectory = arm_trajectory
        
        # Specify zero tolerance for the execution time
        arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    
        # Send the goal to the action server
        arm_client.send_goal(arm_goal)
        
        if not sync:
            # Wait for up to 5 seconds for the motion to complete 
            arm_client.wait_for_result(rospy.Duration(5.0))
        
        rospy.loginfo('...done')
        
if __name__ == '__main__':
    try:
        TrajectoryDemo()
    except rospy.ROSInterruptException:
        pass