Kinect2和六轴机械臂的实时映射（初步）

本文暂时不贴代码，只是总结一下这两天的思路，做的过程中遇到的问题，尝试的各种方法以及结果。

首先说下现阶段想要的一个效果吧：利用Kinect2采集人的骨架参数，实时映射到机械臂的几个自由度让其模仿人右臂的动作。

Kinect2部分流程图大概如下：

1）这里的初始化选择对象的意思是让电脑知道要跟踪哪一副骨架，之前有做过一个向上举手达到两秒选中的小Demo所以直接拿过来用了，作用也十分直观，因为kinect2可以追踪六个骨架，而且这样可以中途换人。
2）知道要追踪哪一个骨架之后自然是得考虑哪些数据是有效的哪些要舍弃，这里取了右臂的肩关节，肘关节以及手关节三个点的三维坐标用来计算映射方式，对应关系如下：
①左右挥手的角度->机械臂的底座
②上臂和两肩连线夹角->机械臂自下而上第二个自由度
③上下臂夹角->机械臂自下而上的第三个自由度
④为了测试机械臂稳定程度，通过计算保持手部垂直向上（在上方可以放一些物体）
3）这里的退出暂时还没有加，初步的设想是利用kinect2的手势保持特定的几秒即可（很容易实现）

下面是这几天遇到的问题以及解决思路
1）TCP/IP通讯一直不成功，但IP可以ping得通。
解决方法：控制指令最后加上“\n” //吐了半升血
2）直接在刷图的地方给机器人发送控制指令，机器人的移动速度可以说是龟速。
初步分析：刷图的速率大概是30ms，机械臂接受指令的方式是非阻塞的，也就是说后一条会覆盖前一条，但每条指令执行的时候需要几十毫秒的“启动时间”，所以每次刚刚启动就被下一条指令覆盖了。
处理方法：判定指令必须和上一条指令有超过15度的偏差之后才会给机械臂发送指令。
3）在跟随的时候机械臂卡顿感非常严重！！！！（还没有完全解决）
初步分析：在给某个电机正向运动时一个反向的指令，机械臂会立即卡住再反向（不会平滑）
处理方法1：记录三条控制指令，判定一旦有反向的时候Sleep一秒，但这样会让画面卡住，时间少于一秒效果不是很好。
处理方法2：在1的基础之上给发送指令的部分单独写一个线程，不至于让画面卡住以及某些指令的丢失（指令确实可以有条件地舍去，但不能因为Sleep而丢失，这不好……）
处理方法3：（现阶段采取的方法）给发送指令加了一个最小间隔，大概为600ms，也就是两次指令之间至少间隔二十帧，效果比不加处理的要好很多，但很多时候还是有一定卡顿。

暂时的效果如下：（还是存在卡顿感，这也是之后可能要重点解决的问题）

最后祝大家开发愉快~