UR5——逆解问题的解决

TASK：

1.电脑控制real robot（UR5）

2.优化逆解——更好的实现UR5的运动规划

——电脑控制UR5  ：

            ①连接上UR5 ：      

                     

       $  roslaunch ur_bringup ur5_bringup.launch robot_ip:=IP_OF_THE_ROBOT

         ②运行test文件

               

         $ rosrun ur_driver test_move.py    

 

         基本上就是这样去控制，我在连接的时候出现过连接不上的问题，但是后面又尝试去连接，不知道为什么又连接好了，可能是不稳定。

          详细的参考资料：https://github.com/ros-industrial/universal_robot

——逆解的解决 ：   

                   ①最终目的：优化八个解，找到规划路径最短的那一个             

         ②初始思路：

                                     从源代码中逆回去找到求逆解的算法（查阅了资料发现应该是去找moveit package

                                     里面的一 个文件KDL，一种求逆解的算法）

                                                               

                 

                 当时我就不断的去找 right.arm.plan 这个方法的定义，想通过这种方法来找到它是怎么算出这个

                 trajectory的，但最后没有找到，原因是看不懂那些python代码

                 最后硬是在moveit的包里面找到了这个算法：

                 

                  打开我标记的文件

                

                 打开后，（一脸懵逼） ，慢慢，仔细地还是找到了那个求逆解的方法：

                 

                但是还是很复杂，不知道怎么样去找逆解（初学c++）

               ③第二种思路：

                        自定义一个逆解算法的插件：

                        虽然说当时想去自己加入一个求逆解的算法，但是吧github上的包下载下来后再编译，

                        这个算法自动就被插入进去了（包的下载在我发的上面的链接上）

                       （这个包里面自带一个kinematic算法）

                       

                                            

                   

            这个函数的定义就比第一种思路里面的函数定义要简单多了，返回值是解的个数，q_sols就是一个指针，

            指向的是一段内存，放的是八个解。

           

             现在要做的只需在Setup assistant 上面重新定义解的算法，就可以了，因为原来包里面的解的算法是KDL

             可以在这个文件里查看：   

           

                    

                打开后：

                

                 

                 可以看到manipulator这个move_group用的是KDL去解的

                  现在我们改变一下：

                   1.首先创建一个文件夹（我是把这个文件夹和原来的config文件放在同一个目录下的）：

                        

                    

                      2.运行moveit_setup_assistant

                               

                     

                        创建一个新的SRDF文件

                           ·选择的路径是

                                  

                                  

                                   这里有个小问题就是moveit可能加载不了xacro文件，你只需要勾选下面

                                   红色区域的地方就行了。

                              

                               

 

                              ·其余的什么定义move_group的话，大家可以看看moveit的教程，这里

                          关键是选取move_group的解算法

                                     

                                     

                                  

                                   这样就ok啦

                   最后在你新建的文件夹里面：

                      

                  

                       config文件里面你可以查看kinematic文件：

                               

                                确认后，这就是新的算法

                                最后，文件里面也会生成一个demo文件，就是一个演示文件，打开后rviz自动就出来了，

                                然后demo里面都是调用的新的算法来算逆解的。