API tools faq
paste
Advertisement
Guest User

Matlab full code UR3

a guest
Jan 22nd, 2021
341
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
MatLab 22.20 KB | None | 0 0
raw download clone embed print report
  1. %%%-------------------------------------------------------------------------------------------------
  2. %%% Ansteuerung eines UR3 Roboters mit Matlab Robotics Toolbox
  3. %%% Erstellt von Studenten der HSHL
  4. %%%-------------------------------------------------------------------------------------------------
  5.  
  6. %% Initialisierung von Matlab
  7. startup_rvc
  8. %close all
  9. clear all %#ok<*CLALL>
  10. clc
  11.  
  12. %#ok<*NUSED>        %Matlabfehler unterdrücken bei ungenutzter globaler Variable
  13. %#ok<*SEPEX>        %Matlabfehler unterdrücken bei Code in einer Zeile, zB. kurze "If"
  14. %#ok<*UNRCH>        %Matlabfehler unterdrücken bei nicht erreichbarem Code, Ur3_connected=false
  15. %#ok<*NOPRT>        %Matlabfehler unterdrücken bei Ausgabe in Console
  16. %#ok<*DEFNU>        %Matlabfehler unterdrücken bei unbenutzten Funktionen
  17. %#ok<*NASGU>        %Matlabfehler unterdrücken bei globalen Variablen
  18.  
  19. %% Globale Variablen
  20. %Einstellparameter
  21. global UR3_connected; UR3_connected=true;                                  %true nur mit angeschlossenem UR3, UR3 führt Bewegungen aus!
  22. global UR3_plot; UR3_plot=true;                                             %true für Erstellen einer Simulation der Bewegung
  23.  
  24. global pausetimeOnMovement; pausetimeOnMovement=false;
  25. global StopOnCalibrationPosition; StopOnCalibrationPosition=false;          %Roboter fährt nur zur Ausgangspostition und hält an.
  26. global SicherheitsabstandUeberEndposition; SicherheitsabstandUeberEndposition=0.1;  %Wert in m!
  27. global brickUpperLeft;   brickUpperLeft   = [313.32;  -43.17; -145.69; -80.14; 59.98; 356.99];    %Linke obere Ecke der Platte. Die Ecke, die am nächsten am Roboterfuß ist.
  28. global brickLowerRight;  brickLowerRight  = [285.95; -128.22;  -73.51; -52.57; 63.79; 326.26];    %Rechte untere Ecke
  29. global magazinePosition; magazinePosition = [273.42;  -89.74; -124.73; -33.60; 68.79; 311.05];
  30.  
  31. global normaleGeschwindigkeitVar; normaleGeschwindigkeitVar = 0.3;          %Faktor 0-1
  32. global langsameGeschwindigkeitVar; langsameGeschwindigkeitVar = 0.01;       %Faktor 0-1
  33. global ParameterGeschwindigkeitMax_mm; ParameterGeschwindigkeitMax_mm = 1;
  34. global ParameterGeschwindigkeitMax_mm_accel; ParameterGeschwindigkeitMax_mm_accel = 1;
  35.  
  36.  
  37. %Programmparameter
  38. global accel_radss;                                 %Beschleunigung in Radiant/s²
  39. global accel_m;                                    %Beschleunigung in mm/s²
  40. global vel_m;                                      %Geschwindigkeit in mm/s
  41. global vel_rads;                                    %Geschwindigkeit in Radiant/s
  42. global q_alt;                                       %Gelenkwinkel der aktuellen Position
  43. global q_neu;                                       %Gelenkwinkel neu der anzufahrenden Position
  44. global T_alt;                                       %Transformationsmatrix der aktuellen Position
  45. global T_neu;                                       %Transformationsmatrix der anzufahrenden Position
  46.  
  47. %-------------------------------------------------------------------------------------------------------------------
  48. %-------------------------------------------------------------------------------------------------------------------
  49. %-------------------------------------------------------------------------------------------------------------------
  50.  
  51. %% Ausführen der Bewegungen/Aufruf der Funktionen
  52. connectRobot();
  53. normaleGeschwindigkeit();
  54. ResetUR3_startPositionUp();
  55. %startPositionWithTurn();                % Alternativ startPositionNoTurn() für andere Ausrichtung des LEGO-Stein Magazins
  56. Startposition_MoveJ();
  57. if(StopOnCalibrationPosition) return; end
  58. langsameGeschwindigkeit();
  59. moveTo_XYZ(0,-0.3,0.3);
  60. moveTo_XYZ(-0.3,-0.3,0.3);
  61. testPlateCoordinate();
  62.  
  63.  
  64.  
  65.  
  66.  
  67. % testPlace();
  68. %
  69. % testNewPosition();
  70. % %testArbeitsposition();
  71. %
  72. % langsameGeschwindigkeit();
  73. % %testBrickMagazine();
  74. % testTranslatory3();
  75.  
  76.  
  77.  
  78.  
  79.  
  80.  
  81.  
  82.  
  83. %-------------------------------------------------------------------------------------------------------------------
  84. %-------------------------------------------------------------------------------------------------------------------
  85. %-------------------------------------------------------------------------------------------------------------------
  86. %% Neue Bewegung nach q_neu/T_neu ausführen
  87. function runQ(time,movement)
  88.     %time ist die Zeit der auszuführenden Bewegung in Sekunden
  89.     %movement ist "L"inear oder "J"unction
  90.  
  91.     getMissingTQ(movement);                 % Fehlende Werte T oder q bestimmen
  92.     plotSimulation(time,movement);          % Bewegung Simulieren
  93.     moveRobotToQ(time,movement);            % Roboter Bewegen
  94.     saveToAlt();                            % Aktuelle Position als "alt" speichern
  95. end
  96.  
  97. function getMissingTQ(movement)             % Bestimmung fehlender Werte T_neu oder q_neu
  98.     global q_neu; global q_alt; global T_neu; global T_alt;
  99.     mdl_ur3;
  100.     %if(movement=="J")                       % T_neu wird bei "J" nicht berechnet, aber für anschließende Bewegungen benötigt
  101.     if(q_neu ~= q_alt)                       % Änderung, da auch mit T "J" gefahren werden darf
  102.         T_neu = ur3.fkine(q_neu);
  103.     %elseif(movement=="L")                   % q_neu wird nicht erzeugt, nur T mit Koordinaten vorgegeben
  104.     elseif(T_neu ~= T_alt)    
  105.         q_neu = ur3.ikine(T_neu,'q0',q_alt);
  106.     end
  107. end
  108. function plotSimulation(time,movement)      % Bewegung Simulieren
  109.     global q_alt; global q_neu; global T_alt; global T_neu; global UR3_plot;
  110.     if (UR3_plot==false) return; end
  111.    
  112.     mdl_ur3;
  113.     t = time*20;
  114.     %t = [0:.05:time];
  115.     if(movement=="L")
  116.         T_bewegungMitZwischenpunkten = ctraj(T_alt, T_neu, t);                      % Bewegungsschritte linear berechnen als T
  117.         q_bewegung = ur3.ikine(T_bewegungMitZwischenpunkten,'q0',q_alt);            % Bewegung in Gelenkwinkel umrechnen
  118.     elseif(movement=="J")
  119.         q_bewegung = jtraj(q_alt, q_neu, t);                                        % Bewegung der Achsen ermitteln
  120.     end
  121.     figure(1);                                                                      % Plot öffnen
  122.     ur3.plot(q_bewegung);                                                           % Anzeigen der Bewegung
  123. end
  124. function moveRobotToQ(time,movement)        % Roboter Bewegen
  125.     global pausetimeOnMovement; global q_neu; global accel_m; global vel_m; global accel_radss; global vel_rads; global Socket_conn; global UR3_connected;
  126.     if(UR3_connected == false) return; end                  %Roboter nur verfahren, wenn angeschlossen
  127.    
  128.    
  129.     if(movement=="L")
  130.         Befehl = command2stringMoveL(q_neu,accel_m,vel_m,0,0);
  131.     elseif(movement=="J")
  132.         Befehl = command2string(q_neu,accel_radss,vel_rads,0,0);
  133.     end
  134.     fprintf(1, 'Verfahre den UR3.\n');
  135.     fprintf(Socket_conn, Befehl);
  136.     %-----
  137.  
  138. %     RobotSteady=false;
  139. %     Befehl='is_steady()';
  140. %     while RobotSteady==false
  141. %         RobotSteady=fprintf(Socket_conn, Befehl);
  142. %         if(RobotSteady==false) pause(0.2); end
  143. %     end
  144.  
  145.  
  146.    
  147.    
  148.     %-----
  149.     if(pausetimeOnMovement)
  150.         pausetime = time / (rad2deg(vel_rads)/130) ;            %Wartezeit für Matlab, damit Roboter Bewegung beenden kann.
  151.         pause(pausetime);
  152.     end
  153. end
  154. function saveToAlt()                        % Aktuelle Position als "alt" speichern
  155. % Nach abgeschlossenem Befehl ist die Position "alt" und die neue Bewegung kann die Variablen überschreiben
  156.     global q_alt; global q_neu; global T_alt; global T_neu;
  157.     q_alt = q_neu;
  158.     T_alt = T_neu;
  159. end
  160.  
  161. %% Roboter konfigurieren
  162. function normaleGeschwindigkeit()           % Geschwindigkeit für Roboter einstellen
  163.     global ParameterGeschwindigkeitMax_mm_accel; global ParameterGeschwindigkeitMax_mm; global normaleGeschwindigkeitVar; global vel_rads; global accel_m; global vel_m; global accel_radss;
  164.     if (normaleGeschwindigkeitVar <= 1 && normaleGeschwindigkeitVar > 0)
  165.         vel_deg = normaleGeschwindigkeitVar*130;
  166.         accel_radss = 3;
  167.         vel_rads = deg2rad(vel_deg);
  168.         accel_m = ParameterGeschwindigkeitMax_mm_accel*normaleGeschwindigkeitVar;  %10*normaleGeschwindigkeitVar;
  169.         vel_m = ParameterGeschwindigkeitMax_mm*normaleGeschwindigkeitVar;
  170.         fprintf('Geschwindigkeit auf %d Prozent eingestellt.\n', (normaleGeschwindigkeitVar*100))
  171.     else
  172.         error('Normale Geschwindigkeit falsch eingestellt. Faktor zwischen 0 und 1 erwartet.');
  173.         pause(2);
  174.         return;
  175.     end
  176. end
  177. function langsameGeschwindigkeit()          % Geschwindigkeit für Roboter einstellen
  178.     global ParameterGeschwindigkeitMax_mm_accel; global ParameterGeschwindigkeitMax_mm; global langsameGeschwindigkeitVar; global vel_rads; global accel_m; global vel_m; global accel_radss;
  179.     if (langsameGeschwindigkeitVar <= 1 && langsameGeschwindigkeitVar > 0)
  180.         vel_deg = langsameGeschwindigkeitVar*130;
  181.         accel_radss = 3;
  182.         vel_rads = deg2rad(vel_deg);
  183.         accel_m = ParameterGeschwindigkeitMax_mm_accel*langsameGeschwindigkeitVar;  %10*langsameGeschwindigkeitVar;
  184.         vel_m = ParameterGeschwindigkeitMax_mm*langsameGeschwindigkeitVar;
  185.         fprintf('Geschwindigkeit auf %d Prozent reduziert.\n', (langsameGeschwindigkeitVar*100))
  186.     else
  187.         error('Langsame Geschwindigkeit falsch eingestellt. Faktor zwischen 0 und 1 erwartet.');
  188.         pause(2);
  189.         return;
  190.     end
  191. end
  192.  
  193. function connectRobot()                     % Verbindung herstellen
  194.     global Socket_conn; global UR3_connected;
  195.     if(UR3_connected == false) return; end
  196.    
  197.     mdl_ur3;
  198.     Robot_IP = '192.168.56.101';
  199.     Port_NR = 30003;
  200.     Socket_conn = tcpip(Robot_IP,Port_NR);
  201.     fclose(Socket_conn); % ggf. bestehende Verbindung schließen
  202.     pause(2);
  203.     fprintf(1, 'Verbindungsaufbau mit UR3...\n');
  204.     try
  205.         fopen(Socket_conn);
  206.     catch
  207.         error('Connection to UR3 not possible');
  208.         pause(2);
  209.         return;
  210.     end
  211.     fprintf(1, 'Verbindung mit UR3 hergestellt.\n');
  212.     pause(2);
  213. end
  214. function ResetUR3_startPositionUp()         % Roboter in gerade Startposition fahren
  215.     global UR3_connected; global accel_radss; global vel_rads; global Socket_conn; global T_alt; global T_neu; global q_alt; global q_neu;
  216.    
  217.     mdl_ur3;
  218.     fprintf('Fahre in verikale Ausgangsposition.\n');
  219.        
  220.     %Startposition des Roboters
  221.     q_start = [0,-pi/2,0,-pi/2,-pi/2,0];
  222.     q_neu = q_start;
  223.     q_alt = q_start;
  224.     T_alt = ur3.fkine(q_start);
  225.     T_neu = ur3.fkine(q_start);
  226.    
  227.     runQ(4,"J");
  228.    
  229.     %Startposition anfahren
  230. %         if(UR3_connected)
  231. %             Befehl = command2string(q_neu,accel_radss,vel_rads,0,0);
  232. %             fprintf(Socket_conn, Befehl);
  233. %             %Warten auf das Erreichen der Startposition
  234. %             pausetime = 4 / (rad2deg(vel_rads)/130) ;
  235. %             pause(pausetime);
  236. %         end
  237. end
  238. function startPositionNoTurn()              % Startposition ohne gedrehtem Greifer (LEGO-Stein horizontal)
  239.     global q_neu; global q_alt;
  240.     fprintf('Fahre in Startposition.\n');
  241.     q1 = deg2rad(45);
  242.     q2 = deg2rad(-90);
  243.     q3 = deg2rad(90);
  244.     q4 = deg2rad(-90);
  245.     q5 = deg2rad(-120);
  246.     q6 = deg2rad(0);
  247.     q_alt = q_neu;
  248.     q_neu = [q1, q2, q3, q4, q5, q6];
  249.     runQ(2,"J")
  250. end
  251. function startPositionWithTurn()            % Startpsoition mit gedrehtem Greifer (LEGO-Stein vertikal)
  252.     global q_alt; global q_neu;
  253.     fprintf('Fahre in Startposition.\n');
  254.     q1 = deg2rad(135);
  255.     q2 = deg2rad(-90);
  256.     q3 = deg2rad(90);
  257.     q4 = deg2rad(-90);
  258.     q5 = deg2rad(-120);
  259.     q6 = deg2rad(0);
  260.     q_alt = q_neu;
  261.     q_neu = [q1, q2, q3, q4, q5, q6];
  262.     runQ(2,"J");
  263. %     plotMovementPoint();
  264. %     moveJToQ(2);
  265. end
  266. function Startposition_MoveJ()            % Startpsoition mit gedrehtem Greifer (LEGO-Stein vertikal)
  267.     global q_alt; global q_neu;
  268.     fprintf('Fahre in Startposition.\n');
  269.     q1 = deg2rad(276.19);
  270.     q2 = deg2rad(-71.00);
  271.     q3 = deg2rad(-90.53);
  272.     q4 = deg2rad(-87.66);
  273.     q5 = deg2rad(67.69);
  274.     q6 = deg2rad(313.83);
  275.     q_alt = q_neu;
  276.     q_neu = [q1, q2, q3, q4, q5, q6];
  277.     runQ(2,"J");
  278. %     plotMovementPoint();
  279. %     moveJToQ(2);
  280. end
  281.  
  282. %% Neue Position Roboter - Test
  283. function testNewPosition()  %Über Winkel
  284.     global q_alt; global q_neu;
  285.     q1 = deg2rad(268.65);
  286.     q2 = deg2rad(-82.00);
  287.     q3 = deg2rad(-121.34);
  288.     q4 = deg2rad(-43.18);
  289.     q5 = deg2rad(70.50);
  290.     q6 = deg2rad(306.34);
  291.     q_alt = q_neu;
  292.     q_neu = [q1, q2, q3, q4, q5, q6];
  293.     runQ(4,"J");
  294. %     plotMovementPoint();
  295. %     moveJToQ(4);
  296. end
  297. function testArbeitsposition()  %Über T
  298.     global q_alt; global q_neu; global T_alt; global T_neu;
  299.     mdl_ur3;
  300.    
  301.     %Neue Position vorgeben
  302.     T_neu = SE3([   -0.5740   -0.7438   -0.3426    -0.146;
  303.                     -0.6467    0.6684   -0.3676   -0.2676;
  304.                      0.5024    0.0105   -0.8646    0.2719;
  305.                           0         0         0         1]);  
  306.     %Bewegung erzeugen
  307.     q_neu = ur3.ikine(T_neu,'q0',q_alt);
  308.     runQ(2,"J");
  309. end
  310. function testBrickMagazine()    %Über Winkel
  311.     global magazinePosition;
  312.     mdl_ur3;
  313.     T_magazine = ur3_fkine(deg2rad(magazinePosition));
  314.     plotMovement();
  315.     moveJToQ(2);
  316. end
  317. function testTranslatory1()     %Funktion fehlerhaft
  318.        
  319.     % pause(5);
  320.     % Befehl = command2string(q,accel_radss,vel_rads,time_robot,blend_radius);
  321.     % fprintf(1, 'Verfahre den UR3.\n');
  322.     % fprintf(Socket_conn, Befehl);
  323. %     test=loadrobot("UniversalUR3");
  324. %     show(test);
  325.  
  326.     global q;
  327.     mdl_ur3;
  328.  
  329.     T1 = transl(-0.146, -0.26787, 0.23509);
  330.     T2 = transl(-0.14598, -0.26787, 0.49710);
  331.     T = ctraj(T1, T2, 50);
  332.     %X = cart2sph(x,y,z)
  333.     %q = ur3.ikine6s(T);    %Fehler: Wrist is not sperical
  334.     %q = ur3.ikunc(T);
  335.     %q = ur3.ikine3(T)      %Fehler: Solution only applicable for standard DH conventions
  336.     %q = ur3.ikine(T)       %Fehler: Warning: failed to converge: try a different initial value of joint coordinates, 100 Versuche ohne Lösung
  337.     q = ur3.ikcon(T)
  338.    
  339.    
  340.     %figure(1);                          % Plot öffnen
  341. %     q_alt = q_neu;                      % Alte Position speichern
  342. %     q_neu = q;                          % Neue Position speichern
  343. %     t = [0:.05:3]';                     % Bewegung in kleinere Bewegungssprünge teilen
  344. %     q_plot = jtraj(q_alt, q_neu, t);         % generate joint coordinate trajectory
  345. %     ur3.plot(q_plot);
  346.     %ur3.plot(q);
  347.     plotMovement();
  348.     moveJToQ(2);
  349.     %     ur3.plot(q);
  350.    
  351. end
  352. function testTranslatory2()     %Funktioniert mit fest vorgegebener Bewegung Linear und invers
  353.     global q;
  354.     mdl_ur3;
  355.  
  356.     T1 = transl(-0.16057, -0.28109, 0.08207);
  357.     T2 = transl(-0.16056, -0.28111, 0.27125);
  358.     T = ctraj(T1, T2, 50);
  359.     %h=hgtransform
  360.     %q = ur3.ikcon(T);
  361.    
  362.     global q_alt;
  363.     q_alt=q;
  364.    
  365.     %Aktuelle Position bestimmen
  366.     Ta = ur3.fkine(q)
  367.    
  368.     %Tn = Ta + [0 0 0 0; 0 0 0 0; 0 0 0 0.2; 0 0 0 0]
  369. %     Tn=[1   0   0    0.1;
  370. %         0   1   0    0.1;
  371. %         0   0   1    0.1;
  372. %         0   0   0     1]
  373.          
  374.     Tn=[-0.5740   -0.7438   -0.3426    -0.146;
  375.         -0.6467    0.6684   -0.3676   -0.2676;
  376.          0.5024    0.0105   -0.8646    0.4719;
  377.               0         0         0         1]
  378.          
  379.     Ta = [-0.5740   -0.7438   -0.3426    -0.146;
  380.    -0.6467    0.6684   -0.3676   -0.2676;
  381.     0.5024    0.0105   -0.8646    0.2719;
  382.          0         0         0         1]          
  383.          
  384.     q_plot = ctraj(Ta, Tn, 20)
  385.     q_pq = ur3.ikine(q_plot,'q0',q_alt)
  386.     ur3.plot(q_pq);
  387.          
  388.          
  389. %           Tadd=[0 0 0 0; 0 0 0 0; 0 0 0 0.2; 0 0 0 0]
  390. %           Ttest= Ta + Tadd
  391.     q = ur3.ikine(Tn,'q0',q_alt);
  392. %     q = ur3.ikcon(Tn);
  393.    
  394.  %   plotMovementPoint();
  395.         moveLToQ(2);    
  396.  
  397. %_________
  398.     q_alt=q;
  399.     Ta = ur3.fkine(q)
  400.  
  401.     Tn=[-0.5740   -0.7438   -0.3426    -0.146;
  402.         -0.6467    0.6684   -0.3676   -0.0676;
  403.          0.5024    0.0105   -0.8646    0.4719;
  404.               0         0         0         1]
  405.  
  406.     q = ur3.ikine(Tn,'q0',q_alt);  
  407.     plotMovementPoint();
  408. %____________
  409.    
  410.     q_plot = ctraj(Ta, Tn, 50);
  411.     ur3.plot(q_plot);
  412.    
  413.     %plotMovement();
  414.    
  415. %     global q; global accel_radss; global vel_rads; global Socket_conn; global UR3_connected;
  416. %     if(UR3_connected == false) return; end
  417. %     Befehl = command2stringMoveL(q,accel_radss,vel_rads,0,0);
  418. %     fprintf(1, 'Verfahre den UR3.\n');
  419. %     fprintf(Socket_conn, Befehl);
  420. %    
  421. %     pausetime = time / (rad2deg(vel_rads)/130) ;
  422. %     pause(pausetime);
  423. %    
  424.  
  425.     moveJToQ(2);    
  426. end
  427. function testTranslatory3()     %Funktioniert, mehrere Positionen anfahren über T
  428.     global q_alt; global q_neu; global T_alt; global T_neu;
  429.     mdl_ur3;
  430.    
  431.     %Neue Position vorgeben - 20cm y
  432.     T_neu = SE3([   -0.5740   -0.7438   -0.3426   -0.1460;
  433.                     -0.6467    0.6684   -0.3676   -0.2676;
  434.                      0.5024    0.0105   -0.8646    0.4719;
  435.                           0         0         0         1]);  
  436.     runQ(1,"L");
  437.  
  438.     %Neue Position vorgeben - zurück
  439.     T_neu = SE3([   -0.5740   -0.7438   -0.3426   -0.1460;
  440.                     -0.6467    0.6684   -0.3676   -0.2676;
  441.                      0.5024    0.0105   -0.8646    0.2719;
  442.                           0         0         0         1]);  
  443.     runQ(1,"L");
  444.    
  445.     %Neue Position vorgeben - 30cm x
  446.     T_neu = SE3([   -0.5740   -0.7438   -0.3426    0.2460;
  447.                     -0.6467    0.6684   -0.3676   -0.2676;
  448.                      0.5024    0.0105   -0.8646    0.2719;
  449.                           0         0         0         1]);    
  450.     runQ(1,"L");
  451.      
  452. end
  453.  
  454.  
  455. function testPlateCoordinate()
  456.     global brickUpperLeft; global brickLowerRight; global SicherheitsabstandUeberEndposition;
  457.     mdl_ur3;
  458.    
  459. %     brickUpperLeft = [313.32; -43.17; -145.69; -80.14; 59.98; 356.99];
  460. %     brickLowerRight = [285.95; -128.22; -73.51; -52.57; 63.79; 326.26];
  461.    
  462.     T_UpperLeft = ur3.fkine(deg2rad(brickUpperLeft))
  463.     T_LowerRight = ur3.fkine(deg2rad(brickLowerRight))
  464.    
  465.     %Welche Ecke der Platte?
  466. %     T_test = T_LowerRight + SE3([   0 0 0 0;
  467. %                                     0 0 0 0;
  468. %                                     0 0 0 SicherheitsabstandUeberEndposition;
  469. %                                     0 0 0 0]);
  470.     T_test = T_UpperLeft + SE3([   0 0 0 0;
  471.                                    0 0 0 0;
  472.                                    0 0 0 SicherheitsabstandUeberEndposition;
  473.                                    0 0 0 0]);
  474.     global T_neu;
  475.     T_neu = SE3(T_test);
  476.     runQ(2,"L");
  477.     langsameGeschwindigkeit();
  478.     %T_neu = SE3(T_LowerRight);
  479.     T_neu = SE3(T_UpperLeft);
  480.     runQ(0.5,"L");
  481.     %%geht!
  482. end
  483.  
  484. function testPlace()
  485.     global T_neu; global q_alt; global q_neu;
  486.     mdl_ur3;
  487.    
  488.     %aktuellepos -1cm x
  489.       T_neu = SE3([ -0.5743   -0.7438   -0.3420   -0.1078;
  490.    -0.6466    0.6683   -0.3677   -0.2438;
  491.     0.5020    0.0100   -0.8648    0.4093;
  492.          0         0         0         1]);
  493.      runQ(2,"L");
  494.    
  495. %      %neuepos nur x
  496. %      T_neu = SE3([ -0.5743   -0.7438   -0.3420   -0.08988;
  497. %    -0.6466    0.6683   -0.3677   -0.2438;
  498. %     0.5020    0.0100   -0.8648    0.4093;
  499. %          0         0         0         1]);
  500. %      runQ(2,"L");
  501. %      
  502. %           %neuepos nur xy
  503. %      T_neu = SE3([ -0.5743   -0.7438   -0.3420   -0.08988;
  504. %    -0.6466    0.6683   -0.3677   0.04943;
  505. %     0.5020    0.0100   -0.8648    0.4093;
  506. %          0         0         0         1]);
  507. %      runQ(2,"L");
  508. %      
  509. %           %neuepos nur xyz
  510. %      T_neu = SE3([ -0.5743   -0.7438   -0.3420   -0.08988;
  511. %    -0.6466    0.6683   -0.3677   0.04943;
  512. %     0.5020    0.0100   -0.8648    0.5366;
  513. %          0         0         0         1]);
  514. %      runQ(2,"L");
  515. %      
  516. %    
  517.  
  518. %drehen
  519.      T_neu = SE3([ -0.9698   -0.0829    0.2294   -0.1078  ;
  520.                    -0.2319    0.0216   -0.9725   -0.2438  ;
  521.                     0.0756   -0.9963   -0.0402    0.4093  ;
  522.                          0         0         0         1]);
  523.      runQ(2,"J");    
  524. %neuepos nur x
  525.      T_neu = SE3([ -0.5743   -0.7438   -0.3420   -0.08988 ;
  526.                    -0.6466    0.6683   -0.3677   -0.2438  ;
  527.                     0.5020    0.0100   -0.8648    0.4093  ;
  528.                          0         0         0         1]);
  529.      runQ(2,"L");
  530.      
  531.           %neuepos nur xy
  532.      T_neu = SE3([ -0.5743   -0.7438   -0.3420   -0.08988;
  533.    -0.6466    0.6683   -0.3677   0.04943;
  534.     0.5020    0.0100   -0.8648    0.4093;
  535.          0         0         0         1]);
  536.      runQ(2,"L");
  537.      
  538.           %neuepos nur xyz
  539.      T_neu = SE3([ -0.5743   -0.7438   -0.3420   -0.08988;
  540.    -0.6466    0.6683   -0.3677   0.04943;
  541.     0.5020    0.0100   -0.8648    0.5366;
  542.          0         0         0         1]);
  543.      runQ(2,"L");
  544.      
  545.          
  546.      
  547.      
  548.      
  549.      
  550.      
  551.      
  552.      
  553.      
  554.      
  555. %     T_neu = SE3([0.5333    0.0801   -0.8422   -0.09757;
  556. %              0.7673    0.3733    0.5214    0.1534;
  557. %              0.3562   -0.9242    0.1376    0.614;
  558. %              0         0         0         1]);
  559. %          
  560.     T_neu = SE3([-0.4125   -0.2152    0.8852  -0.08988;
  561.     0.6336    0.6304    0.4485   0.04943;
  562.    -0.6546    0.7458   -0.1237    0.5366;
  563.          0         0         0         1]);
  564.      
  565.     runQ(5,"L");
  566. %     T_neu = SE3([0.5333    0.0801   -0.8422   -0.09757;
  567. %              0.7673    0.3733    0.5214    0.1534;
  568. %              0.3562   -0.9242    0.1376    0.514;
  569. %              0         0         0         1]);
  570. %          
  571.     T_neu = SE3([-0.4125   -0.2152    0.8852  -0.08988;
  572.     0.6336    0.6304    0.4485   0.04943;
  573.    -0.6546    0.7458   -0.1237    0.4366;
  574.          0         0         0         1]);
  575.     langsameGeschwindigkeit();
  576.     runQ(1,"L");
  577. end
  578.  
  579. function moveTo_XYZ(x,y,z)
  580.     global T_neu;
  581.     mdl_ur3;
  582.    
  583.     %Startposition ist ca: x=0.11, y=-0.24, z=0.40
  584.     T_standartorientierung =  [ -0.5743   -0.7438   -0.3420   x   ;  %Ungedrehte Ausgangslage
  585.                                 -0.6466    0.6683   -0.3677   y   ;
  586.                                  0.5020    0.0100   -0.8648   z   ;
  587.                                  0         0         0        1  ];
  588.                      
  589.     T_neu = SE3(T_standartorientierung);
  590.     runQ(1,"L");                        
  591. end
  592.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!