## 陈狗翔 / UR5-control-with-RG2 .gitee-modal { width: 500px !important; }

Create your Gitee Account
Explore and code with more than 6 million developers，Free private repositories ！：）
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
util.py 2.03 KB
chen authored 2019-10-28 03:30 . nothing special
import numpy as np

# 旋转矢量转旋转矩阵
def rv2rm(rx, ry, rz):
theta = np.linalg.norm([rx, ry, rz])
kx = rx / theta
ky = ry / theta
kz = rz / theta

c = np.cos(theta)
s = np.sin(theta)
v = 1 - c

R = np.zeros((3, 3))
R[0][0] = kx * kx * v + c
R[0][1] = kx * ky * v - kz * s
R[0][2] = kx * kz * v + ky * s

R[1][0] = ky * kx * v + kz * s
R[1][1] = ky * ky * v + c
R[1][2] = ky * kz * v - kx * s

R[2][0] = kz * kx * v - ky * s
R[2][1] = kz * ky * v + kx * s
R[2][2] = kz * kz * v + c

return R

# 旋转矩阵转rpy
def rm2rpy(R):
sy = np.sqrt(R[0][0] * R[0][0] + R[1][0] * R[1][0])
singular = sy < 1e-6

if not singular:
x = np.arctan2(R[2][1], R[2][2])
y = np.arctan2(-R[2][0], sy)
z = np.arctan2(R[1][0], R[0][0])
else:
x = np.arctan2(-R[1][2], R[1][1])
y = np.arctan2(-R[2][0], sy)
z = 0

return np.asarray([x, y, z])

# rpy转旋转矩阵
def rpy2rm(rpy):
# Rx = np.zeros((3, 3), dtype=rpy.dtype)
# Ry = np.zeros((3, 3), dtype=rpy.dtype)
# Rz = np.zeros((3, 3), dtype=rpy.dtype)

R0 = np.zeros((3, 3), dtype=rpy.dtype)

thetaX = rpy[0]
thetaY = rpy[1]
thetaZ = rpy[2]

cx = np.cos(thetaX)
sx = np.sin(thetaX)

cy = np.cos(thetaY)
sy = np.sin(thetaY)

cz = np.cos(thetaZ)
sz = np.sin(thetaZ)

R0[0][0] = cz * cy
R0[0][1] = cz * sy * sx - sz * cx
R0[0][2] = cz * sy * cx + sz * sx
R0[1][0] = sz * cy
R0[1][1] = sz * sy * sx + cz * cx
R0[1][2] = sz * sy * cx - cz * sx
R0[2][0] = -sy
R0[2][1] = cy * sx
R0[2][2] = cy * cx
return R0

# 旋转矩阵转旋转矢量
def rm2rv(R):
theta = np.arccos((R[0][0] + R[1][1] + R[2][2] - 1) / 2)
K = (1 / (2 * np.sin(theta))) * np.asarray([R[2][1] - R[1][2], R[0][2] - R[2][0], R[1][0] - R[0][1]])
r = theta * K
return r

def rv2rpy(rx, ry, rz):
R = rv2rm(rx, ry, rz)
rpy = rm2rpy(R)
return rpy

def rpy2rv(rpy):
R = rpy2rm(rpy)
rv = rm2rv(R)
return rv