手写ROS程序控制ur5机械臂笛卡尔空间运动（Python）

本文不借助moveit！！！，所有运动学功能全部手写！！！

 

一、ur5机械臂运动学

csdn上诸多关于ur机械臂的正逆运动学求解思路，参考UR机械臂正逆运动学求解。

由于多数开源代码基本为MATLAB所写，ur官方提供的求解算法为cpp文件，不方便python文件调用，故本文将主要要用到的运动学算法全部采用python语言重写，所有python模块已发布至https://github.com/Wujinshan/my_ur_kinematics。

 

二、ur5的笛卡尔空间运动代码

参考手写ROS程序控制ur5机械臂运动（Python），在其基础之上进行改写，具体代码如下：

from select_ik_solution import select_ik_solution
from trajectory_msgs.msg import *
from control_msgs.msg import *
import rospy
import actionlib
from sensor_msgs.msg import JointState
import n