Skip to content

Commit 93900f8

Browse files
cedricWassenaarcedricWassenaar
committed
Initial Commit
Requirements: - PyQt5 - pyqtgraph/opengl - numpy
1 parent d644a21 commit 93900f8

File tree

4 files changed

+880
-0
lines changed

4 files changed

+880
-0
lines changed

‎DOF3_Robot.py‎

Lines changed: 289 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,289 @@
1+
import rm_utilitiesV3 as Rm
2+
import numpy as np
3+
import pyqtgraph.opengl as gl
4+
5+
6+
"""
7+
Simulation of a 6 DOF robot.
8+
- Cédric Wassenaar & Joeri Verkerk
9+
- C.M.Wassenaar@student.hhs.nl
10+
- 24-09-2018
11+
"""
12+
13+
14+
class Link(object):
15+
"""Storage type for robot arm object"""
16+
def __init__(self):
17+
self.arm = None
18+
self.arm_length = 0
19+
self.frame = None
20+
self.joint = None
21+
self.DH_parameters = [0, 0, 0, 0]
22+
self.homogenious_matrix = None
23+
self.hg_matrix_list = []
24+
self.p = None
25+
26+
27+
class Robot(object):
28+
"""Class to create robot object for OpenGl implementation"""
29+
def __init__(self, view3D):
30+
# Save pointer to 3d renderer
31+
self.view3D = view3D
32+
# declare presets
33+
self.radius = 0.2
34+
self.width = .6 * 2 * self.radius
35+
self.depth = self.width
36+
self.depth_cylinder = 1.2 * self.width
37+
self.segments = 40
38+
self.joint_color = (0.8, 0.2, 1, 1) # yellow
39+
self.arm_color = (0.4, 0.4, 0.4, 1) # light blue
40+
self.cycle = 0
41+
self.iteration = 0
42+
self.N = 0
43+
self.ready = False
44+
self.print_text = []
45+
# define arm lengths
46+
self.arm_lengths = [1.75,
47+
1.60,
48+
1.25,
49+
0.00,
50+
0.00,
51+
0.00,
52+
0.50,
53+
0.00]
54+
55+
# Create a list of arm pieces
56+
self.link = [Link()] * (len(self.arm_lengths) + 1)
57+
58+
# define Target rotation for the end effector
59+
# self.target_rotation = Rm.make_rotation_matrix("x", np.radians(00))
60+
self.target_rotation = np.eye(3, 3)
61+
# define trajectory
62+
# X, Y, Z, Ax, Ay, Az (degrees)
63+
self.trajectory = np.array([[-1.5, 1, 2, 0, 0, 0],
64+
[1.5, 0.5, 4, 0, 0, np.pi/2],
65+
[1.5, 2.0, 1, 0, np.pi, np.pi/2],
66+
[-0.0, 2.0, 3, 0, np.pi, 0],
67+
[-1.5, 2, 2, 0, 0, 0]])
68+
self.setup()
69+
70+
def setup(self):
71+
"""
72+
Function for user defined objects and functions
73+
:return:
74+
"""
75+
# create all arm pieces
76+
self.link[0].frame = self.create_main_axis()
77+
self.link[1] = self.add_arm(self.link[0], self.arm_lengths[0])
78+
self.link[1].frame.setSize(0.5, 0.5, 0.5)
79+
self.link[2] = self.add_arm(self.link[1], self.arm_lengths[1])
80+
self.link[2].frame.setSize(0.5, 0.5, 0.5)
81+
self.link[3] = self.add_arm(self.link[2], self.arm_lengths[2])
82+
self.link[3].frame.setSize(0.1, 0.1, 0.1)
83+
self.link[4] = self.add_arm(self.link[3], self.arm_lengths[3])
84+
self.link[4].frame.setSize(0.1, 0.1, 0.1)
85+
self.link[5] = self.add_arm(self.link[4], self.arm_lengths[4])
86+
self.link[5].frame.setSize(0.1, 0.1, 0.1)
87+
self.link[6] = self.add_arm(self.link[5], self.arm_lengths[5])
88+
self.link[6].frame.setSize(0.1, 0.1, 0.1)
89+
self.link[7] = self.add_arm(self.link[6], self.arm_lengths[6])
90+
self.link[7].frame.setSize(0.1, 0.1, 0.1)
91+
self.link[8] = self.add_arm(self.link[7], self.arm_lengths[7])
92+
93+
# Rotate arms to work with Denavit Hartenberg parameters
94+
self.link[1].arm.rotate(90, 1, 0, 0)
95+
self.link[2].arm.rotate(-90, 0, 1, 0)
96+
self.link[3].arm.rotate(-90, 0, 1, 0)
97+
98+
# Predefine static values of the Denavit Hartenberg parameters
99+
self.link[0].DH_parameters = [0, 0, 0, 0]
100+
self.link[1].DH_parameters = [0, 1.57079, self.arm_lengths[0], 0]
101+
self.link[2].DH_parameters = [self.arm_lengths[1], 0, 0, 0]
102+
self.link[3].DH_parameters = [self.arm_lengths[2], 0, 0, 0]
103+
self.link[4].DH_parameters = [0, 1.57079, 0, 1.57079]
104+
self.link[5].DH_parameters = [0, -1.57079, 0, 0]
105+
self.link[6].DH_parameters = [0, 1.57079, 0, 0]
106+
self.link[7].DH_parameters = [0, 0, 0, 0]
107+
self.link[8].DH_parameters = [0, 0, self.arm_lengths[6], 0]
108+
109+
def set_new_trajectory(self, new_trajectory, precision):
110+
"""Sets and calculates new trajectory for robot"""
111+
self.trajectory = Rm.interpolate(new_trajectory, precision)
112+
self.N = self.trajectory.shape[0]
113+
self.iteration = 0
114+
self.calculate_path()
115+
116+
def calculate_path(self):
117+
"""In order to reduce CPU use the inverse kinematics are pre-calculated and saved in a list,
118+
the update_window methods will then iterate through this list."""
119+
# Clear old path
120+
self.print_text = []
121+
for i in range(1, len(self.link)):
122+
self.link[i].hg_matrix_list = []
123+
124+
# Create new path
125+
length = len(self.trajectory)
126+
for current in range(length):
127+
# In order to prevent call by reference the trajectory values are copied.
128+
pos = self.trajectory[current].copy()
129+
rotation = Rm.rotate_xyz(pos[3:])
130+
pos[0] = pos[0] - self.arm_lengths[6] * rotation[0, 2]
131+
pos[1] = pos[1] - self.arm_lengths[6] * rotation[1, 2]
132+
pos[2] = pos[2] - self.arm_lengths[6] * rotation[2, 2]
133+
134+
# The Inverse kinematics are calculated in the rm_utilitiesV3 file
135+
angles, error = Rm.inverse_algorithm_3DOF(self.arm_lengths, pos)
136+
137+
# If the robot arm is not capable of reaching the given coordinate it will raise an error
138+
if not error:
139+
# The dynamic values of the Denavit Hartenberg parameters are now updated.
140+
self.link[1].DH_parameters[3] = angles[0]
141+
self.link[2].DH_parameters[3] = angles[1]
142+
self.link[3].DH_parameters[3] = angles[2]
143+
144+
# The homogenious matrices are created for link 0 to 4
145+
for i in range(5):
146+
self.link[i].homogenious_matrix = Rm.make_DH_matrix(self.link[i].DH_parameters)
147+
148+
# The H03 matrix is calculated using the @ symbol (matrix multiplication)
149+
H03 = self.link[0].homogenious_matrix @ \
150+
self.link[1].homogenious_matrix @ \
151+
self.link[2].homogenious_matrix @ \
152+
self.link[3].homogenious_matrix @ \
153+
self.link[4].homogenious_matrix
154+
155+
# The 3x3-rotation matrix R36 is calculated
156+
R36 = np.matrix.transpose(H03[:3, :3]) @ rotation
157+
158+
# The inverse kinematics for the wrist is calculated with R36
159+
rotate, R_compare = Rm.inverse_kinematics_wrist(R36)
160+
161+
self.link[5].DH_parameters[3] = rotate[0]
162+
self.link[6].DH_parameters[3] = rotate[1]
163+
self.link[7].DH_parameters[3] = rotate[2]
164+
165+
for i in range(5, 9):
166+
self.link[i].homogenious_matrix = Rm.make_DH_matrix(self.link[i].DH_parameters)
167+
168+
# The end effector matrices are calculated
169+
H36 = self.link[5].homogenious_matrix @ \
170+
self.link[6].homogenious_matrix @ \
171+
self.link[7].homogenious_matrix @ \
172+
self.link[8].homogenious_matrix
173+
174+
H06 = H03 @ H36
175+
176+
p0 = np.array([self.trajectory[current, 0],
177+
self.trajectory[current, 1],
178+
self.trajectory[current, 2],
179+
1])
180+
181+
TH6 = np.linalg.inv(H06).dot(p0)
182+
183+
# All flattened homogenious matrices are saved in a list
184+
for i in range(1, len(self.link)):
185+
# self.link[i].frame.setTransform(self.link[i].homogenious_matrix.flatten())
186+
matrix = self.link[i].homogenious_matrix.flatten()
187+
self.link[i].hg_matrix_list.append(matrix)
188+
189+
# The positions are saved in textformat
190+
angles = str(format(np.degrees(angles[0]), '.1f')) + \
191+
"\t" + str(format(np.degrees(angles[1]), '.1f')) + \
192+
"\t" + str(format(np.degrees(angles[2]), '.1f')) + "\n\n"
193+
194+
rotate = str(format(np.degrees(rotate[0]), '.1f')) + \
195+
"\t" + str(format(np.degrees(rotate[1]), '.1f')) + \
196+
"\t" + str(format(np.degrees(rotate[2]), '.1f')) + "\n\n"
197+
198+
self.print_text.append(
199+
"R36:\n" + str(R36) +
200+
"\n\nR36 check:\n" + str(R_compare) +
201+
"\n\nArm Angles:\n" + angles +
202+
"Wrist Angles:\n" + rotate +
203+
"Target: " + str(self.trajectory[current]) +
204+
"\noc: " + str(pos) +
205+
"\nTH6: " + str(TH6))
206+
207+
# Each 5th step of the trajectory a small block is rendered to show the path of the robot
208+
if (current % 6) > 4:
209+
self.show_path(H06)
210+
211+
else:
212+
print("Error Calculating, position possibly out of range")
213+
self.ready = True
214+
215+
def __str__(self):
216+
# The standard str() function is overwritten to write the coordinates of the robot when
217+
# str(Robot) is called
218+
return self.print_text[self.iteration]
219+
220+
def add_arm(self, parent_object, length):
221+
"""
222+
:param parent_object:
223+
:param length:
224+
:return:
225+
"""
226+
link = Link()
227+
link.frame = self.set_new_axis(parent_object.frame)
228+
link.arm = self.create_arm(length)
229+
link.arm.setParentItem(link.frame)
230+
link.arm_length = length
231+
link.arm.rotate(-90, 0, 1, 0)
232+
link.arm.translate(self.width / 2, -self.width / 2, self.width / 2)
233+
link.joint = self.create_joint()
234+
link.joint.setParentItem(link.frame)
235+
link.joint.translate(0, 0, -self.depth_cylinder/ 2)
236+
return link
237+
238+
def create_arm(self, length):
239+
"""Creates a box that is attached to a certain object"""
240+
size = (length, self.width, self.depth)
241+
vertices_arm, faces_arm = Rm.box(size)
242+
arm = gl.GLMeshItem(vertexes=vertices_arm, faces=faces_arm,
243+
rawEdges=False, drawFaces=True, color=self.arm_color)
244+
self.view3D.addItem(arm)
245+
return arm
246+
247+
def create_joint(self):
248+
"""Creates a joint that is attached to a certain object"""
249+
vertices_joint, faces_joint = Rm.cylinder(self.radius, self.depth_cylinder, self.segments)
250+
joint = gl.GLMeshItem(vertexes=vertices_joint, faces=faces_joint,
251+
drawEdges=False, drawFaces=True, color=self.joint_color)
252+
self.view3D.addItem(joint)
253+
return joint
254+
255+
def set_new_axis(self, parent_object):
256+
"""Adds axis to given object"""
257+
axis = gl.GLAxisItem(antialias=True, glOptions='opaque')
258+
axis.updateGLOptions({'glLineWidth': (3,)})
259+
axis.setParentItem(parent_object)
260+
self.view3D.addItem(axis)
261+
return axis
262+
263+
def create_main_axis(self):
264+
"""Create the basis axis"""
265+
axis = gl.GLAxisItem()
266+
axis.updateGLOptions({'glLineWidth': (6,)})
267+
self.view3D.addItem(axis)
268+
return axis
269+
270+
def show_path(self, frame):
271+
vertices_arm, faces_arm = Rm.box((0.05, 0.05, 0.05))
272+
box = gl.GLMeshItem(vertexes=vertices_arm, faces=faces_arm,
273+
rawEdges=False, drawFaces=True, color=(1, 0.3, 0.4, 1))
274+
box.setParentItem(self.link[0].frame)
275+
box.setTransform(frame.flatten())
276+
self.view3D.addItem(box)
277+
278+
def update_window(self):
279+
"""Render new frame of 3D view"""
280+
# When ready the robot is rendered and updated.
281+
if self.ready:
282+
for i in range(1, len(self.link)):
283+
self.link[i].frame.setTransform(self.link[i].hg_matrix_list[self.iteration])
284+
# increase the counter, such that next time we get the next point
285+
self.iteration += 1
286+
# if the counter arives at N, reset it to 0, to repeat the trajectory
287+
if self.iteration == self.N:
288+
self.iteration = 0
289+
self.cycle += 1

0 commit comments

Comments
 (0)