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)