ur机械臂的位姿获取

UR机械臂的位姿获取

之前就玩过ur机械臂，但是今天再研究的时候，发现ur机械臂通过30003端口发送过来的居然是rotation vector, 以前一直当作欧拉角处理…问题是居然还蒙混过关了…

于是借机学习了一下旋转向量到旋转矩阵的转换。

转换一般采用Rodrigues变换，公式非常简单，对于给定旋转向量 $r$
$\theta =norm(r)$
$r=r/\theta$
$R=\cos (\theta )I+(1-\cos (\theta ))r{r}^T+\sin (\theta )r^{\wedge }$
$其中,r^{\wedge }是r对应的反对称矩阵=\left[\begin{array}{ccc}0& -r_x& r_y\\ r_z& 0& -r_x\\ -r_y& r_x& 0\end{array}\right]$

证明可以参考罗德里格斯公式的证明
附上我的ur机械臂获取位姿程序

# socket and connect
addr = (UR_HOST_IP, 30003)
client = socket(AF_INET, SOCK_STREAM)
client.connect(addr)

# get robot initial position
data = bytes(client.recv(1060))
x, y, z = struct.unpack('!ddd', data[444:468])
rx, ry, rz = struct.unpack('!ddd', data[468:492])

#rodrigues tranform
theta = sqrt(rx*rx + ry*ry + rz*rz)
r = np.array([rx, ry, rz]).reshape((3,1))/theta
r1 = np.array([[0,-rz,ry],[rz,0,-rx],[-ry,rx,0]])

R = np.cos(theta) * np.eye(3) + (1-np.cos(theta)) * np.dot(r, r.T) + np.sin(theta)*r1