R-Thetaタイプのスカラロボットをpyqtgraphでシミュレーションしてみた
2022/07/21 categories:SCARA| tags:pyqtgraph|SCARA|ROBOT|python|
座標の計算
座標は上図のような感じで計算しました。R-Thetaタイプのスカラロボットでは、上図の赤、緑、青のアームがタイミングベルトなどでリンクして動作するので、t2の回転がr1の動きに変換されます。r2の角度はt0の回転角で決まるので、t0とt2だけでxyの移動が出来ます。基本的に直交座標を極座標に変換するだけで計算できます。計算内容は以下の通りです。
def calcurate(self, x3, y3):
a, b = 100, 80
r1 = math.sqrt(x3**2 + y3**2)
t0 = np.sign(y3) * math.acos( x3 / math.sqrt(x3**2 + y3**2) )
r2 = r1 - b
t2 = math.acos( (r2 / 2) / a )
t1 = t0 + t2
x1, y1 = a * math.cos(t1), a * math.sin(t1)
x2, y2 = r2 * math.cos(t0), r2 * math.sin(t0)実行結果
計算結果はpyqtgraphのPlotDataItemに表示しました。グラフ上で動いているように見せるためにQThreadで別スレッドを作成して一定間隔でグラフを書き換えるというプログラムにしました。
Pythonコード
# -*- coding: utf-8 -*-
import math
import numpy as np
import pyqtgraph as pg
import pandas as pd
import sys
import time
from PyQt5 import QtWidgets, QtCore, QtGui
class MainWindow(QtWidgets.QMainWindow):
def __init__(self, parent=None):
super(MainWindow, self).__init__(parent)
self.plot_items = []
self.resize(800, 600)
self.plot_widget = pg.PlotWidget()
self.plot_widget.setBackground('#FFFFFFFF')
self.plot_widget.plotItem.getAxis('bottom').setPen(pg.mkPen(color='#000000'))
self.plot_widget.plotItem.getAxis('left').setPen(pg.mkPen(color='#000000'))
self.plot_widget.plotItem.showGrid(True, True, 0.3)
self.plot_widget.setAspectLocked(True)
self.setCentralWidget(self.plot_widget)
self.j0 = pg.PlotDataItem(x=[0, 0], y=[0, 0], pen=pg.mkPen(color='#FF0000', width=2), antialias=True)
self.j1 = pg.PlotDataItem(x=[0, 0], y=[0, 0], pen=pg.mkPen(color='#00FF00', width=2), antialias=True)
self.j2 = pg.PlotDataItem(x=[0, 0], y=[0, 0], pen=pg.mkPen(color='#0000FF', width=2), antialias=True)
self.subline = pg.PlotDataItem(x=[0, 0], y=[0, 0], pen=pg.mkPen(color='#E0E0E0', width=2), antialias=True)
self.plot_widget.addItem(self.j0)
self.plot_widget.addItem(self.j1)
self.plot_widget.addItem(self.j2)
self.plot_widget.addItem(self.subline)
self.robot = Robot()
self.robot.coordinates = [
[250, 0],
[250, 100],
[ 50, 100],
[ 50,-100],
[250,-100],
[250, 0]
]
self.thread1 = QtCore.QThread()
self.robot.moveToThread(self.thread1)
self.thread1.started.connect(self.robot.run)
self.robot.updated.connect(self.plot_update)
self.thread1.start()
def plot_update(self, x, y):
self.j0.setData(x=[ 0, x[1]], y=[ 0, y[1]])
self.j1.setData(x=[x[1], x[2]], y=[y[1], y[2]])
self.j2.setData(x=[x[2], x[3]], y=[y[2], y[3]])
self.subline.setData(x=[0, x[2]], y=[0, y[2]])
def closeEvent(self, a0: QtGui.QCloseEvent) -> None:
self.thread1.terminate()
return super().closeEvent(a0)
class Robot(QtCore.QObject):
coordinates = []
updated = QtCore.pyqtSignal(list, list)
def run(self):
while True:
for i in range(len(self.coordinates) - 1):
d0, d1 = self.coordinates[i], self.coordinates[i+1]
distance = math.sqrt((d1[0] - d0[0]) ** 2 + (d1[1] - d0[1]) ** 2)
split_count = int(distance / 2.0)
x_arr = np.linspace(d0[0], d1[0], split_count)
y_arr = np.linspace(d0[1], d1[1], split_count)
for x, y in zip(x_arr, y_arr):
self.calcurate(x, y)
time.sleep(0.01)
def calcurate(self, x3, y3):
a, b = 100, 80
r1 = math.sqrt(x3**2 + y3**2)
t0 = np.sign(y3) * math.acos( x3 / math.sqrt(x3**2 + y3**2) )
r2 = r1 - b
t2 = math.acos( (r2 / 2) / a )
t1 = t0 + t2
x1, y1 = a * math.cos(t1), a * math.sin(t1)
x2, y2 = r2 * math.cos(t0), r2 * math.sin(t0)
self.updated.emit([0, x1, x2, x3], [0, y1, y2, y3])
def main():
app = QtWidgets.QApplication(sys.argv)
window = MainWindow()
window.show()
app.exec()
if __name__ == '__main__':
main()