from robolink import * # import the robolink library (bridge with RoboDK) from robodk import * # import the robodk library (robotics toolbox) # Import and install libarys: import_install("numpy") import_install("math") import_install("cmath") import_install("matplotlib") #import_install("numpy-stl") #falls nicht vorinstalliert import_install("mpl_toolkits") import_install("timeit") # Load numpy: import numpy as np import math from numpy import linalg as LA # Gefährdungserkennung: from stl import mesh from mpl_toolkits import mplot3d from matplotlib import pyplot import timeit import Gefährdungserkennung as GE import Vorwärtskin_Rückwärtskin as Vkin_Rkin #*************************************************************************************************************************************************** # Function definitions: def TCP_On(toolitem): #Sauggreifer AN """Attach the closest object to the toolitem tool pose, furthermore, it will output appropriate function calls on the generated robot program (call to TCP_On)""" toolitem.AttachClosest() toolitem.RDK().RunMessage('Set air valve on') toolitem.RDK().RunProgram('TCP_On()') def TCP_Off(toolitem, itemleave=0): #Sauggreifer AUS """Detaches the closest object attached to the toolitem tool pose, furthermore, it will output appropriate function calls on the generated robot program (call to TCP_Off)""" toolitem.DetachAll(itemleave) toolitem.RDK().RunMessage('Set air valve off') toolitem.RDK().RunProgram('TCP_Off()') def objekt_ausrichten(roboterbasis_pose, gefahrpunkt, drehpunkt): # translatorisch Hochfahren: pose1 = robot.Pose()*transl(0,0,-250) #Hier könnte noch automatisch erkannt werden wie "hoch" der Roboter anhand der Bauteilgröße hochfahren muss robot.MoveL(pose1) #robot.MoveJ((0, 0, 0,1, 0, 0, 0),acc =0.5, vel=0.2, relative=true) # move relative to current pose #robot.translate((0.1, 0, 0), a, v) #move tool and keep orientation # Koordinaten bestimmen: drehpunkt_array = np.array(drehpunkt) drehpunkt_koord = drehpunkt_array[3][0:3] roboterbasis_array = np.array(roboterbasis_pose) roboterbasis_koord = roboterbasis_array[3][0:3] #print("Drehpunkt: ", drehpunkt_koord) #print("gef. Punkt: ",p1) #print("Roboterbasis: ", roboterbasis_koord) # Richtungsvektoren berechnen: g1 = p1-drehpunkt_koord g2 = roboterbasis_koord -drehpunkt_koord #print("Richtungsvektor vom gef. Punkt zum Drehpunkt: ",g1) #print("Richtungsvektor von der Roboterbasis zum Drehpunkt: ",g2) test5 = LA.norm(g1[0]) #print("test5 ",test5) #Drehwinkel ausrechnen: drehwinkel = np.dot(g1[:2],g2[:2])/(LA.norm(g1[:2])*LA.norm(g2[:2])) drehwinkel = math.acos(round(drehwinkel,6)) drehwinkel = np.rad2deg(drehwinkel) #print("Drehwinkel: ",drehwinkel) g3 = p1 - roboterbasis_koord #print("Richtungsvektor von der Robotorbasis zum gef. Punkt: ",g3) #Anhand Kreuzprodukt Drehrichtung festlegen: joints = robot.Joints().list() kreuzprodukt = np.cross(g2[:2],g3[:2]) #print("Kreuzprodukt",kreuzprodukt) if kreuzprodukt>0: joints[5] = joints[5]+drehwinkel # -> gegen den Uhrzeitersinn Drehrichtung robot.MoveJ(joints) else: joints[5] =joints[5]-drehwinkel # -> mit dem Uhrzeigersinn Drehrichtung robot.MoveJ(joints) joints[4] = joints[4]-90 # Greifer hochkant positionieren robot.MoveJ(joints) #***************************** Hauptprogramm****************************************************************************************************** # Global: RDK = Robolink() # establish a link with the simulator RDK.setCollisionActive(False) # Collisiondetection True = On, False = Off #RDK.setSimulationSpeed(0.5) # set a Simulationspeed 1 = 1 sec RDK.Render(False) # Rendering robot = RDK.Item('UR10e') # retrieve the robot by name robot.setJoints([0,-90,-90,0,90,0]) # set the robot to the home position toolitem = RDK.Item('GripperB') # Get an item named GripperB (hier Sauggreifer) #posepart = partitem.Pose() posetool = toolitem.Pose() # get the toolreference ##print(posetool) home = RDK.Item('Home') # Homeposition of the robot TCP_Off(toolitem) startort_blech = RDK.Item('Startort Blech') # Startort des Blechs ablageort = RDK.Item('Ablageort4') # Ablageort des Blechs roboterbasis = RDK.Item('UR10e Base') # Roboterbasis # Blechteil reinladen: filename = '45_Grad_Blech_5mm_speziell2' #45_Grad_Blech_5mm_gedreht 60_Grad_Blech_5mm_gegenüberliegend 45_Grad_Blech_5mm_eingeschnitten 45_Grad_Blech_5mm_speziell2 blechteil = RDK.Item(filename) # falls Blech noch nicht reingeladen ist, dies auskommentieren und nachfolgende Zeile einkommentieren #blechteil = RDK.AddFile('c:/Users/Oliver Roers/Desktop/Uni/Masterarbeit/RoboDK/'+filename+'.stl') blechteil_pose = blechteil.setPose(startort_blech.Pose()) # Position des Blechs auf Startposition setzen x,y,z,rx,ry,rz = pose_2_xyzrpw(startort_blech.Pose()) # Abspeichern in UR-Schreibweise #aufnahmeort_array = np.array(startort_blech.Pose()) aufnahmeort_koord = [x,y,z] # aufnahmeort_array[3][0:3] # als Vektor abspeichern #print("Aufnahmekoord vom Blech:",aufnahmeort_koord) roboterbasis_pose = roboterbasis.Pose() # Ausgangsstellung des Roboters # Gefährdungserkennung aufrufen und gefährlichen Punkt zurückgeben: p1 = GE.Gefährdungserkennung(filename) #print('gefährliche Punkt ohne Bezug zum Aufnahmeort: ', p1) p1 = p1 + aufnahmeort_koord # wegen Objektreverenz Blech zur Roboterbasis #print('gefährlicher Punkt: ', p1) # Bewegung zum Blech: robot.MoveJ(startort_blech.Pose()*transl(0,0,6)*roty(pi)) # variiert bei unterschiedlichen Blechendicken hier 5 mm Blech, daher 6 mm oberhalb greifen TCP_On(toolitem) objekt_ausrichten(roboterbasis_pose, p1,robot.Pose()) # Gefährdung Richtung Roboterbasis ausrichten robot_pose = robot.Pose() # aktuelle Roboterstellung #print("Roboterpose nach dem Ausrichten: ",robot_pose) # Vorwärts Kinematik: # Theta-Werte aus Excel: #th1 = np.deg2rad(62.228113) #die Werte noch automatisch einladen! #th2 = np.deg2rad(-113.714791) #th3 = np.deg2rad(-100.546591) #th4 = np.deg2rad(-146.186724) #th5 = np.deg2rad(62.228835) #th6 = np.deg2rad(90.208794) #theta = np.matrix([[th1], [th2], [th3], [th4], [th5], [th6]]) #c = [0] #result = Vkin_Rkin.HTrans(theta, c) ##print("The result of Vorwärtskin. is: ",result) # Ablageort des Blechs: ablage_koord = np.array(ablageort.Pose()*transl(0,0,-250))[3][0:3] #ablage_koord = ablageort.Pose().Pos() robot_pose_array = np.array(robot.Pose()*rotz(-pi/2)) #robot_pose_array[0][3] = ablage_koord[0] #robot_pose_array[1][3] = ablage_koord[1] #robot_pose_array[2][3] = ablage_koord[2] robot_pose_array[3][0:3] = ablage_koord desired_pos = robot_pose_array.transpose() #print("Ort über dem Ablageort ist: ",desired_pos) x,y,z,rx,ry,rz = pose_2_xyzrpw(robot.Pose()) # Abspeichern in UR-Schreibweise #new_pos = [ablage_koord[0],ablage_koord[1],ablage_koord[2],rx,ry,rz] #desired_pos = xyzrpw_2_pose(new_pos) #new = rotx(rx)*roty(ry)*rotz(rz)*transl(x,y,z) #new = robot.Pose() ##print("test2: ",new) # Erreichbarkeit des Ablageortes des Bleches prüfen: robot_tool = robot.PoseTool() robot_base = robot.PoseFrame() jnts_sol1 = robot.SolveIK(ablageort.Pose(), None, robot_tool, robot_base) #hier auch noch desired_pos überprüfen und ggfs. Anfahren der Tischkante mit Blech überprüfen #print("Joints: ",jnts_sol1.list()) #jnts_sol2 = robot.SolveIK(setPose(desired_pos), None, robot_tool, robot_base) #hier auch noch desired_pos überprüfen ##print(jnts_sol2.list()) if len(jnts_sol1.list()) <= 1: #& len(jnts_sol2.list()) <= 1: print("Not reachable") else: print("reachable") th = Vkin_Rkin.invKine(desired_pos) #print("All examples (8) of Theta-Winkel are: ", th) theta_array = np.array(th) # Kollisionsüberprüfung und Entscheidung für eine Version und Verfahren joints_old = robot.Joints().list() joints = robot.Joints().list() kollisionsfreie_versionen =[] for version in range(0,8): joints[0] = np.rad2deg(theta_array[0][version]) joints[1] = np.rad2deg(theta_array[1][version]) joints[2] = np.rad2deg(theta_array[2][version]) joints[3] = np.rad2deg(theta_array[3][version]) joints[4] = np.rad2deg(theta_array[4][version]) joints[5] = np.rad2deg(theta_array[5][version]) RDK.Render(False) collision = robot.MoveJ_Test(joints_old,joints,minstep_deg=- 1) if collision == 0: robot.setJoints(joints_old) robot.MoveJ(joints) RDK.Render(True) #print("keine Kollision bei Version", version) kollisionsfreie_versionen.append(version) break #break später auskommentieren else: robot.Stop() robot.setJoints(joints_old) RDK.Render(True) #print("Kollision entdeckt bei Version", version) # Für eine der kollisionsfreien Stellungen entscheiden: anzahl_versionen =len(kollisionsfreie_versionen) kollisionfreie_joints = np.empty((anzahl_versionen,6)) for versionen in range(0,anzahl_versionen): for joint_index in range(0,6): kollisionfreie_joints[versionen][joint_index] = np.rad2deg(theta_array[joint_index][versionen]) #print("Alte Joints sind ", joints_old) #print("Kollisionsfreie Joints sind ", kollisionfreie_joints) # Ablegen des Gegenstands: #print(robot.Pose()) # HIER HAT ER NOCH BEI STARTORT BLECH ZU ABLAGEORT EINE KOLLISION VOM BLECHSPITZE MIT TISCH x,y,z,rx,ry,rz = pose_2_xyzrpw(robot.Pose()) new_pos = [x,y,z,0,180,0] # hier -250 rausnehmen, falls erst orientiert und dann erst translatorisch verfahren werden soll new_pose = xyzrpw_2_pose(new_pos) # Abspeichern in UR-Schreibweise robot.MoveJ(new_pose) # Wenn ich hier anstellle MoveL -> MoveJ benutze detektiert er bei Startprt zu Ablageort eine Kollision wegen MoveJ-Test pose2 = robot.Pose()*transl(0,0,250) robot.MoveJ(pose2) # Sauggreifer Off: TCP_Off(toolitem) #program = RDK.ItemUserPick('Select a program to get estimated travel distance', ITEM_TYPE_PROGRAM) #if not program.Valid(): #quit() # the user cancelled the selection #[n_instructions, program_time, program_distance, valid_ratio, readable_msg] = program.Update() #RDK.ShowMessage('Estimated travel distance is %.1f mm' % program_distance)