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')