-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathlbr3.py
48 lines (42 loc) · 1.65 KB
/
lbr3.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
from PyKDL import *
from math import pi
#segments = [
# Segment(Joint(Joint.None),
# Frame(Rotation(),Vector(0,0,0.110))),
# Segment(Joint(Joint.RotZ),
# Frame(Rotation.RotX(-pi/2),Vector(0,0,0.200))),
# Segment(Joint(Joint.RotZ),
# Frame(Rotation.RotX(pi/2),Vector(0,-0.200,0))),
# Segment(Joint(Joint.RotZ),
# Frame(Rotation.RotX(-pi/2),Vector(0,0.0,0.200))),
# Segment(Joint(Joint.RotZ,-1),
# Frame(Rotation.RotX(pi/2),Vector(0,-0.200,0))),
# Segment(Joint(Joint.RotZ),
# Frame(Rotation.RotX(-pi/2),Vector(0,0,0.190))),
# Segment(Joint(Joint.RotZ),
# Frame(Rotation.RotX(pi/2),Vector(0,-0.078,0))),
# Segment(Joint(Joint.RotZ),
# Frame(Vector(0,0,0.100))),
# ]
segments = [
Segment(Joint(Joint.None),
Frame(Rotation.Identity(), Vector(0.0, 0.0, 0.11))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(-pi/2), Vector(0.0, 0.0, 0.20))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(pi/2), Vector(0.0, -0.20, 0.0))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(pi/2), Vector(0, 0, .20))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(-pi/2), Vector(0, 0.2, 0))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(-pi/2), Vector(0, 0, 0.19))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(pi/2), Vector(0, -0.078, 0.0))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotZ(pi/4)*Rotation.RotY(pi/2), Vector(0.075, 0.075, -0.094))), #To wrist
]
limits_min = [-169.9, -119.9, -169.9, -119.9, -169.9, -130.1, -170.1]
limits_max = [ 169.9, 119.9, 169.9, 119.9, 169.9, 130.1, 170.1]
limits_min = [v*pi/180.0 for v in limits_min]
limits_max = [v*pi/180.0 for v in limits_max]