-
Notifications
You must be signed in to change notification settings - Fork 0
/
Forward_Kinematics.py
164 lines (124 loc) · 5.12 KB
/
Forward_Kinematics.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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
"""
Brandon Sawyer and Dylan Thompson
CSE 4280
Forward Kinematics
04/30/2020
"""
import vpython as vp
from vpython import canvas, vector, color, box, sphere, rate, norm, cylinder
import time
import numpy as np
from scipy.spatial.transform import Rotation as R
# pass in theta between two frames in degrees, and length of arm between the frames
def createAdjacentTx_Ty(thetaX, thetaZ,length):
# used roatation matrix for clockwise rotations
Tx_Ty = np.array([[np.cos(thetaX), -np.sin(thetaX), np.sin(thetaX)*np.sin(thetaZ), 0],
[np.sin(thetaX), np.cos(thetaX)*np.cos(thetaZ), -np.cos(thetaX)*np.sin(thetaZ), length],
[0, np.sin(thetaZ), np.cos(thetaZ), 0],
[0, 0, 0, 1]])
return Tx_Ty
def end_effector(P1_P2, P2_P3, P3_P4, P4_P5):
T0_2 = np.matmul(P0_P1, P1_P2)
T0_3 = np.matmul(T0_2, P2_P3)
T0_4 = np.matmul(T0_3, P3_P4)
T0_5 = np.matmul(T0_4, P4_P5)
return P0_P1, T0_2, T0_3, T0_4, T0_5
def posCal(start, offset):
return start + vector(offset[0],offset[1],offset[2])
side = 50.0
thk = 0.3
s3 = 2 * side + thk
zero = vector(0, 20, 0)
# transformation matric between frames 0 and 1.
# hardcoded beacuse it is the only transformation with an x translation
P0_P1 = np.array([[1, 0, 0, 0],
[0, 1, 0, 0.3],
[0, 0, 1, 0],
[0, 0, 0, 1]])
scene2 = canvas(title='Collision Avoidance',
width=1800,
height=900,
center=zero,
background=color.gray(.2),
forward=vector(0, -2, -5),
fov=70)
floor = box(pos=vector(0, 0, 0),
size=vector(s3, thk, s3),
color=color.gray(1))
base = cylinder(axis=vector(0, 1, 0),
pos=vector(0, thk, 0),
color=color.red,
radius=3,
length=5)
joint1 = sphere(pos=posCal(base.pos, [0, base.length + 1, 0]),
radius=2.9,
color=color.white)
arm1 = cylinder(axis=vector(0, 1, 0),
pos=joint1.pos,
color=color.red,
length=10 + joint1.radius)
joint2 = sphere(pos=posCal(arm1.pos, [0, arm1.length, 0]),
radius=1.5,
color=color.white)
arm2 = cylinder(axis=vector(0, 1, 0),
pos=joint2.pos,
color=color.red,
length=5 + joint2.radius)
joint3 = sphere(pos=posCal(arm2.pos, [0, arm2.length, 0]),
radius=1.5,
color=color.white)
arm3 = cylinder(axis=vector(0, 1, 0),
pos=joint3.pos,
color=color.red,
length=5 + joint3.radius)
effector = sphere(pos=posCal(arm3.pos, [0, arm3.length, 0]),
radius=0.5,
color=color.white,
# make_trail=True,
# retain=400
)
def main():
P1_P2 = createAdjacentTx_Ty(vp.radians(-45), vp.radians(0), base.length + 1)
P2_P3 = createAdjacentTx_Ty(vp.radians(90), vp.radians(0), arm1.length)
P3_P4 = createAdjacentTx_Ty(vp.radians(-45), vp.radians(0), arm2.length)
P4_P5 = createAdjacentTx_Ty(vp.radians(0), vp.radians(0), arm3.length)
P0_1, P0_2, P0_3, P0_4, P0_5 = end_effector(P1_P2, P2_P3, P3_P4, P4_P5)
r1 = R.from_matrix(P0_1[:3,:3])
r2 = R.from_matrix(P0_2[:3,:3])
r3 = R.from_matrix(P0_3[:3,:3])
r4 = R.from_matrix(P0_4[:3,:3])
r5 = R.from_matrix(P0_5[:3,:3])
# print(P1_P2[:3,:3])
# print(r1.as_euler('zyx', degrees=True))
print(r2.as_euler('zyx', degrees=True))
print(r3.as_euler('zyx', degrees=True))
print(r4.as_euler('zyx', degrees=True))
# print(r5.as_euler('zyx', degrees=True))
bR = r1.as_euler('zyx', degrees=True)
a1R = r2.as_euler('zyx', degrees=True)
a2R = r3.as_euler('zyx', degrees=True)
a3R = r4.as_euler('zyx', degrees=True)
eR = r5.as_euler('zyx', degrees=True)
time.sleep(5)
dt = 0.01
for theta in range(0, 100):
rate(20)
# a1RX = vp.radians(a1R[0]*dt)
# a2RX = vp.radians(a2R[0]*dt)
# a3RX = vp.radians(a3R[0]*dt)
arm1.rotate(angle=vp.radians(a1R[0]*dt), axis=vector(0, 0, 1))
arm1.rotate(angle=vp.radians(a1R[1]*dt), axis=vector(0, 1, 0))
arm1.rotate(angle=vp.radians(a1R[2]*dt), axis=vector(1, 0, 0))
joint2.pos = arm1.pos + arm1.axis
arm2.pos = joint2.pos
arm2.rotate(angle=vp.radians(a2R[0]*dt), axis=vector(0, 0, 1))
arm2.rotate(angle=vp.radians(a2R[1]*dt), axis=vector(0, 1, 0))
arm2.rotate(angle=vp.radians(a2R[2]*dt), axis=vector(1, 0, 0))
joint3.pos = arm2.pos + arm2.axis
arm3.pos = joint3.pos
arm3.rotate(angle=vp.radians(a3R[0]*dt), axis=vector(0, 0, 1))
arm3.rotate(angle=vp.radians(a3R[1]*dt), axis=vector(0, 1, 0))
arm3.rotate(angle=vp.radians(a3R[2]*dt), axis=vector(1, 0, 0))
effector.pos = arm3.pos + arm3.axis
if __name__ == '__main__':
main()