-
Notifications
You must be signed in to change notification settings - Fork 2
/
ForwardKinematic.py
88 lines (54 loc) · 1.55 KB
/
ForwardKinematic.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
# -*- coding: utf-8 -*-
# <nbformat>3.0</nbformat>
# <codecell>
import numpy as np
# <codecell>
def calcT(y, p, r, t):
T = np.eye(4)
# from Degree to Radians
y = y*np.pi/180.0
p = p*np.pi/180.0
r = r*np.pi/180.0
cr = np.cos(r)
sr = np.sin(r)
cp = np.cos(p)
sp = np.sin(p)
cy = np.cos(y)
sy = np.sin(y)
Rr = np.matrix([[1.0, 0.0, 0.0],[0.0, cr, -sr],[0.0, sr, cr]])
Rp = np.matrix([[cp, 0.0, sp],[0.0, 1.0, 0.0],[-sp, 0.0, cp]])
Ry = np.matrix([[cy, -sy, 0.0],[sy, cy, 0.0],[0.0, 0.0, 1.0]])
T[np.ix_([0,1,2],[0,1,2])] = Ry*Rp*Rr
# Translationsteil
T[np.ix_([0],[3])] = t
return T
# <codecell>
conf = {}
# <codecell>
T = np.eye(4)
for i in range(5):
T = calcT(0.0,0.0,0.0,i)
print T
# <codecell>
endeffector = T*np.array([0.0, 0.0, 0.0, 1.0])
endeffector
# <codecell>
import sympy
import sympybotics
# <codecell>
rbtdef = sympybotics.RobotDef('Example Robot', # robot name
[(0, 0, 0, 0), # list of tuples with Denavit-Hartenberg parameters
( 'pi/2', 0, 0, 0)], # (alpha, a, d, theta)
dh_convention='standard' # either 'standard' or 'modified'
)
# <codecell>
rbtdef.frictionmodel = {'Coulomb', 'viscous'} # options are None or a combination of 'Coulomb', 'viscous' and 'offset'
# <codecell>
rbtdef.dynparms()
# <codecell>
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
# <codecell>
rbt.geo.z
# <codecell>
rbt.kin.J[-1]
# <codecell>