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

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()