测试symply

mport sympy as sp
import math
from sympy import *
import time 
import matplotlib.pyplot as plt


def get_R(a_x, a_y, a_z):                                                           
                                                        
   #a_x, a_y, a_z = sp.symbols('a_x a_y a_z')                          
   #x, y , z = sp.symbols('x y z')   
   #lis = np.array(lis).reshape(-1,3)                                  
  # li = Matrix(lis).T                                               
   Rx = [[1, 0, 0],                                                   
             [0, sp.cos(a_x), -sp.sin(a_x)],                        
             [0, sp.sin(a_x), sp.cos(a_x)]]                           
                                                                      
   Rx = Matrix(Rx)                                                  
   #print(Rx)                                                                   
   Ry = [[sp.cos(a_y), 0, sp.sin(a_y)],                               
         [0,1,0],                                                   
         [-1*sp.sin(a_y),0, sp.cos(a_y)]]                               
   Ry = Matrix(Ry)                                                   
                                                                      
   Rz = [[sp.cos(a_z), -sp.sin(a_z),0],                               
         [sp.sin(a_z), sp.cos(a_z), 0],                               
         [0, 0, 1]]                                                  
                                                                      
   Rz = Matrix(Rz)                                                                                                                                                                  
   #                                                                  
                                                                      
   R = Rz*Ry*Rx                                                          
   return R

vector = [[1,2,3],[2,3,-3]]

def get_vertical_vector(vector):
    a, b = sp.symbols('a, b') 
    vv = np.array([a,b,1])
    v0 = np.array(vector[0])
    v1 = np.array(vector[1])
    r0 = np.dot(v0,vv)
    r1 = np.dot(v1,vv)
    #print(ab_dict[a])
    ab_dict = sp.solve([r0,r1],[a,b])   # get a,b values by function group [r0, r1]
    #print(sp.solve([r0,r1],[a,b]))
   
    v_v = [ab_dict[a],ab_dict[b],1]
    #print(v_v)
    return v_v

vv = get_vertical_vector(vector)

def get_rotate_mat():
    a_x, a_y, a_z = sp.symbols('a_x a_y a_z')    
    R = get_R(a_x, a_y, a_z)
    #print(R, Matrix(vv))
    len_vv = math.sqrt(vv[0]**2+vv[1]**2+vv[2]**2)
    ff = R*Matrix(vv) - Matrix([0,0,len_vv])
    angls = sp.solve([ff[0],ff[1],ff[2]],[a_x, a_y, a_z])
    print(angls)

get_rotate_mat()