机器人四元数转欧拉角、欧拉角转四元数、欧拉角转旋转矩阵python实现

太过爱你忘了你带给我的痛 2022-12-06 05:23 191阅读 0赞

import numpy as np
    import math
    import csv, sys, os
    import matplotlib.pyplot as plt
    from mpl_toolkits.mplot3d import Axes3D
    
    def ma2qua(m):
        q1 = math.sqrt(m[0,0] + m[1,1] + m[2,2] + 1) / 2
        if q1 != 0:
            q2 = (m[2,1] - m[1,2])/4/q1
            q3 = (m[0,2] - m[2,0])/4/q1
            q4 = (m[1,0] - m[0,1])/4/q1
        else:
            tr = m[0,0] + m[1,1] + m[2,2]
            if tr>0:
                s = math.sqrt(tr + 1.0) * 2
                q1 = 0.25*s
                q2 = (m[2,1] - m[1,2])/s
                q3 = (m[0,2] - m[2,0])/s
                q4 = (m[1,0] - m[0,1])/s
            elif (m[0,0]>m[1,1]) & (m[0,0]>m[2,2]):
                s = math.sqrt(1.0 + m[0,0] - m[1,1] - m[2,2]) * 2
                q1 = (m[2,1] - m[1,2]) / s
                q2 = 0.25 * s
                q3 = (m[0,1] + m[1,0]) / s
                q4 = (m[0,2] + m[2,0]) / s
            elif (m[1,1]>m[2,2]):
                s = math.sqrt(1.0 + m[1,1] - m[0,0] - m[2,2]) * 2
                q1 = (m[0,2] - m[2,0]) / s
                q2 = (m[0,1] + m[1,0]) / s
                q3 = 0.25 * s
                q4 = (m[1,2] + m[2,1]) / s
            else:
                s = math.sqrt(1.0 + m[2,2] - m[0,0] - m[1,1]) * 2
                q1 = (m[1,0] - m[0,1]) / s
                q2 = (m[0,2] + m[2,0]) / s
                q3 = (m[1,2] + m[2,1]) / s
                q4 = 0.25 * s
        #r = np.array([q1,q2,q3,q4])
        r = (q1, q2, q3, q4)
        # print(q1,q2,q3,q4, q1**2+q2**2+q3**2+q4**2)
        return r
    
    def qua2ma(q0,q1,q2,q3):
        r = np.array([[1-2*q2**2-2*q3**2, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2],
                      [2*q1*q2+2*q0*q3, 1-2*q1**2-2*q3**2, 2*q2*q3-2*q0*q1],
                      [2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, 1-2*q1**2-2*q2**2]])
        return r
    
    def ma2abc(m):
        '''
        rotz(c)roty(b)rotx(a)
        '''
        c = math.atan2(m[1,0], m[0, 0]) # and c = math.atan2(-m[1,0], -m[0, 0])
        b = math.atan2(-m[2, 0], (m[0,0] * math.cos(c) + m[1, 0] * math.sin(c)))
        a = math.atan2((-m[1, 2] * math.cos(c) + m[0, 2] * math.sin(c)), (m[1, 1] * math.cos(c) - m[0, 1] * math.sin(c)))
        return (a, b, c)
    
    def plot_frame(plot, pos):
        m = qua2ma(pos[3], pos[4], pos[5], pos[6])
    
        plot.quiver(pos[0], pos[1], pos[2], m[0,0], m[1,0], m[2,0], color='r', length=10)
        plot.quiver(pos[0], pos[1], pos[2], m[0,1], m[1,1], m[2,1], color='g', length=10)
        plot.quiver(pos[0], pos[1], pos[2], m[0,2], m[1,2], m[2,2], color='b', length=10)
        
    def plot_pos(pos_lst):
        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')
        ax.set_xlim(-150, +150)
        ax.set_ylim(0, 300)
        ax.set_zlim(-150, +150)
        #plt.axis('equal')
        
        ax.quiver(0, 0, 0, 1, 0, 0, color='c', length=50)#Cyan 青/天蓝
        ax.quiver(0, 0, 0, 0, 1, 0, color='m', length=50)#Magenta 洋红
        ax.quiver(0, 0, 0, 0, 0, 1, color='y', length=50)#Yellow 黄
        ax.set_xlabel('x')
        ax.set_ylabel('y')
        ax.set_zlabel('z')
        for i in pos_lst:
            if i:
                plot_frame(ax, i)
        plt.show()
    
    def read_csv(filename):
        """
        逗号分隔格式文件返回为一个列表，列表元素为浮点数组列表
        """
        a = []
        if os.path.isabs(filename):
            p = filename
        else:
            p = os.path.join(sys.path[0], filename)
        with open(p, 'r', ) as csvfile:
            r = csv.reader(csvfile)
            for row in r:
                a.append([float(i) for i in row])
        return a
    
    def write_csv(filename, lst):
        if os.path.isabs(filename):
            p = filename
        else:
            p = os.path.join(sys.path[0], filename)
        with open(p, 'w', newline='') as csvfile:
            w = csv.writer(csvfile)
            for row in lst:
                w.writerow(row)
    
    def diff(datalist):
        '''
        取前3列: x y z值，计算差值, 返回(x, y, z)列表
        '''
        r = []
        for i in range(len(datalist) - 1):
            c, n = datalist[i], datalist[i + 1]
            r.append((n[0] - c[0], n[1] - c[1], n[2] - c[2]))
        return r
    
    def rotr(r, a):
        '''
        绕r转a弧度
        '''
        r11 = r[0] * r[0] - r[0] * r[0] * math.cos(a) + math.cos(a)
        r22 = r[1] * r[1] - r[1] * r[1] * math.cos(a) + math.cos(a)
        r33 = r[2] * r[2] - r[2] * r[2] * math.cos(a) + math.cos(a)
    
        r12 = r[0] * r[1] - r[0] * r[1] * math.cos(a) - r[2] * math.sin(a)
        r13 = r[0] * r[2] - r[0] * r[2] * math.cos(a) + r[1] * math.sin(a)
        r23 = r[1] * r[2] - r[1] * r[2] * math.cos(a) - r[0] * math.sin(a)
    
        r21 = r[0] * r[1] - r[0] * r[1] * math.cos(a) + r[2] * math.sin(a)
        r31 = r[0] * r[2] - r[0] * r[2] * math.cos(a) - r[1] * math.sin(a)
        r32 = r[1] * r[2] - r[1] * r[2] * math.cos(a) + r[0] * math.sin(a)
        return np.array([
            [r11, r12, r13],
            [r21, r22, r23],
            [r31, r32, r33],
        ])
    
    
    def compute_ori(vectors):
        res = []
        init_ori = np.eye(3)
        z = np.array([0, 0, 1]) #xoy
        for i in vectors:
            a = np.array(i)
            a_z = a - a.dot(z)*z # xoy的投影
            r = a_z / math.sqrt(a_z.dot(a_z))
            res.append(np.matmul(init_ori, rotr(r, -15 * math.pi / 180))) ##偏移角
        return res
    
    def compose_points(infile, outfile):
        d = read_csv(infile)
        df = diff(d)
        ori = compute_ori(df)
        r = []
        for i in range(len(ori)):
            a = (d[i][0], d[i][1], d[i][2]) + ma2qua(ori[i]) + tuple([i * 180 / math.pi for i in ma2abc(ori[i])])
            r.append(a)
        return r
    
    import socket
    def server_it(reply):
        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
            s.bind(('', 50007))
            s.listen(1)
            while True:
                conn, addr = s.accept()
                with conn:
                    print('Connected by', addr)
                    while True:
                        data = conn.recv(1024)
                        if not data: break
                        conn.sendall(reply)
    
    def main():
        print(ma2qua(np.array([[0, -1, 0],[1,0,0],[0,0,1]])))
    
    if __name__ == '__main__':
        main()