﻿ 机械臂沿末端xyz轴移动及旋转（python）_人工智能_开心洋葱网
• 欢迎访问开心洋葱网站，在线教程，推荐使用最新版火狐浏览器和Chrome浏览器访问本网站，欢迎加入开心洋葱` QQ群`
• 为方便开心洋葱网用户，开心洋葱官网已经开启复制功能！
• 欢迎访问开心洋葱网站，手机也能访问哦~欢迎加入开心洋葱多维思维学习平台` QQ群`
• 如果您觉得本站非常有看点，那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~！
• 由于近期流量激增，小站的ECS没能经的起亲们的访问，本站依然没有盈利，如果各位看如果觉着文字不错，还请看官给小站打个赏~~~~~~~~~~~~~！

2962次浏览

[隐藏]

# 2.接口代码

## 2.1.核心函数

``````# 移动
def move_endEffector(axis, dist, x, y, z, rx, ry, rz):
Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])
Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])
Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
R = Rz * Ry * Rx

unitVector = np.mat([[0], [0], [1]])
T = R*unitVector

pose_move = [0.0] * 6
if axis == "x":
pose_move[0]=dist
if axis == "y":
pose_move[1]=dist
if axis == "z":
pose_move[2]=dist
T_move= getT_fromPose(pose_move[0],pose_move[1],pose_move[2],pose_move[3],pose_move[4],pose_move[5])

T_now = getT_fromPose(x, y, z, rx, ry, rz)
T_moveTarget = T_now*T_move
pose = getPose_fromT(T_moveTarget)
output_x = pose[0]
output_y = pose[1]
output_z = pose[2]
rx = pose[3]
ry = pose[4]
rz = pose[5]

return output_x, output_y, output_z, rx, ry, rz

#旋转
def spin_endEffector(axis, angle, x, y, z, rx, ry, rz):
Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])
Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])
Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
R = Rz * Ry * Rx

unitVector = np.mat([[0], [0], [1]])
T = R*unitVector

pose_spin = [0.0] * 6
if axis == "x":
pose_spin[3]=angle/180.0*math.pi
if axis == "y":
pose_spin[4]=angle/180.0*math.pi
if axis == "z":
pose_spin[5]=angle/180.0*math.pi
print(axis)
print(pose_spin)
T_spin= getT_fromPose(pose_spin[0],pose_spin[1],pose_spin[2],pose_spin[3],pose_spin[4],pose_spin[5])

T_now = getT_fromPose(x, y, z, rx, ry, rz)
T_spinTarget = T_now*T_spin
pose = getPose_fromT(T_spinTarget)
output_x = pose[0]
output_y = pose[1]
output_z = pose[2]
rx = pose[3]
ry = pose[4]
rz = pose[5]

return output_x, output_y, output_z, rx, ry, rz

``````

## 2.2.需要调用的函数

``````def getPose_fromT(T):
x = T[0, 3]
y = T[1, 3]
z = T[2, 3]
rx = math.atan2(T[2, 1], T[2, 2])
ry = math.asin(-T[2, 0])
rz = math.atan2(T[1, 0], T[0, 0])

return x, y, z, rx, ry, rz

def getT_fromPose(x, y, z, rx, ry, rz):
Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])
Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])
Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
t = np.mat([[x], [y], [z]])

R = Rz * Ry * Rx
R_ = np.array(R)
t_ = np.array(t)
T_1 = np.append(R_, t_, axis = 1)

zero = np.mat([0,0,0,1])
T_2 = np.array(zero)

T = np.append(T_1, T_2, axis = 0)
T = np.mat(T)

return T
``````

# 3.使用例子

## 3.1.python例子

``````import math
import numpy as np
import scipy.linalg as la

def move_endEffector(axis, dist, x, y, z, rx, ry, rz):
Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])
Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])
Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
R = Rz * Ry * Rx

unitVector = np.mat([[0], [0], [1]])
T = R*unitVector

pose_move = [0.0] * 6
if axis == "x":
pose_move[0]=dist
if axis == "y":
pose_move[1]=dist
if axis == "z":
pose_move[2]=dist
T_move= getT_fromPose(pose_move[0],pose_move[1],pose_move[2],pose_move[3],pose_move[4],pose_move[5])

T_now = getT_fromPose(x, y, z, rx, ry, rz)
T_moveTarget = T_now*T_move
pose = getPose_fromT(T_moveTarget)
output_x = pose[0]
output_y = pose[1]
output_z = pose[2]
rx = pose[3]
ry = pose[4]
rz = pose[5]

return output_x, output_y, output_z, rx, ry, rz

def rotate_endEffector(axis, angle, x, y, z, rx, ry, rz):
Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])
Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])
Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
R = Rz * Ry * Rx

unitVector = np.mat([[0], [0], [1]])
T = R*unitVector

pose_rotate = [0.0] * 6
if axis == "x":
pose_rotate[3]=angle/180.0*math.pi
if axis == "y":
pose_rotate[4]=angle/180.0*math.pi
if axis == "z":
pose_rotate[5]=angle/180.0*math.pi

T_rotate= getT_fromPose(pose_rotate[0],pose_rotate[1],pose_rotate[2],pose_rotate[3],pose_rotate[4],pose_rotate[5])

T_now = getT_fromPose(x, y, z, rx, ry, rz)
T_rotateTarget = T_now*T_rotate
pose = getPose_fromT(T_rotateTarget)
output_x = pose[0]
output_y = pose[1]
output_z = pose[2]
rx = pose[3]
ry = pose[4]
rz = pose[5]

return output_x, output_y, output_z, rx, ry, rz

def getPose_fromT(T):
x = T[0, 3]
y = T[1, 3]
z = T[2, 3]
rx = math.atan2(T[2, 1], T[2, 2])
ry = math.asin(-T[2, 0])
rz = math.atan2(T[1, 0], T[0, 0])

return x, y, z, rx, ry, rz

def getT_fromPose(x, y, z, rx, ry, rz):
Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])
Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])
Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
t = np.mat([[x], [y], [z]])

R = Rz * Ry * Rx
R_ = np.array(R)
t_ = np.array(t)
T_1 = np.append(R_, t_, axis = 1)

zero = np.mat([0,0,0,1])
T_2 = np.array(zero)

T = np.append(T_1, T_2, axis = 0)
T = np.mat(T)

return T

# 随便设置了一个位姿
pose_now = [-0.072944147641399, -0.06687830562048944, 0.4340418493881254, -0.2207496117519063, 0.0256862005614321, 0.1926014162476009]

# 得到目标位置的位姿
# 沿x轴移动5cm
pose_move_x = move_endEffector("x", 0.05, pose_now[0], pose_now[1], pose_now[2], pose_now[3], pose_now[4], pose_now[5])
print(pose_move_x)

# 沿y轴移动-5cm
pose_move_y = move_endEffector("y", -0.05, pose_now[0], pose_now[1], pose_now[2], pose_now[3], pose_now[4], pose_now[5])
print(pose_move_y)

# 沿z轴旋转0.05弧度
pose_rotate_z = rotate_endEffector("z", 0.05, pose_now[0], pose_now[1], pose_now[2], pose_now[3], pose_now[4], pose_now[5])
print(pose_rotate_z)

# 沿x轴旋转-0.05弧度
pose_rotate_x = rotate_endEffector("x", -0.05, pose_now[0], pose_now[1], pose_now[2], pose_now[3], pose_now[4], pose_now[5])
print(pose_rotate_x)
``````

[开心洋葱]

• 版权声明

本站的文章和资源来自互联网或者站长的原创，按照 CC BY -NC -SA 3.0 CN协议发布和共享，转载或引用本站文章应遵循相同协议。如果有侵犯版权的资源请尽快联系站长，我们会在24h内删除有争议的资源。
• 合作网站

• 友情链接

• 关于我们

一群热爱思考，热爱生活，有理想的新社会主义接班人的多维思维学习平台，天行健，君子以自强不息。地势坤，君子以厚德载物。
……