Esempio n. 1
0
class RobotSimulator(QMainWindow):

	def __init__(self, *args):
		super(RobotSimulator, self).__init__(*args)

		loadUi('main.ui', self)
		self.timer  = QTimer()
		self.count = 0
		self.timer.timeout.connect(self.timeEvent)
		self.objRB = Robot()
		self.RB = DrawRB.GLWidget(self, self.objRB)
		# self.objRB.cf = ConfigRobot()
		self.UpdateData(1)
		self.UpdateData(2)
		self.fileName = None
		self.AllPoints = np.array([[None, None, None]])
		self.AllJVars = np.array([[None, None, None, None]])
		self.toolstatus = np.array([None])
		self.isRun = True


	def setupUI(self):	
		self.setCentralWidget(self.RB)
		self.sliderQ1.setMinimum(-360)
		self.sliderQ1.setMaximum(360)
		self.sliderQ1.setValue(RadToDeg(self.objRB.cf.q_init[1]))
		self.sliderQ1.setTickInterval(1)
		self.sliderQ1.sliderMoved.connect(lambda: self.valueChangeJVars(0, self.sliderQ1.value()))

		self.sliderQ2.setMinimum(-360)
		self.sliderQ2.setMaximum(360)
		self.sliderQ2.setValue(RadToDeg(self.objRB.cf.q_init[2]))
		self.sliderQ2.setTickInterval(1)
		self.sliderQ2.sliderMoved.connect(lambda: self.valueChangeJVars(1, self.sliderQ2.value()))

		self.sliderQ3.setMinimum(-360)
		self.sliderQ3.setMaximum(360)
		self.sliderQ3.setValue(RadToDeg(self.objRB.cf.q_init[3]))
		self.sliderQ3.setTickInterval(1)
		self.sliderQ3.sliderMoved.connect(lambda: self.valueChangeJVars(2, self.sliderQ3.value()))

		self.sliderQ4.setMinimum(-360)
		self.sliderQ4.setMaximum(360)
		self.sliderQ4.setValue(RadToDeg(self.objRB.cf.q_init[4]))
		self.sliderQ4.setTickInterval(1)
		self.sliderQ4.sliderMoved.connect(lambda: self.valueChangeJVars(3, self.sliderQ4.value()))

		# self.setCentralWidget(self.RB)
		self.sliderX.setMinimum(-1000)
		self.sliderX.setMaximum(1000)
		self.sliderX.setValue(self.objRB.EVars[0])
		self.sliderX.setTickInterval(1)
		self.sliderX.sliderMoved.connect(self.valueChangeEVars)

		self.sliderY.setMinimum(-1000)
		self.sliderY.setMaximum(1000)
		self.sliderY.setValue(self.objRB.EVars[1])
		self.sliderY.setTickInterval(1)
		self.sliderY.sliderMoved.connect(self.valueChangeEVars)

		self.sliderZ.setMinimum(-1000)
		self.sliderZ.setMaximum(1000)
		self.sliderZ.setValue(self.objRB.EVars[2])
		self.sliderZ.setTickInterval(1)
		self.sliderZ.sliderMoved.connect(self.valueChangeEVars)

		self.sliderAngleR.setMinimum(-360)
		self.sliderAngleR.setMaximum(360)
		self.sliderAngleR.setValue(RadToDeg(self.objRB.EVars[3]))
		self.sliderAngleR.setTickInterval(1)
		self.sliderAngleR.sliderMoved.connect(self.valueChangeEVars)

		self.sliderAngleP.setMinimum(-360)
		self.sliderAngleP.setMaximum(360)
		self.sliderAngleP.setValue(RadToDeg(self.objRB.EVars[4]))
		self.sliderAngleP.setTickInterval(1)
		self.sliderAngleP.sliderMoved.connect(self.valueChangeEVars)

		self.sliderAngleY.setMinimum(-360)
		self.sliderAngleY.setMaximum(360)
		self.sliderAngleY.setValue(RadToDeg(self.objRB.EVars[5]))
		self.sliderAngleY.setTickInterval(1)
		self.sliderAngleY.sliderMoved.connect(self.valueChangeEVars)

		self.sliderAngleR.setEnabled = False
		self.btnOpenFile.clicked.connect(self.openFileNameDialog)
		self.btnLoadFile.clicked.connect(self.LoadFile)
		self.btnRun.clicked.connect(self.Run)
		self.btnStop.clicked.connect(self.Stop)
		# self.btnClear.clicked.connect(self.Clear)
		# self.actionDock_Control_Panel.addAction(self.ViewGrid)
		self.actionDock_Control_Panel.triggered.connect(self.ViewGrid)
		self.actionAbout.triggered.connect(self.ShowAbout)
		self.btnUpdateStatusTab1.clicked.connect(lambda: self.UpdateData(1))
		self.btnUpdateStatusTab2.clicked.connect(lambda: self.UpdateData(2))

	def ShowAbout(self):
		msg = QMessageBox()
		msg.setIcon(QMessageBox.Information)
		msg.setText("Robot Simulator")
		msg.setInformativeText("Project: Robot design\
			\n\nCode by: Nguyen Van Khuong\
			\nSource code: https://github.com/khuonghust/RobotSimulator\
			\nVideo demo: https://tinyurl.com/robot-simulation-python-opengl")
		msg.setWindowTitle("About Dialog")
		msg.exec_()

	def Stop(self):
		self.isRun = False

	def ViewGrid(self):
		self.RB.isDrawGrid = not (self.RB.isDrawGrid)
		self.RB.updateGL()

	def valueChangeJVars(self, index, value):
		self.objRB.JVars[index] = DegToRad(value)
		self.objRB.CalFwdPostion(self.objRB.JVars)
		self.RB.updateGL()
		self.UpdateData(1)

	def valueChangeEVars(self):
		x = self.sliderX.value()
		y = self.sliderY.value()
		z = self.sliderZ.value()
		psi = DegToRad(self.sliderAngleR.value())
		theta = DegToRad(self.sliderAngleP.value())
		phi =  DegToRad(self.sliderAngleY.value())
		EVars = np.array([x, y, z, psi, theta, phi])
		self.objRB.CalInvPostion(EVars)
		self.UpdateData(2)
		self.RB.updateGL()

	def openFileNameDialog(self):    
		options = QFileDialog.Options()
		options |= QFileDialog.DontUseNativeDialog
		self.fileName, _ = QFileDialog.getOpenFileName(self,"Openfile", "","Gcode Files (*.gcode);;All Files (*)", options=options)
		self.processLoadFile.setValue(0)
		self.valueStatus.setText("None...")

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

	def Run(self):
		if len(self.AllJVars)> 1:
			self.isRun = True
			self.timeEvent()
		else:
			self.valueStatus.setText("Error: Data is empty.")

	def timeEvent(self):
		try:
			self.objRB.JVars = self.AllJVars[self.count]
			self.RB.listPoints = np.append(self.RB.listPoints, [self.AllPoints[self.count]], axis = 0)
			if self.toolstatus[self.count] == 1:
				self.RB.color = np.append(self.RB.color, 1)
			else:
				self.RB.color = np.append(self.RB.color, 0)

			self.count +=1
			self.RB.updateGL()
		finally:
			if (self.count < len(self.AllJVars) - 1) and self.isRun:
				QTimer.singleShot(0, self.timeEvent)
			else:
				self.valueStatus.setText("Done")

	def UpdateData(self, tabsel):
		if tabsel == 1:
			self.sliderX.setValue(int(self.objRB.EVars[0]))
			self.sliderY.setValue(int(self.objRB.EVars[1]))
			self.sliderZ.setValue(int(self.objRB.EVars[2]))
			self.sliderAngleR.setValue(int(RadToDeg(self.objRB.EVars[3])))
			self.sliderAngleP.setValue(int(RadToDeg(self.objRB.EVars[4])))
			self.sliderAngleY.setValue(int(RadToDeg(self.objRB.EVars[5])))
		elif tabsel == 2:
			self.valueQ1.setText(str(int(RadToDeg(self.objRB.JVars[0]))))
			self.valueQ2.setText(str(int(RadToDeg(self.objRB.JVars[1]))))
			self.valueQ3.setText(str(int(RadToDeg(self.objRB.JVars[2]))))
			self.valueQ4.setText(str(int(RadToDeg(self.objRB.JVars[3]))))
			self.sliderQ1.setValue(int(RadToDeg(self.objRB.JVars[0])))
			self.sliderQ2.setValue(int(RadToDeg(self.objRB.JVars[1])))
			self.sliderQ3.setValue(int(RadToDeg(self.objRB.JVars[2])))
			self.sliderQ4.setValue(int(RadToDeg(self.objRB.JVars[3])))

		self.valueQ1.setText(str(int(RadToDeg(self.objRB.JVars[0]))))
		self.valueQ2.setText(str(int(RadToDeg(self.objRB.JVars[1]))))
		self.valueQ3.setText(str(int(RadToDeg(self.objRB.JVars[2]))))
		self.valueQ4.setText(str(int(RadToDeg(self.objRB.JVars[3]))))

		self.valueX.setText(str(format(self.objRB.EVars[0], '.2f')))
		self.valueY.setText(str(format(self.objRB.EVars[1], '.2f')))
		self.valueZ.setText(str(format(self.objRB.EVars[2], '.2f')))
		self.valueAngleR.setText(str(int(RadToDeg(self.objRB.EVars[3]))))
		self.valueAngleP.setText(str(int(RadToDeg(self.objRB.EVars[4]))))
		self.valueAngleY.setText(str(int(RadToDeg(self.objRB.EVars[5]))))
Esempio n. 2
0
class RobotSimulator(QMainWindow):

	def __init__(self, *args):
		super(RobotSimulator, self).__init__(*args)
		loadUi('main.ui', self)
		self.objRB = Robot()
		self.RB = DrawRB.GLWidget(self, self.objRB)
		# self.objRB.cf = ConfigRobot()
		self.UpdateData()

	def setupUI(self):	
		self.setCentralWidget(self.RB)
		self.sliderQ1.setMinimum(-360)
		self.sliderQ1.setMaximum(360)
		self.sliderQ1.setValue(RadToDeg(self.objRB.cf.q_init[1]))
		self.sliderQ1.setTickInterval(1)
		self.sliderQ1.sliderMoved.connect(lambda: self.valueChangeJVars(1, self.sliderQ1.value()))

		self.sliderQ2.setMinimum(-360)
		self.sliderQ2.setMaximum(360)
		self.sliderQ2.setValue(RadToDeg(self.objRB.cf.q_init[2]))
		self.sliderQ2.setTickInterval(1)
		self.sliderQ2.sliderMoved.connect(lambda: self.valueChangeJVars(2, self.sliderQ2.value()))

		self.sliderQ3.setMinimum(-360)
		self.sliderQ3.setMaximum(360)
		self.sliderQ3.setValue(RadToDeg(self.objRB.cf.q_init[3]))
		self.sliderQ3.setTickInterval(1)
		self.sliderQ3.sliderMoved.connect(lambda: self.valueChangeJVars(3, self.sliderQ3.value()))

		self.sliderQ4.setMinimum(-360)
		self.sliderQ4.setMaximum(360)
		self.sliderQ4.setValue(RadToDeg(self.objRB.cf.q_init[4]))
		self.sliderQ4.setTickInterval(1)
		self.sliderQ4.sliderMoved.connect(lambda: self.valueChangeJVars(4, self.sliderQ4.value()))

		# self.setCentralWidget(self.RB)
		self.sliderX.setMinimum(-1000)
		self.sliderX.setMaximum(1000)
		self.sliderX.setValue(self.objRB.x)
		self.sliderX.setTickInterval(1)
		self.sliderX.sliderMoved.connect(self.valueChangeEVars)

		self.sliderY.setMinimum(-1000)
		self.sliderY.setMaximum(1000)
		self.sliderY.setValue(self.objRB.y)
		self.sliderY.setTickInterval(1)
		self.sliderY.sliderMoved.connect(self.valueChangeEVars)

		self.sliderZ.setMinimum(-1000)
		self.sliderZ.setMaximum(1000)
		self.sliderZ.setValue(self.objRB.z)
		self.sliderZ.setTickInterval(1)
		self.sliderZ.sliderMoved.connect(self.valueChangeEVars)

		self.sliderAngleR.setMinimum(-360)
		self.sliderAngleR.setMaximum(360)
		self.sliderAngleR.setValue(RadToDeg(self.objRB.psi))
		self.sliderAngleR.setTickInterval(1)
		self.sliderAngleR.sliderMoved.connect(self.valueChangeEVars)

		self.sliderAngleP.setMinimum(-360)
		self.sliderAngleP.setMaximum(360)
		self.sliderAngleP.setValue(RadToDeg(self.objRB.theta))
		self.sliderAngleP.setTickInterval(1)
		self.sliderAngleP.sliderMoved.connect(self.valueChangeEVars)

		self.sliderAngleY.setMinimum(-360)
		self.sliderAngleY.setMaximum(360)
		self.sliderAngleY.setValue(RadToDeg(self.objRB.phi))
		self.sliderAngleY.setTickInterval(1)
		self.sliderAngleY.sliderMoved.connect(self.valueChangeEVars)


	def valueChangeJVars(self, index, value):
		self.objRB.q[index] = DegToRad(value)
		self.objRB.CalFwdPostion(*tuple(self.objRB.q[1:]))
		self.RB.updateGL()
		self.UpdateData()

	def valueChangeEVars(self):
		x = self.sliderX.value()
		y = self.sliderY.value()
		z = self.sliderZ.value()
		psi = DegToRad(self.sliderAngleR.value())
		theta = DegToRad(self.sliderAngleP.value())
		phi =  DegToRad(self.sliderAngleY.value())
		# print(x, " ", y)
		self.objRB.CalInvPostion(x, y, z, psi, theta, phi)
		self.RB.updateGL()

		# self.UpdateData()

	def UpdateData(self):
		self.valueQ1.setText(str(int(RadToDeg(self.objRB.q[1]))))
		self.valueQ2.setText(str(int(RadToDeg(self.objRB.q[2]))))
		self.valueQ3.setText(str(int(RadToDeg(self.objRB.q[3]))))
		self.valueQ4.setText(str(int(RadToDeg(self.objRB.q[4]))))
		self.valueX.setText(str(format(self.objRB.x, '.2f')))
		self.valueY.setText(str(format(self.objRB.y, '.2f')))
		self.valueZ.setText(str(format(self.objRB.z, '.2f')))
		self.valueAngleR.setText(str(int(RadToDeg(self.objRB.psi))))
		self.valueAngleP.setText(str(int(RadToDeg(self.objRB.theta))))
		self.valueAngleY.setText(str(int(RadToDeg(self.objRB.phi))))
		self.sliderX.setValue(int(self.objRB.x))
		self.sliderY.setValue(int(self.objRB.y))
		self.sliderZ.setValue(int(self.objRB.x))
		self.sliderAngleR.setValue(int(RadToDeg(self.objRB.psi)))
		self.sliderAngleP.setValue(int(RadToDeg(self.objRB.theta)))
		self.sliderAngleY.setValue(int(RadToDeg(self.objRB.phi)))