-
Notifications
You must be signed in to change notification settings - Fork 0
/
frames visualization.py
114 lines (80 loc) · 3.13 KB
/
frames visualization.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
#packages
import numpy as np
from numpy.linalg import multi_dot
import pandas as pd
import plotly.graph_objects as go
import plotly.offline as py
from ipywidgets import interactive, HBox, VBox
py.init_notebook_mode()
#angle values
q=["some times arrays are better started at 1",0,45,0,0]
a1,a2,a3,a4=3,8,9,10
t0=[0,0,0] #zero vector
#Transformation of Rotation around x || y || z functions
def TRX(phi,t):
phi= phi*np.pi/180
TRX=np.array([[1,0,0,t[0]],[0,np.cos(phi),-np.sin(phi),t[1]],[0,np.sin(phi),np.cos(phi),t[2]],[0,0,0,1]])
return TRX
def TRY(phi,t):
phi= phi*np.pi/180
TRY=np.array([[np.cos(phi),0,np.sin(phi),t[0]],[0,1,0,t[1]],[-np.sin(phi),0,np.cos(phi),t[2]],[0,0,0,1]])
return TRY
def TRZ(phi,t):
phi= phi*np.pi/180
TRZ=np.array([[np.cos(phi),-np.sin(phi),0,t[0]],[np.sin(phi),np.cos(phi),0,t[1]],[0,0,1,t[2]],[0,0,0,1]])
return TRZ
#_______________________________________________________________
#TRZ(q1,[0,0,a1]) . TRX(90,t0) . TRZ(q2,[a2,0,0]) . TRZ(q3,t0) . TRX(90,t0) . TRZ(q4,[a3+a4,0,0])
def zxy():
T0_1=TRZ(q[1],[0,0,a1])@TRX(90,t0) #transformation matrix from frame0 to frame 1
T0_2=TRZ(q[1],[0,0,a1])@TRX(90,t0)@TRZ(q[2],[a2,0,0]) #transformation matrix from frame0 to frame 2
T0_3=TRZ(q[1],[0,0,a1])@TRX(90,t0)@TRZ(q[2],[a2,0,0])@TRZ(q[3],t0)@TRX(90,t0)#transformation matrix from frame0 to frame 3
T0_4=TRZ(q[1],[0,0,a1])@TRX(90,t0)@TRZ(q[2],[a2,0,0])@TRZ(q[3],t0)@TRX(90,t0)@TRZ(q[4],[a3+a4,0,0]) #transformation matrix from frame0 to frame 4
p0=np.array([0,0,0]) #frame0 origin
p1=np.around(T0_1[:3,3:4]) #frame1 origin
p2=np.around(T0_2[:3,3:4]) #frame2 origin
p3=np.around(T0_3[:3,3:4]) #frame3 origin
p4=np.around(T0_4[:3,3:4]) #frame4 origin
x=[int(p0[0]),int(p1[0]),int(p2[0]),int(p3[0]),int(p4[0])] #x array of all frames
y=[int(p0[1]),int(p1[1]),int(p2[1]),int(p3[1]),int(p4[1])] #y array of all frames
z=[int(p0[2]),int(p1[2]),int(p2[2]),int(p3[2]),int(p4[2])] #z array of all frames
return z,x,y
#frames visualization
f = go.FigureWidget(
data=[
go.Scatter3d(z=zxy()[0], x=zxy()[1], y=zxy()[2])],
layout=go.Layout(scene=go.layout.Scene(
camera=go.layout.scene.Camera(
up=dict(x=0, y=0, z=1),
center=dict(x=0, y=0, z=0),
eye=dict(x=1.25, y=1.25, z=1.25))
))
)
#upadting angles and positions each time the slider moves
def update_q1(q1):
q[1]=q1
f.data[0].z=zxy()[0]
f.data[0].x=zxy()[1]
f.data[0].y=zxy()[2]
def update_q2(q2):
q[2]=q2
f.data[0].z=zxy()[0]
f.data[0].x=zxy()[1]
f.data[0].y=zxy()[2]
def update_q3(q3):
q[3]=q3
f.data[0].z=zxy()[0]
f.data[0].x=zxy()[1]
f.data[0].y=zxy()[2]
def update_q4(q4):
q[4]=q4
f.data[0].z=zxy()[0]
f.data[0].x=zxy()[1]
f.data[0].y=zxy()[2]
q1_slider = interactive(update_q1, q1=(0, 180, 0.01))
q2_slider = interactive(update_q2, q2=(0, 180, 0.01))
q3_slider = interactive(update_q3, q3=(0, 180, 0.01))
q4_slider = interactive(update_q4, q4=(0, 180, 0.01))
vb = VBox((f, q1_slider,q2_slider,q3_slider,q4_slider))
vb.layout.align_items = 'center'
vb