-
Notifications
You must be signed in to change notification settings - Fork 16
/
GlobalFunc.py
executable file
·68 lines (61 loc) · 2 KB
/
GlobalFunc.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
from math import *
import numpy as np
def RadToDeg(value):
return value * 180.0 / pi
def DegToRad(value):
return value * pi / 180.0
def ConvertRPYToMat(psi, theta, phi):
Matrix = np.array([[cos(phi)*cos(theta), cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi), cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)],
[sin(phi)*cos(theta), sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi), sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi)],
[-sin(theta), cos(theta)*sin(psi), cos(theta)*cos(psi)]])
return Matrix
def ConvertMatToRPY(Matrix):
a11 = Matrix[0][0]
a21 = Matrix[1][0]
a31 = Matrix[2][0]
a32 = Matrix[2][1]
a33 = Matrix[2][2]
theta = -asin(a31)
psi = atan2(a32/cos(theta), a33/cos(theta))
phi = atan2(a21/cos(theta), a11/cos(theta))
return psi, theta, phi
def DHMatrix(q, d, a, alpha):
return np.array([[cos(q), -cos(alpha)*sin(q), sin(alpha)*sin(q), a*cos(q)],
[sin(q), cos(alpha)*cos(q), -sin(alpha)*cos(q), a*sin(q)],
[0, sin(alpha), cos(alpha), d],
[0,0,0,1]])
def SmartDegSubstraction(v2, v1):
# v2 and v1 is numpy array
size = v2.shape[0]
Result = v2 - v1
for i in np.arange(size):
if Result[i] > pi:
Result[i] -= 2 * pi
elif (Result[i] < -pi):
Result[i] += 2 * pi
return Result
def LoadGCode(filename, offsetx, offsety, offsetz):
file_obj = open(filename, "r")
list_of_line = file_obj.readlines()
list_of_gcode = []
act = int(0)
list_of_point = []
for ls in list_of_line:
if ("G1" in ls) or ("M300" in ls):
list_of_gcode.append(ls)
if "(end of print job)" in ls:
break
for ls in list_of_gcode:
if "M300" in ls:
ls_split_space = ls.split(" ")
if ls_split_space[1] == "S30.00":
act = int(1)
elif(ls_split_space[1] == "S50.00"):
act = int(0)
if "G1" in ls:
ls_split_space = ls.split(" ")
x = offsetx + float(ls_split_space[1][1:])
y = offsety + float(ls_split_space[2][1:])
z = offsetz
list_of_point.append([x, y, z, act])
return np.asarray(list_of_point)