├── LICENSE.md
├── ReadMe.md
├── Screenshot (53).png
├── Screenshot (54).png
├── Screenshot (55).png
├── SerialCom.py
├── armControl.py
├── design.ui
├── down.png
├── invKin.py
├── kukadesign.gif
├── roboticArm.png
└── up.png


/LICENSE.md:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2020 Md. Zahidul Islam
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/ReadMe.md:
--------------------------------------------------------------------------------
 1 | <img src="https://github.com/Zedd1558/Inverse-Kinematics-6-DOF-for-ERC-2019/blob/master/roboticArm.png"  width="100" height="100" />
 2 | 
 3 | # InverseKinematics 6 DOF 
 4 | In robotics, inverse kinematics makes use of the kinematics equations to determine the joint parameters that provide a desired position for each of the robot's end-effectors. In this program, we will apply inverse kinematics on a 6 DOF robotic arm. (KUKA based design)
 5 | We have the robotic arm's measurements i.e. link lengths, link configuration, rotation information. Given coordinates X, Y, Z and roll, pitch, yaw of the end effector, We have to determine every joint angle from q1, q2, q3 ,q4, q5, q6.
 6 | 
 7 | ### The mathematical calculations of IK are taken from https://github.com/mithi/arm-ik/
 8 | 
 9 | # Procedure
10 | In the terminal enter "python armControl.py". This will open up a GUI window where you give inputs and output angles are calculated.
11 | Inverse Kinematics is calculated in the file invKin.py. It is called by armControl.py. Arm control has a GUI that we use to control X Y Z roll pitch yaw. To see whether the output of the Inverse Kinematics is correct we use a live Matplotlib 3d graph. invKin.py has functions forwardKinematics() and getAngles() and some other helper functions. Given, the joint angles q1..q6 forwardKinematics() calculates the joint coordinates along with the end effector's coordinates. This helps us to draw the arm in matplotlib. Matplotlib window automatically pop up when you run armControl.py.
12 | # References 
13 | some scholarly aritcles where the algorithms is explaind in detail.\
14 | http://scholar.google.com/scholar_url?url=https://forum.linuxcnc.org/media/kunena/attachments/1549/6-axis-serial-robot-2.pdf&hl=en&sa=X&scisig=AAGBfm0JxooSQg2Bp5iJwO5Te8mIan0TtA&nossl=1&oi=scholarr \
15 | \
16 | http://atmia.put.poznan.pl/Woluminy/Fil/ATMiA_34_3_5.pdf
17 | # Some instructions
18 | ```
19 | python armControl.py 
20 | ```
21 | A GUI having control of X,Y,Z,roll,pitch,yaw will be initiated. A matplotlib window will be initiated. Play with GUI buttons to see inverse kinematics in action.
22 | ![](https://github.com/Zedd1558/Inverse-Kinematics-6-DOF-for-ERC-2019/blob/master/Screenshot%20(55).png)
23 | ![](https://github.com/Zedd1558/Inverse-Kinematics-6-DOF-for-ERC-2019/blob/master/Screenshot%20(54).png)
24 | ![](https://github.com/Zedd1558/Inverse-Kinematics-6-DOF-for-ERC-2019/blob/master/Screenshot%20(53).png)
25 | 
26 | # Author 
27 | Md. Zahidul Islam,
28 | Lecturer, CSE, IUT,
29 | Islamic University of Technology. 
30 | https://github.com/Zedd1558
31 | 


--------------------------------------------------------------------------------
/Screenshot (53).png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zahid58/Inverse-Kinematics-6-DOF-Python-With-Visualizer/495c4de76a4cf5836e2cd7e8c7f409c8930d24d4/Screenshot (53).png


--------------------------------------------------------------------------------
/Screenshot (54).png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zahid58/Inverse-Kinematics-6-DOF-Python-With-Visualizer/495c4de76a4cf5836e2cd7e8c7f409c8930d24d4/Screenshot (54).png


--------------------------------------------------------------------------------
/Screenshot (55).png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zahid58/Inverse-Kinematics-6-DOF-Python-With-Visualizer/495c4de76a4cf5836e2cd7e8c7f409c8930d24d4/Screenshot (55).png


--------------------------------------------------------------------------------
/SerialCom.py:
--------------------------------------------------------------------------------
 1 | import serial 
 2 | 
 3 | 
 4 | class SerialCom:    
 5 |     #arduinoOut = serial.Serial('COM7',115200,timeout =.2)
 6 |     
 7 |     def getConn(self,comm,baudRate):
 8 |         self.arduinoOut = serial.Serial(comm,baudRate)
 9 |         return self.arduinoOut
10 | 
11 |     def sendString(self,data):
12 |         data += "\n"
13 |         self.arduinoOut.write(data.encode())
14 | 
15 |     def sendAngles(self,data):
16 |         data = "{},{},{},{}\n".format(data[0],data[1],data[2],data[3])
17 |         self.sendString(data)
18 | 
19 |     def receiveString(self):
20 |         data = self.arduinoOut.readline()
21 |         #print(data)
22 |         return data
23 |     
24 |     def receiveAngles(self):
25 |         data = self.receiveString()
26 |         data = data.strip(" ")
27 |         angles  = []
28 |         for angle in data:
29 |             angles.append(int(angle))
30 |         return angles
31 |         
32 |     def closeConn(self):
33 |         self.arduinoOut.close()
34 | 
35 | 
36 | def main():
37 |     SerialObj = SerialCom()
38 |     arduinoOut = SerialObj.getConn('COM9',9600)
39 |     while True:
40 |         string_ = input('>')
41 |         SerialObj.sendString(string_)
42 | 
43 | 
44 |     
45 | 
46 | 
47 | if __name__=="__main__":
48 |     main()


--------------------------------------------------------------------------------
/armControl.py:
--------------------------------------------------------------------------------
  1 | # -*- coding: utf-8 -*-
  2 | 
  3 | # Form implementation generated from reading ui file 'design.ui'
  4 | #
  5 | # Created by: PyQt5 UI code generator 5.11.3
  6 | #
  7 | # WARNING! All changes made in this file will be lost!
  8 | 
  9 | from PyQt5 import QtCore, QtGui, QtWidgets
 10 | import sys
 11 | import SerialCom
 12 | from invKin import *
 13 | 
 14 | class Ui_ArmControl(object):
 15 |     def setupUi(self, ArmControl):
 16 |         ArmControl.setObjectName("ArmControl")
 17 |         ArmControl.setWindowModality(QtCore.Qt.NonModal)
 18 |         ArmControl.resize(482, 537)
 19 |         ArmControl.setMaximumSize(QtCore.QSize(16777215, 16777215))
 20 |         icon = QtGui.QIcon()
 21 |         icon.addPixmap(QtGui.QPixmap("../roboticArm.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off)
 22 |         ArmControl.setWindowIcon(icon)
 23 |         ArmControl.setWindowOpacity(1.0)
 24 |         ArmControl.setAutoFillBackground(False)
 25 |         self.xUp = QtWidgets.QPushButton(ArmControl)
 26 |         self.xUp.setGeometry(QtCore.QRect(360, 130, 41, 41))
 27 |         font = QtGui.QFont()
 28 |         font.setFamily("Roboto Slab")
 29 |         font.setPointSize(16)
 30 |         self.xUp.setFont(font)
 31 |         self.xUp.setStyleSheet("color : rgb(255, 255, 255);\n"
 32 | "")
 33 |         self.xUp.setText("")
 34 |         icon1 = QtGui.QIcon()
 35 |         icon1.addPixmap(QtGui.QPixmap("up.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off)
 36 |         self.xUp.setIcon(icon1)
 37 |         self.xUp.setIconSize(QtCore.QSize(32, 32))
 38 |         self.xUp.setFlat(True)
 39 |         self.xUp.setObjectName("xUp")
 40 |         self.xVal = QtWidgets.QLabel(ArmControl)
 41 |         self.xVal.setGeometry(QtCore.QRect(260, 130, 81, 41))
 42 |         font = QtGui.QFont()
 43 |         font.setFamily("Roboto Slab")
 44 |         font.setPointSize(14)
 45 |         self.xVal.setFont(font)
 46 |         self.xVal.setFrameShape(QtWidgets.QFrame.Box)
 47 |         self.xVal.setFrameShadow(QtWidgets.QFrame.Raised)
 48 |         self.xVal.setLineWidth(1)
 49 |         self.xVal.setMidLineWidth(0)
 50 |         self.xVal.setAlignment(QtCore.Qt.AlignCenter)
 51 |         self.xVal.setWordWrap(False)
 52 |         self.xVal.setObjectName("xVal")
 53 |         self.yVal = QtWidgets.QLabel(ArmControl)
 54 |         self.yVal.setGeometry(QtCore.QRect(260, 190, 81, 41))
 55 |         font = QtGui.QFont()
 56 |         font.setFamily("Roboto Slab")
 57 |         font.setPointSize(14)
 58 |         self.yVal.setFont(font)
 59 |         self.yVal.setFrameShape(QtWidgets.QFrame.Box)
 60 |         self.yVal.setFrameShadow(QtWidgets.QFrame.Raised)
 61 |         self.yVal.setLineWidth(1)
 62 |         self.yVal.setMidLineWidth(0)
 63 |         self.yVal.setAlignment(QtCore.Qt.AlignCenter)
 64 |         self.yVal.setWordWrap(False)
 65 |         self.yVal.setObjectName("yVal")
 66 |         self.rollVal = QtWidgets.QLabel(ArmControl)
 67 |         self.rollVal.setGeometry(QtCore.QRect(260, 310, 81, 41))
 68 |         font = QtGui.QFont()
 69 |         font.setFamily("Roboto Slab")
 70 |         font.setPointSize(14)
 71 |         self.rollVal.setFont(font)
 72 |         self.rollVal.setFrameShape(QtWidgets.QFrame.Box)
 73 |         self.rollVal.setFrameShadow(QtWidgets.QFrame.Raised)
 74 |         self.rollVal.setLineWidth(1)
 75 |         self.rollVal.setMidLineWidth(0)
 76 |         self.rollVal.setAlignment(QtCore.Qt.AlignCenter)
 77 |         self.rollVal.setWordWrap(False)
 78 |         self.rollVal.setObjectName("rollVal")
 79 |         self.zVal = QtWidgets.QLabel(ArmControl)
 80 |         self.zVal.setGeometry(QtCore.QRect(260, 250, 81, 41))
 81 |         font = QtGui.QFont()
 82 |         font.setFamily("Roboto Slab")
 83 |         font.setPointSize(14)
 84 |         self.zVal.setFont(font)
 85 |         self.zVal.setFrameShape(QtWidgets.QFrame.Box)
 86 |         self.zVal.setFrameShadow(QtWidgets.QFrame.Raised)
 87 |         self.zVal.setLineWidth(1)
 88 |         self.zVal.setMidLineWidth(0)
 89 |         self.zVal.setAlignment(QtCore.Qt.AlignCenter)
 90 |         self.zVal.setWordWrap(False)
 91 |         self.zVal.setObjectName("zVal")
 92 |         self.xLabel = QtWidgets.QLabel(ArmControl)
 93 |         self.xLabel.setGeometry(QtCore.QRect(80, 130, 81, 41))
 94 |         font = QtGui.QFont()
 95 |         font.setFamily("Roboto Slab")
 96 |         font.setPointSize(12)
 97 |         self.xLabel.setFont(font)
 98 |         self.xLabel.setAutoFillBackground(False)
 99 |         self.xLabel.setStyleSheet("background-color: rgb(237, 255, 248)")
100 |         self.xLabel.setFrameShape(QtWidgets.QFrame.Panel)
101 |         self.xLabel.setFrameShadow(QtWidgets.QFrame.Raised)
102 |         self.xLabel.setLineWidth(1)
103 |         self.xLabel.setMidLineWidth(0)
104 |         self.xLabel.setAlignment(QtCore.Qt.AlignCenter)
105 |         self.xLabel.setWordWrap(False)
106 |         self.xLabel.setObjectName("xLabel")
107 |         self.yUp = QtWidgets.QPushButton(ArmControl)
108 |         self.yUp.setGeometry(QtCore.QRect(360, 190, 41, 41))
109 |         font = QtGui.QFont()
110 |         font.setFamily("Roboto Slab")
111 |         font.setPointSize(16)
112 |         self.yUp.setFont(font)
113 |         self.yUp.setStyleSheet("color : rgb(255, 255, 255);\n"
114 | "")
115 |         self.yUp.setText("")
116 |         self.yUp.setIcon(icon1)
117 |         self.yUp.setIconSize(QtCore.QSize(32, 32))
118 |         self.yUp.setFlat(True)
119 |         self.yUp.setObjectName("yUp")
120 |         self.rollup = QtWidgets.QPushButton(ArmControl)
121 |         self.rollup.setGeometry(QtCore.QRect(360, 310, 41, 41))
122 |         font = QtGui.QFont()
123 |         font.setFamily("Roboto Slab")
124 |         font.setPointSize(16)
125 |         self.rollup.setFont(font)
126 |         self.rollup.setStyleSheet("color : rgb(255, 255, 255);\n"
127 | "")
128 |         self.rollup.setText("")
129 |         self.rollup.setIcon(icon1)
130 |         self.rollup.setIconSize(QtCore.QSize(32, 32))
131 |         self.rollup.setFlat(True)
132 |         self.rollup.setObjectName("rollup")
133 |         self.zUp = QtWidgets.QPushButton(ArmControl)
134 |         self.zUp.setGeometry(QtCore.QRect(360, 250, 41, 41))
135 |         font = QtGui.QFont()
136 |         font.setFamily("Roboto Slab")
137 |         font.setPointSize(16)
138 |         self.zUp.setFont(font)
139 |         self.zUp.setStyleSheet("color : rgb(255, 255, 255);\n"
140 | "")
141 |         self.zUp.setText("")
142 |         self.zUp.setIcon(icon1)
143 |         self.zUp.setIconSize(QtCore.QSize(32, 32))
144 |         self.zUp.setFlat(True)
145 |         self.zUp.setObjectName("zUp")
146 |         self.xDown = QtWidgets.QPushButton(ArmControl)
147 |         self.xDown.setGeometry(QtCore.QRect(200, 130, 41, 41))
148 |         font = QtGui.QFont()
149 |         font.setFamily("Roboto Slab")
150 |         font.setPointSize(16)
151 |         self.xDown.setFont(font)
152 |         self.xDown.setStyleSheet("color : rgb(255, 255, 255);\n"
153 | "")
154 |         self.xDown.setText("")
155 |         icon2 = QtGui.QIcon()
156 |         icon2.addPixmap(QtGui.QPixmap("down.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off)
157 |         self.xDown.setIcon(icon2)
158 |         self.xDown.setIconSize(QtCore.QSize(32, 32))
159 |         self.xDown.setFlat(True)
160 |         self.xDown.setObjectName("xDown")
161 |         self.yDown = QtWidgets.QPushButton(ArmControl)
162 |         self.yDown.setGeometry(QtCore.QRect(200, 190, 41, 41))
163 |         font = QtGui.QFont()
164 |         font.setFamily("Roboto Slab")
165 |         font.setPointSize(16)
166 |         self.yDown.setFont(font)
167 |         self.yDown.setStyleSheet("color : rgb(255, 255, 255);\n"
168 | "")
169 |         self.yDown.setText("")
170 |         self.yDown.setIcon(icon2)
171 |         self.yDown.setIconSize(QtCore.QSize(32, 32))
172 |         self.yDown.setFlat(True)
173 |         self.yDown.setObjectName("yDown")
174 |         self.zDown = QtWidgets.QPushButton(ArmControl)
175 |         self.zDown.setGeometry(QtCore.QRect(200, 250, 41, 41))
176 |         font = QtGui.QFont()
177 |         font.setFamily("Roboto Slab")
178 |         font.setPointSize(16)
179 |         self.zDown.setFont(font)
180 |         self.zDown.setStyleSheet("color : rgb(255, 255, 255);\n"
181 | "")
182 |         self.zDown.setText("")
183 |         self.zDown.setIcon(icon2)
184 |         self.zDown.setIconSize(QtCore.QSize(32, 32))
185 |         self.zDown.setFlat(True)
186 |         self.zDown.setObjectName("zDown")
187 |         self.rollDown = QtWidgets.QPushButton(ArmControl)
188 |         self.rollDown.setGeometry(QtCore.QRect(200, 310, 41, 41))
189 |         font = QtGui.QFont()
190 |         font.setFamily("Roboto Slab")
191 |         font.setPointSize(16)
192 |         self.rollDown.setFont(font)
193 |         self.rollDown.setStyleSheet("color : rgb(255, 255, 255);\n"
194 | "")
195 |         self.rollDown.setText("")
196 |         self.rollDown.setIcon(icon2)
197 |         self.rollDown.setIconSize(QtCore.QSize(32, 32))
198 |         self.rollDown.setFlat(True)
199 |         self.rollDown.setObjectName("rollDown")
200 |         self.yLabel = QtWidgets.QLabel(ArmControl)
201 |         self.yLabel.setGeometry(QtCore.QRect(80, 190, 81, 41))
202 |         font = QtGui.QFont()
203 |         font.setFamily("Roboto Slab")
204 |         font.setPointSize(12)
205 |         self.yLabel.setFont(font)
206 |         self.yLabel.setAutoFillBackground(False)
207 |         self.yLabel.setStyleSheet("background-color: rgb(237, 255, 248)")
208 |         self.yLabel.setFrameShape(QtWidgets.QFrame.Panel)
209 |         self.yLabel.setFrameShadow(QtWidgets.QFrame.Raised)
210 |         self.yLabel.setLineWidth(1)
211 |         self.yLabel.setMidLineWidth(0)
212 |         self.yLabel.setAlignment(QtCore.Qt.AlignCenter)
213 |         self.yLabel.setWordWrap(False)
214 |         self.yLabel.setObjectName("yLabel")
215 |         self.zLabel = QtWidgets.QLabel(ArmControl)
216 |         self.zLabel.setGeometry(QtCore.QRect(80, 250, 81, 41))
217 |         font = QtGui.QFont()
218 |         font.setFamily("Roboto Slab")
219 |         font.setPointSize(12)
220 |         self.zLabel.setFont(font)
221 |         self.zLabel.setAutoFillBackground(False)
222 |         self.zLabel.setStyleSheet("background-color: rgb(237, 255, 248)")
223 |         self.zLabel.setFrameShape(QtWidgets.QFrame.Panel)
224 |         self.zLabel.setFrameShadow(QtWidgets.QFrame.Raised)
225 |         self.zLabel.setLineWidth(1)
226 |         self.zLabel.setMidLineWidth(0)
227 |         self.zLabel.setAlignment(QtCore.Qt.AlignCenter)
228 |         self.zLabel.setWordWrap(False)
229 |         self.zLabel.setObjectName("zLabel")
230 |         self.rollLabel = QtWidgets.QLabel(ArmControl)
231 |         self.rollLabel.setGeometry(QtCore.QRect(80, 310, 81, 41))
232 |         font = QtGui.QFont()
233 |         font.setFamily("Roboto Slab")
234 |         font.setPointSize(12)
235 |         self.rollLabel.setFont(font)
236 |         self.rollLabel.setAutoFillBackground(False)
237 |         self.rollLabel.setStyleSheet("background-color: rgb(237, 255, 248)")
238 |         self.rollLabel.setFrameShape(QtWidgets.QFrame.Panel)
239 |         self.rollLabel.setFrameShadow(QtWidgets.QFrame.Raised)
240 |         self.rollLabel.setLineWidth(1)
241 |         self.rollLabel.setMidLineWidth(0)
242 |         self.rollLabel.setAlignment(QtCore.Qt.AlignCenter)
243 |         self.rollLabel.setWordWrap(False)
244 |         self.rollLabel.setObjectName("rollLabel")
245 |         self.titleLabel = QtWidgets.QLabel(ArmControl)
246 |         self.titleLabel.setGeometry(QtCore.QRect(30, 40, 161, 41))
247 |         font = QtGui.QFont()
248 |         font.setFamily("Roboto Slab")
249 |         font.setPointSize(20)
250 |         font.setBold(False)
251 |         font.setWeight(50)
252 |         self.titleLabel.setFont(font)
253 |         self.titleLabel.setObjectName("titleLabel")
254 |         self.sendButton = QtWidgets.QPushButton(ArmControl)
255 |         self.sendButton.setGeometry(QtCore.QRect(350, 30, 41, 51))
256 |         self.sendButton.setText("")
257 |         self.sendButton.setIcon(icon)
258 |         self.sendButton.setIconSize(QtCore.QSize(32, 32))
259 |         self.sendButton.setFlat(True)
260 |         self.sendButton.setObjectName("sendButton")
261 |         self.closeButton = QtWidgets.QPushButton(ArmControl)
262 |         self.closeButton.setGeometry(QtCore.QRect(400, 30, 51, 51))
263 |         self.closeButton.setText("")
264 |         icon3 = QtGui.QIcon()
265 |         icon3.addPixmap(QtGui.QPixmap("../closeIcon.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off)
266 |         self.closeButton.setIcon(icon3)
267 |         self.closeButton.setIconSize(QtCore.QSize(48, 48))
268 |         self.closeButton.setFlat(True)
269 |         self.closeButton.setObjectName("closeButton")
270 |         self.pitchVal = QtWidgets.QLabel(ArmControl)
271 |         self.pitchVal.setGeometry(QtCore.QRect(260, 370, 81, 41))
272 |         font = QtGui.QFont()
273 |         font.setFamily("Roboto Slab")
274 |         font.setPointSize(14)
275 |         self.pitchVal.setFont(font)
276 |         self.pitchVal.setFrameShape(QtWidgets.QFrame.Box)
277 |         self.pitchVal.setFrameShadow(QtWidgets.QFrame.Raised)
278 |         self.pitchVal.setLineWidth(1)
279 |         self.pitchVal.setMidLineWidth(0)
280 |         self.pitchVal.setAlignment(QtCore.Qt.AlignCenter)
281 |         self.pitchVal.setWordWrap(False)
282 |         self.pitchVal.setObjectName("pitchVal")
283 |         self.pitchDown = QtWidgets.QPushButton(ArmControl)
284 |         self.pitchDown.setGeometry(QtCore.QRect(200, 370, 41, 41))
285 |         font = QtGui.QFont()
286 |         font.setFamily("Roboto Slab")
287 |         font.setPointSize(16)
288 |         self.pitchDown.setFont(font)
289 |         self.pitchDown.setStyleSheet("color : rgb(255, 255, 255);\n"
290 | "")
291 |         self.pitchDown.setText("")
292 |         self.pitchDown.setIcon(icon2)
293 |         self.pitchDown.setIconSize(QtCore.QSize(32, 32))
294 |         self.pitchDown.setFlat(True)
295 |         self.pitchDown.setObjectName("pitchDown")
296 |         self.pitchLabel = QtWidgets.QLabel(ArmControl)
297 |         self.pitchLabel.setGeometry(QtCore.QRect(80, 370, 81, 41))
298 |         font = QtGui.QFont()
299 |         font.setFamily("Roboto Slab")
300 |         font.setPointSize(12)
301 |         self.pitchLabel.setFont(font)
302 |         self.pitchLabel.setAutoFillBackground(False)
303 |         self.pitchLabel.setStyleSheet("background-color: rgb(237, 255, 248)")
304 |         self.pitchLabel.setFrameShape(QtWidgets.QFrame.Panel)
305 |         self.pitchLabel.setFrameShadow(QtWidgets.QFrame.Raised)
306 |         self.pitchLabel.setLineWidth(1)
307 |         self.pitchLabel.setMidLineWidth(0)
308 |         self.pitchLabel.setAlignment(QtCore.Qt.AlignCenter)
309 |         self.pitchLabel.setWordWrap(False)
310 |         self.pitchLabel.setObjectName("pitchLabel")
311 |         self.pitchUp = QtWidgets.QPushButton(ArmControl)
312 |         self.pitchUp.setGeometry(QtCore.QRect(360, 370, 41, 41))
313 |         font = QtGui.QFont()
314 |         font.setFamily("Roboto Slab")
315 |         font.setPointSize(16)
316 |         self.pitchUp.setFont(font)
317 |         self.pitchUp.setStyleSheet("color : rgb(255, 255, 255);\n"
318 | "")
319 |         self.pitchUp.setText("")
320 |         self.pitchUp.setIcon(icon1)
321 |         self.pitchUp.setIconSize(QtCore.QSize(32, 32))
322 |         self.pitchUp.setFlat(True)
323 |         self.pitchUp.setObjectName("pitchUp")
324 |         self.yawVal = QtWidgets.QLabel(ArmControl)
325 |         self.yawVal.setGeometry(QtCore.QRect(260, 430, 81, 41))
326 |         font = QtGui.QFont()
327 |         font.setFamily("Roboto Slab")
328 |         font.setPointSize(14)
329 |         self.yawVal.setFont(font)
330 |         self.yawVal.setFrameShape(QtWidgets.QFrame.Box)
331 |         self.yawVal.setFrameShadow(QtWidgets.QFrame.Raised)
332 |         self.yawVal.setLineWidth(1)
333 |         self.yawVal.setMidLineWidth(0)
334 |         self.yawVal.setAlignment(QtCore.Qt.AlignCenter)
335 |         self.yawVal.setWordWrap(False)
336 |         self.yawVal.setObjectName("yawVal")
337 |         self.yawDown = QtWidgets.QPushButton(ArmControl)
338 |         self.yawDown.setGeometry(QtCore.QRect(200, 430, 41, 41))
339 |         font = QtGui.QFont()
340 |         font.setFamily("Roboto Slab")
341 |         font.setPointSize(16)
342 |         self.yawDown.setFont(font)
343 |         self.yawDown.setStyleSheet("color : rgb(255, 255, 255);\n"
344 | "")
345 |         self.yawDown.setText("")
346 |         self.yawDown.setIcon(icon2)
347 |         self.yawDown.setIconSize(QtCore.QSize(32, 32))
348 |         self.yawDown.setFlat(True)
349 |         self.yawDown.setObjectName("yawDown")
350 |         self.yawLabel = QtWidgets.QLabel(ArmControl)
351 |         self.yawLabel.setGeometry(QtCore.QRect(80, 430, 81, 41))
352 |         font = QtGui.QFont()
353 |         font.setFamily("Roboto Slab")
354 |         font.setPointSize(12)
355 |         self.yawLabel.setFont(font)
356 |         self.yawLabel.setAutoFillBackground(False)
357 |         self.yawLabel.setStyleSheet("background-color: rgb(237, 255, 248)")
358 |         self.yawLabel.setFrameShape(QtWidgets.QFrame.Panel)
359 |         self.yawLabel.setFrameShadow(QtWidgets.QFrame.Raised)
360 |         self.yawLabel.setLineWidth(1)
361 |         self.yawLabel.setMidLineWidth(0)
362 |         self.yawLabel.setAlignment(QtCore.Qt.AlignCenter)
363 |         self.yawLabel.setWordWrap(False)
364 |         self.yawLabel.setObjectName("yawLabel")
365 |         self.yawUp = QtWidgets.QPushButton(ArmControl)
366 |         self.yawUp.setGeometry(QtCore.QRect(360, 430, 41, 41))
367 |         font = QtGui.QFont()
368 |         font.setFamily("Roboto Slab")
369 |         font.setPointSize(16)
370 |         self.yawUp.setFont(font)
371 |         self.yawUp.setStyleSheet("color : rgb(255, 255, 255);\n"
372 | "")
373 |         self.yawUp.setText("")
374 |         self.yawUp.setIcon(icon1)
375 |         self.yawUp.setIconSize(QtCore.QSize(32, 32))
376 |         self.yawUp.setFlat(True)
377 |         self.yawUp.setObjectName("yawUp")
378 |         self.subtitleLabel = QtWidgets.QLabel(ArmControl)
379 |         self.subtitleLabel.setGeometry(QtCore.QRect(190, 10, 131, 21))
380 |         font = QtGui.QFont()
381 |         font.setFamily("Roboto Slab")
382 |         font.setPointSize(10)
383 |         font.setBold(False)
384 |         font.setWeight(50)
385 |         self.subtitleLabel.setFont(font)
386 |         self.subtitleLabel.setObjectName("subtitleLabel")
387 | 
388 |         self.retranslateUi(ArmControl)
389 |         QtCore.QMetaObject.connectSlotsByName(ArmControl)
390 | 
391 |     def retranslateUi(self, ArmControl):
392 |         _translate = QtCore.QCoreApplication.translate
393 |         ArmControl.setWindowTitle(_translate("ArmControl", "Arm Control"))
394 |         self.xVal.setText(_translate("ArmControl", "0.4"))
395 |         self.yVal.setText(_translate("ArmControl", "1.3"))
396 |         self.rollVal.setText(_translate("ArmControl", "1"))
397 |         self.zVal.setText(_translate("ArmControl", "3"))
398 |         self.xLabel.setText(_translate("ArmControl", "x"))
399 |         self.yLabel.setText(_translate("ArmControl", "y"))
400 |         self.zLabel.setText(_translate("ArmControl", "z"))
401 |         self.rollLabel.setText(_translate("ArmControl", "roll"))
402 |         self.titleLabel.setText(_translate("ArmControl", "Arm Control"))
403 |         self.pitchVal.setText(_translate("ArmControl", "-1"))
404 |         self.pitchLabel.setText(_translate("ArmControl", "pitch"))
405 |         self.yawVal.setText(_translate("ArmControl", "-1"))
406 |         self.yawLabel.setText(_translate("ArmControl", "yaw"))
407 |         self.subtitleLabel.setText(_translate("ArmControl", "Team Avijatrik"))
408 |         self.closeButton.clicked.connect(ArmControl.close)
409 |         self.sendButton.clicked.connect(self.send)
410 |         self.xDown.clicked.connect(self.xDownMethod)
411 |         self.xUp.clicked.connect(self.xUpMethod)
412 |         self.yDown.clicked.connect(self.yDownMethod)
413 |         self.yUp.clicked.connect(self.yUpMethod)
414 |         self.zDown.clicked.connect(self.zDownMethod)
415 |         self.zUp.clicked.connect(self.zUpMethod)
416 |         self.rollDown.clicked.connect(self.rollDownMethod)
417 |         self.rollup.clicked.connect(self.rollupMethod)
418 |         self.pitchDown.clicked.connect(self.pitchDownMethod)
419 |         self.pitchUp.clicked.connect(self.pitchUpMethod)
420 |         self.yawDown.clicked.connect(self.yawDownMethod)
421 |         self.yawUp.clicked.connect(self.yawUpMethod)
422 |         self.window = ArmControl
423 | 
424 |     def send(self):
425 |         data = []
426 |         val = int(self.xVal.text())
427 |         data.append(str(val))
428 |         val = int(self.yVal.text())
429 |         data.append(str(val))
430 |         val = int(self.zVal.text())
431 |         data.append(str(val))
432 |         val = int(self.rollVal.text())
433 |         data.append(str(val))
434 |         val = int(self.pitchVal.text())
435 |         data.append(str(val))
436 |         val = int(self.yawVal.text())
437 |         data.append(str(val))
438 |         dataString = ','.join(data)
439 |         #self.window.SerialObj.sendString(dataString)
440 |         print(dataString)
441 |     
442 |     
443 |     def plot_data(self):
444 | 
445 |         px = float(self.xVal.text())
446 |         py = float(self.yVal.text())
447 |         pz = float(self.zVal.text())
448 |         roll = float(self.rollVal.text())
449 |         pitch = float(self.pitchVal.text())
450 |         yaw = float(self.yawVal.text())
451 |         q1,q2,q3,q4,q5,q6 = get_angles(px,py,pz,roll,pitch,yaw)
452 |         X,Y,Z = forward_kin(q1,q2,q3,q4,q5,q6)
453 |         update_plot(X,Y,Z,self.window.fig,self.window.ax)
454 |     
455 |     
456 |     def xUpMethod(self):
457 |         val = float(self.xVal.text())
458 |         if(val<2):
459 |                 val+=.1
460 |                 self.xVal.setText(str(round(val,2))) 
461 |                 self.plot_data()
462 |     def xDownMethod(self):
463 |         val = float(self.xVal.text())
464 |         if(val>0):
465 |                 val -= .1
466 |                 self.xVal.setText(str(round(val,2)))
467 |                 self.plot_data()
468 | 
469 |    
470 |     def yUpMethod(self):
471 |         val = float(self.yVal.text())
472 |         if(val<3):
473 |                 val+=.1
474 |                 self.yVal.setText(str(round(val,2)))
475 |                 self.plot_data()
476 |     def yDownMethod(self):
477 |         val = float(self.yVal.text())
478 |         if(val>0):
479 |                 val -= .1
480 |                 self.yVal.setText(str(round(val,2)))
481 |                 self.plot_data()
482 |   
483 |   
484 |     def zUpMethod(self):
485 |         val = float(self.zVal.text())
486 |         if(val<5):
487 |                 val+=.1
488 |                 self.zVal.setText(str(round(val,2)))
489 |                 self.plot_data()
490 |     def zDownMethod(self):
491 |         val = float(self.zVal.text())
492 |         if(val>0):
493 |                 val -= .1
494 |                 self.zVal.setText(str(round(val,2)))
495 |                 self.plot_data()
496 | 
497 |     def rollupMethod(self):
498 |         val = float(self.rollVal.text())
499 |         if(val<3.14):
500 |                 val+=.1
501 |                 self.rollVal.setText(str(round(val,2)))
502 |                 self.plot_data()
503 |     def rollDownMethod(self):
504 |         val = float(self.rollVal.text())
505 |         if(val>-3.14):
506 |                 val -= .1
507 |                 self.rollVal.setText(str(round(val,2)))
508 |                 self.plot_data()    
509 | 
510 |     def pitchUpMethod(self):
511 |         val = float(self.pitchVal.text())
512 |         if(val<3.14):
513 |                 val+=.1
514 |                 self.pitchVal.setText(str(round(val,2)))
515 |                 self.plot_data()
516 |     def pitchDownMethod(self):
517 |         val = float(self.pitchVal.text())
518 |         if(val>-3.14):
519 |                 val -= .1
520 |                 self.pitchVal.setText(str(round(val,2)))
521 |                 self.plot_data()  
522 |     def yawUpMethod(self):
523 |         val = float(self.yawVal.text())
524 |         if(val<3.14):
525 |                 val+=.1
526 |                 self.yawVal.setText(str(round(val,2)))
527 |                 self.plot_data()
528 |     def yawDownMethod(self):
529 |         val = float(self.yawVal.text())
530 |         if(val>-3.14):
531 |                 val -= .1
532 |                 self.yawVal.setText(str(round(val,2)))
533 |                 self.plot_data()  
534 |     
535 |     
536 | 
537 | 
538 | class Main_window(QtWidgets.QMainWindow):
539 |     def __init__(self):
540 |         super().__init__()
541 |         self.InitSerial()
542 |         self.InitPlot()
543 | 
544 |     def InitPlot(self):
545 |         self.fig,self.ax = create_plot()
546 |         
547 | 
548 | 
549 |     def InitSerial(self):
550 |         #self.SerialObj = SerialCom.SerialCom()
551 |         #self.arduinoOut = self.SerialObj.getConn('COM9',9600) 
552 |         pass
553 |     def close(self):
554 |         sys.exit()
555 |     
556 | 
557 | 
558 | 
559 | def main():
560 |         app = QtWidgets.QApplication(sys.argv)
561 |         MainWindow = Main_window()
562 |         ui = Ui_ArmControl()
563 |         ui.setupUi(MainWindow)
564 |         MainWindow.show()
565 |         plt.show()
566 |         sys.exit(app.exec_())
567 |         
568 | 
569 | 
570 | if __name__ == "__main__":
571 |         main()
572 | 
573 | 
574 | 


--------------------------------------------------------------------------------
/design.ui:
--------------------------------------------------------------------------------
   1 | <?xml version="1.0" encoding="UTF-8"?>
   2 | <ui version="4.0">
   3 |  <class>ArmControl</class>
   4 |  <widget class="QWidget" name="ArmControl">
   5 |   <property name="windowModality">
   6 |    <enum>Qt::NonModal</enum>
   7 |   </property>
   8 |   <property name="geometry">
   9 |    <rect>
  10 |     <x>0</x>
  11 |     <y>0</y>
  12 |     <width>482</width>
  13 |     <height>537</height>
  14 |    </rect>
  15 |   </property>
  16 |   <property name="maximumSize">
  17 |    <size>
  18 |     <width>16777215</width>
  19 |     <height>16777215</height>
  20 |    </size>
  21 |   </property>
  22 |   <property name="windowTitle">
  23 |    <string>Arm Control</string>
  24 |   </property>
  25 |   <property name="windowIcon">
  26 |    <iconset>
  27 |     <normaloff>../roboticArm.png</normaloff>../roboticArm.png</iconset>
  28 |   </property>
  29 |   <property name="windowOpacity">
  30 |    <double>1.000000000000000</double>
  31 |   </property>
  32 |   <property name="autoFillBackground">
  33 |    <bool>false</bool>
  34 |   </property>
  35 |   <widget class="QPushButton" name="xUp">
  36 |    <property name="geometry">
  37 |     <rect>
  38 |      <x>360</x>
  39 |      <y>130</y>
  40 |      <width>41</width>
  41 |      <height>41</height>
  42 |     </rect>
  43 |    </property>
  44 |    <property name="font">
  45 |     <font>
  46 |      <family>Roboto Slab</family>
  47 |      <pointsize>16</pointsize>
  48 |     </font>
  49 |    </property>
  50 |    <property name="styleSheet">
  51 |     <string notr="true">color : rgb(255, 255, 255);
  52 | </string>
  53 |    </property>
  54 |    <property name="text">
  55 |     <string/>
  56 |    </property>
  57 |    <property name="icon">
  58 |     <iconset>
  59 |      <normaloff>up.png</normaloff>up.png</iconset>
  60 |    </property>
  61 |    <property name="iconSize">
  62 |     <size>
  63 |      <width>32</width>
  64 |      <height>32</height>
  65 |     </size>
  66 |    </property>
  67 |    <property name="flat">
  68 |     <bool>true</bool>
  69 |    </property>
  70 |   </widget>
  71 |   <widget class="QLabel" name="xVal">
  72 |    <property name="geometry">
  73 |     <rect>
  74 |      <x>260</x>
  75 |      <y>130</y>
  76 |      <width>81</width>
  77 |      <height>41</height>
  78 |     </rect>
  79 |    </property>
  80 |    <property name="font">
  81 |     <font>
  82 |      <family>Roboto Slab</family>
  83 |      <pointsize>14</pointsize>
  84 |     </font>
  85 |    </property>
  86 |    <property name="frameShape">
  87 |     <enum>QFrame::Box</enum>
  88 |    </property>
  89 |    <property name="frameShadow">
  90 |     <enum>QFrame::Raised</enum>
  91 |    </property>
  92 |    <property name="lineWidth">
  93 |     <number>1</number>
  94 |    </property>
  95 |    <property name="midLineWidth">
  96 |     <number>0</number>
  97 |    </property>
  98 |    <property name="text">
  99 |     <string>123</string>
 100 |    </property>
 101 |    <property name="alignment">
 102 |     <set>Qt::AlignCenter</set>
 103 |    </property>
 104 |    <property name="wordWrap">
 105 |     <bool>false</bool>
 106 |    </property>
 107 |    <property name="margin">
 108 |     <number>4</number>
 109 |    </property>
 110 |   </widget>
 111 |   <widget class="QLabel" name="yVal">
 112 |    <property name="geometry">
 113 |     <rect>
 114 |      <x>260</x>
 115 |      <y>190</y>
 116 |      <width>81</width>
 117 |      <height>41</height>
 118 |     </rect>
 119 |    </property>
 120 |    <property name="font">
 121 |     <font>
 122 |      <family>Roboto Slab</family>
 123 |      <pointsize>14</pointsize>
 124 |     </font>
 125 |    </property>
 126 |    <property name="frameShape">
 127 |     <enum>QFrame::Box</enum>
 128 |    </property>
 129 |    <property name="frameShadow">
 130 |     <enum>QFrame::Raised</enum>
 131 |    </property>
 132 |    <property name="lineWidth">
 133 |     <number>1</number>
 134 |    </property>
 135 |    <property name="midLineWidth">
 136 |     <number>0</number>
 137 |    </property>
 138 |    <property name="text">
 139 |     <string>123</string>
 140 |    </property>
 141 |    <property name="alignment">
 142 |     <set>Qt::AlignCenter</set>
 143 |    </property>
 144 |    <property name="wordWrap">
 145 |     <bool>false</bool>
 146 |    </property>
 147 |    <property name="margin">
 148 |     <number>4</number>
 149 |    </property>
 150 |   </widget>
 151 |   <widget class="QLabel" name="rollVal">
 152 |    <property name="geometry">
 153 |     <rect>
 154 |      <x>260</x>
 155 |      <y>310</y>
 156 |      <width>81</width>
 157 |      <height>41</height>
 158 |     </rect>
 159 |    </property>
 160 |    <property name="font">
 161 |     <font>
 162 |      <family>Roboto Slab</family>
 163 |      <pointsize>14</pointsize>
 164 |     </font>
 165 |    </property>
 166 |    <property name="frameShape">
 167 |     <enum>QFrame::Box</enum>
 168 |    </property>
 169 |    <property name="frameShadow">
 170 |     <enum>QFrame::Raised</enum>
 171 |    </property>
 172 |    <property name="lineWidth">
 173 |     <number>1</number>
 174 |    </property>
 175 |    <property name="midLineWidth">
 176 |     <number>0</number>
 177 |    </property>
 178 |    <property name="text">
 179 |     <string>123</string>
 180 |    </property>
 181 |    <property name="alignment">
 182 |     <set>Qt::AlignCenter</set>
 183 |    </property>
 184 |    <property name="wordWrap">
 185 |     <bool>false</bool>
 186 |    </property>
 187 |    <property name="margin">
 188 |     <number>4</number>
 189 |    </property>
 190 |   </widget>
 191 |   <widget class="QLabel" name="zVal">
 192 |    <property name="geometry">
 193 |     <rect>
 194 |      <x>260</x>
 195 |      <y>250</y>
 196 |      <width>81</width>
 197 |      <height>41</height>
 198 |     </rect>
 199 |    </property>
 200 |    <property name="font">
 201 |     <font>
 202 |      <family>Roboto Slab</family>
 203 |      <pointsize>14</pointsize>
 204 |     </font>
 205 |    </property>
 206 |    <property name="frameShape">
 207 |     <enum>QFrame::Box</enum>
 208 |    </property>
 209 |    <property name="frameShadow">
 210 |     <enum>QFrame::Raised</enum>
 211 |    </property>
 212 |    <property name="lineWidth">
 213 |     <number>1</number>
 214 |    </property>
 215 |    <property name="midLineWidth">
 216 |     <number>0</number>
 217 |    </property>
 218 |    <property name="text">
 219 |     <string>123</string>
 220 |    </property>
 221 |    <property name="alignment">
 222 |     <set>Qt::AlignCenter</set>
 223 |    </property>
 224 |    <property name="wordWrap">
 225 |     <bool>false</bool>
 226 |    </property>
 227 |    <property name="margin">
 228 |     <number>4</number>
 229 |    </property>
 230 |   </widget>
 231 |   <widget class="QLabel" name="xLabel">
 232 |    <property name="geometry">
 233 |     <rect>
 234 |      <x>80</x>
 235 |      <y>130</y>
 236 |      <width>81</width>
 237 |      <height>41</height>
 238 |     </rect>
 239 |    </property>
 240 |    <property name="font">
 241 |     <font>
 242 |      <family>Roboto Slab</family>
 243 |      <pointsize>12</pointsize>
 244 |     </font>
 245 |    </property>
 246 |    <property name="autoFillBackground">
 247 |     <bool>false</bool>
 248 |    </property>
 249 |    <property name="styleSheet">
 250 |     <string notr="true">background-color: rgb(237, 255, 248)</string>
 251 |    </property>
 252 |    <property name="frameShape">
 253 |     <enum>QFrame::Panel</enum>
 254 |    </property>
 255 |    <property name="frameShadow">
 256 |     <enum>QFrame::Raised</enum>
 257 |    </property>
 258 |    <property name="lineWidth">
 259 |     <number>1</number>
 260 |    </property>
 261 |    <property name="midLineWidth">
 262 |     <number>0</number>
 263 |    </property>
 264 |    <property name="text">
 265 |     <string>x</string>
 266 |    </property>
 267 |    <property name="alignment">
 268 |     <set>Qt::AlignCenter</set>
 269 |    </property>
 270 |    <property name="wordWrap">
 271 |     <bool>false</bool>
 272 |    </property>
 273 |    <property name="margin">
 274 |     <number>4</number>
 275 |    </property>
 276 |   </widget>
 277 |   <widget class="QPushButton" name="yUp">
 278 |    <property name="geometry">
 279 |     <rect>
 280 |      <x>360</x>
 281 |      <y>190</y>
 282 |      <width>41</width>
 283 |      <height>41</height>
 284 |     </rect>
 285 |    </property>
 286 |    <property name="font">
 287 |     <font>
 288 |      <family>Roboto Slab</family>
 289 |      <pointsize>16</pointsize>
 290 |     </font>
 291 |    </property>
 292 |    <property name="styleSheet">
 293 |     <string notr="true">color : rgb(255, 255, 255);
 294 | </string>
 295 |    </property>
 296 |    <property name="text">
 297 |     <string/>
 298 |    </property>
 299 |    <property name="icon">
 300 |     <iconset>
 301 |      <normaloff>up.png</normaloff>up.png</iconset>
 302 |    </property>
 303 |    <property name="iconSize">
 304 |     <size>
 305 |      <width>32</width>
 306 |      <height>32</height>
 307 |     </size>
 308 |    </property>
 309 |    <property name="flat">
 310 |     <bool>true</bool>
 311 |    </property>
 312 |   </widget>
 313 |   <widget class="QPushButton" name="rollup">
 314 |    <property name="geometry">
 315 |     <rect>
 316 |      <x>360</x>
 317 |      <y>310</y>
 318 |      <width>41</width>
 319 |      <height>41</height>
 320 |     </rect>
 321 |    </property>
 322 |    <property name="font">
 323 |     <font>
 324 |      <family>Roboto Slab</family>
 325 |      <pointsize>16</pointsize>
 326 |     </font>
 327 |    </property>
 328 |    <property name="styleSheet">
 329 |     <string notr="true">color : rgb(255, 255, 255);
 330 | </string>
 331 |    </property>
 332 |    <property name="text">
 333 |     <string/>
 334 |    </property>
 335 |    <property name="icon">
 336 |     <iconset>
 337 |      <normaloff>up.png</normaloff>up.png</iconset>
 338 |    </property>
 339 |    <property name="iconSize">
 340 |     <size>
 341 |      <width>32</width>
 342 |      <height>32</height>
 343 |     </size>
 344 |    </property>
 345 |    <property name="flat">
 346 |     <bool>true</bool>
 347 |    </property>
 348 |   </widget>
 349 |   <widget class="QPushButton" name="zUp">
 350 |    <property name="geometry">
 351 |     <rect>
 352 |      <x>360</x>
 353 |      <y>250</y>
 354 |      <width>41</width>
 355 |      <height>41</height>
 356 |     </rect>
 357 |    </property>
 358 |    <property name="font">
 359 |     <font>
 360 |      <family>Roboto Slab</family>
 361 |      <pointsize>16</pointsize>
 362 |     </font>
 363 |    </property>
 364 |    <property name="styleSheet">
 365 |     <string notr="true">color : rgb(255, 255, 255);
 366 | </string>
 367 |    </property>
 368 |    <property name="text">
 369 |     <string/>
 370 |    </property>
 371 |    <property name="icon">
 372 |     <iconset>
 373 |      <normaloff>up.png</normaloff>up.png</iconset>
 374 |    </property>
 375 |    <property name="iconSize">
 376 |     <size>
 377 |      <width>32</width>
 378 |      <height>32</height>
 379 |     </size>
 380 |    </property>
 381 |    <property name="flat">
 382 |     <bool>true</bool>
 383 |    </property>
 384 |   </widget>
 385 |   <widget class="QPushButton" name="xDown">
 386 |    <property name="geometry">
 387 |     <rect>
 388 |      <x>200</x>
 389 |      <y>130</y>
 390 |      <width>41</width>
 391 |      <height>41</height>
 392 |     </rect>
 393 |    </property>
 394 |    <property name="font">
 395 |     <font>
 396 |      <family>Roboto Slab</family>
 397 |      <pointsize>16</pointsize>
 398 |     </font>
 399 |    </property>
 400 |    <property name="styleSheet">
 401 |     <string notr="true">color : rgb(255, 255, 255);
 402 | </string>
 403 |    </property>
 404 |    <property name="text">
 405 |     <string/>
 406 |    </property>
 407 |    <property name="icon">
 408 |     <iconset>
 409 |      <normaloff>down.png</normaloff>down.png</iconset>
 410 |    </property>
 411 |    <property name="iconSize">
 412 |     <size>
 413 |      <width>32</width>
 414 |      <height>32</height>
 415 |     </size>
 416 |    </property>
 417 |    <property name="flat">
 418 |     <bool>true</bool>
 419 |    </property>
 420 |   </widget>
 421 |   <widget class="QPushButton" name="yDown">
 422 |    <property name="geometry">
 423 |     <rect>
 424 |      <x>200</x>
 425 |      <y>190</y>
 426 |      <width>41</width>
 427 |      <height>41</height>
 428 |     </rect>
 429 |    </property>
 430 |    <property name="font">
 431 |     <font>
 432 |      <family>Roboto Slab</family>
 433 |      <pointsize>16</pointsize>
 434 |     </font>
 435 |    </property>
 436 |    <property name="styleSheet">
 437 |     <string notr="true">color : rgb(255, 255, 255);
 438 | </string>
 439 |    </property>
 440 |    <property name="text">
 441 |     <string/>
 442 |    </property>
 443 |    <property name="icon">
 444 |     <iconset>
 445 |      <normaloff>down.png</normaloff>down.png</iconset>
 446 |    </property>
 447 |    <property name="iconSize">
 448 |     <size>
 449 |      <width>32</width>
 450 |      <height>32</height>
 451 |     </size>
 452 |    </property>
 453 |    <property name="flat">
 454 |     <bool>true</bool>
 455 |    </property>
 456 |   </widget>
 457 |   <widget class="QPushButton" name="zDown">
 458 |    <property name="geometry">
 459 |     <rect>
 460 |      <x>200</x>
 461 |      <y>250</y>
 462 |      <width>41</width>
 463 |      <height>41</height>
 464 |     </rect>
 465 |    </property>
 466 |    <property name="font">
 467 |     <font>
 468 |      <family>Roboto Slab</family>
 469 |      <pointsize>16</pointsize>
 470 |     </font>
 471 |    </property>
 472 |    <property name="styleSheet">
 473 |     <string notr="true">color : rgb(255, 255, 255);
 474 | </string>
 475 |    </property>
 476 |    <property name="text">
 477 |     <string/>
 478 |    </property>
 479 |    <property name="icon">
 480 |     <iconset>
 481 |      <normaloff>down.png</normaloff>down.png</iconset>
 482 |    </property>
 483 |    <property name="iconSize">
 484 |     <size>
 485 |      <width>32</width>
 486 |      <height>32</height>
 487 |     </size>
 488 |    </property>
 489 |    <property name="flat">
 490 |     <bool>true</bool>
 491 |    </property>
 492 |   </widget>
 493 |   <widget class="QPushButton" name="rollDown">
 494 |    <property name="geometry">
 495 |     <rect>
 496 |      <x>200</x>
 497 |      <y>310</y>
 498 |      <width>41</width>
 499 |      <height>41</height>
 500 |     </rect>
 501 |    </property>
 502 |    <property name="font">
 503 |     <font>
 504 |      <family>Roboto Slab</family>
 505 |      <pointsize>16</pointsize>
 506 |     </font>
 507 |    </property>
 508 |    <property name="styleSheet">
 509 |     <string notr="true">color : rgb(255, 255, 255);
 510 | </string>
 511 |    </property>
 512 |    <property name="text">
 513 |     <string/>
 514 |    </property>
 515 |    <property name="icon">
 516 |     <iconset>
 517 |      <normaloff>down.png</normaloff>down.png</iconset>
 518 |    </property>
 519 |    <property name="iconSize">
 520 |     <size>
 521 |      <width>32</width>
 522 |      <height>32</height>
 523 |     </size>
 524 |    </property>
 525 |    <property name="flat">
 526 |     <bool>true</bool>
 527 |    </property>
 528 |   </widget>
 529 |   <widget class="QLabel" name="yLabel">
 530 |    <property name="geometry">
 531 |     <rect>
 532 |      <x>80</x>
 533 |      <y>190</y>
 534 |      <width>81</width>
 535 |      <height>41</height>
 536 |     </rect>
 537 |    </property>
 538 |    <property name="font">
 539 |     <font>
 540 |      <family>Roboto Slab</family>
 541 |      <pointsize>12</pointsize>
 542 |     </font>
 543 |    </property>
 544 |    <property name="autoFillBackground">
 545 |     <bool>false</bool>
 546 |    </property>
 547 |    <property name="styleSheet">
 548 |     <string notr="true">background-color: rgb(237, 255, 248)</string>
 549 |    </property>
 550 |    <property name="frameShape">
 551 |     <enum>QFrame::Panel</enum>
 552 |    </property>
 553 |    <property name="frameShadow">
 554 |     <enum>QFrame::Raised</enum>
 555 |    </property>
 556 |    <property name="lineWidth">
 557 |     <number>1</number>
 558 |    </property>
 559 |    <property name="midLineWidth">
 560 |     <number>0</number>
 561 |    </property>
 562 |    <property name="text">
 563 |     <string>y</string>
 564 |    </property>
 565 |    <property name="alignment">
 566 |     <set>Qt::AlignCenter</set>
 567 |    </property>
 568 |    <property name="wordWrap">
 569 |     <bool>false</bool>
 570 |    </property>
 571 |    <property name="margin">
 572 |     <number>4</number>
 573 |    </property>
 574 |   </widget>
 575 |   <widget class="QLabel" name="zLabel">
 576 |    <property name="geometry">
 577 |     <rect>
 578 |      <x>80</x>
 579 |      <y>250</y>
 580 |      <width>81</width>
 581 |      <height>41</height>
 582 |     </rect>
 583 |    </property>
 584 |    <property name="font">
 585 |     <font>
 586 |      <family>Roboto Slab</family>
 587 |      <pointsize>12</pointsize>
 588 |     </font>
 589 |    </property>
 590 |    <property name="autoFillBackground">
 591 |     <bool>false</bool>
 592 |    </property>
 593 |    <property name="styleSheet">
 594 |     <string notr="true">background-color: rgb(237, 255, 248)</string>
 595 |    </property>
 596 |    <property name="frameShape">
 597 |     <enum>QFrame::Panel</enum>
 598 |    </property>
 599 |    <property name="frameShadow">
 600 |     <enum>QFrame::Raised</enum>
 601 |    </property>
 602 |    <property name="lineWidth">
 603 |     <number>1</number>
 604 |    </property>
 605 |    <property name="midLineWidth">
 606 |     <number>0</number>
 607 |    </property>
 608 |    <property name="text">
 609 |     <string>z</string>
 610 |    </property>
 611 |    <property name="alignment">
 612 |     <set>Qt::AlignCenter</set>
 613 |    </property>
 614 |    <property name="wordWrap">
 615 |     <bool>false</bool>
 616 |    </property>
 617 |    <property name="margin">
 618 |     <number>4</number>
 619 |    </property>
 620 |   </widget>
 621 |   <widget class="QLabel" name="rollLabel">
 622 |    <property name="geometry">
 623 |     <rect>
 624 |      <x>80</x>
 625 |      <y>310</y>
 626 |      <width>81</width>
 627 |      <height>41</height>
 628 |     </rect>
 629 |    </property>
 630 |    <property name="font">
 631 |     <font>
 632 |      <family>Roboto Slab</family>
 633 |      <pointsize>12</pointsize>
 634 |     </font>
 635 |    </property>
 636 |    <property name="autoFillBackground">
 637 |     <bool>false</bool>
 638 |    </property>
 639 |    <property name="styleSheet">
 640 |     <string notr="true">background-color: rgb(237, 255, 248)</string>
 641 |    </property>
 642 |    <property name="frameShape">
 643 |     <enum>QFrame::Panel</enum>
 644 |    </property>
 645 |    <property name="frameShadow">
 646 |     <enum>QFrame::Raised</enum>
 647 |    </property>
 648 |    <property name="lineWidth">
 649 |     <number>1</number>
 650 |    </property>
 651 |    <property name="midLineWidth">
 652 |     <number>0</number>
 653 |    </property>
 654 |    <property name="text">
 655 |     <string>roll</string>
 656 |    </property>
 657 |    <property name="alignment">
 658 |     <set>Qt::AlignCenter</set>
 659 |    </property>
 660 |    <property name="wordWrap">
 661 |     <bool>false</bool>
 662 |    </property>
 663 |    <property name="margin">
 664 |     <number>4</number>
 665 |    </property>
 666 |   </widget>
 667 |   <widget class="QLabel" name="titleLabel">
 668 |    <property name="geometry">
 669 |     <rect>
 670 |      <x>30</x>
 671 |      <y>40</y>
 672 |      <width>161</width>
 673 |      <height>41</height>
 674 |     </rect>
 675 |    </property>
 676 |    <property name="font">
 677 |     <font>
 678 |      <family>Roboto Slab</family>
 679 |      <pointsize>20</pointsize>
 680 |      <weight>50</weight>
 681 |      <bold>false</bold>
 682 |     </font>
 683 |    </property>
 684 |    <property name="text">
 685 |     <string>Arm Control</string>
 686 |    </property>
 687 |   </widget>
 688 |   <widget class="QPushButton" name="sendButton">
 689 |    <property name="geometry">
 690 |     <rect>
 691 |      <x>350</x>
 692 |      <y>30</y>
 693 |      <width>41</width>
 694 |      <height>51</height>
 695 |     </rect>
 696 |    </property>
 697 |    <property name="text">
 698 |     <string/>
 699 |    </property>
 700 |    <property name="icon">
 701 |     <iconset>
 702 |      <normaloff>../roboticArm.png</normaloff>../roboticArm.png</iconset>
 703 |    </property>
 704 |    <property name="iconSize">
 705 |     <size>
 706 |      <width>32</width>
 707 |      <height>32</height>
 708 |     </size>
 709 |    </property>
 710 |    <property name="flat">
 711 |     <bool>true</bool>
 712 |    </property>
 713 |   </widget>
 714 |   <widget class="QPushButton" name="closeButton">
 715 |    <property name="geometry">
 716 |     <rect>
 717 |      <x>400</x>
 718 |      <y>30</y>
 719 |      <width>51</width>
 720 |      <height>51</height>
 721 |     </rect>
 722 |    </property>
 723 |    <property name="text">
 724 |     <string/>
 725 |    </property>
 726 |    <property name="icon">
 727 |     <iconset>
 728 |      <normaloff>../closeIcon.png</normaloff>../closeIcon.png</iconset>
 729 |    </property>
 730 |    <property name="iconSize">
 731 |     <size>
 732 |      <width>48</width>
 733 |      <height>48</height>
 734 |     </size>
 735 |    </property>
 736 |    <property name="flat">
 737 |     <bool>true</bool>
 738 |    </property>
 739 |   </widget>
 740 |   <widget class="QLabel" name="pitchVal">
 741 |    <property name="geometry">
 742 |     <rect>
 743 |      <x>260</x>
 744 |      <y>370</y>
 745 |      <width>81</width>
 746 |      <height>41</height>
 747 |     </rect>
 748 |    </property>
 749 |    <property name="font">
 750 |     <font>
 751 |      <family>Roboto Slab</family>
 752 |      <pointsize>14</pointsize>
 753 |     </font>
 754 |    </property>
 755 |    <property name="frameShape">
 756 |     <enum>QFrame::Box</enum>
 757 |    </property>
 758 |    <property name="frameShadow">
 759 |     <enum>QFrame::Raised</enum>
 760 |    </property>
 761 |    <property name="lineWidth">
 762 |     <number>1</number>
 763 |    </property>
 764 |    <property name="midLineWidth">
 765 |     <number>0</number>
 766 |    </property>
 767 |    <property name="text">
 768 |     <string>123</string>
 769 |    </property>
 770 |    <property name="alignment">
 771 |     <set>Qt::AlignCenter</set>
 772 |    </property>
 773 |    <property name="wordWrap">
 774 |     <bool>false</bool>
 775 |    </property>
 776 |    <property name="margin">
 777 |     <number>4</number>
 778 |    </property>
 779 |   </widget>
 780 |   <widget class="QPushButton" name="pitchDown">
 781 |    <property name="geometry">
 782 |     <rect>
 783 |      <x>200</x>
 784 |      <y>370</y>
 785 |      <width>41</width>
 786 |      <height>41</height>
 787 |     </rect>
 788 |    </property>
 789 |    <property name="font">
 790 |     <font>
 791 |      <family>Roboto Slab</family>
 792 |      <pointsize>16</pointsize>
 793 |     </font>
 794 |    </property>
 795 |    <property name="styleSheet">
 796 |     <string notr="true">color : rgb(255, 255, 255);
 797 | </string>
 798 |    </property>
 799 |    <property name="text">
 800 |     <string/>
 801 |    </property>
 802 |    <property name="icon">
 803 |     <iconset>
 804 |      <normaloff>down.png</normaloff>down.png</iconset>
 805 |    </property>
 806 |    <property name="iconSize">
 807 |     <size>
 808 |      <width>32</width>
 809 |      <height>32</height>
 810 |     </size>
 811 |    </property>
 812 |    <property name="flat">
 813 |     <bool>true</bool>
 814 |    </property>
 815 |   </widget>
 816 |   <widget class="QLabel" name="pitchLabel">
 817 |    <property name="geometry">
 818 |     <rect>
 819 |      <x>80</x>
 820 |      <y>370</y>
 821 |      <width>81</width>
 822 |      <height>41</height>
 823 |     </rect>
 824 |    </property>
 825 |    <property name="font">
 826 |     <font>
 827 |      <family>Roboto Slab</family>
 828 |      <pointsize>12</pointsize>
 829 |     </font>
 830 |    </property>
 831 |    <property name="autoFillBackground">
 832 |     <bool>false</bool>
 833 |    </property>
 834 |    <property name="styleSheet">
 835 |     <string notr="true">background-color: rgb(237, 255, 248)</string>
 836 |    </property>
 837 |    <property name="frameShape">
 838 |     <enum>QFrame::Panel</enum>
 839 |    </property>
 840 |    <property name="frameShadow">
 841 |     <enum>QFrame::Raised</enum>
 842 |    </property>
 843 |    <property name="lineWidth">
 844 |     <number>1</number>
 845 |    </property>
 846 |    <property name="midLineWidth">
 847 |     <number>0</number>
 848 |    </property>
 849 |    <property name="text">
 850 |     <string>pitch</string>
 851 |    </property>
 852 |    <property name="alignment">
 853 |     <set>Qt::AlignCenter</set>
 854 |    </property>
 855 |    <property name="wordWrap">
 856 |     <bool>false</bool>
 857 |    </property>
 858 |    <property name="margin">
 859 |     <number>4</number>
 860 |    </property>
 861 |   </widget>
 862 |   <widget class="QPushButton" name="pitchUp">
 863 |    <property name="geometry">
 864 |     <rect>
 865 |      <x>360</x>
 866 |      <y>370</y>
 867 |      <width>41</width>
 868 |      <height>41</height>
 869 |     </rect>
 870 |    </property>
 871 |    <property name="font">
 872 |     <font>
 873 |      <family>Roboto Slab</family>
 874 |      <pointsize>16</pointsize>
 875 |     </font>
 876 |    </property>
 877 |    <property name="styleSheet">
 878 |     <string notr="true">color : rgb(255, 255, 255);
 879 | </string>
 880 |    </property>
 881 |    <property name="text">
 882 |     <string/>
 883 |    </property>
 884 |    <property name="icon">
 885 |     <iconset>
 886 |      <normaloff>up.png</normaloff>up.png</iconset>
 887 |    </property>
 888 |    <property name="iconSize">
 889 |     <size>
 890 |      <width>32</width>
 891 |      <height>32</height>
 892 |     </size>
 893 |    </property>
 894 |    <property name="flat">
 895 |     <bool>true</bool>
 896 |    </property>
 897 |   </widget>
 898 |   <widget class="QLabel" name="yawVal">
 899 |    <property name="geometry">
 900 |     <rect>
 901 |      <x>260</x>
 902 |      <y>430</y>
 903 |      <width>81</width>
 904 |      <height>41</height>
 905 |     </rect>
 906 |    </property>
 907 |    <property name="font">
 908 |     <font>
 909 |      <family>Roboto Slab</family>
 910 |      <pointsize>14</pointsize>
 911 |     </font>
 912 |    </property>
 913 |    <property name="frameShape">
 914 |     <enum>QFrame::Box</enum>
 915 |    </property>
 916 |    <property name="frameShadow">
 917 |     <enum>QFrame::Raised</enum>
 918 |    </property>
 919 |    <property name="lineWidth">
 920 |     <number>1</number>
 921 |    </property>
 922 |    <property name="midLineWidth">
 923 |     <number>0</number>
 924 |    </property>
 925 |    <property name="text">
 926 |     <string>123</string>
 927 |    </property>
 928 |    <property name="alignment">
 929 |     <set>Qt::AlignCenter</set>
 930 |    </property>
 931 |    <property name="wordWrap">
 932 |     <bool>false</bool>
 933 |    </property>
 934 |    <property name="margin">
 935 |     <number>4</number>
 936 |    </property>
 937 |   </widget>
 938 |   <widget class="QPushButton" name="yawDown">
 939 |    <property name="geometry">
 940 |     <rect>
 941 |      <x>200</x>
 942 |      <y>430</y>
 943 |      <width>41</width>
 944 |      <height>41</height>
 945 |     </rect>
 946 |    </property>
 947 |    <property name="font">
 948 |     <font>
 949 |      <family>Roboto Slab</family>
 950 |      <pointsize>16</pointsize>
 951 |     </font>
 952 |    </property>
 953 |    <property name="styleSheet">
 954 |     <string notr="true">color : rgb(255, 255, 255);
 955 | </string>
 956 |    </property>
 957 |    <property name="text">
 958 |     <string/>
 959 |    </property>
 960 |    <property name="icon">
 961 |     <iconset>
 962 |      <normaloff>down.png</normaloff>down.png</iconset>
 963 |    </property>
 964 |    <property name="iconSize">
 965 |     <size>
 966 |      <width>32</width>
 967 |      <height>32</height>
 968 |     </size>
 969 |    </property>
 970 |    <property name="flat">
 971 |     <bool>true</bool>
 972 |    </property>
 973 |   </widget>
 974 |   <widget class="QLabel" name="yawLabel">
 975 |    <property name="geometry">
 976 |     <rect>
 977 |      <x>80</x>
 978 |      <y>430</y>
 979 |      <width>81</width>
 980 |      <height>41</height>
 981 |     </rect>
 982 |    </property>
 983 |    <property name="font">
 984 |     <font>
 985 |      <family>Roboto Slab</family>
 986 |      <pointsize>12</pointsize>
 987 |     </font>
 988 |    </property>
 989 |    <property name="autoFillBackground">
 990 |     <bool>false</bool>
 991 |    </property>
 992 |    <property name="styleSheet">
 993 |     <string notr="true">background-color: rgb(237, 255, 248)</string>
 994 |    </property>
 995 |    <property name="frameShape">
 996 |     <enum>QFrame::Panel</enum>
 997 |    </property>
 998 |    <property name="frameShadow">
 999 |     <enum>QFrame::Raised</enum>
1000 |    </property>
1001 |    <property name="lineWidth">
1002 |     <number>1</number>
1003 |    </property>
1004 |    <property name="midLineWidth">
1005 |     <number>0</number>
1006 |    </property>
1007 |    <property name="text">
1008 |     <string>yaw</string>
1009 |    </property>
1010 |    <property name="alignment">
1011 |     <set>Qt::AlignCenter</set>
1012 |    </property>
1013 |    <property name="wordWrap">
1014 |     <bool>false</bool>
1015 |    </property>
1016 |    <property name="margin">
1017 |     <number>4</number>
1018 |    </property>
1019 |   </widget>
1020 |   <widget class="QPushButton" name="yawUp">
1021 |    <property name="geometry">
1022 |     <rect>
1023 |      <x>360</x>
1024 |      <y>430</y>
1025 |      <width>41</width>
1026 |      <height>41</height>
1027 |     </rect>
1028 |    </property>
1029 |    <property name="font">
1030 |     <font>
1031 |      <family>Roboto Slab</family>
1032 |      <pointsize>16</pointsize>
1033 |     </font>
1034 |    </property>
1035 |    <property name="styleSheet">
1036 |     <string notr="true">color : rgb(255, 255, 255);
1037 | </string>
1038 |    </property>
1039 |    <property name="text">
1040 |     <string/>
1041 |    </property>
1042 |    <property name="icon">
1043 |     <iconset>
1044 |      <normaloff>up.png</normaloff>up.png</iconset>
1045 |    </property>
1046 |    <property name="iconSize">
1047 |     <size>
1048 |      <width>32</width>
1049 |      <height>32</height>
1050 |     </size>
1051 |    </property>
1052 |    <property name="flat">
1053 |     <bool>true</bool>
1054 |    </property>
1055 |   </widget>
1056 |   <widget class="QLabel" name="subtitleLabel">
1057 |    <property name="geometry">
1058 |     <rect>
1059 |      <x>190</x>
1060 |      <y>10</y>
1061 |      <width>131</width>
1062 |      <height>21</height>
1063 |     </rect>
1064 |    </property>
1065 |    <property name="font">
1066 |     <font>
1067 |      <family>Roboto Slab</family>
1068 |      <pointsize>10</pointsize>
1069 |      <weight>50</weight>
1070 |      <bold>false</bold>
1071 |     </font>
1072 |    </property>
1073 |    <property name="text">
1074 |     <string>Team Avijatrik</string>
1075 |    </property>
1076 |   </widget>
1077 |  </widget>
1078 |  <resources/>
1079 |  <connections/>
1080 | </ui>
1081 | 


--------------------------------------------------------------------------------
/down.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zahid58/Inverse-Kinematics-6-DOF-Python-With-Visualizer/495c4de76a4cf5836e2cd7e8c7f409c8930d24d4/down.png


--------------------------------------------------------------------------------
/invKin.py:
--------------------------------------------------------------------------------
  1 | from sympy import symbols, cos, sin, pi, simplify, pprint, tan, expand_trig, sqrt, trigsimp, atan2
  2 | from sympy.matrices import Matrix
  3 | 
  4 | from mpl_toolkits.mplot3d import Axes3D
  5 | import matplotlib.pyplot as plt
  6 | import time
  7 | import numpy as np
  8 | 
  9 | def pose(theta, alpha, a, d):
 10 |   # returns the pose T of one joint frame i with respect to the previous joint frame (i - 1)
 11 |   # given the parameters:
 12 |   # theta: theta[i]
 13 |   # alpha: alpha[i-1]
 14 |   # a: a[i-1]
 15 |   # d: d[i]
 16 | 
 17 |   r11, r12 = cos(theta), -sin(theta)
 18 |   r23, r33 = -sin(alpha), cos(alpha)
 19 |   r21 = sin(theta) * cos(alpha)
 20 |   r22 = cos(theta) * cos(alpha)
 21 |   r31 = sin(theta) * sin(alpha)
 22 |   r32 = cos(theta) * sin(alpha)
 23 |   y = -d * sin(alpha)
 24 |   z = d * cos(alpha)
 25 |     
 26 |   T = Matrix([
 27 |     [r11, r12, 0.0, a],
 28 |     [r21, r22, r23, y],
 29 |     [r31, r32, r33, z],
 30 |     [0.0, 0.0, 0.0, 1]
 31 |   ])
 32 |   
 33 |   T = simplify(T)
 34 | 
 35 |   return T
 36 | 
 37 | # under construction
 38 | def forward_kin(q1,q2,q3,q4,q5,q6):
 39 |   X = []
 40 |   Y = []
 41 |   Z = []
 42 |   d90 = pi/2
 43 |   T01 = pose(q1, 0, 0, 0.75)
 44 |   T0g = T01
 45 |   px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3]
 46 |   X.append(px)
 47 |   Y.append(py)
 48 |   Z.append(pz)
 49 |   T12 = pose(q2 - d90, -d90, 0.35, 0)
 50 |   T0g = T0g* T12
 51 |   px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3]
 52 |   X.append(px)
 53 |   Y.append(py)
 54 |   Z.append(pz)
 55 |   T23 = pose(q3, 0, 1.25, 0)
 56 |   T0g = T0g* T23
 57 |   px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3]
 58 |   X.append(px)
 59 |   Y.append(py)
 60 |   Z.append(pz)
 61 |   T34 = pose(q4, -d90, -0.054, 1.5)
 62 |   T0g = T0g* T34
 63 |   px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3]
 64 |   X.append(px)
 65 |   Y.append(py)
 66 |   Z.append(pz)
 67 |   T45 = pose(q5, d90, 0, 0)
 68 |   T0g = T0g* T45
 69 |   px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3]
 70 |   X.append(px)
 71 |   Y.append(py)
 72 |   Z.append(pz)
 73 |   T56 = pose(q6, -d90, 0, 0)
 74 |   T0g = T0g* T56
 75 |   px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3]
 76 |   X.append(px)
 77 |   Y.append(py)
 78 |   Z.append(pz)
 79 |   T6g = pose(0, 0, 0, 0.303)
 80 |   #final position and rotation
 81 |   T0g = T0g* T6g
 82 |   px,py,pz = T0g[0,3], T0g[1,3], T0g[2,3]
 83 |   X.append(px)
 84 |   Y.append(py)
 85 |   Z.append(pz)
 86 |   #fig = plt.figure()
 87 |   #ax = fig.add_subplot(111,projection = '3d')
 88 |   #ax.set_xlabel('x axis')
 89 |   #ax.set_ylabel('y axis')
 90 |   #ax.set_zlabel('z axis')
 91 |   
 92 |   X = np.reshape(X,(1,7))
 93 |   Y = np.reshape(Y,(1,7))
 94 |   Z = np.reshape(Z,(1,7))
 95 | 
 96 |   return X,Y,Z
 97 |   #ax.cla()
 98 |   #ax.plot_wireframe(X,Y,Z)
 99 |   #plt.draw()
100 |   #plt.pause(3)
101 |   #ax.cla()
102 |   #ax.plot_wireframe(Z,Y,X,color='r')
103 |   
104 |   #plt.show()
105 | 
106 | 
107 | 
108 | def create_plot():
109 | 
110 |   fig = plt.figure()
111 |   ax = fig.add_subplot(111,projection = '3d')
112 |   ax.set_xlabel('x axis')
113 |   ax.set_ylabel('y axis')
114 |   ax.set_zlabel('z axis')
115 |   ax.set_xlim3d([0, 2])
116 |   ax.set_ylim3d([0, 3])
117 |   ax.set_zlim3d([0, 3])
118 |   ax.set_autoscale_on(False)
119 |   return fig,ax
120 | 
121 | def update_plot(X,Y,Z,fig,ax):
122 |   X = np.reshape(X,(1,7))
123 |   Y = np.reshape(Y,(1,7))
124 |   Z = np.reshape(Z,(1,7))
125 |   ax.cla()
126 |   ax.plot_wireframe(X,Y,Z)
127 |   #plt.draw()
128 |   ax.set_xlabel('x axis')
129 |   ax.set_ylabel('y axis')
130 |   ax.set_zlabel('z axis')
131 |   ax.set_xlim3d([0, 2])
132 |   ax.set_ylim3d([0, 3])
133 |   ax.set_zlim3d([0, 3])
134 |   ax.set_autoscale_on(False)
135 |   fig.canvas.draw()
136 |   fig.canvas.flush_events()
137 |   #plt.pause(3)
138 |   #ax.cla()
139 |   #ax.plot_wireframe(Z,Y,X,color='r')
140 | 
141 | 
142 | #------------  Rotation Matrix and Euler Angles -----------
143 | # Calculates Rotation Matrix given euler angles.
144 | def eulerAnglesToRotationMatrix(theta) :
145 |      
146 |     R_x = np.array([[1,         0,                  0                   ],
147 |                     [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
148 |                     [0,         math.sin(theta[0]), math.cos(theta[0])  ]
149 |                     ])
150 |          
151 |          
152 |                      
153 |     R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
154 |                     [0,                     1,      0                   ],
155 |                     [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
156 |                     ])
157 |                  
158 |     R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
159 |                     [math.sin(theta[2]),    math.cos(theta[2]),     0],
160 |                     [0,                     0,                      1]
161 |                     ])
162 |                      
163 |                      
164 |     R = np.dot(R_z, np.dot( R_y, R_x ))
165 |  
166 |     return R
167 | 
168 | 
169 | # Checks if a matrix is a valid rotation matrix.
170 | def isRotationMatrix(R) :
171 |     Rt = np.transpose(R)
172 |     shouldBeIdentity = np.dot(Rt, R)
173 |     I = np.identity(3, dtype = R.dtype)
174 |     n = np.linalg.norm(I - shouldBeIdentity)
175 |     return n < 1e-6
176 | 
177 | # Calculates rotation matrix to euler angles
178 | # The result is the same as MATLAB except the order
179 | # of the euler angles ( x and z are swapped ).
180 | def rotationMatrixToEulerAngles(R) :
181 |  
182 |     assert(isRotationMatrix(R))
183 |      
184 |     sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
185 |      
186 |     singular = sy < 1e-6
187 |  
188 |     if  not singular :
189 |         x = math.atan2(R[2,1] , R[2,2])
190 |         y = math.atan2(-R[2,0], sy)
191 |         z = math.atan2(R[1,0], R[0,0])
192 |     else :
193 |         x = math.atan2(-R[1,2], R[1,1])
194 |         y = math.atan2(-R[2,0], sy)
195 |         z = 0
196 |  
197 |     return np.array([x, y, z])
198 | #---------------------------------------
199 | 
200 | 
201 | def get_hypotenuse(a, b):
202 |   # calculate the longest side given the two shorter sides 
203 |   # of a right triangle using pythagorean theorem
204 |   return sqrt(a*a + b*b)
205 | 
206 | 
207 | def get_cosine_law_angle(a, b, c):    
208 |   # given all sides of a triangle a, b, c
209 |   # calculate angle gamma between sides a and b using cosine law
210 |   cos_gamma = (a*a + b*b - c*c) / (2*a*b)
211 |   sin_gamma = sqrt(1 - cos_gamma * cos_gamma)
212 |   gamma = atan2(sin_gamma, cos_gamma)
213 | 
214 |   return gamma
215 | 
216 | 
217 | def get_wrist_center(gripper_point, R0g, dg = 0.303):
218 |   # get the coordinates of the wrist center wrt to the base frame (xw, yw, zw)
219 |   # given the following info:
220 |   # the coordinates of the gripper (end effector) (x, y, z)
221 |   # the rotation of the gripper in gripper frame wrt to the base frame (R0u)
222 |   # the distance between gripper and wrist center dg which is along common z axis
223 |   # check WRITEUP.pdf for more info
224 |   xu, yu, zu = gripper_point 
225 |     
226 |   nx, ny, nz = R0g[0, 2], R0g[1, 2], R0g[2, 2]
227 |   xw = xu - dg * nx
228 |   yw = yu - dg * ny
229 |   zw = zu - dg * nz 
230 | 
231 |   return xw, yw, zw
232 | 
233 | 
234 | def get_first_three_angles(wrist_center):
235 |   # given the wrist center which a tuple of 3 numbers x, y, z
236 |   # (x, y, z) is the wrist center point wrt base frame
237 |   # return the angles q1, q2, q3 for each respective joint
238 |   # given geometry of the kuka kr210
239 |   # check WRITEUP.pdf for more info
240 |   x, y, z  = wrist_center
241 |     
242 |   a1, a2, a3 = 0.35, 1.25, -0.054
243 |   d1, d4 = 0.75, 1.5
244 |   l = 1.50097168527591 # get_hypotenuse(d4, abs(a3))
245 |   phi = 1.53481186671284 # atan2(d4, abs(a3))
246 |   
247 |   x_prime = get_hypotenuse(x, y)
248 |   mx = x_prime -  a1
249 |   my = z - d1 
250 |   m = get_hypotenuse(mx, my)
251 |   alpha = atan2(my, mx)
252 |   
253 |   gamma = get_cosine_law_angle(l, a2, m)
254 |   beta = get_cosine_law_angle(m, a2, l)
255 |   
256 |   q1 = atan2(y, x)
257 |   q2 = pi/2 - beta - alpha 
258 |   q3 = -(gamma - phi)
259 |     
260 |   return q1, q2, q3 
261 | 
262 | 
263 | def get_last_three_angles(R):
264 |   #Recall that from our simplification, R36 (R) equals the following:
265 |   #Matrix([
266 |   #[-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)],
267 |   #[                           sin(q5)*cos(q6),                           -sin(q5)*sin(q6),          cos(q5)],
268 |   #[-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4),  sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6),  sin(q4)*sin(q5)]])
269 |   #From trigonometry we can get q4, q5, q6 if we know numerical values of all cells of matrix R36 (R)
270 |   #check WRITEUP.pdf for more info    
271 |   sin_q4 = R[2, 2]
272 |   cos_q4 =  -R[0, 2]
273 |     
274 |   sin_q5 = sqrt(R[0, 2]**2 + R[2, 2]**2) 
275 |   cos_q5 = R[1, 2]
276 |     
277 |   sin_q6 = -R[1, 1]
278 |   cos_q6 = R[1, 0] 
279 |   
280 |   q4 = atan2(sin_q4, cos_q4)
281 |   q5 = atan2(sin_q5, cos_q5)
282 |   q6 = atan2(sin_q6, cos_q6)
283 |     
284 |   return q4, q5, q6
285 | 
286 | 
287 | def get_angles(x, y, z, roll, pitch, yaw):
288 |   # input: given position and orientation of the gripper_URDF wrt base frame
289 |   # output: angles q1, q2, q3, q4, q5, q6
290 |     
291 |   gripper_point = x, y, z
292 | 
293 |   ################################################################################
294 |   # All important symbolic transformations matrices are declared below 
295 |   ################################################################################
296 | 
297 |   q1, q2, q3, q4, q5, q6 = symbols('q1:7')
298 |   alpha, beta, gamma = symbols('alpha beta gamma', real = True)
299 |   px, py, pz = symbols('px py pz', real = True)
300 | 
301 |   # Rotation of joint 3 wrt to the base frame interms the first three angles q1, q2, q3
302 |   R03 = Matrix([
303 |     [sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],
304 |     [sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],
305 |     [        cos(q2 + q3),        -sin(q2 + q3),        0]])
306 | 
307 |   # Transpose of R03 
308 |   R03T = Matrix([
309 |     [sin(q2 + q3)*cos(q1), sin(q1)*sin(q2 + q3),  cos(q2 + q3)],
310 |     [cos(q1)*cos(q2 + q3), sin(q1)*cos(q2 + q3), -sin(q2 + q3)],
311 |     [            -sin(q1),              cos(q1),             0]])
312 | 
313 |   # Rotation of joint 6 wrt to frame of joint 3 interms of the last three angles q4, q5, q6
314 |   R36 = Matrix([
315 |     [-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)],
316 |     [                           sin(q5)*cos(q6),                           -sin(q5)*sin(q6),          cos(q5)],
317 |     [-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4),  sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6),  sin(q4)*sin(q5)]])
318 | 
319 |   # Rotation of urdf_gripper with respect to the base frame interms of alpha = yaw, beta = pitch, gamma = roll
320 |   R0u = Matrix([
321 |     [1.0*cos(alpha)*cos(beta), -1.0*sin(alpha)*cos(gamma) + sin(beta)*sin(gamma)*cos(alpha), 1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma)],
322 |     [1.0*sin(alpha)*cos(beta),  sin(alpha)*sin(beta)*sin(gamma) + 1.0*cos(alpha)*cos(gamma), sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha)],
323 |     [          -1.0*sin(beta),                                     1.0*sin(gamma)*cos(beta),                                    1.0*cos(beta)*cos(gamma)]])
324 | 
325 |   # Total transform of gripper wrt to base frame given orientation yaw (alpha), pitch (beta), roll (beta) and position px, py, pz
326 |   T0g_b = Matrix([
327 |     [1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma),  1.0*sin(alpha)*cos(gamma) - 1.0*sin(beta)*sin(gamma)*cos(alpha), 1.0*cos(alpha)*cos(beta), px],
328 |     [sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha), -1.0*sin(alpha)*sin(beta)*sin(gamma) - 1.0*cos(alpha)*cos(gamma), 1.0*sin(alpha)*cos(beta), py],
329 |     [                                   1.0*cos(beta)*cos(gamma),                                        -1.0*sin(gamma)*cos(beta),           -1.0*sin(beta), pz],
330 |     [                                                          0,                                                                0,                        0,  1]])
331 | 
332 |   # Total transform of gripper wrt to base frame given angles q1, q2, q3, q4, q5, q6
333 |   T0g_a = Matrix([
334 |     [((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*cos(q6) - (-sin(q1)*cos(q4) + sin(q4)*sin(q2 + q3)*cos(q1))*sin(q6), -((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*sin(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*cos(q6), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), -0.303*sin(q1)*sin(q4)*sin(q5) + 1.25*sin(q2)*cos(q1) - 0.303*sin(q5)*sin(q2 + q3)*cos(q1)*cos(q4) - 0.054*sin(q2 + q3)*cos(q1) + 0.303*cos(q1)*cos(q5)*cos(q2 + q3) + 1.5*cos(q1)*cos(q2 + q3) + 0.35*cos(q1)],
335 |     [ ((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*cos(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*sin(q6), -((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*sin(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*cos(q6), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3),  1.25*sin(q1)*sin(q2) - 0.303*sin(q1)*sin(q5)*sin(q2 + q3)*cos(q4) - 0.054*sin(q1)*sin(q2 + q3) + 0.303*sin(q1)*cos(q5)*cos(q2 + q3) + 1.5*sin(q1)*cos(q2 + q3) + 0.35*sin(q1) + 0.303*sin(q4)*sin(q5)*cos(q1)],
336 |     [                                                                -(sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*cos(q6) - sin(q4)*sin(q6)*cos(q2 + q3),                                                                  (sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*sin(q6) - sin(q4)*cos(q6)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),                                                                                 -0.303*sin(q5)*cos(q4)*cos(q2 + q3) - 0.303*sin(q2 + q3)*cos(q5) - 1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],
337 |     [                                                                                                                                                            0,                                                                                                                                                             0,                                                                                        0,                                                                                                                                                                                                              1]])
338 | 
339 |   # Rotation of urdf_gripper wrt (DH) gripper frame from rotz(pi) * roty(-pi/2) and it's transpose
340 |   Rgu_eval = Matrix([[0, 0, 1], [0, -1.00000000000000, 0], [1.00000000000000, 0, 0]])
341 |   RguT_eval = Matrix([[0, 0, 1], [0, -1.00000000000000, 0], [1.00000000000000, 0, 0]])
342 | 
343 |   # Inverse kinematics transformations starts below
344 | 
345 |   R0u_eval = R0u.evalf(subs = {alpha: yaw, beta: pitch, gamma: roll})
346 |   R0g_eval = R0u_eval * RguT_eval
347 | 
348 |   wrist_center = get_wrist_center(gripper_point, R0g_eval, dg = 0.303)
349 | 
350 |   j1, j2, j3 = get_first_three_angles(wrist_center)
351 | 
352 |   R03T_eval = R03T.evalf(subs = {q1: j1.evalf(), q2: j2.evalf(), q3: j3.evalf()})
353 |   R36_eval = R03T_eval * R0g_eval
354 | 
355 |   j4, j5, j6 = get_last_three_angles(R36_eval)
356 | 
357 |   j1 = j1.evalf()
358 |   j2 = j2.evalf()
359 |   j3 = j3.evalf()
360 |   j4 = j4.evalf()
361 |   j5 = j5.evalf()
362 |   j6 = j6.evalf()
363 | 
364 |   return j1, j2, j3, j4, j5, j6
365 | 
366 | 
367 | def main():
368 |         px, py, pz = 0.49792, 1.3673,3
369 |         #px, py, pz = 0.49792, 1.3673, 2.4988
370 |         roll, pitch, yaw = 1, -1, -1
371 |         #roll, pitch, yaw = 0.366, -0.078, 2.561
372 |         q1,q2,q3,q4,q5,q6 = get_angles(px,py,pz,roll,pitch,yaw)
373 |         print("q1 : ",q1)
374 |         print("q2 : ",q2)
375 |         print("q3 : ",q3)
376 |         print("q4 : ",q4)
377 |         print("q5 : ",q5)
378 |         print("q6 : ",q6)
379 | 
380 |         #now plot the arm with updated angles
381 |         forward_kin(q1,q2,q3,q4,q5,q6)
382 | 
383 | if __name__=="__main__":
384 |   main()
385 | 


--------------------------------------------------------------------------------
/kukadesign.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zahid58/Inverse-Kinematics-6-DOF-Python-With-Visualizer/495c4de76a4cf5836e2cd7e8c7f409c8930d24d4/kukadesign.gif


--------------------------------------------------------------------------------
/roboticArm.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zahid58/Inverse-Kinematics-6-DOF-Python-With-Visualizer/495c4de76a4cf5836e2cd7e8c7f409c8930d24d4/roboticArm.png


--------------------------------------------------------------------------------
/up.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zahid58/Inverse-Kinematics-6-DOF-Python-With-Visualizer/495c4de76a4cf5836e2cd7e8c7f409c8930d24d4/up.png


--------------------------------------------------------------------------------