自用两轴加旋转3D机械臂python仿真

#encoding='utf-8'
import matplotlib.pyplot as plt
import numpy as np
#导入三维工具包mplot3d
from mpl_toolkits import mplot3d

def rototeZ(x,goal):
    rotate=np.array([[np.cos(x),-np.sin(x),0],
         [np.sin(x),np.cos(x),0],
         [0,0,1]])
    #goal=np.array(goal)
    return  np.dot(rotate,goal)
def CalRotote(x,y):
    x1=np.arctan2(y[1],y[0])-np.arctan2(x[1],x[0])
    return x1


l1=6
l2=5
l0_l1=2
l1_l2=1
x1=[0,3]
y1=[0,4]
i=0.1
diff=l1_l2-l0_l1
x2=np.zeros(len(x1))
y2=np.zeros(len(x1))
spoint=np.array([-4,2,3])#初始位置
gpoint=np.array([2,4,8])#目标位置

theltXx=np.arctan2(spoint[1],spoint[0])#计算初始位置相对x轴角度
theltYy=np.arctan2(gpoint[1],gpoint[0])#计算终点位置相对X轴角度

l1t=np.sqrt((spoint[1])**2+(spoint[0])**2)
l2t=np.sqrt((gpoint[1])**2