自用两轴加旋转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+(gpoint[0])**2)
sint1=np.arcsin(diff/l1t)#计算初始位置由于轴一轴2存在偏差引起的角度
sint2=np.arcsin(diff/l2t)#计算终止位置由于轴一轴2存在偏差引起的角度

l1t = rototeZ(theltXx+np.pi/2+sint1, [diff, 0, 0])
ssspoint=spoint+l1t
l2t = rototeZ(theltYy+np.pi/2+sint2, [diff, 0, 0])
gggpoint=gpoint+l2t
theltX=np.arctan2(ssspoint[1],ssspoint[0])
theltY=np.arctan2(gggpoint[1],gggpoint[0])

sspoint=rototeZ(-theltX,ssspoint)
ggpoint=rototeZ(-theltY,gggpoint)
print(ggpoint[1]-sspoint[1])
print(sspoint)
print(ggpoint)
thelt=np.arctan2((ggpoint[2]-sspoint[2]),(ggpoint[0]-sspoint[0]))
locus=[]
angle=[]
l=np.sqrt((ggpoint[2]-sspoint[2])**2+(ggpoint[1]-sspoint[1])**2+(ggpoint[0]-sspoint[0])**2)
print(l)

k=100
for i in range(k+1):
    x=sspoint[0]+l*i/k*np.cos(thelt)
    y=sspoint[2]+l*i/k*np.sin(thelt)
    Rotate=(theltY-theltX)*i/k+theltX
    locus.append([x,y])
    tem=-(pow(l2,2)-pow(l1,2)-pow(x,2)-pow(y,2))/2/l1/np.sqrt(pow(x,2)+pow(y,2))
    fthelt=np.arccos(tem)
    thelt1=np.arctan2(y,x)-fthelt
    thelt2=np.arccos((x**2+y**2-l1**2-l2**2)/2/l1/l2)
    angle.append([thelt1,thelt2+thelt1,Rotate])
# plt.plot(locus[:,0],locus[:,1])
# plt.show()


point1=[]
point2=[]
point3=[]
fig = plt.figure()
# fig.canvas.manager.window.showFullScreen()

#创建3d绘图区域
ax = fig.add_subplot(projection='3d')

q=[]
w=[]
l1ss=[0,0,0]
for i in range(k):
    plt.cla()
    x1[1]=l1*np.cos(angle[i][0])
    y1[1]=l1*np.sin(angle[i][0])
    x2[0]=x1[1]
    y2[0]=y1[1]
    x2[1]=x1[1]+l2*np.cos(angle[i][1])
    y2[1]=y1[1]+l2*np.sin(angle[i][1])

    l1sss=rototeZ(angle[i][2]-np.pi/2,[l1_l2,0,0])
    l0sss = rototeZ(angle[i][2] + np.pi / 2, [l0_l1, 0, 0])

    l1s=rototeZ(angle[i][2],[x1[1],0,y1[1]])
    l01s=l1s+l0sss
    l1ss=l1sss+l1s+l0sss




    l2s=rototeZ(angle[i][2],[x2[1],0,y2[1]])
    # print(l1sss)
    l2s=l1sss+l2s+l0sss


    point1.append(l2s[0])
    point2.append(l2s[1])
    point3.append(l2s[2])
    q.append(l1s)
    w.append(l2s)
    # x1[1]=
    # y1[1]=
    # x2[0]=
    # y2[0]=
    # x2[1]=
    # y2[1]=


#    plt.scatter(x1[1], y1[1], color='black', s=5)
    ax.scatter(spoint[0], spoint[1],spoint[2], color='black', s=5)
    ax.scatter(gpoint[0], gpoint[1],gpoint[2], color='black', s=5)
    # ax.scatter(sspoint[0], sspoint[1], sspoint[2], color='b', s=5)
    # ax.scatter(ggpoint[0], ggpoint[1], ggpoint[2], color='b', s=5)
    # ax.scatter(ssspoint[0], ssspoint[1], ssspoint[2], color='y', s=5)
    # ax.scatter(gggpoint[0], gggpoint[1], gggpoint[2], color='y', s=5)
    ax.scatter(0, 0, 0, color='b', s=10)
    ax.scatter(point1,point2, point3, color='r', s=2)

    ax.plot([0,l0sss[0]],[0,l0sss[1]], [0,l0sss[2]], linewidth=2, color='black')
    ax.plot([l0sss[0],l01s[0]],[l0sss[1],l01s[1]], [l0sss[2],l01s[2]], linewidth=2, color='g')
    ax.plot([l1ss[0],l2s[0]],[l1ss[1],l2s[1]], [l1ss[2],l2s[2]], linewidth=2, color='g')
    ax.plot([l01s[0], l1ss[0]], [l01s[1], l1ss[1]], [l01s[2], l1ss[2]], linewidth=2, color='b')
    ax.set_xlim(-7,7)
    ax.set_ylim(-7,7)
    ax.set_zlim(-7,7)
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_zlabel('z')
    plt.pause(0.01)
    # l = np.sqrt((l01s[0] - l1ss[0]) ** 2 + (l01s[1] - l1ss[1]) ** 2 + (l01s[2] - l1ss[2]) ** 2)
    # # l = np.sqrt((l1s[0]) ** 2 + (l1s[1]) ** 2 + (l1s[2]) ** 2)
    # print(l)
    # print(l1s[2]-l1ss[2])
# plt.subplot(1,2,1)
# plt.plot(angle[:,0])
# plt.subplot(1,2,2)
# plt.plot(angle[:,1])

plt.show()

最终运行效果

在补一张画音乐的图