基于ROS控制UR5末端画圆--ROS_UR5_CIRCLE_V0

相信很多朋友上手UR5后面，最迫切的问题是如何自由的控制机械臂按照自己规定的路径进行移动，当然这里说的路径不是简单的pick 和 place。比如画圆，走直线等等都是对控制机械臂非常好的例子，接下来文章告诉你如何控制UR5进行画圆，代码后面有备注于github，可以进行参考。
总结一下，要让UR5进行简单的画圆，你需要大概五步：
一、确定机械臂末端画圆平面
二、根据画圆平面确定末端姿态
三、根据姿态依据圆方程给出末端姿态T矩阵
四、根据正逆运动学进行求解joint角度
五、publish joint角度控制UR5
第一步：明确自己的机械臂末端，是如何进行画圆，比如保证Z轴不动，然后让机械臂末端在平面方向上进行操作。
文中本着一切从简的原则，我们设置末端姿态保持不变，末端只在水平面上进行平动，即对于矩阵T我们只需要关注translation matrix,而且我们只需要关注translation matrix 的x,y即可。
第二步：根据第一步确定的末端姿态，我们可以获取如下公式：

x = xy_center_pos[0] + self.radius * math.cos( 2 * math.pi * t / self.cont )
y = xy_center_pos[1] + self.radius * math.sin( 2 * math.pi * t / self.cont)
其中 xy_center_pos表示机械臂起始姿态，后面表示圆方程

第三步和第四步：末端姿态已获取，根据6-DOF自由度的逆运动学，对上述更新的末端姿态矩阵进行更新，即可计算末端关节角（关于6自由度正逆运动学，可参考机器人学导论，或者参考我的github,我提供一个基于python的正逆运动学求解函数，同时写有最优解选择函数连接为:https://github.com/Musyue/CSDN_BLOG_CODE/blob/master/ROS_UR5_CIRCLE_V0/ur_kinematics.py）
第五步：发布上述两步计算的角度即可：代码可参考：

ss = "movej([" + str(qq[0]) + "," + str(qq[1]) + "," + str(qq[2]) + "," + str(
    qq[3]) + "," + str(qq[4]) + "," + str(qq[5]) + "]," + "a=" + str(ace) + "," + "v=" + str(
    vel) + "," + "t=" + str(t) + ")"

视频效果：
https://raw.githubusercontent.com/Musyue/CSDN_BLOG_CODE/master/ROS_UR5_CIRCLE_V0/demo/webwxgetvideo
代码位置：https://github.com/Musyue/CSDN_BLOG_CODE