def modellrechnung(self, xP2, yP2, xP4, yP4, xP5, yP5):
'Kartesische Koordinaten'
'Punkt2, Punkt4 und Punkt5'
self.xP2 = xP2
self.yP2 = yP2
self.xP4 = xP4
self.yP4 = yP4
self.xP5 = xP5
self.yP5 = yP5
'Polarkoordinaten'
'Radien von Punkt2, Punkt4 und Punkt5'
self.r2 = ((self.xP2 - self.xP1)**2 + (self.yP2 - self.yP1)**2)**(1/2)
self.r4 = ((self.xP4 - self.xP1)**2 + (self.yP4 - self.yP1)**2)**(1/2)
self.r5 = ((self.xP5 - self.xP1)**2 + (self.yP5 - self.yP1)**2)**(1/2)
'Winkel im Intervall [0, 2*pi]'
'Winkel von Punkt2 berechnen'
self.phi2 = winkel_berechnen(self.xP2, self.yP2)
'Winkel von Punkt4 berechnen'
self.phi4 = winkel_berechnen(self.xP4, self.yP4)
'Winkel von Punkt5 berechnen'
self.phi5 = self.phi4
'Theta1 (Denavit-Hartenberg-Parameter) berechnen'
self.theta1 = self.phi4
def modellrechnung(self, xP6, yP6, xP7, yP7):
'Kartesische Koordinaten'
'Punkt6 und Punkt7'
self.xP6 = xP6
self.yP6 = yP6
self.xP7 = xP7
self.yP7 = yP7
'Polarkoordinaten'
'Radien von Punkt6 und Punkt7'
self.r6 = ((self.xP6)**2 + (self.yP6)**2)**(1/2)
self.r7 = self.r6
'Winkel im Intervall [0, 2*pi]'
'Winkel von Punkt6 berechnen'
self.phi6 = winkel_berechnen(self.xP6, self.yP6)
'Winkel von Punkt7 berechnen'
self.phi7 = self.phi6 + pi
'Winkel theta5 (Denavit-Hartenberg-Parameter) berechnen'
self.theta5 = pi - self.phi6
def on_motion(self, event):
'''Die Methode wird weiter ausgeführt wenn das Klassenattribut
mit dem Wert self belegt ist. Das Circle-Objekt darf ohne angeklickt
zu sein nicht bewegt werden.'''
if DraufsichtRoboter.lock is not self: return
'''Die Methode wird weiter ausgeführt wenn der Mauszeiger
innerhalb des Koordinatensystems bewegt wird.'''
if event.inaxes != self.point.axes: return
'''Den Mittelpunkt des Circle-Objektes und die x- und y-Koordinaten
des Mauszeigers zum Zeitpunkt des Drückens der linken Maustaste
zuweisen.'''
self.point.center, xpress, ypress = self.press
'Winkel aller Circle-Objekte neu berechnen'
'Winkel berechnen - Kreismittelpunkt'
phi = winkel_berechnen(self.point4.center[0], self.point4.center[1])
'Winkel berechnen - Mauszeiger beim Anklicken des Punktes'
phi_p = winkel_berechnen(xpress, ypress)
'Winkel berechnen - Mauszeiger bei der Bewegung'
phi_e = winkel_berechnen(event.xdata, event.ydata)
'Winkeländerung berechnen'
dphi = phi_e - phi_p
'neuen Positionswinkel berechnen'
phi_neu = phi + dphi
'Winkel auf [0, 2*pi] normieren um Rundungsfehler auszugleichen'
phi_neu = winkel_normieren(phi_neu)
'Winkelgrenzen zuweisen'
phi_min = self.theta1_min
phi_max = self.theta1_max
'Winkelbegrenzung [phi_min, phi_max]'
if phi_neu <= phi_min or phi_neu > 3*pi/2 and phi_neu < 2*pi:
phi_neu = phi_min
elif phi_neu >= phi_max:
phi_neu = phi_max
'Winkel von Punkt2 aktualisieren'
#Rundungsfehler berücksichtigen
if round(self.phi2, 3) == round(self.phi4, 3):
self.phi2 = phi_neu
elif round(self.phi2, 3) != round(self.phi4, 3):
self.phi2 = phi_neu + pi
'Winkel von Punkt4 aktualisieren'
self.phi4 = phi_neu
'Winkel von Punkt5 aktualisieren'
self.phi5 = self.phi4
'Theta1 (Denavit-Hartenberg-Parameter) aktualisieren'
self.theta1 = self.phi4
'Mittelpunktkoordinaten der Circle-Objekte aktualisieren'
'Koordinaten von Punkt2'
self.xP2 = self.r2*cos(self.phi2)
self.yP2 = self.r2*sin(self.phi2)
'Koordinaten akualisieren'
self.point2.center = ((self.xP2, self.yP2))
'Koordinaten von Punkt4'
self.xP4 = self.r4*cos(self.phi4)
self.yP4 = self.r4*sin(self.phi4)
'Koordinaten akualisieren'
self.point4.center = ((self.xP4, self.yP4))
'Koordinaten von Punkt5'
self.xP5 = self.r5*cos(self.phi5)
self.yP5 = self.r5*sin(self.phi5)
'Koordinaten akualisieren'
self.point5.center = ((self.xP5, self.yP5))
'Endpunktkoordinaten der Line2D-Objekte aktualisieren'
'Koordinaten von Linie1'
self.xL1 = (self.point1.center[0], self.point2.center[0])
self.yL1 = (self.point1.center[1], self.point2.center[1])
'Koordinaten akualisieren'
self.line1.set_data(self.xL1, self.yL1)
'Koordinaten von Linie2'
self.xL2 = (self.point2.center[0], self.point4.center[0])
self.yL2 = (self.point2.center[1], self.point4.center[1])
'Koordinaten akualisieren'
self.line2.set_data(self.xL2, self.yL2)
'Koordinaten von Linie3'
self.xL3 = (self.point4.center[0], self.point5.center[0])
self.yL3 = (self.point4.center[1], self.point5.center[1])
'Koordinaten akualisieren'
self.line3.set_data(self.xL3, self.yL3)
'Methoden der Animation-Blit-Technik ausführen'
canvas = self.point.figure.canvas
axes = self.point.axes
canvas.restore_region(self.background)
axes.draw_artist(self.point2)
axes.draw_artist(self.point4)
axes.draw_artist(self.point5)
axes.draw_artist(self.line1)
axes.draw_artist(self.line2)
axes.draw_artist(self.line3)
canvas.blit(axes.bbox)
'Signal mit den neuen Koordinaten senden'
self.xy_neu.emit(self.xP2, self.yP2, self.xP4, self.yP4, \
self.xP5, self.yP5)
def modellrechnung(self, xP2, yP2, xP3, yP3, xP4, yP4, xP5, yP5):
'Kartesische Koordinaten'
'Punkt2, Punkt3, Punkt4 und Punkt5'
self.xP2 = xP2
self.yP2 = yP2
self.xP3 = xP3
self.yP3 = yP3
self.xP4 = xP4
self.yP4 = yP4
self.xP5 = xP5
self.yP5 = yP5
'Länge des Ortsvektors zu Punkt4 berechnen'
self.r4 = (self.xP4**2 + self.yP4**2)**(1 / 2)
'Hilfswinkel berechnen - Cosinussatz'
b = (self.a3**2 - self.a2**2 - self.r4**2) / (-2 * self.a2 * self.r4)
if b >= 1: b = 1
elif b <= -1: b = -1
beta1 = acos(b)
beta2 = acos(self.xP4 / self.r4)
'Winkel von Punkt2 berechnen'
self.phi2 = beta1 + beta2
'Winkel auf [0, 2*pi] normieren um Rundungsfehler auszugleichen'
self.phi2 = winkel_normieren(self.phi2)
'Theta2 (Denavit-Hartenberg-Parameter) berechnen'
self.theta2 = self.phi2
'Hilfswinkel berechnen - Cosinussatz'
b = (self.r4**2 - self.a2**2 - self.a3**2) / (-2 * self.a2 * self.a3)
if b >= 1: b = 1
elif b <= -1: b = -1
beta3 = acos(b)
'Winkel von Punkt4 berechnen'
self.phi4 = beta3 - (pi - self.phi2)
'Theta3 (Denavit-Hartenberg-Parameter) berechnen'
self.theta3 = -(pi - beta3)
'Winkel von Punkt3 berechnen'
self.phi3 = self.phi4 + pi / 4 - 8 * pi / 180
'Winkel auf [0, 2*pi] normieren um Rundungsfehler auszugleichen'
self.phi3 = winkel_normieren(self.phi3)
'Verschiebung von Punkt5 relativ zu Punkt4'
dx = self.xP5 - self.xP4
dy = self.yP5 - self.yP4
'Winkel von Punkt5 berechnen'
self.phi5 = winkel_berechnen(dx, dy)
'Winkel normieren'
self.phi5 = winkel_normieren(self.phi5)
'Theta4 (Denavit-Hartenberg-Parameter) berechnen'
if self.phi5 >= 0 and self.phi5 <= pi / 2:
self.theta4 = self.phi5 + pi / 2 - 8 * pi / 180
elif self.phi5 >= 3 * pi / 2 and self.phi5 < 2 * pi:
self.theta4 = pi / 2 - 8 * pi / 180 - 2 * pi + self.phi5
def on_motion(self, event):
'''Die Methode wird weiter ausgeführt wenn das Klassenattribut
mit dem Wert self belegt ist. Das Circle-Objekt darf ohne angeklickt
zu sein nicht bewegt werden.'''
if SeitenansichtRoboter.lock is not self: return
'''Die Methode wird weiter ausgeführt wenn der Mauszeiger
innerhalb des Koordinatensystems bewegt wird.'''
if event.inaxes != self.point.axes: return
'''Den Mittelpunkt des Circle-Objektes und die x- und y-Koordinaten
des Mauszeigers zum Zeitpunkt des Drückens der linken Maustaste
zuweisen.'''
self.point.center, xpress, ypress = self.press
'Fallunterscheidung - Punkt4 oder Punkt5'
if self.point == self.point4:
'Verschiebung von Punkt4 in x-, und y-Richtung'
dx = event.xdata - xpress
dy = event.ydata - ypress
'neue Koordinaten berechnen'
x_neu = self.point.center[0] + dx
y_neu = self.point.center[1] + dy
'Begrenzung auf [x_min,...[ und [y_min...['
if x_neu < self.x_min and dx < 0:
x_neu = self.x_min
if y_neu < self.y_min and dy < 0:
y_neu = self.y_min
'Länge des Ortsvektors zu Punkt4 berechnen'
self.r4 = (x_neu**2 + y_neu**2)**(1 / 2)
'Hilfswinkel berechnen - Cosinussatz'
b = (self.a3**2 - self.l1**2 - self.r4**2) / (-2 * self.l1 *
self.r4)
if b >= 1: b = 1
elif b <= -1: b = -1
beta1 = acos(b)
beta2 = acos(x_neu / self.r4)
'Winkel von Punkt2 berechnen'
self.phi2 = beta1 + beta2
'Winkel auf [0, 2*pi] normieren um Rundungsfehler auszugleichen'
self.phi2 = winkel_normieren(self.phi2)
'Winkelgrenzen zuweisen'
phi2_min = self.theta2_min
phi2_max = self.theta2_max
'Winkelbegrenzung [phi_min, phi_max]'
if self.phi2 < phi2_min:
self.phi2 = phi2_min
elif self.phi2 > phi2_max:
self.phi2 = phi2_max
'Theta2 (Denavit-Hartenberg-Parameter) aktualisieren'
self.theta2 = self.phi2
'Hilfswinkel berechnen - Cosinussatz'
b = (self.r4**2 - self.l1**2 - self.a3**2) / (-2 * self.l1 *
self.a3)
if b >= 1: b = 1
elif b <= -1: b = -1
beta3 = acos(b)
'Winkelgrenzen zuweisen'
beta3_min = self.theta3_min + pi
beta3_max = self.theta3_max + pi
'Winkelbegrenzung [phi_min, phi_max]'
if beta3 < beta3_min:
beta3 = beta3_min
elif beta3 > beta3_max:
beta3 = beta3_max
'Winkel von Punkt4 berechnen'
self.phi4 = beta3 - (pi - self.phi2)
'Theta3 (Denavit-Hartenberg-Parameter) berechnen'
self.theta3 = -(pi - beta3)
'Winkel von Punkt3 berechnen'
self.phi3 = self.phi4 + pi / 4 - 8 * pi / 180
'Winkel auf [0, 2*pi] normieren um Rundungsfehler auszugleichen'
self.phi3 = winkel_normieren(self.phi3)
'Mittelpunktkoordinaten der Circle-Objekte aktualisieren'
'Koordinaten von Punkt2'
self.xP2 = self.l1 * cos(self.phi2)
self.yP2 = self.l1 * sin(self.phi2)
'Koordinaten akualisieren'
self.point2.center = ((self.xP2, self.yP2))
'Koordinaten von Punkt3'
self.xP3 = self.point2.center[0] + self.l2 * cos(self.phi3)
self.yP3 = self.point2.center[1] + self.l2 * sin(self.phi3)
'Koordinaten akualisieren'
self.point3.center = ((self.xP3, self.yP3))
'Koordinaten von Punkt4'
self.xP4 = self.point2.center[0] + self.a3 * cos(self.phi4)
self.yP4 = self.point2.center[1] + self.a3 * sin(self.phi4)
'Koordinaten akualisieren'
self.point4.center = ((self.xP4, self.yP4))
elif self.point == self.point5:
'relative Verschiebung'
dx = self.point5.center[0] - self.point4.center[0]
dy = self.point5.center[1] - self.point4.center[1]
'Winkel berechnen - Kreismittelpunkt'
phi = winkel_berechnen(dx, dy)
'relative Verschiebung'
dx_p = xpress - self.point4.center[0]
dy_p = ypress - self.point4.center[1]
'Winkel berechnen - Mauszeiger beim Anklicken des Punktes'
phi_p = winkel_berechnen(dx_p, dy_p)
'relative Verschiebung'
dx_e = event.xdata - self.point4.center[0]
dy_e = event.ydata - self.point4.center[1]
'Winkel berechnen - Mauszeiger bei der Bewegung'
phi_e = winkel_berechnen(dx_e, dy_e)
'Winkeländerung berechnen'
dphi = phi_e - phi_p
'neuen Positionswinkel berechnen'
phi_neu = phi + dphi
'Winkel auf [0, 2*pi] normieren um Rundungsfehler auszugleichen'
phi_neu = winkel_normieren(phi_neu)
'Winkel von Punkt5 berechnen'
self.phi5 = phi_neu
'Winkelgrenzen - senkrechter Anschlag'
if self.phi5 > pi / 2 and self.phi5 < pi:
self.phi5 = pi / 2
elif self.phi5 >= pi and self.phi5 < 3 * pi / 2:
self.phi5 = 3 * pi / 2
'Winkelgrenzen - rechtwinkeliger Anschlag'
phi5_min = self.phi4 - 8 * pi / 180 + 3 * pi / 2
phi5_max = self.phi4 - 8 * pi / 180 + pi / 2
'Winkelbegrenzung [phi_min, phi_max]'
if self.phi5 >= pi and self.phi5 < phi5_min:
self.phi5 = phi5_min
elif self.phi5 > phi5_max and self.phi5 < pi:
self.phi5 = phi5_max
'Theta4 (Denavit-Hartenberg-Parameter) aktualisieren'
if self.phi5 >= 0 and self.phi5 <= pi / 2:
self.theta4 = pi / 2 - self.phi4 + self.phi5
elif self.phi5 >= 3 * pi / 2 and self.phi5 < 2 * pi:
self.theta4 = pi / 2 - self.phi4 + self.phi5 - 2 * pi
'Koordinaten von Punkt5'
self.xP5 = self.point4.center[0] + self.l4 * cos(self.phi5)
self.yP5 = self.point4.center[1] + self.l4 * sin(self.phi5)
'Koordinaten akualisieren'
self.point5.center = ((self.xP5, self.yP5))
'Endpunktkoordinaten der Line2D-Objekte aktualisieren'
'Koordinaten von Linie1'
self.xL1 = (self.point1.center[0], self.point2.center[0])
self.yL1 = (self.point1.center[1], self.point2.center[1])
'Koordinaten akualisieren'
self.line1.set_data(self.xL1, self.yL1)
'Koordinaten von Linie2'
self.xL2 = (self.point2.center[0], self.point3.center[0])
self.yL2 = (self.point2.center[1], self.point3.center[1])
'Koordinaten akualisieren'
self.line2.set_data(self.xL2, self.yL2)
'Koordinaten von Linie3'
self.xL3 = (self.point3.center[0], self.point4.center[0])
self.yL3 = (self.point3.center[1], self.point4.center[1])
'Koordinaten akualisieren'
self.line3.set_data(self.xL3, self.yL3)
'Koordinaten von Linie4'
self.xL4 = (self.point4.center[0], self.point5.center[0])
self.yL4 = (self.point4.center[1], self.point5.center[1])
'Koordinaten akualisieren'
self.line4.set_data(self.xL4, self.yL4)
'Methoden der Animation-Blit-Technik ausführen'
canvas = self.point.figure.canvas
axes = self.point.axes
canvas.restore_region(self.background)
axes.draw_artist(self.point2)
axes.draw_artist(self.point3)
axes.draw_artist(self.point4)
axes.draw_artist(self.point5)
axes.draw_artist(self.line1)
axes.draw_artist(self.line2)
axes.draw_artist(self.line3)
axes.draw_artist(self.line4)
canvas.blit(axes.bbox)
'Signal mit den neuen Koordinaten senden'
self.xy_neu.emit(self.xP2, self.yP2, self.xP3, self.yP3, \
self.xP4, self.yP4, self.xP5, self.yP5)
def on_motion(self, event):
'''Die Methode wird weiter ausgeführt wenn das Klassenattribut
mit dem Wert self belegt ist. Das Circle-Objekt darf ohne angeklickt
zu sein nicht bewegt werden.'''
if RueckansichtGreifer.lock is not self: return
'''Die Methode wird weiter ausgeführt wenn der Mauszeiger
innerhalb des Koordinatensystems bewegt wird.'''
if event.inaxes != self.point.axes: return
'''Den Mittelpunkt des Circle-Objektes und die x- und y-Koordinaten
des Mauszeigers zum Zeitpunkt des Drückens der linken Maustaste
zuweisen.'''
self.point.center, xpress, ypress = self.press
'Radien aller Circle-Objekte neu berechnen'
'Verschiebung von Punkt6 in x- und y-Richtung'
dx = event.xdata - xpress
dy = event.ydata - ypress
'neue Koordinaten berechnen'
x_neu = self.point.center[0] + dx
y_neu = self.point.center[1] + dy
'neuen Radius berechnen'
r_neu = (x_neu**2 + y_neu**2)**(1/2)
'Radiusbegrenzung [r_min, r_max]'
if r_neu <= self.r_min:
r_neu = self.r_min
elif r_neu >= self.r_max:
r_neu = self.r_max
'Radien aktualisieren'
self.r6 = r_neu
self.r7 = self.r6
'Winkel aller Circle-Objekte neu berechnen'
'Winkel berechnen - Kreismittelpunkt'
phi = winkel_berechnen(self.point.center[0], self.point.center[1])
'Winkel berechnen - Mauszeiger beim Anklicken des Punktes'
phi_p = winkel_berechnen(xpress, ypress)
'Winkel berechnen - Mauszeiger bei der Bewegung'
phi_e = winkel_berechnen(event.xdata, event.ydata)
'Winkeländerung berechnen'
dphi = phi_e - phi_p
'neuen Positionswinkel berechnen'
phi_neu = phi + dphi
'Winkel auf [0, 2*pi] normieren um Rundungsfehler auszugleichen'
phi_neu = winkel_normieren(phi_neu)
'Winkel von Punkt6 aktualisieren'
self.phi6 = phi_neu
'Winkelbegrenzung [phi_min, phi_max]'
if self.phi6 < self.phi6_min or self.phi6 > 3*pi/2 and \
self.phi6 < 2*pi:
self.phi6 = self.phi6_min
elif self.phi6 > self.phi6_max and self.phi6 < 3*pi/2:
self.phi6 = self.phi6_max
'Winkel von Punkt7 aktualisieren'
self.phi7 = self.phi6 + pi
'Winkel auf [0, 2*pi] normieren um Rundungsfehler auszugleichen'
self.phi7 = winkel_normieren(self.phi7)
'Theta5 (Denavit-Hartenberg-Parameter) aktualisieren'
self.theta5 = pi - self.phi6
'Mittelpunktkoordinaten der Circle-Objekte aktualisieren'
'Koordinaten von Punkt6'
self.xP6 = self.r6*cos(self.phi6)
self.yP6 = self.r6*sin(self.phi6)
'Koordinaten akualisieren'
self.point6.center = ((self.xP6, self.yP6))
'Koordinaten von Punkt7'
self.xP7 = self.r7*cos(self.phi7)
self.yP7 = self.r7*sin(self.phi7)
'Koordinaten akualisieren'
self.point7.center = ((self.xP7, self.yP7))
'Endpunktkoordinaten der Line2D-Objekte aktualisieren'
'Koordinaten von Linie1'
self.xL1 = (0, self.xP6)
self.yL1 = (0, self.yP6)
'Koordinaten akualisieren'
self.line1.set_data(self.xL1, self.yL1)
'Koordinaten von Linie2'
self.xL2 = (0, self.xP7)
self.yL2 = (0, self.yP7)
'Koordinaten akualisieren'
self.line2.set_data(self.xL2, self.yL2)
'Methoden der Animation-Blit-Technik ausführen'
canvas = self.point.figure.canvas
axes = self.point.axes
canvas.restore_region(self.background)
axes.draw_artist(self.point6)
axes.draw_artist(self.point7)
axes.draw_artist(self.line1)
axes.draw_artist(self.line2)
canvas.blit(axes.bbox)
'Signal mit den neuen Koordinaten senden'
self.xy_neu.emit(self.xP6, self.yP6, self.xP7, self.yP7)