delta 机器人和传送带之间坐标转换代码 [Python]

import scipy as sci
from scipy import spatial

#求比例因子, p_r1:机械手末端位置1,
# p_r2:机械手末端位置2
def factor_c(p_r1, p_r2,encoder_v1,encoder_v2):
    v_2 = (p_r1[0] - p_r2[0])**2 + (p_r1[1] - p_r2[1])**2 + (p_r1[2] - p_r2[2])**2
    factor = sci.sqrt(v_2) / (encoder_v2 - encoder_v1)
    return factor

#计算传输带坐标系原点在机器人坐标系下的坐标
#p_r1,p_r2,p_r3是传输带坐标系的点在机器人坐标系下的点
def cal_Ocr(p_r1, p_r2, p_r3):

    x1,y1,z1 = p_r1[0],p_r1[1],p_r1[2]
    x2,y2,z2 = p_r2[0],p_r2[1],p_r2[2]
    x3,y3,z3 = p_r3[0],p_r3[1],p_r3[2]

    ori_x_c = (x1**2*x3-2*x1*x2*x3-x1*y1*y2+x1*y1*y3 + x1*y2**2-\
                x1*y2*y3 - x1*z1*z2 + x1*z1*z3 + x1*z2**2 - x1*z2*z3 + \
                x2**2*x3 + x2*y1**2 - x2*y1*y2 - x2*y1*y3 + x2*y2*y3 + \
                x2*z1**2 - x2*z1*z2 - x2*z1*z3 + x2*z2*z3) / ((x1-x2)**2*(y1-y2)**2*(z1-z2)**2)
    ori_y_c = (x1**2*y2 - x1*x2*y1 - x1*x2*y2 + x1*x3*y1 - x1*x3*y2 + \
                x2**2*y1 - x2*x3*y1 + x2*x3*y2 + y1**2*y3 - 2*y1*y2*y3 - \
                y1*z1*z2 + y1*z1*z3 - y1*z2**2 - y1*z2*z3 + y2**2*y3 + \
                y2*z1**2 - y2*z1*z2 - y2*z1*z3 + y2*z2*z3) / ((x1-x2)**2*(y1-y2)**2*(z1-z2)**2)

    ori_z_c = (x1**2*z2 - x1*x2*z1 - x1*x2*z2 + x1*x3*z1 - x1*x3*z2 + \
                x1*x3*z1 - x1*x3*z2 + x2**2*z1 - x2*x3*z1 + x2*x3*z2 + \
                y1**2*z2 - y1*y2*z1 - y1*y2*z2 + y1*y3*z1 - y1*y3*z2 + \
                y2**2*z1 - y2*y3*z2 + z1**2*z3 - 2*z1*z2*z3+ z2**2*z3) / ((x1-x2)**2*(y1-y2)**2*(z1-z2)**2)

    Ocr = sci.array([ori_x_c, ori_y_c, ori_z_c])

    return Ocr

def cal_Hcr(p_r1, p_r2, p_r3):
    Ocr = cal_Ocr(p_r1, p_r2, p_r3)
    Ocr_T = Ocr.reshape(Ocr.shape[0], 1)

    d_pr2_Ocr = spatial.distance.pdist(sci.matrix([p_r2,Ocr]))
    d_pr3_Ocr = spatial.distance.pdist(sci.matrix([p_r3,Ocr]))

    p_r2 = sci.array(p_r2)
    p_r3 = sci.array(p_r3)
    Xc = (p_r2 - Ocr) / d_pr2_Ocr
    Yc = (p_r3 - Ocr) / d_pr3_Ocr
    Zc = sci.cross(Xc,Yc)

    Xc_T = Xc.reshape(Xc.shape[0], 1)
    Yc_T = Yc.reshape(Yc.shape[0], 1)
    Zc_T = Zc.reshape(Zc.shape[0], 1)
    #水平拼接
    Hcr = sci.concatenate([Xc_T,Yc_T,Zc_T,Ocr_T],1)
    #在列上合并
    Hcr = sci.vstack((Hcr, [0,0,0,1]))
    return Hcr

#Hcr:4x4 matrix
def cal_Hcr_extends(Hcr,deltaL):
    Trans_x_deltaL = sci.matrix([[1,0,0,deltaL],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
    return sci.dot(Hcr,Trans_x_deltaL) #矩阵相乘


#-----------------for test-----------------
p_r1 = [2,3,4]
p_r2 = [4,5,6]
p_r3 = [6,7,8]

Hcr = cal_Hcr(p_r1,p_r2,p_r3)
Hcr_ext = cal_Hcr_extends(Hcr, 3)
#print(Hcr_ext)
#exit(0);
Ocr = cal_Ocr(p_r1, p_r2, p_r3)
#两个列表转矩阵
d_pr2_Ocr = spatial.distance.pdist(sci.matrix([p_r2,Ocr]))
print(sci.matrix([p_r2,Ocr]))
print(p_r2)
print(Ocr)
print(d_pr2_Ocr)

d_pr3_Ocr = spatial.distance.pdist(sci.matrix([p_r3,Ocr]))
Xc = (sci.array(p_r2) - sci.array(Ocr)) / d_pr2_Ocr
Yc = (sci.array(p_r3) - sci.array(Ocr)) / d_pr3_Ocr
Zc = sci.cross(Xc,Yc)
#sci.dot()
print(Xc)
print(Yc)
print(Zc)

追求梦想,做最好的自己