机械臂导纳控制

对于柔顺控制，主流的控制算法有导纳控制算法，由于网上对于算法的介绍以及讲解较多，而代码以及实现较少（可以说几乎没有）。博主在本科毕设中用到了导纳控制，并付诸应用于实验中，奈何当时也是糊里糊涂，在几个月的打磨，终于对自己的代码能力以及ROS较为熟悉，能够熟练应用。这里需要感谢的人很多，感谢相遇，也感谢每一个老师的指导。

上干货！~ 导纳算法可以简单的看作一个二阶系统，公式如下：
$$M\ast (\ddot{\theta }-{\ddot{\theta }}_d)+D\ast (\dot{\theta }-{\dot{\theta }}_d)+K(\theta -{\theta }_d)=\tau$$
​ 具体不了解的可以参考机器人学导论以及网上关于导纳控制的资料，我认为讲的还是蛮清楚的。

​ 接着，剩下的就是算法的实现部分了，实现中用到了ROS作为主要框架以及开发环境，Gazebo为主要仿真环境，机械臂采用开源的Universal Robot，开源的KDL库。最后，完成了导纳控制效果。

首先，启动一个仿真环境，载入机械臂配置文件，和笛卡尔坐标系下的控制器（基于ROS control 实现，自定义开发，因此也用到了pluginlib），具体运行命令为：

$ roslaunch ur_e_gazebo ur5e.launch

因为Universal Robot提供了多种型号，我选择用ur5e进行测试（这个型号对实验不会存在太多的影响，最大的就应该是工作空间了）。这条命令启动以后，就可以看到gazebo和载入的机器人，接下来就可以开心的启动算法啦，命令如下。

$ roslaunch Admittance Admittance_test.launch

或者

$ roslaunch Admittance Admittance.launch

这里提供了两种模式，前者是嵌入了一个节点，用于发布末端力矩信号，用来模拟有个人在往下压和向上拉的信号，后者就是不会这个节点，大家可以通过/wrench这个节点来完成话题发布后完成控制。

最后，那就是代码啦~

代码网址：https://github.com/MingshanHe/Robotic_Arm_Algorithms

视频网址：https://www.bilibili.com/video/BV1aZ4y197SP?spm_id_from=333.999.0.0

希望大家能够给仓库点小星星啦！~~