def LoadFile(self):
self.UpdateData(1)
self.UpdateData(2)
if self.fileName == None:
self.valueStatus.setText("Error: file is not found")
else:
self.count = 0
self.RB.listPoints = np.array([[0,0,0]])
self.RB.color=np.array([0])
self.AllPoints = np.array([[None, None, None]])
self.AllJVars = np.array([[None, None, None, None]])
self.toolstatus = np.array([None])
self.valueStatus.setText("Processing...")
self.processLoadFile.setValue(30)
listPoint = LoadGCode(self.fileName, self.objRB.EVars[0], self.objRB.EVars[1], self.objRB.EVars[2])
self.RB.updateGL()
trj = Trajectory()
listPoint = np.insert(listPoint, 0, [self.objRB.EVars[0], self.objRB.EVars[1], self.objRB.EVars[2], 0], axis = 0)
listPoint = np.append(listPoint, [[self.objRB.EVars[0], self.objRB.EVars[1], self.objRB.EVars[2], 0]], axis = 0)
toolstt_tmp = np.array([None])
trj.SetSpTime(0.1)
for i in np.arange(len(listPoint)-1):
p1 = listPoint[i][:3]
p2 = listPoint[i+1][:3]
if i==0 or i == len(listPoint)-2:
trj.SetPoint(p1,p2,100)
else:
trj.SetPoint(p1, p2, 100)
points = trj.Calculate()
if points[0] == False:
pass
else:
self.AllPoints = np.append(self.AllPoints, points[1], axis = 0)
self.toolstatus = np.append(self.toolstatus, [listPoint[i+1][3]]*len(points[1]))
self.toolstatus = np.delete(self.toolstatus, 0)
self.AllPoints = np.delete(self.AllPoints, 0, axis = 0)
toolstt_tmp = np.delete(toolstt_tmp, 0)
q1P = self.objRB.q1P
q2P = self.objRB.q2P
for p in self.AllPoints:
EVars = np.append(p, [self.objRB.EVars[3], self.objRB.EVars[4], self.objRB.EVars[5]])
JVar = self.objRB.CalInvPositionEx(EVars, q1P, q2P)
if JVar[0] == False:
break
self.AllJVars = np.append(self.AllJVars, [JVar[1]], axis = 0)
q2P = q1P
q1P = JVar[1]
self.AllJVars = np.delete(self.AllJVars, 0, axis = 0)
self.processLoadFile.setValue(100)
if len(self.AllJVars) == len(self.AllPoints):
self.valueStatus.setText("All done")
else:
self.valueStatus.setText("Some points is missing")
from sys import argv
from GlobalFunc import *
import OpenGLControl as DrawRB
from Robot import *
from Trajectory import *
import numpy as np
import matplotlib.pyplot as plt
script, first = argv
trj = Trajectory()
f_point = np.array([220, 310, 70])
e_point = np.array([-100, 430, 260])
trj.SetSpTime(0.1)
trj.SetPoint(f_point, e_point, 30)
points = trj.Calculate()
T = 0.1 * len(points[1])
t = np.arange(0., T, 0.1)
objRB = Robot()
AllJVar = np.array([[None, None, None, None]])
for p in points[1]:
p = np.append(p, [objRB.EVars[3], objRB.EVars[4], objRB.EVars[5]])
JVar = objRB.CalInvPositionEx(p, sol=4)
AllJVar = np.append(AllJVar, [JVar[1]], axis=0)
AllJVar = np.delete(AllJVar, 0, axis=0)
if first == 'x':
plt.plot(t, points[1][:, 0], 'r', t, points[1][:, 1], 'g--', t,
points[1][:, 2], 'b:')
plt.xlabel('t(s)')
plt.ylabel('mm')
plt.title('Đồ thị điểm thao tác theo thời gian')
