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

机械臂沿末端xyz轴移动及旋转(python)

人工智能 绿竹巷人 1900次浏览 0个评论

描述

机械臂末端的位姿是p时,不想沿着基坐标系来移动,而是想沿着末端的xyz轴来移动

接口代码

核心函数

# 移动
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

需要调用的函数

我把需要用的代码贴在这里,讲解在另外一篇博客里欧氏空间位姿与变换矩阵的转换

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

使用例子

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)


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明机械臂沿末端xyz轴移动及旋转(python)
喜欢 (0)

您必须 登录 才能发表评论!

加载中……