ros melodic控制真实机械臂之等周期输出插补点

        在上一节中，通过修改相关配置文件，使moveit能够输出真实的插补点，并可通过

control_msgs::FollowJointTrajectoryGoalConstPtr

获取轨迹信息，并将获取的插补点写进文件里面。但有个问题是，该插补点为等距而非等周期，且 1s之内输出的插补点个数极其有限，与实际工业控制中的机器人的控制周期（一般是2ms，甚至1ms）差距极为明显。

        通过查阅相关资料发现，可以通过Industrial_trajectory_filters包改善这种情况。根据该资料中的描述，其方法如下：

1、打开由moveit_setup_assistant生成的功能包中的ompl_planning_pipeline.launch.xml文件，在name=“planning_adapters”的value值中加入filter，并紧随其后指定参数值（sample_duration），添加后如下：

<!-- The request adapters (plugins) used when planning with OMPL.
       ORDER MATTERS -->
  <arg name="planning_adapters" value="
               industrial_trajectory_filters/UniformSampleFilter
               default_planner_request_adapters/AddTimeParameterization
				       default_planner_request_adapters/FixWorkspaceBounds
				       default_planner_request_adapters/FixStartStateBounds
				       default_planner_request_adapters/FixStartStateCollision
				       default_planner_request_adapters/FixStartStatePathConstraints" />

  <param name="sample_duration" value="0.004"/>

2、可修改各个轴的速度、加速度限制（可选）， 打开由moveit_setup_assistant配置的功能包中config文件件下的joint_limits.ymal文件，如果在助手设置的时候没有进行相关设置，其has_velocity_limits、has_acceleration_limits参数应该为false，max_velocity、max_acceleration参数应该为0，根据实际需要进行设置，这里仅仅为举例，设置完成后内容如下：

# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
  joint1:
    has_velocity_limits: true
    max_velocity: 420
    has_acceleration_limits: true
    max_acceleration: 1600
  joint2:
    has_velocity_limits: true
    max_velocity: 420
    has_acceleration_limits: true
    max_acceleration: 1600
  joint3:
    has_velocity_limits: true
    max_velocity: 420
    has_acceleration_limits: true
    max_acceleration: 1600
  joint4:
    has_velocity_limits: true
    max_velocity: 420
    has_acceleration_limits: true
    max_acceleration: 1600
  joint5:
    has_velocity_limits: true
    max_velocity: 420
    has_acceleration_limits: true
    max_acceleration: 1600
  joint6:
    has_velocity_limits: true
    max_velocity: 420
    has_acceleration_limits: true
    max_acceleration: 1600

3、修改完成后，就可以使用该filter了。先打开通过上一节修改后的程序节点，用于接受插补点并输入文件，注意，在文件操作中fopen函数输入绝对路径更为可靠，用相对路径存在程序崩溃的情况。

rosrun trajectory_server cubicSpline_test

然后打开**_moveit_config功能包中的demo.launch文件，如下：

roslaunch csrobot_moveit_config demo.launch 

就会弹出自己的机器人模型，然后拖动末端小球，点击Plan & Excute。这里仅以J1轴为例子，形成的插补点文件内容如下：

0.000000,0.004000,0.008000,0.012000,0.016000,0.020000,0.024000,0.028000,0.032000,0.036000,0.040000,0.044000,0.048000,0.052000,0.056000,0.060000,0.064000,0.068000,0.072000,0.076000,0.080000,0.084000,0.088000,0.092000,0.096000,0.100000,
0.000000,0.008508,0.031574,0.069994,0.133243,0.213557,0.311100,0.423583,0.528749,0.616772,0.686561,0.735851,0.752813,0.760593,0.792597,0.855078,0.932860,0.998843,1.036645,1.045811,1.053848,1.091950,1.134949,1.155846,1.166266,1.167497,
0.000000,4.219867,7.106975,14.149067,17.942939,22.375410,26.570125,28.604643,24.108166,19.790137,15.161314,7.749884,1.459739,4.150592,13.226828,17.712687,18.399999,15.253648,5.250950,0.568299,4.781373,17.563799,6.720773,3.881950,1.293986,0.000000,
944.640841,931.287255,660.441550,1879.362077,749.157338,1208.360042,1146.323672,-1613.341556,-1120.832075,-1338.734795,89.320520,-1688.544459,-697.779219,1780.192490,3413.014350,1011.182244,-721.810792,-1853.932346,-1862.489525,-82.326113,1839.611648,1537.805861,-869.398796,-631.226688,-675.784472,-677.755254,

 分为4行，第一行为时间，第二行为position，第三行为velocity，第四行为accleration。

通过python程序绘制出图如下：

综上，从插补点可以看出，通过该filter之后的插补点为等周期插补点。

参考：

1、https://blog.csdn.net/shenyan0712/article/details/96997856

2、https://blog.csdn.net/lejou_lee/article/details/109676723

3、https://blog.csdn.net/qq_34935373/article/details/98482706