相信很多朋友上手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的正逆運動學求解函式，同時寫有最優解選擇函式連線為：

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) + ")"