manipulators/reference_frames.py at master · zebbarry/manipulators · GitHub

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
import robodk as rdk

GLOBAL = "global"
SILVIA = "silvia"
GRINDER = "grinder"
CUPSTACK = "cupstack"
CUP = "cup"
CROSS = "cross"
TCP = "tcp"
TOOL = "tool"
PUSHER = "pusher"
PULLER = "puller"
LEVER = "lever"
GRINDERMOUNT = "grindermount"
FILTERMOUNT = "filtermount"
CUPMOUNT = "cupmount"
FILTER = "filter"
SCRAPER = "scraper"
TAMPER = "tamper"
BALL = "ball"
ENTRY = "entry"
SILVIAPOWERON = "silviapoweron"
SILVIAPOWEROFF = "silviapoweroff"
GRINDERPOWERON = "grinderpoweron"
GRINDERPOWEROFF = "grinderpoweroff"


def read_frames(filename):
    file = open(filename, "r")
    lines = file.readlines()
    file.close()

    frames = {}
    for line in lines:
        segments = line.rstrip().split(",")
        name = segments[0]
        values = [float(i) for i in segments[1:]]
        transform = rdk.Mat([values[0:4],
                             values[4:8],
                             values[8:12],
                             values[12:16]])
        frames[name] = transform
    return frames


def read_joint_angles(filename):
    file = open(filename, "r")
    lines = file.readlines()
    file.close()

    joint_angles = {}
    for line in lines:
        segments = line.rstrip().split(",")
        name = segments[0]
        values = [float(i) for i in segments[1:]]
        angles = rdk.Mat(values)
        joint_angles[name] = angles
    return joint_angles