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

拿到了 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.113