使用 MoveIt 控制自己的真实机械臂【3】——优化轨迹发给真实机械臂执行

黄小白的进阶之路

已于 2022-07-29 19:03:40 修改

阅读量5.8k

点赞数 21

分类专栏： moveit 文章标签： ubuntu moveit 机械臂

于 2022-07-27 10:52:39 首次发布

本文链接：https://blog.csdn.net/huangjunsheng123/article/details/125971767

版权

moveit 专栏收录该内容

17 篇文章 74 订阅

订阅专栏

上一篇文章中，使用 MoveIt 控制自己的真实机械臂【2】——编写 action server 端代码，已经实现了 action server 端代码，并且拿到了 MoveIt 规划的轨迹数据。当然下一步就是把拿到的轨迹数据作为控制指令来驱动机械臂执行。

那么在这之前，首先你得确定，让机械臂动起来，需要发给它什么样的指令数据，可能有机械臂能够直接执行 MoveIt 生成的指令，但是通常来讲很多机械臂是不能直接执行的。
但是，机械臂厂家都会提供对应的 SDK，一般而言，机械臂可以执行关节路点指令。比如一个七自由度机械臂，有七个独立关节，这七个关节的关节角 [joint1, joint2, joint3, joint4, joint5, joint6, joint7] 就确定了机械臂在空间中的唯一构型。我们只需要将 MoveIt 规划出来的轨迹中对应的关节角按顺序发给机械臂执行就可以了。这里由于大家手中的实际机械臂用户驱动接口不尽相同，不做细致阐述（自己的机械臂自己应该是会用的吧哈哈）。

在这里插入图片描述

我们看下 MoveIt 完成上图所示运动所规划出来的轨迹信息：

// 时间戳、关节位置（关节1～7）、关节速度（关节1～7）、关节加速度（关节1～7）
0.000000000,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.857861,0,0,0,0,0,0
0.307578714,0.0405788,-0.00694363,-0.0244031,0.0358453,-0.04523,0.0208521,0.037594,0.228918,-0.0391713,-0.137666,0.202215,-0.255157,0.117633,0.21208,0.897857,-0.153636,-0.539947,0.793121,-1.00077,0.461377,0.831814
0.432089212,0.0811577,-0.0138873,-0.0488061,0.0716906,-0.0904601,0.0417041,0.075188,0.371675,-0.0635989,-0.223515,0.328319,-0.414276,0.190991,0.344336,0.82569,-0.141287,-0.496548,0.729373,-0.920331,0.424293,0.764955
0.529297445,0.121737,-0.0208309,-0.0732092,0.107536,-0.13569,0.0625562,0.112782,0.453931,-0.0776741,-0.272982,0.40098,-0.505961,0.233259,0.420541,0.811068,-0.138786,-0.487755,0.716457,-0.904034,0.41678,0.751409
0.612040669,0.162315,-0.0277745,-0.0976122,0.143381,-0.18092,0.0834082,0.150376,0.524281,-0.089712,-0.315289,0.463123,-0.584374,0.26941,0.485717,0.871342,-0.149099,-0.524002,0.769699,-0.971216,0.447752,0.807249
0.684744045,0.202894,-0.0347181,-0.122015,0.179227,-0.22615,0.10426,0.18797,0.584287,-0.09998,-0.351375,0.51613,-0.651259,0.300245,0.541309,0.751403,-0.128576,-0.451874,0.663751,-0.837529,0.38612,0.696133
0.751219638,0.243473,-0.0416618,-0.146418,0.215072,-0.27138,0.125112,0.225564,0.632449,-0.108221,-0.380338,0.558674,-0.704941,0.324994,0.585929,0.685464,-0.117293,-0.41222,0.605505,-0.764033,0.352236,0.635044
0.813222623,0.284052,-0.0486054,-0.170821,0.250917,-0.31661,0.145964,0.263158,0.674597,-0.115433,-0.405685,0.595905,-0.75192,0.346652,0.624977,0.668748,-0.114432,-0.402168,0.590738,-0.745401,0.343646,0.619558
0.871632239,0.324631,-0.055549,-0.195224,0.286762,-0.36184,0.166816,0.300752,0.716099,-0.122535,-0.430643,0.632566,-0.798179,0.367978,0.663425,0.753562,-0.128945,-0.453173,0.665659,-0.839936,0.387229,0.698133
0.926656740,0.36521,-0.0624927,-0.219628,0.322608,-0.40707,0.187668,0.338346,0.756278,-0.12941,-0.454806,0.668058,-0.842963,0.388625,0.700649,0.700668,-0.119894,-0.421363,0.618935,-0.780979,0.360049,0.64913
0.979010665,0.405788,-0.0694363,-0.244031,0.358453,-0.4523,0.208521,0.37594,0.790823,-0.135321,-0.47558,0.698573,-0.881468,0.406376,0.732653,0.613094,-0.104909,-0.368699,0.541576,-0.683367,0.315047,0.567997
1.029321758,0.446367,-0.0763799,-0.268434,0.394298,-0.49753,0.229373,0.413534,0.822933,-0.140816,-0.494891,0.726938,-0.917259,0.422877,0.762402,0.663893,-0.113602,-0.399248,0.586449,-0.739989,0.341151,0.615059
1.077669729,0.486946,-0.0833235,-0.292837,0.430144,-0.54276,0.250225,0.451128,0.852024,-0.145794,-0.512385,0.752635,-0.949684,0.437825,0.789352,0.533868,-0.0913525,-0.321054,0.471592,-0.59506,0.274336,0.494599
1.124595793,0.527525,-0.0902672,-0.31724,0.465989,-0.58799,0.271077,0.488722,0.877841,-0.150211,-0.527911,0.775441,-0.97846,0.451092,0.813271,0.566712,-0.0969726,-0.340806,0.500604,-0.631669,0.291213,0.525026
1.170141769,0.568104,-0.0972108,-0.341643,0.501834,-0.63322,0.291929,0.526316,0.904441,-0.154763,-0.543907,0.798937,-1.00811,0.464761,0.837914,0.601576,-0.102938,-0.361772,0.531402,-0.670529,0.309129,0.557326
1.214348245,0.608683,-0.104154,-0.366046,0.53768,-0.67845,0.312781,0.56391,0.931846,-0.159452,-0.560388,0.823146,-1.03866,0.478843,0.863303,0.638585,-0.109271,-0.384028,0.564094,-0.71178,0.328146,0.591613
1.257254614,0.649262,-0.111098,-0.390449,0.573525,-0.72368,0.333633,0.601504,0.955258,-0.163459,-0.574467,0.843827,-1.06475,0.490874,0.884993,0.447458,-0.0765665,-0.269089,0.395262,-0.498746,0.229933,0.414545
1.299315559,0.68984,-0.118042,-0.414852,0.60937,-0.768911,0.354485,0.639098,0.974459,-0.166744,-0.586014,0.860788,-1.08615,0.50074,0.902782,0.465626,-0.0796754,-0.280016,0.411311,-0.518997,0.239269,0.431377
1.340547737,0.730419,-0.124985,-0.439255,0.645216,-0.814141,0.375337,0.676692,0.994046,-0.170096,-0.597793,0.87809,-1.10798,0.510805,0.920927,0.484533,-0.0829106,-0.291385,0.428012,-0.54007,0.248984,0.448892
1.380967478,0.770998,-0.131929,-0.463658,0.681061,-0.859371,0.396189,0.714286,1.01403,-0.173515,-0.609809,0.895739,-1.13025,0.521072,0.939438,0.504207,-0.0862771,-0.303217,0.445391,-0.561999,0.259094,0.467119
1.420590790,0.811577,-0.138873,-0.488061,0.716906,-0.904601,0.417041,0.75188,1.03441,-0.177002,-0.622066,0.913744,-1.15297,0.531546,0.958321,0.524679,-0.0897803,-0.315529,0.463475,-0.584819,0.269614,0.486086
1.459433367,0.852156,-0.145816,-0.512464,0.752751,-0.949831,0.437893,0.789474,1.0552,-0.18056,-0.634569,0.93211,-1.17615,0.54223,0.977583,0.545984,-0.0934257,-0.32834,0.482294,-0.608565,0.280562,0.505823
1.497510591,0.892735,-0.15276,-0.536867,0.788597,-0.995061,0.458745,0.827069,1.07641,-0.184189,-0.647324,0.950845,-1.19979,0.553129,0.997232,0.568153,-0.0972191,-0.341672,0.501877,-0.633275,0.291954,0.526362
1.534837544,0.933313,-0.159703,-0.56127,0.824442,-1.04029,0.479597,0.864663,1.10359,-0.18884,-0.66367,0.974855,-1.23008,0.567096,1.02241,0.895664,-0.153261,-0.538629,0.791185,-0.998326,0.460251,0.829783
1.571066716,0.973892,-0.166647,-0.585673,0.860287,-1.08552,0.500449,0.902257,1.12566,-0.192617,-0.676943,0.994352,-1.25468,0.578437,1.04286,0.310698,-0.0531649,-0.186846,0.274455,-0.34631,0.159657,0.287844
1.606937184,1.01447,-0.173591,-0.610076,0.896133,-1.13075,0.521301,0.939851,1.13692,-0.194543,-0.683712,1.0043,-1.26723,0.584222,1.05329,0.316943,-0.0542335,-0.190601,0.279971,-0.353271,0.162866,0.29363
1.642452499,1.05505,-0.180534,-0.63448,0.931978,-1.17598,0.542153,0.977445,1.14829,-0.196488,-0.690549,1.01434,-1.2799,0.590064,1.06382,0.323313,-0.0553236,-0.194432,0.285599,-0.360372,0.166139,0.299532
1.677616176,1.09563,-0.187478,-0.658883,0.967823,-1.22121,0.563005,1.01504,1.14829,-0.196488,-0.690549,1.01434,-1.2799,0.590064,1.06382,-0.323313,0.0553236,0.194432,-0.285599,0.360372,-0.166139,-0.299532
1.713131491,1.13621,-0.194422,-0.683286,1.00367,-1.26644,0.583857,1.05263,1.13692,-0.194543,-0.683712,1.0043,-1.26723,0.584222,1.05329,-0.316943,0.0542335,0.190601,-0.279971,0.353271,-0.162866,-0.29363
1.749001959,1.17679,-0.201365,-0.707689,1.03951,-1.31167,0.604709,1.09023,1.12566,-0.192617,-0.676943,0.994352,-1.25468,0.578437,1.04286,-0.310698,0.0531649,0.186846,-0.274455,0.34631,-0.159657,-0.287844
1.785231131,1.21737,-0.208309,-0.732092,1.07536,-1.3569,0.625562,1.12782,1.11452,-0.19071,-0.67024,0.984506,-1.24226,0.57271,1.03254,-0.304576,0.0521173,0.183164,-0.269047,0.339487,-0.156511,-0.282172
1.821822596,1.25794,-0.215252,-0.756495,1.1112,-1.40213,0.646414,1.16541,1.10348,-0.188821,-0.663604,0.974759,-1.22996,0.56704,1.02231,-0.298574,0.0510904,0.179555,-0.263746,0.332797,-0.153427,-0.276612
1.858779974,1.29852,-0.222196,-0.780898,1.14705,-1.44736,0.667266,1.20301,1.08717,-0.186031,-0.653797,0.960354,-1.21179,0.55866,1.0072,-0.579573,0.0991733,0.34854,-0.511965,0.646004,-0.297822,-0.536941
1.896480197,1.3391,-0.22914,-0.805301,1.1829,-1.49259,0.688118,1.2406,1.06575,-0.182365,-0.640915,0.941431,-1.18791,0.547652,0.987359,-0.556958,0.0953035,0.33494,-0.491988,0.620797,-0.286201,-0.51599
1.934938193,1.37968,-0.236083,-0.829704,1.21874,-1.53782,0.70897,1.2782,1.04475,-0.178772,-0.628286,0.922881,-1.1645,0.536862,0.967904,-0.535225,0.0915848,0.321871,-0.472791,0.596574,-0.275034,-0.495856
1.974169196,1.42026,-0.243027,-0.854107,1.25459,-1.58305,0.729822,1.31579,1.02417,-0.17525,-0.615907,0.904697,-1.14156,0.526283,0.948832,-0.514341,0.0880112,0.309311,-0.454343,0.573295,-0.264302,-0.476508
2.014188741,1.46084,-0.249971,-0.87851,1.29043,-1.62828,0.750674,1.35338,1.00399,-0.171797,-0.603771,0.886871,-1.11906,0.515913,0.930137,-0.494272,0.0845771,0.297242,-0.436615,0.550926,-0.253989,-0.457915
2.055012680,1.50142,-0.256914,-0.902913,1.32628,-1.67351,0.771526,1.39098,0.984203,-0.168411,-0.591874,0.869396,-1.09701,0.505748,0.911809,-0.474985,0.0812769,0.285644,-0.419578,0.529429,-0.244078,-0.440047
2.096657179,1.542,-0.263858,-0.927316,1.36212,-1.71874,0.792378,1.42857,0.964811,-0.165093,-0.580212,0.852265,-1.0754,0.495783,0.893843,-0.456452,0.0781055,0.274498,-0.403206,0.508771,-0.234555,-0.422877
2.139138733,1.58257,-0.270801,-0.951719,1.39797,-1.76397,0.81323,1.46617,0.941165,-0.161047,-0.565992,0.831377,-1.04904,0.483632,0.871936,-0.65142,0.111467,0.391747,-0.575432,0.726087,-0.334742,-0.603504
2.182907521,1.62315,-0.277745,-0.976122,1.43381,-1.8092,0.834082,1.50376,0.916065,-0.156752,-0.550897,0.809205,-1.02106,0.470734,0.848682,-0.499002,0.0853865,0.300087,-0.440793,0.556198,-0.25642,-0.462298
2.227745486,1.66373,-0.284689,-1.00053,1.46966,-1.85443,0.854934,1.54135,0.893566,-0.152902,-0.537367,0.789331,-0.995987,0.459172,0.827839,-0.503957,0.0862343,0.303066,-0.44517,0.561721,-0.258966,-0.466888
2.273746922,1.70431,-0.291632,-1.02493,1.5055,-1.89966,0.875786,1.57895,0.86915,-0.148724,-0.522684,0.767763,-0.968773,0.446626,0.805218,-0.555545,0.0950618,0.33409,-0.490741,0.619222,-0.285475,-0.514681
2.321142247,1.74489,-0.298576,-1.04933,1.54135,-1.94489,0.896638,1.61654,0.840216,-0.143773,-0.505284,0.742204,-0.936522,0.431758,0.778413,-0.660779,0.113069,0.397375,-0.583699,0.736518,-0.339551,-0.612175
2.370373249,1.78547,-0.30552,-1.07373,1.57719,-1.99012,0.91749,1.65414,0.803546,-0.137498,-0.483232,0.709812,-0.895649,0.412914,0.74444,-0.819579,0.140242,0.492874,-0.723975,0.91352,-0.421153,-0.759294
2.422208818,1.82605,-0.312463,-1.09814,1.61304,-2.03535,0.938342,1.69173,0.763841,-0.130704,-0.459354,0.674738,-0.851393,0.392511,0.707655,-0.714751,0.122304,0.429833,-0.631375,0.796677,-0.367286,-0.662177
2.476688523,1.86663,-0.319407,-1.12254,1.64888,-2.08058,0.959194,1.72933,0.719786,-0.123166,-0.432861,0.635823,-0.802289,0.369873,0.666841,-0.887852,0.151924,0.533931,-0.784284,0.989619,-0.456236,-0.822545
2.535098139,1.90721,-0.326351,-1.14694,1.68473,-2.12581,0.980046,1.76692,0.674597,-0.115433,-0.405685,0.595905,-0.75192,0.346652,0.624977,-0.668748,0.114432,0.402168,-0.590738,0.745401,-0.343646,-0.619558
2.597101124,1.94778,-0.333294,-1.17135,1.72057,-2.17104,1.0009,1.80451,0.631451,-0.10805,-0.379738,0.557792,-0.703829,0.324481,0.585004,-0.715313,0.1224,0.430171,-0.631871,0.797303,-0.367574,-0.662697
2.663794729,1.98836,-0.340238,-1.19575,1.75642,-2.21627,1.02175,1.84211,0.580527,-0.0993365,-0.349114,0.512808,-0.647067,0.298312,0.537825,-0.79673,0.136332,0.479132,-0.703791,0.888051,-0.409412,-0.738125
2.737225139,2.02894,-0.347181,-1.22015,1.79227,-2.2615,1.0426,1.8797,0.522878,-0.0894721,-0.314445,0.461885,-0.582811,0.268689,0.484418,-0.763895,0.130713,0.459386,-0.674786,0.851453,-0.392539,-0.707706
2.819511690,2.06952,-0.354125,-1.24456,1.82811,-2.30673,1.06345,1.9173,0.457765,-0.0783302,-0.275288,0.404366,-0.510234,0.235229,0.424093,-0.793377,0.135758,0.477116,-0.700829,0.884314,-0.407689,-0.735019
2.915581587,2.1101,-0.361069,-1.26896,1.86396,-2.35196,1.08431,1.95489,0.372383,-0.0637201,-0.223941,0.328944,-0.415065,0.191354,0.344992,-0.901239,0.154215,0.541982,-0.796109,1.00454,-0.463115,-0.834947
3.041455671,2.15068,-0.368012,-1.29336,1.8998,-2.39719,1.10516,1.99248,0.227814,-0.0389823,-0.137002,0.20124,-0.253927,0.117066,0.211057,-0.878829,0.15038,0.528505,-0.776313,0.979561,-0.451599,-0.814186
3.345982495,2.19126,-0.374956,-1.31777,1.93565,-2.44242,1.12601,2.03008,0,0,0,0,0,0,0,-0.875142,0.149749,0.526288,-0.773056,0.975452,-0.449705,-0.81077

用 Matlab 脚本绘制出来，如下图所示：
在这里插入图片描述

每一行数据代表一个路点，该示例共有55行，即该轨迹共有55个路点构成；
每个路点中，包含经过该路点的时间戳信息、关节1～7的关节位置、关节速度、关节加速度；

可以发现，有两个特点：

时间戳不是等步长增加的；
相邻两路点之间关节角变化比较大；

事实上，我的 xmate7_pro 机械臂是以 1ms 为控制周期进行控制指令的下发和机械臂关节状态的反馈的，因此，决不能直接把 MoveIt 规划完的轨迹数据直接在每个控制周期发送给机械臂执行；另一方面，由于相邻两路点之间关节角度变化范围大，倘若在相邻两控制周期内发给机械臂执行，势必不合理，给机械臂带来巨大的冲击，甚至触发其保护程序丢失关节零点也不是没有可能。

以上分析表明，MoveIt 默认配置情况下产生的轨迹是等距而非等时的。而在市面上的大多数机器人通常都是按照固定周期来控制关节轴运动。这表明 MoveIt 原生轨迹与实际机器人之间的控制指令需求不匹配。为解决上述问题，笔者了解到以下几种解决方案：

方案一：在拿到的非等时轨迹数据基础上，自己编写三次样条插补算法对原生轨迹进行处理，重采样生成自己的机械臂可执行的等时间间距（即真实机械臂采样周期）的轨迹数据。可参考通过ROS控制真实机械臂(7)—三次样条插补，机械臂轨迹规划——三次样条对比研究。

方案二：查阅资料了解到，软件包 Industrial_trajectory_filters 包含一些对机器人轨迹进行滤波的方法，专注于工业机器人和应用的需求，关于其详细说明参见 industrial_trajectory_filters。其中的 uniform_sample_filter 可以完成对一个规划的轨迹进行重采样（实现方式也是在关节空间中用高阶样条曲线来完成），然后在采样点之间使用统一的时间步进，这个统一的时间步进值可由用户根据实际情况自行设置。首先将该软件包 clone 到工作空间，编译后配置 MoveIt 配置助手生成的 config 包中 ompl_planning_pipeline.launch.xml 文件，该文件主要针对 OMPL 运动规划库规划适配器 planning request adapters 进行设置，这一概念在下一节中展开详细介绍。该软件包具体配置过程可参考industrial_trajectory_filtersTutorialsfilters_inside_moveit 。

<launch>

  <!-- OMPL Plugin for MoveIt! -->
  <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />

  <!-- 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" />

  <arg name="start_state_max_bounds_error" value="0.1" />

  <!-- 机械臂的控制周期是 1ms -->
  <param name="sample_duration" value="0.1"/>
  

  <param name="planning_plugin" value="$(arg planning_plugin)" />
  <param name="request_adapters" value="$(arg planning_adapters)" />
  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
     

  <rosparam command="load" file="$(find xmate7_moveit_config_new)/config/ompl_planning.yaml"/>

</launch>

将该滤波器包加入后，再次启动 MoveIt，生成的轨迹点绘制如下：
在这里插入图片描述此时，轨迹被处理为等时间步进值，调整 sample_duration 参数值以适配实际机械臂控制周期即可。

注意：以上两种方案实际上都是对规划后的轨迹二次处理，仅仅作为一种可执行方案，实际上可以自己开发轨迹规划算法封装为 plugin 集成到 MoveIt 框架中。

说了这么多，想要强调的就是，在把轨迹指令发给机械臂之前，一定要好好将 MoveIt 规划后的轨迹数据绘制出来看一下是否符合真实机械臂的使用要求，也要弄清楚机械臂的控制周期。然后，根据实际机械臂的控制周期，将经重采样处理之后的对应各个周期的路点数据发给机械臂执行，这样才能保证机械臂的安全性以及轨迹执行正确性。

参考文章：
关于如何让moveit输出固定时间步长的轨迹点