1 Star 0 Fork 0

陈狗翔 / UR5-control-with-RG2

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Clone or download
util.py 2.03 KB
Copy Edit Web IDE Raw Blame History
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

Comment ( 0 )

Sign in for post a comment

Python
1
https://gitee.com/ChenGouXiang/UR5-control-with-RG2.git
git@gitee.com:ChenGouXiang/UR5-control-with-RG2.git
ChenGouXiang
UR5-control-with-RG2
UR5-control-with-RG2
master

Search