├── README.md ├── delta_documentation ├── PROJECTE D’INTEGRACIÓ - ROBOT DELTA - PRESENTACIÓ.pdf ├── Readme.md ├── Scilab_resources │ ├── PlotFunction.sce │ ├── Readme.md │ ├── deltaOptimization.sce │ ├── inverseDelta.sce │ ├── trajectoria.sce │ ├── trajectoriaV2.sce │ └── workSpaceValidation.sce └── wxMax_eqs │ ├── Eqs_estat.html │ └── Eqs_estat_htmlimg │ ├── Eqs_estat.css │ ├── Eqs_estat_0.png │ └── Eqs_estat_1.png ├── delta_mechanical ├── Readme.txt ├── base.stp ├── biceps.stp ├── mountingbracket.stp └── unio.stp ├── delta_robot_drivers ├── CMakeLists.txt ├── README.md ├── include │ └── delta_robot_drivers │ │ ├── delta_angles_streamer.h │ │ └── delta_hw_driver.h ├── package.xml └── src │ ├── delta_angles_streamer.cpp │ ├── delta_angles_streamer_node.cpp │ └── delta_hw_driver.cpp ├── delta_robot_firmware ├── CMakeLists.txt ├── README.md ├── package.xml └── src │ ├── CMakeLists.txt │ └── firmware.cpp ├── delta_robot_img_processor ├── CMakeLists.txt ├── README.md ├── include │ └── delta_robot_img_processor │ │ ├── camera.h │ │ ├── circle_detector.h │ │ └── ros_img_processor.h ├── launch │ └── delta_robot_img_processor.launch ├── package.xml └── src │ ├── delta_img_processor_node.cpp │ └── ros_img_processor.cpp ├── delta_robot_kinematics ├── CMakeLists.txt ├── README.md ├── include │ └── delta_robot_kinematics │ │ └── delta_kinematics.h ├── package.xml └── src │ └── delta_kinematics.cpp └── delta_robot_support ├── CMakeLists.txt ├── README.md ├── config ├── controllers.yaml └── delta.rviz ├── launch ├── delta_robot_real.launch └── delta_robot_sim.launch ├── package.xml ├── urdf ├── delta_robot.urdf └── meshes │ ├── Platform.STL │ ├── fixed_base2.dae │ └── upper_leg.STL └── world └── delta_robot.world /README.md: -------------------------------------------------------------------------------- 1 | ## ROS delta robot. 2 | 3 | * delta_robot_support 4 | Project containing global launch files,URDFs and meshes. 5 | 6 | * delta_robot_kinematics 7 | Package kinematics library 8 | 9 | * delta_robot_img_processor 10 | ROS image processor for detecting circles. 11 | 12 | * delta_robot_firmware 13 | Arduino src code 14 | 15 | * delta_robot_drivers 16 | Delta robot driver and kinematic solver. 17 | 18 | 19 | * How to launch 20 | 21 | * Load firmware to arduiono UNO 22 | 23 | catkin_make delta_robot_firmware_src_delta_arduino-upload 24 | 25 | roslaunch delta_robot_support delta_robot_sim.launch 26 | 27 | roslaunch delta_robot_support delta_robot_real.launch 28 | 29 | **Looking for more information?** Visit the delta_documentation folder. There can be found simulations in scilab and all the equation development for the kinematic problems. There are also all the mechanical parts printed for the project in the delta_mechanical folder. 30 | 31 | [![Assembly test one](https://img.youtube.com/vi/qOeS8mAfZcc/0.jpg)](https://www.youtube.com/watch?v=qOeS8mAfZcc) 32 | [![Assembly test two](https://img.youtube.com/vi/pI4uqf1SEYo/0.jpg)](https://www.youtube.com/watch?v=pI4uqf1SEYo) 33 | 34 | -------------------------------------------------------------------------------- /delta_documentation/PROJECTE D’INTEGRACIÓ - ROBOT DELTA - PRESENTACIÓ.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/serrauvic/ros_delta_robot/5940f173e6c5ca05aa4bb1cbf7bc135b6618412b/delta_documentation/PROJECTE D’INTEGRACIÓ - ROBOT DELTA - PRESENTACIÓ.pdf -------------------------------------------------------------------------------- /delta_documentation/Readme.md: -------------------------------------------------------------------------------- 1 | There are two folders in delta_documentation: 2 | * The Scilab_resources: where it can be found some tools for designing the delta robot. Basically, there is the function that 3 | solves the direct kinematics problem and around it there are some scripts for plotting, checking designs and so on. 4 | * The wxMax_eqs: contains an .html file that can be opened with any explorer. This file explains the symbolic equation development 5 | to get the equations applied in the Scilab resources. 6 | -------------------------------------------------------------------------------- /delta_documentation/Scilab_resources/PlotFunction.sce: -------------------------------------------------------------------------------- 1 | //This script plots the 3D Delta Robot 2 | 3 | //Ull, primer cal executar el inverseDelta.sce 4 | 5 | //Calcula la base 6 | B1x = [-sB/2,sB/2] 7 | B1y = [-tan(%pi/6)*sB/2,-tan(%pi/6)*sB/2] 8 | B1z = [0,0] 9 | 10 | BaseX = [-sB/2,sB/2,0,-sB/2] 11 | BaseY = [-wB,-wB,uB,-wB] 12 | BaseZ = [0,0,0,0] 13 | 14 | //Calcula la cinemàtica inversa 15 | x = 0; 16 | y = 0.1; 17 | z = -1.0; 18 | [theta1,theta2,theta3] = invDelta(x,y,z); 19 | [A1,A2,A3] = jointsPosition(theta1,theta2,theta3); 20 | 21 | //Calcula l'end-effector 22 | EFX = [0+x,sP/2+x,-sP/2+x,0+x]; 23 | EFY = [-uP+y,wP+y,wP+y,-uP+y]; 24 | EFZ = [z,z,z,z] 25 | 26 | //Calcula el braç 1 27 | b1x = [0,A1(1),x] 28 | b1y = [-wB,A1(2),-uP+y] 29 | b1z = [0,A1(3),z] 30 | //Calcula el braç 2 31 | b2x = [sqrt(3)*wB/2,A2(1),sP/2+x] 32 | b2y = [0.5*wB,A2(2),wP+y] 33 | b2z = [0,A2(3),z] 34 | //Calcula el braç 3 35 | b3x = [-sqrt(3)/2*wB,A3(1),-sP/2] 36 | b3y = [0.5*wB,A3(2),wP+y] 37 | b3z = [0,A3(3),z] 38 | 39 | close 40 | plot3d(BaseX',BaseY',BaseZ', flag=[0,8,4]) 41 | plot3d(EFX',EFY',EFZ', flag=[2,8,4]) 42 | param3d(b1x',b1y',b1z') 43 | param3d(b2x',b2y',b2z') 44 | param3d(b3x',b3y',b3z') 45 | 46 | 47 | -------------------------------------------------------------------------------- /delta_documentation/Scilab_resources/Readme.md: -------------------------------------------------------------------------------- 1 | Com heu de fer servir els arxius: 2 | 3 | Bàsicament hi ha dos grups, els que van sols (deltaOptimization) i els que fan servir el fitxer inverseDelta.sce estretament. 4 | 5 | En el deltaOptimization, el primer que heu de fer és canviar el "exec('/home/txema/Documents/IntegrationProject/Personal/inverseDelta.sce', -1)" per la direcció corresponent al lloc on heu deixat el vostre inverseDelta.sce. La resta és anar seguint els passos indicats als comentaris. 6 | 7 | Els altres arxius estan basats en l'inverseDelta.sce i només provaràn la configuració carregada per defecte en aquest arxiu. La configuració carregada per defecte es pot trobar en el propi fitxer: 8 | 9 | sB = 0.18; 10 | sP = 0.08; 11 | L = 0.09 12 | l = 0.32; 13 | 14 | Per executar qualsevol dels altres fitxers, primer heu de guardar i executar aquest. 15 | 16 | workSpaceValidation.sce: Entreu l'espai que voleu provar: 17 | x_space = [-0.2:0.01:0.2] 18 | y_space = [-0.2:0.01:0.2] 19 | z_space = linspace(-0.30,-0.2,9) 20 | 21 | El primer número és la coordenada incial, el segon l'increment i el darrer la coordenada final. El z-space va diferent. Es posa la coordenada inicial, la coordenada final i el número de gràfics que voleu que us faci. Aquest número pot ser 1, 4, 9 o 16. Aquest fitxer pintarà les configuracions possibles i com de factibles són amb un color que indica el grau de proximitat a un punt singular. 22 | 23 | trajectoria.sce: 24 | Poca cosa a dir, és bastant explicit. Si no dóna resultats és que els punts no es poden assolir. L'error típic és posar-li una coordenada z positiva. Al mig del codi hi ha un sleep(200). Si descomenteu la linia, entre gràfic i gràfic esperarà 200ms. Està bé perquè el resultat queda com una mena d'animació a càmera lenta. 25 | 26 | 27 | trajectoriaV2.sce: 28 | Aquest és bastant experimental, són proves per evitar punts crítics dividint la trajectoria en passos. Si es troba que no pot passar per un lloc, busca trajectories alternatives, però no està provat intensivament i potser peta. 29 | 30 | Per últim, si l'scilab fa coses races, aneu a la pantalla principal i feu ctrl+c. Això ho para tot. 31 | 32 | -------------------------------------------------------------------------------- /delta_documentation/Scilab_resources/deltaOptimization.sce: -------------------------------------------------------------------------------- 1 | // Aquesta funció optimitza el robot delta en funció de l'espai de treball 2 | //Pasos previs, netejar la memòria 3 | clc 4 | clear 5 | close 6 | g = 9.81; 7 | exec('/home/txema/Documents/IntegrationProject/Personal/inverseDelta.sce', -1) 8 | warning('off') 9 | tic(); 10 | // 1er pas: 11 | // Espai de treball: 12 | xWS = [-0.175,0.175]//m 13 | yWS = [-0.175,0.175]//m 14 | zWS = [-0.28,-0.23]//m 15 | 16 | sBRange = [0.1,0.2]; 17 | sPRange = [0.08,0.15]; 18 | LRange = [0.06,0.1]; 19 | lRange = [0.25,0.35]; 20 | 21 | // 2n pas: 22 | // Alguns límits 23 | maxMotorTorque = 0.25 //Nm 24 | 25 | // 3er pas: 26 | // Densitats lineals dels materials i pesos (funció de cost): 27 | densityL = 0.2;// Kg/m 28 | densityl = 0.15*2;//Kg/m 29 | weightEff = 0.1;//Kg 30 | 31 | // 4rt pas 32 | // Resolució dels càlculs en l'espai de treball. Com més petit, més duraran els càlculs, però més precissió. Es recomana al voltant de 0.025 33 | resEspaial = 0.025;//m 34 | 35 | // 5è pas 36 | // Pas triat en les mides, per exemple, per fer càlculs cada 5mm = 0.005m 37 | resConstruccio = 0.005;//m 38 | 39 | // 6è pas 40 | // Calcular (guardeu i executeu) 41 | 42 | mprintf("Calculant . . .\n") 43 | //Redefinicions 44 | x_space= [xWS(1):resEspaial:xWS(2)] 45 | y_space= [yWS(1):resEspaial:yWS(2)] 46 | z_space= [zWS(1):resEspaial:zWS(2)] 47 | 48 | sB_vector = [sBRange(1):resConstruccio:sBRange(2)] 49 | sP_vector = [sPRange(1):resConstruccio:sPRange(2)] 50 | L_vector = [LRange(1):resConstruccio:LRange(2)] 51 | l_vector = [lRange(1):resConstruccio:lRange(2)] 52 | 53 | totalIteracions = length(sB_vector)*length(sP_vector)*length(L_vector)*length(l_vector) 54 | mprintf("Es faran %i iteracions. Per parar ctrl+c.\n",totalIteracions) 55 | iteracioNum = 0 56 | candidats = 0 57 | results = [] 58 | //Iterar, iterar i iterar per cara mida possible en tot l'espai de treball 59 | for i=1:length(sB_vector) 60 | for j=1:length(sP_vector) 61 | for k=1:length(L_vector) 62 | for m=1:length(l_vector) 63 | iteracioNum = iteracioNum + 1 64 | // Mostra alguna cosa per pantalla per mostrar a l'usuari que el programa segueix viu 65 | if modulo(iteracioNum,500)==0 then 66 | clc; 67 | mprintf("Treballant. . . \n\n") 68 | mprintf("Iteracio %i de %i. Trobats %i. Queden %.1fs (%.1fh).\n\n",iteracioNum,totalIteracions,candidats,... 69 | toc()*(totalIteracions-iteracioNum)/500,toc()*(totalIteracions-iteracioNum)/(500*3600) ) 70 | if candidats<10 && candidats >= 1 then 71 | mprintf("Darrer valor trobat:\n") 72 | mprintf("sB = %.3fm sP = %.3fm L = %.3fm l = %.3fm max torque = %.3fNm ratio %.2f%%\n",... 73 | results(candidats,1),results(candidats,2),results(candidats,3),results(candidats,4),results(candidats,5),results(candidats,8) ) 74 | end 75 | if candidats>10 then 76 | mprintf("Darrers valors trobats:\n") 77 | for ii=(candidats-9):candidats 78 | mprintf("sB = %.3fm sP = %.3fm L = %.3fm l = %.3fm max torque = %.3fNm ratio %.2f%%\n",... 79 | results(ii,1),results(ii,2),results(ii,3),results(ii,4),results(ii,5),results(ii,8) ) 80 | end 81 | end 82 | tic(); 83 | end 84 | sB = sB_vector(i); 85 | sP = sP_vector(j); 86 | L = L_vector(k); 87 | l = l_vector(m); 88 | 89 | wB = sB*sqrt(3)/6; 90 | uB = sB*sqrt(3)/3; 91 | wP = sP*sqrt(3)/6; 92 | uP = sP*sqrt(3)/3; 93 | 94 | a = wB-uP; 95 | b = (sP/2)-(wB*sqrt(3)/2); 96 | c = wP-(0.5*wB); 97 | 98 | pesL = L*densityL*g; 99 | pesl = l*densityl*g; 100 | pesEff = weightEff*g; 101 | 102 | torqueValidated = [] 103 | numFrobenius = [] 104 | 105 | members_found = 0; 106 | members_lost = 0; 107 | 108 | for x=1:length(x_space) 109 | for y=1:length(y_space) 110 | for z=1:length(z_space) 111 | try 112 | [P1,P2,P3,Jx,Jq] = allTogether(x_space(x),y_space(y),z_space(z)) 113 | nFrobenius = norm(inv(Jq)*Jx) 114 | if nFrobenius < 50.0 then 115 | numFrobenius = [numFrobenius nFrobenius]//[colorMarkers nDet] 116 | torqueValidated = [torqueValidated max([P1,P2,P3])] 117 | members_found = members_found + 1; 118 | else 119 | members_lost = members_lost + 1; 120 | end 121 | catch 122 | members_lost = members_lost + 1; 123 | end 124 | end 125 | end 126 | end 127 | membersRatio=members_found/(members_found+members_lost) 128 | //Avalua si cal guardar-lo 129 | if max(torqueValidated)0.95 then 130 | candidats = candidats + 1 131 | results(candidats,:)=[sB sP L l max(torqueValidated) mean(torqueValidated) mean(numFrobenius) membersRatio*100] 132 | 133 | end 134 | end 135 | end 136 | end 137 | end 138 | toc(); 139 | //Guarda un csv per si de cas... 140 | csvWrite(results, 'Resultats.csv') 141 | 142 | //Calculations 143 | 144 | -------------------------------------------------------------------------------- /delta_documentation/Scilab_resources/inverseDelta.sce: -------------------------------------------------------------------------------- 1 | //IKP equations 2 | //Defines 3 | // En aquestes línies es descriu el cas puntual de robot que es vol testejar. 4 | //Aquestes variables són globals, alguns programes simplement les planxen per 5 | //provar altres valors 6 | sB = 0.2//0.18; 7 | sP = 0.08//0.08; 8 | L = 0.095//0.09 9 | l = 0.315//0.32; 10 | 11 | // Les següents densitats són necessàries per calcular els parells en funció de 12 | //les longituds provades. 13 | densityL = 0.2;// Kg/m 14 | densityl = %pi*0.002^2*8000//0.15*2;//Kg/m 15 | weightEff = 0.1;//Kg 16 | 17 | //Calculations 18 | // Càlculs previs en quant a distàncies, pesos i altres variables que es poden 19 | //trobar a l'article 20 | // The Delta Robot: Kinematics Solutions de Robert L. Williams 21 | g = 9.81; 22 | wB = sB*sqrt(3)/6; 23 | uB = sB*sqrt(3)/3; 24 | wP = sP*sqrt(3)/6; 25 | uP = sP*sqrt(3)/3; 26 | 27 | a = wB-uP; 28 | b = (sP/2)-(wB*sqrt(3)/2); 29 | c = wP-(0.5*wB); 30 | 31 | pesL = L*densityL*g; 32 | pesl = l*densityl*g*2; 33 | pesEff = weightEff*g; 34 | 35 | //La següent funció resol el problema cinemàtic invers: Donades unes coordenades, 36 | //retorna els àngles. En cas que no es pugui trobar una solució, fa saltar 37 | //un error abans de retornar números imaginaris o més enllà de -pi..pi. És per 38 | //aquesta raó que es recomana cridar aquesta funció amb un "try - catch" 39 | function[theta1,theta2,theta3] = invDelta(x,y,z) 40 | E1 = 2*L*(y+a); 41 | F1 = 2*z*L; 42 | G1 = x^2+y^2+z^2+a^2+L^2+2*y*a-l^2; 43 | 44 | E2 = -L*(sqrt(3)*(x+b)+y+c); 45 | F2 = 2*z*L; 46 | G2 = x^2+y^2+z^2+b^2+c^2+L^2+2*(x*b+y*c)-l^2; 47 | 48 | E3 = L*(sqrt(3)*(x-b)-y-c); 49 | F3 = 2*z*L 50 | G3 = x^2+y^2+z^2+b^2+c^2+L^2+2*(-x*b+y*c)-l^2; 51 | 52 | sol1plus = 2*atan((-F1+sqrt(E1^2+F1^2-G1^2))/(G1-E1)); 53 | sol1min = 2*atan((-F1-sqrt(E1^2+F1^2-G1^2))/(G1-E1)); 54 | sol2plus = 2*atan((-F2+sqrt(E2^2+F2^2-G2^2))/(G2-E2)); 55 | sol2min = 2*atan((-F2-sqrt(E2^2+F2^2-G2^2))/(G2-E2)); 56 | sol3plus = 2*atan((-F3+sqrt(E3^2+F3^2-G3^2))/(G3-E3)); 57 | sol3min = 2*atan((-F3-sqrt(E3^2+F3^2-G3^2))/(G3-E3)); 58 | 59 | if isreal(sol1min) && sol1min<%pi && sol1min>-%pi then 60 | theta1 = sol1min; 61 | elseif isreal(sol1plus) && sol1plus<%pi && sol1plus>-%pi then 62 | theta1 = sol1plus; 63 | else 64 | error("theta1 has not practical solution"); 65 | end 66 | if isreal(sol2min) && sol2min<%pi && sol2min>-%pi then 67 | theta2 = sol2min; 68 | elseif isreal(sol2plus) && sol2plus<%pi && sol2plus>-%pi then 69 | theta2 = sol2plus; 70 | else 71 | error("theta2 has not practical solution"); 72 | end 73 | if isreal(sol3min) && sol3min<%pi && sol3min>-%pi then 74 | theta3 = sol3min; 75 | elseif isreal(sol3plus) && sol3plus<%pi && sol3plus>-%pi then 76 | theta3 = sol3plus; 77 | else 78 | error("theta3 has not practical solution"); 79 | end 80 | 81 | //En graus: 82 | /* theta1 = theta1*180/%pi; 83 | theta2 = theta2*180/%pi; 84 | theta3 = theta3*180/%pi; 85 | disp('recorda treure angles') 86 | */ 87 | endfunction 88 | 89 | // Aquesta funció retorna la posició de les joints A_i. Es farà servir més 90 | //endavant 91 | function[A1,A2,A3] = jointsPosition(theta1,theta2,theta3) 92 | 93 | A1 = [0,-wB-L*cos(theta1),-L*sin(theta1)]; 94 | A2 = [sqrt(3)*(wB+L*cos(theta2))/2,(wB+L*cos(theta2))/2,-L*sin(theta2)]; 95 | A3 = [-sqrt(3)*(wB+L*cos(theta3))/2,(wB+L*cos(theta3))/2,-L*sin(theta3)]; 96 | endfunction 97 | 98 | // Aquesta funció dibuixa el robot Delta en una configuració donada. Fa servir 99 | // les funcions invDelta i jointsPosition. Sobre el seu ús: cal notar que no 100 | // obre una finestra nova, de manera que cal fer-ho abans. Es fa així per poder 101 | // pintar diferents configuracions sense esborrar les prèvies. 102 | function plotDelta(x,y,z) 103 | //Calcula la base: posició dels punts B 104 | B1x = [-sB/2,sB/2] 105 | B1y = [-tan(%pi/6)*sB/2,-tan(%pi/6)*sB/2] 106 | B1z = [0,0] 107 | 108 | //Calcula la base: és el triangle vermell (es dóna en 4 punts) 109 | BaseX = [-sB/2,sB/2,0,-sB/2] 110 | BaseY = [-wB,-wB,uB,-wB] 111 | BaseZ = [0,0,0,0] 112 | 113 | //Calcula la cinemàtica inversa 114 | [theta1,theta2,theta3] = invDelta(x,y,z); 115 | [A1,A2,A3] = jointsPosition(theta1,theta2,theta3); 116 | 117 | //Calcula l'end-effector, és el triangle blau (es dóna en 4 punts) 118 | EFX = [0+x,sP/2+x,-sP/2+x,0+x]; 119 | EFY = [-uP+y,wP+y,wP+y,-uP+y]; 120 | EFZ = [z,z,z,z] 121 | 122 | //Braç L 123 | //Calcula el braç 1 124 | b1xL = [0,A1(1)] 125 | b1yL = [-wB,A1(2)] 126 | b1zL = [0,A1(3)] 127 | //Calcula el braç 2 128 | b2xL = [sqrt(3)*wB/2,A2(1)] 129 | b2yL = [0.5*wB,A2(2)] 130 | b2zL = [0,A2(3)] 131 | //Calcula el braç 3 132 | b3xL = [-sqrt(3)/2*wB,A3(1)] 133 | b3yL = [0.5*wB,A3(2)] 134 | b3zL = [0,A3(3)] 135 | 136 | //Braç l 137 | //Calcula el braç 1 138 | b1xl = [A1(1),x] 139 | b1yl = [A1(2),-uP+y] 140 | b1zl = [A1(3),z] 141 | //Calcula el braç 2 142 | b2xl = [A2(1),sP/2+x] 143 | b2yl = [A2(2),wP+y] 144 | b2zl = [A2(3),z] 145 | //Calcula el braç 3 146 | b3xl = [A3(1),-sP/2+x] 147 | b3yl = [A3(2),wP+y] 148 | b3zl = [A3(3),z] 149 | 150 | //A partir d'aquí fa el dibuix, amb diferents línies i colors 151 | plot3d(BaseX',BaseY',BaseZ', flag=[5,8,4]) 152 | plot3d(EFX',EFY',EFZ', flag=[2,8,4]) 153 | param3d(b1xL',b1yL',b1zL') 154 | p=get("hdl"); //get handle on current entity (here the polyline entity) 155 | p.foreground=3; 156 | p.thickness=3; 157 | //p.mark_style=9; 158 | param3d(b2xL',b2yL',b2zL') 159 | p=get("hdl"); //get handle on current entity (here the polyline entity) 160 | p.foreground=3; 161 | p.thickness=3; 162 | //p.mark_style=9; 163 | param3d(b3xL',b3yL',b3zL') 164 | p=get("hdl"); //get handle on current entity (here the polyline entity) 165 | p.foreground=3; 166 | p.thickness=3; 167 | //p.mark_style=9; 168 | 169 | param3d(b1xl',b1yl',b1zl') 170 | param3d(b2xl',b2yl',b2zl') 171 | param3d(b3xl',b3yl',b3zl') 172 | 173 | endfunction 174 | 175 | //Funció copiada d'internet, fa el producte vectorial 176 | function [p] = CrossProd(u,v) 177 | //Calculates the cross-product of two vectors u and v. 178 | //Vectors u and v can be columns or row vectors, but 179 | //they must have only three elements each. 180 | [nu,mu] = size(u); 181 | [nv,mv] = size(v); 182 | if nu*mu <> 3 | nv*mv <> 3 then 183 | error('Vectors must be three-dimensional only') 184 | abort; 185 | end 186 | A1 = [ u(2), u(3); v(2), v(3)]; 187 | A2 = [ u(3), u(1); v(3), v(1)]; 188 | A3 = [ u(1), u(2); v(1), v(2)]; 189 | px = det(A1); py = det(A2); pz = det(A3); 190 | p = [px, py, pz] 191 | //end function 192 | endfunction 193 | 194 | // La següent funció calcula el parell que han de fer els motors del Robot 195 | //en una configuració donada. Aquest parell és estàtic, no es tenen en compte 196 | //inèrcies... Aquesta funció fa servir invDelta, jointsPosition i CrossProd. 197 | //A la mateixa funció s'explica dón surten les equacions. 198 | function[P1,P2,P3] = torqueDelta(x,y,z) 199 | //Calcula la cinemàtica inversa 200 | [theta1,theta2,theta3] = invDelta(x,y,z); 201 | [A1,A2,A3] = jointsPosition(theta1,theta2,theta3); 202 | //Suposicions fetes, descartant el moment en Z i tenint en compte les articulacions. El problema es planteja de la següent forma: 203 | // - Es considera només una branca. 204 | // - La base és estàtica i horitzontal. Això implica que la suma de forces verticals i horitzontals ha de ser zero 205 | // - Com la base està muntada sobre ròtules, el càlcul dels moments respecte qualsevol punt també ha de ser zero (excepte en z) 206 | // - En cas d'un pes mal distribuït al centroide del triangle, aquesta funció no seria vàlida, perquè el pes de l'end-effector 207 | // ja no estaria repartit exactament per 3 a cada branca. 208 | 209 | //Recàlcul dels punts P de la base mòvil 210 | P1B = [x,-uP+y,z]; 211 | P2B = [sP/2+x,wP+y,z]; 212 | P3B = [-sP/2+x,wP+y,z]; 213 | //Recàlcul dels punts B de la base fixa 214 | B1 = [0,-wB,0]; 215 | B2 = [sqrt(3)*wB/2, 0.5*wB,0]; 216 | B3 = [-sqrt(3)*wB/2, 0.5*wB,0]; 217 | //Vector "l" que va del punt A al punt P 218 | lv1 = P1B - A1; 219 | lv2 = P2B - A2; 220 | lv3 = P3B - A3; 221 | // Es calcula el mateix vector en unitari 222 | lv1_u = lv1/norm(lv1); 223 | lv2_u = lv2/norm(lv2); 224 | lv3_u = lv3/norm(lv3); 225 | 226 | //Aquí es resol el sistema d'equacions que afecta a l'end-efector. S'imposa 227 | //el següent: 228 | //[lx ly lz] és un vector unitari de l 229 | //[lx ly lz]*k = F1 230 | //Això vol dir que la força que es farà en un extrem de la base de l'end 231 | //effector tindrà la direcció de l. D'acord amb això i posant el cas de 232 | //les tres barres, el que s'ha de complir és que la suma de forces sigui = 0 233 | // Per tant 234 | //lx1*k+lx2*h+lx3*n = 0; Suma de forces en X 235 | //ly1*k+ly2*h+ly3*n = 0; Suma de forces en Y 236 | //lz1*k+lz2*h+lz3*n + P = 0 Suma de forces en Z 237 | 238 | //res = [k,h,n] 239 | //Multiplicant els vectors unitaris per la seva corresponent solució, 240 | //donarà la força que aplica la barra l 241 | A = [lv1_u' lv2_u' lv3_u']; 242 | b = [0;0;-pesEff]; 243 | res = linsolve(A,b); 244 | //disp(lv1_u*res(1)) 245 | //disp(lv2_u*res(2)) 246 | //disp(lv3_u*res(3)) 247 | //disp(res) 248 | 249 | //El pes de l'end effector no és la única força que actua sobre la barra, 250 | //també està el seu propi pes.Fer saber com es trasmet el pes a través de 251 | //la barra, es procedeix de manera similar a abans. El pes actua en l'eix 252 | //Z sempre. Si l és un vector unitari, la seva component en Z multiplicada 253 | //per alguna cosa ha de donar el Pes de la barra. Per tant: 254 | //-pesl/lv1_u(3) donarà aquesta incògnita. En multiplicar pel vector unitari 255 | //es sabrà com es trasmet la força a la barra. Això mateix sumant-li les 256 | //forces calculades abans, donarà l'estat de tensió de la barra. Cal notar el 257 | //signe negatiu dels valors calculats abans. Això es fa perque es calcula la 258 | //força que subjecta la base (la tensió de la barra) 259 | tensiol1=(-pesl/lv1_u(3))*lv1_u - lv1_u*res(1) 260 | tensiol2=(-pesl/lv2_u(3))*lv2_u - lv2_u*res(2) 261 | tensiol3=(-pesl/lv3_u(3))*lv3_u - lv3_u*res(3) 262 | //disp(tensiol1) 263 | 264 | 265 | //Finalment s'aplica la definició de parell respecte a un punt 266 | P1 = CrossProd((A1-B1), tensiol1) + CrossProd(0.5*(A1-B1), [0,0,-pesL]) 267 | P2 = CrossProd((A2-B2), tensiol2) + CrossProd(0.5*(A2-B2), [0,0,-pesL]) 268 | P3 = CrossProd((A3-B3), tensiol3) + CrossProd(0.5*(A3-B3), [0,0,-pesL]) 269 | 270 | //Si només es vol saber el parell dels motors. Es calcula el vector unitari de l'eix i es projecta. 271 | P1 = [1,0,0]*P1' 272 | P2 = [cos(2*%pi/3),sin(2*%pi/3),0]*P2' 273 | P3 = [cos(4*%pi/3),sin(4*%pi/3),0]*P3' 274 | 275 | endfunction 276 | 277 | function[Jx,Jq] = Jacobians(x,y,z) 278 | // Aquesta funció calcula els jacobians del robot Delta. Per veure la raó 279 | // per la qual apareixen 2, cal revisar l'article al que fa referència 280 | try 281 | [theta1,theta2,theta3] = invDelta(x,y,z) 282 | Jx=[x, y+a+L*cos(theta1), z+L*sin(theta1);... 283 | 2*(x+b)-sqrt(3)*L*cos(theta2),2*(y+c)-L*cos(theta2),2*(z+L*sin(theta2));... 284 | 2*(x-b)+sqrt(3)*L*cos(theta3),2*(y+c)-L*cos(theta3),2*(z+L*sin(theta3))] 285 | 286 | Jq=[L*((y+a)*sin(theta1)-z*cos(theta1)),0,0;... 287 | 0,-L*((sqrt(3)*(x+b)+y+c)*sin(theta2)+2*z*cos(theta2)),0;... 288 | 0,0,L*((sqrt(3)*(x-b)-y-c)*sin(theta3)-2*z*cos(theta3))] 289 | catch 290 | Jx=ones(3,3)*1000 291 | Jq=eye(3,3)*0.001 292 | end 293 | 294 | endfunction 295 | 296 | function[P1,P2,P3,Jx,Jq] = allTogether(x,y,z) 297 | // Aquesta funció és un resum de totes les anteriors, excepte les de fer plots. 298 | // Es disposa d'aquesta forma perquè a l'hora de fer iteracions, cal evitar 299 | // cridar funcions de forma repetitiva. El que s'intenta és sintetitzar al màxim 300 | // les funcions per millorar el temps de càlcul. 301 | E1 = 2*L*(y+a); 302 | F1 = 2*z*L; 303 | G1 = x^2+y^2+z^2+a^2+L^2+2*y*a-l^2; 304 | 305 | E2 = -L*(sqrt(3)*(x+b)+y+c); 306 | F2 = 2*z*L; 307 | G2 = x^2+y^2+z^2+b^2+c^2+L^2+2*(x*b+y*c)-l^2; 308 | 309 | E3 = L*(sqrt(3)*(x-b)-y-c); 310 | F3 = 2*z*L 311 | G3 = x^2+y^2+z^2+b^2+c^2+L^2+2*(-x*b+y*c)-l^2; 312 | 313 | sol1plus = 2*atan((-F1+sqrt(E1^2+F1^2-G1^2))/(G1-E1)); 314 | sol1min = 2*atan((-F1-sqrt(E1^2+F1^2-G1^2))/(G1-E1)); 315 | sol2plus = 2*atan((-F2+sqrt(E2^2+F2^2-G2^2))/(G2-E2)); 316 | sol2min = 2*atan((-F2-sqrt(E2^2+F2^2-G2^2))/(G2-E2)); 317 | sol3plus = 2*atan((-F3+sqrt(E3^2+F3^2-G3^2))/(G3-E3)); 318 | sol3min = 2*atan((-F3-sqrt(E3^2+F3^2-G3^2))/(G3-E3)); 319 | 320 | if isreal(sol1min) && sol1min<%pi && sol1min>-%pi then 321 | theta1 = sol1min; 322 | elseif isreal(sol1plus) && sol1plus<%pi && sol1plus>-%pi then 323 | theta1 = sol1plus; 324 | else 325 | error("theta1 has not practical solution"); 326 | end 327 | if isreal(sol2min) && sol2min<%pi && sol2min>-%pi then 328 | theta2 = sol2min; 329 | elseif isreal(sol2plus) && sol2plus<%pi && sol2plus>-%pi then 330 | theta2 = sol2plus; 331 | else 332 | error("theta2 has not practical solution"); 333 | end 334 | if isreal(sol3min) && sol3min<%pi && sol3min>-%pi then 335 | theta3 = sol3min; 336 | elseif isreal(sol3plus) && sol3plus<%pi && sol3plus>-%pi then 337 | theta3 = sol3plus; 338 | else 339 | error("theta3 has not practical solution"); 340 | end 341 | A1 = [0,-wB-L*cos(theta1),-L*sin(theta1)]; 342 | A2 = [sqrt(3)*(wB+L*cos(theta2))/2,(wB+L*cos(theta2))/2,-L*sin(theta2)]; 343 | A3 = [-sqrt(3)*(wB+L*cos(theta3))/2,(wB+L*cos(theta3))/2,-L*sin(theta3)]; 344 | 345 | P1B = [x,-uP+y,z]; 346 | P2B = [sP/2+x,wP+y,z]; 347 | P3B = [-sP/2+x,wP+y,z]; 348 | B1 = [0,-wB,0]; 349 | B2 = [sqrt(3)*wB/2, 0.5*wB,0]; 350 | B3 = [-sqrt(3)*wB/2, 0.5*wB,0]; 351 | 352 | lv1 = P1B - A1; 353 | lv2 = P2B - A2; 354 | lv3 = P3B - A3; 355 | // Es calcula el mateix vector en unitari 356 | lv1_u = lv1/norm(lv1); 357 | lv2_u = lv2/norm(lv2); 358 | lv3_u = lv3/norm(lv3); 359 | 360 | A = [lv1_u' lv2_u' lv3_u']; 361 | bb = [0;0;-pesEff]; 362 | res = linsolve(A,bb); 363 | 364 | tensiol1=(-pesl/lv1_u(3))*lv1_u - lv1_u*res(1) 365 | tensiol2=(-pesl/lv2_u(3))*lv2_u - lv2_u*res(2) 366 | tensiol3=(-pesl/lv3_u(3))*lv3_u - lv3_u*res(3) 367 | 368 | P1 = CrossProd((A1-B1), tensiol1) + CrossProd(0.5*(A1-B1), [0,0,-pesL]) 369 | P2 = CrossProd((A2-B2), tensiol2) + CrossProd(0.5*(A2-B2), [0,0,-pesL]) 370 | P3 = CrossProd((A3-B3), tensiol3) + CrossProd(0.5*(A3-B3), [0,0,-pesL]) 371 | 372 | //Si només es vol saber el parell dels motors. Es calcula el vector unitari de l'eix i es projecta. 373 | P1 = [1,0,0]*P1' 374 | P2 = [cos(2*%pi/3),sin(2*%pi/3),0]*P2' 375 | P3 = [cos(4*%pi/3),sin(4*%pi/3),0]*P3' 376 | 377 | Jx=[x, y+a+L*cos(theta1), z+L*sin(theta1);... 378 | 2*(x+b)-sqrt(3)*L*cos(theta2),2*(y+c)-L*cos(theta2),2*(z+L*sin(theta2));... 379 | 2*(x-b)+sqrt(3)*L*cos(theta3),2*(y+c)-L*cos(theta3),2*(z+L*sin(theta3))] 380 | 381 | Jq=[L*((y+a)*sin(theta1)-z*cos(theta1)),0,0;... 382 | 0,-L*((sqrt(3)*(x+b)+y+c)*sin(theta2)+2*z*cos(theta2)),0;... 383 | 0,0,L*((sqrt(3)*(x-b)-y-c)*sin(theta3)-2*z*cos(theta3))] 384 | 385 | if abs(det(Jx))<0.000001 || abs(det(Jq))<0.000001then 386 | error("J no invertible"); 387 | end 388 | endfunction 389 | 390 | 391 | -------------------------------------------------------------------------------- /delta_documentation/Scilab_resources/trajectoria.sce: -------------------------------------------------------------------------------- 1 | //Draw trajectory 2 | close(); 3 | 4 | // Aquesta funció requereix l'execució de l'script inverseDelta.sce. Totes les 5 | // subrutines estan en aquest arxiu. Abans, però, cal definir els paràmetres de 6 | // la màquina al mateix inverseDelta.sce (els trobareu a la capçalera). L'usuari 7 | // ha de modificar el següent: 8 | startPoint = [-0.15, -0.15, -0.25] 9 | goalPoint = [0.15,0.15,-0.25] 10 | stepNumber = 100 11 | // 12 | 13 | // Inicialitzacions, primer es calcula un vector trossejat en tantes parts com 14 | // passos definits. 15 | stepVect = (goalPoint - startPoint)*1/stepNumber; 16 | // Altres inicialitzacions 17 | keepTorques = zeros(stepNumber,3); 18 | keepThetas = zeros(stepNumber,3); 19 | keepSample = zeros(stepNumber,1); 20 | figure(1); 21 | 22 | for i=1:stepNumber 23 | // Calcula el següent punt 24 | currentPoint = startPoint + stepVect*i 25 | // Grafica'l 26 | plotDelta(currentPoint(1),currentPoint(2),currentPoint(3)) 27 | //Calcula els angles 28 | [T1,T2,T3]=invDelta(currentPoint(1),currentPoint(2),currentPoint(3)) 29 | keepThetas(i,:)=[T1,T2,T3] 30 | //Calcula els parells 31 | [T1,T2,T3]=torqueDelta(currentPoint(1),currentPoint(2),currentPoint(3)) 32 | keepTorques(i,:)=[T1,T2,T3] 33 | //Guarda la mostra 34 | keepSample(i,:) = i; 35 | 36 | clear T1 37 | clear T2 38 | clear T3 39 | //Si es vol fer una animació, descomentant la linia següent va dibuixant el 40 | //robot pas a pas 41 | //sleep(200) 42 | end 43 | 44 | // Dibuixa els altres gràfics amb les dades obtingudes 45 | figure(2); 46 | plot2d(keepSample(:,1),[keepThetas(:,1),keepThetas(:,2),keepThetas(:,3)],leg="theta1@theta2@theta3") 47 | title("Diagrama dels angles dels motors") 48 | xlabel("Sample") 49 | ylabel("Angle [rad]") 50 | 51 | figure(3); 52 | plot2d(keepSample(:,1),[keepTorques(:,1),keepTorques(:,2),keepTorques(:,3)],leg="theta1@theta2@theta3") 53 | title("Parells motors") 54 | xlabel("Sample") 55 | ylabel("Parell [Nm]") 56 | 57 | // PS, és una forma bastant ineficient de fer els càlculs. S'ha millorat l'algorisme 58 | // amb la funció allTogether, però es deixa per altres scripts més intensius en 59 | // càlcul 60 | 61 | -------------------------------------------------------------------------------- /delta_documentation/Scilab_resources/trajectoriaV2.sce: -------------------------------------------------------------------------------- 1 | //Draw trajectory 2 | // Aquest script dibuixarà una trajectòria del delta esquivant punts singulars. 3 | // Pot ser necessari ajustar els paràmetres de la màquina al fitxer inverseDelta.sce 4 | // Aquest script ÉS TOTALMENT EXPERIMENTAL, no hiha garanties de que funcioni 5 | // Neteja la memòria, figures i executa l'script inverseDelta.sce 6 | close(); 7 | clear 8 | exec('/home/txema/Documents/IntegrationProject/Personal/inverseDelta.sce', -1) 9 | 10 | // Definicions de l'usuari: Pas mínim i zona de treball vàlida 11 | minStepSize = 0.005; 12 | xRange=[-0.3,0.3]; 13 | yRange=[-0.3,0.3]; 14 | zRange=[-0.5,-0.15]; 15 | 16 | // Número de Frobenius, per més info: "Análisis del desempeño cinetostático de un robot 17 | // paralelo tipo Delta reconfigurable". Com més alt, més proper a la inestabilitat 18 | nFrobeniusMax = 50; 19 | 20 | // Coordenades origen i objectiu respectivament: 21 | testPoint = [-0.15,-0.15,-0.2]; 22 | objective=[0.15,0.15,-0.2]; 23 | 24 | // Inicialitzacions 25 | exploredValues = ones(1000,3); 26 | indexExplored = 1; 27 | figure(1); 28 | 29 | // Current point passa a ser l'origen 30 | currentPoint = testPoint; 31 | 32 | // Bucle limitat a 1000 iteracions per assegurar l'estabilitat del programa. 33 | tic(); 34 | for bucles = 1:1000 35 | //Calcula tots els punts al voltant del punt actual on pot anar el End-effector 36 | nextPoints = zeros(27,3); 37 | nextPoints(1,:) = currentPoint + [-1,-1,-1]*minStepSize; 38 | nextPoints(2,:) = currentPoint + [ 0,-1,-1]*minStepSize; 39 | nextPoints(3,:) = currentPoint + [+1,-1,-1]*minStepSize; 40 | nextPoints(4,:) = currentPoint + [-1, 0,-1]*minStepSize; 41 | nextPoints(5,:) = currentPoint + [ 0, 0,-1]*minStepSize; 42 | nextPoints(6,:) = currentPoint + [+1, 0,-1]*minStepSize; 43 | nextPoints(7,:) = currentPoint + [-1,+1,-1]*minStepSize; 44 | nextPoints(8,:) = currentPoint + [ 0,+1,-1]*minStepSize; 45 | nextPoints(9,:) = currentPoint + [+1,+1,-1]*minStepSize; 46 | nextPoints(10,:) = currentPoint + [-1,-1, 0]*minStepSize; 47 | nextPoints(11,:) = currentPoint + [ 0,-1, 0]*minStepSize; 48 | nextPoints(12,:) = currentPoint + [+1,-1, 0]*minStepSize; 49 | nextPoints(13,:) = currentPoint + [-1, 0, 0]*minStepSize; 50 | // Aquest es correspondria al punt actual, en canvi es tria un punt que minimitzi la distància a l'objectiu 51 | nextPoints(14,:) = currentPoint + sqrt(3)*minStepSize*(objective - currentPoint)/norm(objective - currentPoint); 52 | nextPoints(15,:) = currentPoint + [+1, 0, 0]*minStepSize; 53 | nextPoints(16,:) = currentPoint + [-1,+1, 0]*minStepSize; 54 | nextPoints(17,:) = currentPoint + [ 0,+1, 0]*minStepSize; 55 | nextPoints(18,:) = currentPoint + [+1,+1, 0]*minStepSize; 56 | nextPoints(19,:) = currentPoint + [-1,-1,+1]*minStepSize; 57 | nextPoints(20,:) = currentPoint + [ 0,-1,+1]*minStepSize; 58 | nextPoints(21,:) = currentPoint + [+1,-1,+1]*minStepSize; 59 | nextPoints(22,:) = currentPoint + [-1, 0,+1]*minStepSize; 60 | nextPoints(23,:) = currentPoint + [ 0, 0,+1]*minStepSize; 61 | nextPoints(24,:) = currentPoint + [+1, 0,+1]*minStepSize; 62 | nextPoints(25,:) = currentPoint + [-1,+1,+1]*minStepSize; 63 | nextPoints(26,:) = currentPoint + [ 0,+1,+1]*minStepSize; 64 | nextPoints(27,:) = currentPoint + [+1,+1,+1]*minStepSize; 65 | minValue = 50000000; 66 | optimVect = zeros(1,3); 67 | 68 | //Al següent tros de codi es mira per cada punt quin és el més proper i a més 69 | //és una solució vàlida 70 | for k=1:27 71 | if minValue > norm(objective-nextPoints(k,:)) 72 | if nextPoints(k,1) > xRange(1) && nextPoints(k,1) < xRange(2) 73 | if nextPoints(k,2) > yRange(1) && nextPoints(k,2) < yRange(2) 74 | if nextPoints(k,3) > zRange(1) && nextPoints(k,3) < zRange(2) 75 | try 76 | [Jx,Jq] = Jacobians(nextPoints(k,1),nextPoints(k,2),nextPoints(k,3)) 77 | nFrobenius = norm(inv(Jq)*Jx) 78 | if (find(members(exploredValues,nextPoints(k,:),'rows'),1)==[] && nFrobenius < nFrobeniusMax) 79 | minValue = norm(objective-nextPoints(k,:)); 80 | optimVect = nextPoints(k,:); 81 | else 82 | end 83 | catch 84 | end 85 | end 86 | end 87 | end 88 | end 89 | end 90 | //El punt trobat el guarda com a explorat per no repetir-lo 91 | exploredValues(indexExplored,:) = optimVect; 92 | //Dibuixa la màquina 93 | plotDelta(optimVect(1),optimVect(2),optimVect(3)) 94 | //Conserva els angles 95 | [T1,T2,T3]=invDelta(optimVect(1),optimVect(2),optimVect(3)) 96 | keepThetas(indexExplored,:)=[T1,T2,T3] 97 | //Conserva els parells 98 | [T1,T2,T3]=torqueDelta(optimVect(1),optimVect(2),optimVect(3)) 99 | keepTorques(indexExplored,:)=[T1,T2,T3] 100 | //Conserva la mostra 101 | keepSample(indexExplored,:) = indexExplored; 102 | indexExplored = indexExplored + 1; 103 | //Avança, la posició actual és l'òptima trobada 104 | currentPoint = optimVect; 105 | 106 | 107 | if indexExplored > 1000 108 | indexExplored = 1; 109 | end 110 | // Si el punt final és molt proper al punt actual, dóna per acabat el programa 111 | if norm(objective-currentPoint) < minStepSize 112 | break 113 | end 114 | 115 | end 116 | toc() 117 | 118 | //Acaba de graficar i presentar els resultats dels punts guardats a l'apartat anterior 119 | indexExplored = indexExplored -1 120 | figure(2); 121 | plot2d(keepSample(1:indexExplored,1),[keepThetas(1:indexExplored,1),keepThetas(1:indexExplored,2),keepThetas(1:indexExplored,3)],leg="theta1@theta2@theta3") 122 | title("Thetas") 123 | 124 | figure(3); 125 | plot2d(keepSample(1:indexExplored,1),[keepTorques(1:indexExplored,1),keepTorques(1:indexExplored,2),keepTorques(1:indexExplored,3)],leg="theta1@theta2@theta3") 126 | title("Torques") 127 | 128 | figure(4); 129 | scatter3(exploredValues(1:indexExplored,1),exploredValues(1:indexExplored,2),exploredValues(1:indexExplored,3)) 130 | 131 | -------------------------------------------------------------------------------- /delta_documentation/Scilab_resources/workSpaceValidation.sce: -------------------------------------------------------------------------------- 1 | //Work space validation 2 | close 3 | 4 | // Aquest script validarà i dibuixarà l'espai de treball del Delta, seccionat en 5 | // diferents alçades. 6 | // Per tant, es requereix l'execució de l'script inverseDelta.sce. Totes les 7 | // subrutines estan en aquest arxiu. Abans, però, cal definir els paràmetres de 8 | // la màquina al mateix inverseDelta.sce (els trobareu a la capçalera). L'usuari 9 | // ha de modificar el següent: 10 | x_space = [-0.2:0.01:0.2] //El primer número és l'origen, després l'interval de càlcul i el punt final 11 | y_space = [-0.2:0.01:0.2] 12 | // La coordenada z va diferent, el primer número és l'origen, el segon el punt final i el darrer 13 | // és el número de càlculs que es vol fer. ALERTA, més endavant es fa servir la funció subplot 14 | // que mostra els resultats obtinguts. Aquesta funció farà l'arrel quadrada d'aquest darrer 15 | // número i ha de donar enter. Per tant pot ser 4, 9 o 16. TODO: cal arreglar això. 16 | z_space = linspace(-0.30,-0.20,9) 17 | 18 | // Inicialitzacions arbitràries 19 | xValidated = [ ] 20 | yValidated = [ ] 21 | zValidated = [ ] 22 | torqueValidated = [] 23 | numFrobenius = [] 24 | 25 | members_found = 0; 26 | members_lost = 0; 27 | 28 | // Repasa un per un, tots els punts que es volen calcular 29 | for x=1:length(x_space) 30 | for y=1:length(y_space) 31 | for z=1:length(z_space) 32 | // El procediment try...catch intentarà executar el codi següent. Pot passar 33 | // que no hi hagi una solució vàlida i les funcions que depenen de les coordenades 34 | // passades donin error. Si això passa, s'executa el catch i el programa no 35 | // acaba. 36 | try 37 | [theta1,theta2,theta3] = invDelta(x_space(x),y_space(y),z_space(z)) 38 | [P1,P2,P3] = torqueDelta(x_space(x),y_space(y),z_space(z)) 39 | [Jx,Jq] = Jacobians(x_space(x),y_space(y),z_space(z)) 40 | // El número de Frobenius retorna "com d'aprop" estan les sigularitats. 41 | // La seva funció és similar a la del determinant de la jacobiana. 42 | nFrobenius = norm(inv(Jq)*Jx) 43 | nDet = abs(det(inv(Jq)*Jx)) 44 | // En cas de que el punt sigui vàlid, però es trobi proper a una zona 45 | // singular, queda descartat 46 | if nFrobenius < 50.0 && nDet > 0.00000001 then 47 | xValidated = [xValidated x_space(x)] 48 | yValidated = [yValidated y_space(y)] 49 | zValidated = [zValidated z_space(z)] 50 | numFrobenius = [numFrobenius nFrobenius] 51 | // Es mira també que no es superi el parell màxim dels servos 52 | torqueValidated = [torqueValidated max([P1,P2,P3])] 53 | // Si tot OK, considera el punt com vàlid i se'l guarda per fer 54 | // estadística. 55 | members_found = members_found + 1; 56 | else 57 | members_lost = members_lost + 1; 58 | end 59 | 60 | catch 61 | members_lost = members_lost + 1; 62 | end 63 | end 64 | end 65 | end 66 | 67 | f=gcf(); // Create a figure 68 | f.figure_name= "Diagrama de la zona de treball del Delta"; 69 | 70 | // Aquest loop fa els gràfics dels punts trobats 71 | for z=1:length(z_space) 72 | subplot(sqrt(length(z_space)),sqrt(length(z_space)),z) 73 | plotThisX = []; 74 | plotThisY = []; 75 | thisColor = []; 76 | for n=1:members_found 77 | if zValidated(n)==z_space(z) then 78 | plotThisX = [plotThisX xValidated(n)] 79 | plotThisY = [plotThisY yValidated(n)] 80 | thisColor = [thisColor numFrobenius(n)] 81 | end 82 | end 83 | typeMarkers = 1*ones(1,length(plotThisX)) 84 | // set color map 85 | f.color_map = jetcolormap(64); 86 | scatter( plotThisX, plotThisY, typeMarkers,thisColor,"fill") 87 | title(['z = ' string(z_space(z)*1000) ' mm']) 88 | 89 | end 90 | 91 | // Es neteja la pantalla i es fa un petit resum de tota l'estadística adquirida. 92 | clc 93 | mprintf('Statistics:\n') 94 | mprintf("Percentatge de l`espai cobert = %f %%\n",members_found/(members_found+members_lost)) 95 | mprintf("Parell maxim calculat = %f Nm\n",max(torqueValidated)) 96 | mprintf("Parell mig calculat = %f Nm\n",mean(torqueValidated)) 97 | mprintf("Mitjana de les normes de Frobenius = %f \n",mean(numFrobenius)) 98 | 99 | // Nota al peu; si els intervals de càlcul són molt petits, pot ser que costi molt 100 | // fer els darrers diagrames. Per alguna raó són molt exigents en memòria. 101 | 102 | 103 | 104 | 105 | 106 | -------------------------------------------------------------------------------- /delta_documentation/wxMax_eqs/Eqs_estat.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Eqs_estat 5 | 6 | 7 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | \( \DeclareMathOperator{\abs}{abs} 24 | \newcommand{\ensuremath}[1]{\mbox{$#1$}} 25 | \) 26 | 27 | 28 | 29 | 30 |

31 | Equacions cinemàtiques del robot Delta
32 | 33 |

34 | 35 | 36 | 37 | 38 | 39 |

40 | Basat en "The Delta parallel robot: Kinematics Solutions" de Robert L. Williams II
41 |

42 | 43 | 44 | 45 | 46 | 47 | 51 | 54 |
48 | 49 | --> 50 | 52 | kill(all)$
53 | simp:true$
55 | 56 | 57 | 58 | 59 | 60 | 61 |

62 | Figure 1:/home/txema/Documents/IntegrationProject/Personal/wxMax/Diagrama gobal.png 63 |
64 | Diagram 65 | 66 | 67 | 68 | 69 |

70 | Figure 2:/home/txema/Documents/IntegrationProject/Personal/wxMax/Diagrama bases.png 71 |
72 | Diagram 73 | 74 | 75 | 76 | 77 |

78 | Primer pas, es defineixen els punts coneguts. Aquests són B i P. Per fer-ho fàcil es tenen en consideració 2 coses. La primera és que en comptes de representar P en funció dels angles theta, el que es fa és partir des del final. En aquest cas, com es vol resoldre el problema "invers", es considera que x, y, z de l'end effector són coneguts. La segona consideració són les equacions següents:
79 |

80 | 81 | 82 | 83 | 84 | 85 | 89 | 95 |
86 | 87 | --> 88 | 90 | wB = s_B*sqrt(3)/6;
91 | uB = s_B*sqrt(3)/3;
92 | wP = s_P*sqrt(3)/6;
93 | uP = s_P*sqrt(3)/3;
94 |
96 | \[\tag{\% o2} \mathit{wB}=\frac{{s_B}}{2 \sqrt{3}}\] 97 | \[\tag{\% o3} \mathit{uB}=\frac{{s_B}}{\sqrt{3}}\] 98 | \[\tag{\% o4} \mathit{wP}=\frac{{s_P}}{2 \sqrt{3}}\] 99 | \[\tag{\% o5} \mathit{uP}=\frac{{s_P}}{\sqrt{3}}\] 100 | 101 | 102 | 103 | 104 | 105 |

106 | Fet això es defineixen els punts B i P, tenint en compte les definicions anteriors:
107 |

108 | 109 | 110 | 111 | 112 | 113 | 117 | 124 |
114 | 115 | --> 116 | 118 | B1:[0,-w_B,0];
119 | B2:[w_B*sqrt(3)/2,1/2*w_B,0];
120 | B3:[-w_B*sqrt(3)/2,1/2*w_B,0];
121 | P1:[x,y-u_P,z];
122 | P2:[x+s_P/2,y+w_P,z];
123 | P3:[x-s_P/2,y+w_P,z];
125 | \[\tag{B1}[0,-{w_B},0]\] 126 | \[\tag{B2}[\frac{\sqrt{3}\, {w_B}}{2},\frac{{w_B}}{2},0]\] 127 | \[\tag{B3}[-\frac{\sqrt{3}\, {w_B}}{2},\frac{{w_B}}{2},0]\] 128 | \[\tag{P1}[x,y-{u_P},z]\] 129 | \[\tag{P2}[\frac{{s_P}}{2}+x,{w_P}+y,z]\] 130 | \[\tag{P3}[x-\frac{{s_P}}{2},{w_P}+y,z]\] 131 | 132 | 133 | 134 | 135 | 136 |

137 | Ara ve el pas clau. Primer cal no perdre de vista l'objectiu principal del que s'està buscant, que és relacionar en una equació les thetas i les coordenades de l'end-effector {x,y,z}. Per fer-ho, l'autor de l'article proposa el següent:
- Amb les dades que es tenen, es pot arribar a trobar les coordenades del punt A. Simplement és la suma de dos vectors: OB+BA. Com la theta és una de les variables que s'estan buscant, es pot fer servir en l'equació. El que no es pot fer és anar més enllà, perquè els angles que formen L i l no són coneguts i dependran de la posició de l'End-Effector. Dit d'una altra manera, no hi ha forma d'imposar els angles entre L i l, perqué no hi ha actuadors.
- Per altra banda, es pot conèixer fàcilment les coordenades dels punts P. De fet ja estan calculades més amunt. {x,y,z} són desconeguts, però ja va bé que apareguin en l'equació, perquè justament el que es vol és relacionar les coordenades {x,y,z} amb {theta1, theta2, theta3}.

Ara ve la idea feliç, per relacionar els 2 punts anteriors el que proposa l'article és el següent:
- l és una distància coneguda, per tant, si es troba el vector que va des de A fins a P i es busca el seu mòdul, automàticament es té l. Aplicant la fórmula d'extrem menys origen per calcular el vector l i obtenint el seu mòdul, apareixen les equacions del robot Delta.
138 |

139 | 140 | 141 | 142 | 143 | 144 |

145 | Càlcul de OA=OB+BA
146 |

147 | 148 | 149 | 150 | 151 | 152 | 156 | 161 |
153 | 154 | --> 155 | 157 | A1:B1+[0,-L*cos(theta_1),-L*sin(theta_1)];
158 | A2:B2+[sqrt(3)*L*cos(theta_2)/2,L*cos(theta_2)/2,-L*sin(theta_2)];
159 | A3:B3+[-sqrt(3)*L*cos(theta_3)/2,L*cos(theta_3)/2,-L*sin(theta_3)];
160 |
162 | \[\tag{A1}[0,-{w_B}-\cos{\left( {{theta}_1}\right) } L,-\sin{\left( {{theta}_1}\right) } L]\] 163 | \[\tag{A2}[\frac{\sqrt{3}\, {w_B}}{2}+\frac{\sqrt{3}\, \cos{\left( {{theta}_2}\right) } L}{2},\frac{{w_B}}{2}+\frac{\cos{\left( {{theta}_2}\right) } L}{2},-\sin{\left( {{theta}_2}\right) } L]\] 164 | \[\tag{A3}[-\frac{\sqrt{3}\, {w_B}}{2}-\frac{\sqrt{3}\, \cos{\left( {{theta}_3}\right) } L}{2},\frac{{w_B}}{2}+\frac{\cos{\left( {{theta}_3}\right) } L}{2},-\sin{\left( {{theta}_3}\right) } L]\] 165 | 166 | 167 | 168 | 169 | 170 |

171 | Càlcul del vector l=P-OA. S'incorpora un petit canvi de variable per facilitar els càlculs:
a=w_B-u_P
b=-sqrt(3)/2*w_B+s_P/2
c=-w_B/2+w_P
Aquests canvis de variable es fan amb la funció ratsubst de manera que ratsubst(a,w_B-u_P,P1-A1) vol dir que busqui en P1-A1 la seqüència w_B-u_P i hi posi una a
172 |

173 | 174 | 175 | 176 | 177 | 178 | 182 | 188 |
179 | 180 | --> 181 | 183 | vectl1:ratsubst(a,w_B-u_P,P1-A1);
184 | vectl2:ratsubst(b,-sqrt(3)/2*w_B+s_P/2,expand(P2-A2))$
185 | vectl2:ratsubst(c,-w_B/2+w_P,expand(vectl2));
186 | vectl3:ratsubst(b,-sqrt(3)/2*w_B+s_P/2,expand(P3-A3))$
187 | vectl3:ratsubst(c,-w_B/2+w_P,expand(vectl3));
189 | \[\tag{vectl1}[x,\cos{\left( {{theta}_1}\right) } L+y+a,\sin{\left( {{theta}_1}\right) } L+z]\] 190 | \[\tag{vectl2}[-\frac{-2 b-2 x+\sqrt{3}\, \cos{\left( {{theta}_2}\right) } L}{2},-\frac{-2 c-2 y+\cos{\left( {{theta}_2}\right) } L}{2},\sin{\left( {{theta}_2}\right) } L+z]\] 191 | \[\tag{vectl3}[\frac{-2 b+2 x+\sqrt{3}\, \cos{\left( {{theta}_3}\right) } L}{2},-\frac{-2 c-2 y+\cos{\left( {{theta}_3}\right) } L}{2},\sin{\left( {{theta}_3}\right) } L+z]\] 192 | 193 | 194 | 195 | 196 | 197 |

198 | Finalment es calcula el mòdul dels vectors anteriors i s'igualen a l. Per evitar les arrels quadrades, es deixa tot elevat al quadrat:
199 |

200 | 201 | 202 | 203 | 204 | 205 | 209 | 213 |
206 | 207 | --> 208 | 210 | l1: expand(vectl1[1]²+vectl1[2]²+vectl1[3]²=l²);
211 | l2: expand(vectl2[1]²+vectl2[2]²+vectl2[3]²=l²);
212 | l3: expand(vectl3[1]²+vectl3[2]²+vectl3[3]²=l²);
214 | \[\tag{l1}{{\sin{\left( {{theta}_1}\right) }}^{2}} {{L}^{2}}+{{\cos{\left( {{theta}_1}\right) }}^{2}} {{L}^{2}}+2 \sin{\left( {{theta}_1}\right) } z L+2 \cos{\left( {{theta}_1}\right) } y L+2 a\, \cos{\left( {{theta}_1}\right) } L+{{z}^{2}}+{{y}^{2}}+2 a y+{{x}^{2}}+{{a}^{2}}={{l}^{2}}\] 215 | \[\tag{l2}{{\sin{\left( {{theta}_2}\right) }}^{2}} {{L}^{2}}+{{\cos{\left( {{theta}_2}\right) }}^{2}} {{L}^{2}}+2 \sin{\left( {{theta}_2}\right) } z L-\cos{\left( {{theta}_2}\right) } y L-\sqrt{3}\, \cos{\left( {{theta}_2}\right) } x L-c\, \cos{\left( {{theta}_2}\right) } L-\sqrt{3} b\, \cos{\left( {{theta}_2}\right) } L+{{z}^{2}}+{{y}^{2}}+2 c y+{{x}^{2}}+2 b x+{{c}^{2}}+{{b}^{2}}={{l}^{2}}\] 216 | \[\tag{l3}{{\sin{\left( {{theta}_3}\right) }}^{2}} {{L}^{2}}+{{\cos{\left( {{theta}_3}\right) }}^{2}} {{L}^{2}}+2 \sin{\left( {{theta}_3}\right) } z L-\cos{\left( {{theta}_3}\right) } y L+\sqrt{3}\, \cos{\left( {{theta}_3}\right) } x L-c\, \cos{\left( {{theta}_3}\right) } L-\sqrt{3} b\, \cos{\left( {{theta}_3}\right) } L+{{z}^{2}}+{{y}^{2}}+2 c y+{{x}^{2}}-2 b x+{{c}^{2}}+{{b}^{2}}={{l}^{2}}\] 217 | 218 | 219 | 220 | 221 | 222 |

223 | Per simplificar trigonomètricament:
224 |

225 | 226 | 227 | 228 | 229 | 230 | 234 | 238 |
231 | 232 | --> 233 | 235 | EQ_STATE_1:facsum(trigsimp(l1),cos(theta_1));
236 | EQ_STATE_2:facsum(trigsimp(l2),cos(theta_2));
237 | EQ_STATE_3:facsum(trigsimp(l3),cos(theta_3));
239 | \[\tag{EQ\_ STATE\_ 1}{{L}^{2}}+2 \sin{\left( {{theta}_1}\right) } z L+2 \cos{\left( {{theta}_1}\right) } \left( a+y\right) L+{{z}^{2}}+{{y}^{2}}+2 a y+{{x}^{2}}+{{a}^{2}}={{l}^{2}}\] 240 | \[\tag{EQ\_ STATE\_ 2}{{L}^{2}}+2 \sin{\left( {{theta}_2}\right) } z L-\cos{\left( {{theta}_2}\right) } \left( \sqrt{3} b+c+\sqrt{3} x+y\right) L+{{z}^{2}}+{{y}^{2}}+2 c y+{{x}^{2}}+2 b x+{{c}^{2}}+{{b}^{2}}={{l}^{2}}\] 241 | \[\tag{EQ\_ STATE\_ 3}{{L}^{2}}+2 \sin{\left( {{theta}_3}\right) } z L-\cos{\left( {{theta}_3}\right) } \left( \sqrt{3} b+c-\sqrt{3} x+y\right) L+{{z}^{2}}+{{y}^{2}}+2 c y+{{x}^{2}}-2 b x+{{c}^{2}}+{{b}^{2}}={{l}^{2}}\] 242 | 243 | 244 | 245 | 246 | 247 |

248 | Per calcular la matriu Jacobiana es van derivant les funcions anteriors. Per fer-ho, es reescriuen posant totes les variables en funció de X,Y,Z i s'aplica la funció. En aquest cas és molt complicat arribar a la mateixa expressió que l'article, tot i que el resultat és el mateix.
249 |

250 | 251 | 252 | 253 | 254 | 255 | 259 | 264 |
256 | 257 | --> 258 | 260 | EQ_1: ratsubst(theta_1(x,y,z),theta_1,EQ_STATE_1);
261 | EQ_2: ratsubst(theta_2(x,y,z),theta_2,EQ_STATE_2);
262 | EQ_3: ratsubst(theta_3(x,y,z),theta_3,EQ_STATE_3);
263 |
265 | \[\tag{EQ\_ 1}{{L}^{2}}+\left( 2 a\, \cos{\left( {{theta}_1}\left( x,y,z\right) \right) }+2 \cos{\left( {{theta}_1}\left( x,y,z\right) \right) } y+2 \sin{\left( {{theta}_1}\left( x,y,z\right) \right) } z\right) L+{{z}^{2}}+{{y}^{2}}+2 a y+{{x}^{2}}+{{a}^{2}}={{l}^{2}}\] 266 | \[\tag{EQ\_ 2}{{L}^{2}}+\left( \left( -\sqrt{3} x-c-\sqrt{3} b\right) \, \cos{\left( {{theta}_2}\left( x,y,z\right) \right) }-\cos{\left( {{theta}_2}\left( x,y,z\right) \right) } y+2 \sin{\left( {{theta}_2}\left( x,y,z\right) \right) } z\right) L+{{z}^{2}}+{{y}^{2}}+2 c y+{{x}^{2}}+2 b x+{{c}^{2}}+{{b}^{2}}={{l}^{2}}\] 267 | \[\tag{EQ\_ 3}{{L}^{2}}+\left( \left( \sqrt{3} x-c-\sqrt{3} b\right) \, \cos{\left( {{theta}_3}\left( x,y,z\right) \right) }-\cos{\left( {{theta}_3}\left( x,y,z\right) \right) } y+2 \sin{\left( {{theta}_3}\left( x,y,z\right) \right) } z\right) L+{{z}^{2}}+{{y}^{2}}+2 c y+{{x}^{2}}-2 b x+{{c}^{2}}+{{b}^{2}}={{l}^{2}}\] 268 | 269 | 270 | 271 | 272 | 273 | 277 | 279 |
274 | 275 | --> 276 | 278 | facsum(diff(EQ_1,x),diff(theta_1(x,y,z),x));
280 | \[\tag{\% o51} 2 \left( \frac{d}{d x} {{theta}_1}\left( x,y,z\right) \right) \left( -a\, \sin{\left( {{theta}_1}\left( x,y,z\right) \right) }-\sin{\left( {{theta}_1}\left( x,y,z\right) \right) } y+\cos{\left( {{theta}_1}\left( x,y,z\right) \right) } z\right) L+2 x=0\] 281 | 282 | 283 | 284 | 285 | 286 | 290 | 294 |
287 | 288 | --> 289 | 291 | jacobian([lhs(EQ_1),lhs(EQ_2),lhs(EQ_3)],[x,y,z])$
292 | facsum(%,diff(theta_1(x,y,z),x),diff(theta_2(x,y,z),x),diff(theta_3(x,y,z),x));
293 |
295 | \[\tag{\% o55} \begin{pmatrix}2 \left( \frac{d}{d x} {{theta}_1}\left( x,y,z\right) \right) \left( -a\, \sin{\left( {{theta}_1}\left( x,y,z\right) \right) }-\sin{\left( {{theta}_1}\left( x,y,z\right) \right) } y+\cos{\left( {{theta}_1}\left( x,y,z\right) \right) } z\right) L+2 x & 2 \left( \cos{\left( {{theta}_1}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_1}\left( x,y,z\right) \right) z L-\sin{\left( {{theta}_1}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_1}\left( x,y,z\right) \right) y L-a\, \sin{\left( {{theta}_1}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_1}\left( x,y,z\right) \right) L+\cos{\left( {{theta}_1}\left( x,y,z\right) \right) } L+y+a\right) & 2 \left( \cos{\left( {{theta}_1}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_1}\left( x,y,z\right) \right) z L-\sin{\left( {{theta}_1}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_1}\left( x,y,z\right) \right) y L-a\, \sin{\left( {{theta}_1}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_1}\left( x,y,z\right) \right) L+\sin{\left( {{theta}_1}\left( x,y,z\right) \right) } L+z\right) \\ 296 | \left( \frac{d}{d x} {{theta}_2}\left( x,y,z\right) \right) \left( \sqrt{3} b\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) }+c\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) }+\sqrt{3} x\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) }+\sin{\left( {{theta}_2}\left( x,y,z\right) \right) } y+2 \cos{\left( {{theta}_2}\left( x,y,z\right) \right) } z\right) L-\sqrt{3}\, \cos{\left( {{theta}_2}\left( x,y,z\right) \right) } L+2 x+2 b & 2 \cos{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_2}\left( x,y,z\right) \right) z L+\sin{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_2}\left( x,y,z\right) \right) y L+\sqrt{3} x\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_2}\left( x,y,z\right) \right) L+c\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_2}\left( x,y,z\right) \right) L+\sqrt{3} b\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_2}\left( x,y,z\right) \right) L-\cos{\left( {{theta}_2}\left( x,y,z\right) \right) } L+2 y+2 c & 2 \cos{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_2}\left( x,y,z\right) \right) z L+\sin{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_2}\left( x,y,z\right) \right) y L+\sqrt{3} x\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_2}\left( x,y,z\right) \right) L+c\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_2}\left( x,y,z\right) \right) L+\sqrt{3} b\, \sin{\left( {{theta}_2}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_2}\left( x,y,z\right) \right) L+2 \sin{\left( {{theta}_2}\left( x,y,z\right) \right) } L+2 z\\ 297 | \left( \frac{d}{d x} {{theta}_3}\left( x,y,z\right) \right) \left( \sqrt{3} b\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) }+c\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) }-\sqrt{3} x\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) }+\sin{\left( {{theta}_3}\left( x,y,z\right) \right) } y+2 \cos{\left( {{theta}_3}\left( x,y,z\right) \right) } z\right) L+\sqrt{3}\, \cos{\left( {{theta}_3}\left( x,y,z\right) \right) } L+2 x-2 b & 2 \cos{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_3}\left( x,y,z\right) \right) z L+\sin{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_3}\left( x,y,z\right) \right) y L-\sqrt{3} x\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_3}\left( x,y,z\right) \right) L+c\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_3}\left( x,y,z\right) \right) L+\sqrt{3} b\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d y} {{theta}_3}\left( x,y,z\right) \right) L-\cos{\left( {{theta}_3}\left( x,y,z\right) \right) } L+2 y+2 c & 2 \cos{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_3}\left( x,y,z\right) \right) z L+\sin{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_3}\left( x,y,z\right) \right) y L-\sqrt{3} x\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_3}\left( x,y,z\right) \right) L+c\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_3}\left( x,y,z\right) \right) L+\sqrt{3} b\, \sin{\left( {{theta}_3}\left( x,y,z\right) \right) } \left( \frac{d}{d z} {{theta}_3}\left( x,y,z\right) \right) L+2 \sin{\left( {{theta}_3}\left( x,y,z\right) \right) } L+2 z\end{pmatrix}\] 298 | 299 |
300 | Created with wxMaxima. 301 | 302 | 303 | -------------------------------------------------------------------------------- /delta_documentation/wxMax_eqs/Eqs_estat_htmlimg/Eqs_estat.css: -------------------------------------------------------------------------------- 1 | 2 | /*-------------------------------------------------------- 3 | -- Created with wxMaxima version 16.12.0 -------------------------------------------------------- */ 4 | 5 | body { 6 | background-color: rgb(255,255,255); 7 | } 8 | .input { 9 | color: 10 | rgb(0,0,255); 11 | } 12 | .comment { 13 | color: rgb(0,0,0); 14 | background-color: rgb(255,255,255); 15 | padding: 2mm; 16 | } 17 | .code_variable { 18 | color: 19 | rgb(0,128,0); 20 | } 21 | .code_function { 22 | 23 | color: rgb(128,0,0); 24 | } 25 | .code_comment { 26 | color: rgb(64,64,64); 27 | } 28 | .code_number { 29 | color: rgb(128,64,0); 30 | } 31 | .code_string { 32 | color: rgb(0,0,128); 33 | } 34 | .code_operator { 35 | color: rgb(0,0,128); 36 | } 37 | .code_endofline { 38 | color: rgb(192,192,192);} 39 | img { 40 | -ms-interpolation-mode: bicubic; 41 | } 42 | .image { 43 | color: rgb(0,0,0); 44 | padding: 2mm; 45 | } 46 | .section { 47 | color: rgb(0,0,0); 48 | font-size: 1.5em; 49 | padding: 2mm; 50 | } 51 | .subsect { 52 | color: rgb(0,0,0); 53 | font-size: 1.2em; 54 | padding: 2mm; 55 | } 56 | .subsubsect { 57 | color: rgb(0,0,0); 58 | font-size: 1.2em; 59 | padding: 2mm; 60 | } 61 | .title { 62 | color: rgb(0,0,0); 63 | font-size: 2em; 64 | padding: 2mm; 65 | } 66 | .prompt { 67 | color: rgb(255,0,0); 68 | } 69 | table { 70 | border: 0px; 71 | } 72 | td { 73 | vertical-align: top; 74 | padding: 1mm; 75 | } 76 | -------------------------------------------------------------------------------- /delta_documentation/wxMax_eqs/Eqs_estat_htmlimg/Eqs_estat_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/serrauvic/ros_delta_robot/5940f173e6c5ca05aa4bb1cbf7bc135b6618412b/delta_documentation/wxMax_eqs/Eqs_estat_htmlimg/Eqs_estat_0.png -------------------------------------------------------------------------------- /delta_documentation/wxMax_eqs/Eqs_estat_htmlimg/Eqs_estat_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/serrauvic/ros_delta_robot/5940f173e6c5ca05aa4bb1cbf7bc135b6618412b/delta_documentation/wxMax_eqs/Eqs_estat_htmlimg/Eqs_estat_1.png -------------------------------------------------------------------------------- /delta_mechanical/Readme.txt: -------------------------------------------------------------------------------- 1 | Some explanations 2 | 3 | -------------------------------------------------------------------------------- /delta_robot_drivers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(delta_robot_drivers) 3 | 4 | #set C++11 support 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 6 | 7 | ## Find catkin macros and libraries 8 | find_package( catkin REQUIRED COMPONENTS 9 | roscpp 10 | rospy 11 | controller_manager 12 | hardware_interface 13 | joint_limits_interface 14 | transmission_interface 15 | urdf 16 | rosserial_arduino 17 | rosserial_client 18 | image_transport 19 | cv_bridge 20 | delta_robot_kinematics) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | find_package( 24 | delta_robot_kinematics REQUIRED) 25 | 26 | #source files 27 | SET(STREAMER_SRCS 28 | src/delta_angles_streamer.cpp 29 | src/delta_angles_streamer_node.cpp 30 | ) 31 | 32 | SET(HW_SRC 33 | src/delta_hw_driver.cpp 34 | ) 35 | 36 | ## The catkin_package macro generates cmake config files for your package 37 | catkin_package( 38 | CATKIN_DEPENDS 39 | roscpp 40 | rospy 41 | cv_bridge 42 | delta_robot_kinematics) 43 | 44 | add_definitions(-DROS=1) #build using ROS libraries 45 | 46 | ## Specify additional locations of header files 47 | include_directories( 48 | include 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | include_directories(${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 52 | 53 | ## Declare a cpp executable for pipol_tracker_node 54 | add_executable(delta_angles_streamer_node 55 | ${STREAMER_SRCS}) 56 | #add_dependencies(pipol_tracker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 57 | target_link_libraries(delta_angles_streamer_node ${catkin_LIBRARIES}) 58 | 59 | add_executable(delta_hw_driver_node 60 | ${HW_SRC}) 61 | #add_dependencies(pipol_tracker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 62 | target_link_libraries(delta_hw_driver_node ${catkin_LIBRARIES}) 63 | 64 | #install binaries 65 | install(TARGETS delta_angles_streamer_node delta_hw_driver_node 66 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 67 | 68 | #Share launch and configs 69 | foreach(dir launch config) 70 | install(DIRECTORY ${dir}/ 71 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 72 | endforeach(dir) 73 | -------------------------------------------------------------------------------- /delta_robot_drivers/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/serrauvic/ros_delta_robot/5940f173e6c5ca05aa4bb1cbf7bc135b6618412b/delta_robot_drivers/README.md -------------------------------------------------------------------------------- /delta_robot_drivers/include/delta_robot_drivers/delta_angles_streamer.h: -------------------------------------------------------------------------------- 1 | #ifndef DELTA_ANGLES_STREAMER_H 2 | #define DELTA_ANGLES_STREAMER_H 3 | 4 | #include "ros/ros.h" 5 | #include 6 | 7 | class DeltaAnglesStreamer 8 | { 9 | public: 10 | explicit DeltaAnglesStreamer(const double& steps); 11 | virtual ~DeltaAnglesStreamer(); 12 | 13 | void setCurrentTrajectory(const double &x, const double &y, double theta_values[]); 14 | 15 | private: 16 | /* Variables per calcular els vectors dels desplaçaments */ 17 | /* 18 | cv::Mat currentPosition_; 19 | cv::Mat directionVector_; 20 | cv::Mat uDirectionVector_; 21 | */ 22 | double steps_; 23 | cv::Mat currentPosition_ = (cv::Mat_(2,1) << 0.0, 0.0) ; 24 | cv::Mat directionVector_ = (cv::Mat_(2,1) << 0.0, 0.0) ; 25 | cv::Mat uDirectionVector_ = (cv::Mat_(2,1) << 0.0, 0.0) ; 26 | //ros node handle 27 | ros::NodeHandle nh_; 28 | ros::Publisher angles_pub_; 29 | }; 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /delta_robot_drivers/include/delta_robot_drivers/delta_hw_driver.h: -------------------------------------------------------------------------------- 1 | #ifndef DELTA_HW_DRIVER_H 2 | #define DELTA_HW_DRIVER_H 3 | 4 | // SYSTEM 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // ROS headers 12 | #include 13 | #include 14 | #include 15 | 16 | // ROS controls 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | /* 30 | class DeltaHWDriver : public hardware_interface::RobotHW 31 | { 32 | public: 33 | explicit DeltaHWDriver(); 34 | virtual ~DeltaHWDriver(); 35 | 36 | bool init(ros::NodeHandle &nh, std::string robot_name); 37 | void read(ros::Time time, ros::Duration period); 38 | void write(ros::Time time, ros::Duration period); 39 | 40 | private: 41 | 42 | //void updateMsr(const std_msgs::UInt16::ConstPtr& msg); 43 | 44 | int n_joints_; 45 | std::vector joint_names_; 46 | 47 | // limits 48 | std::vector 49 | joint_lower_limits_, 50 | joint_upper_limits_; 51 | 52 | // state and commands 53 | std::vector 54 | joint_position_, 55 | joint_position_prev_, 56 | joint_velocity_, 57 | joint_velocity_command_; 58 | 59 | // aux var to update the angle 60 | double angle_; 61 | 62 | // sensor calibration 63 | double gain_; 64 | double bias_; 65 | 66 | // set all members to default values 67 | void reset(); 68 | 69 | // transmissions in this plugin's scope 70 | std::vector transmissions_; 71 | 72 | } 73 | */ 74 | #endif 75 | -------------------------------------------------------------------------------- /delta_robot_drivers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | delta_robot_drivers 4 | 1.0.0 5 | 6 | Node for driveing delta robot. 7 | 8 | 9 | 10 | Sebastián Serra Landete 11 | José Fernández Mola 12 | David Vives Colom 13 | Gabriel Felez Palacios 14 | Sergi Fabrego i Serrat 15 | Xavier Ortiz-Quintana Escardivol 16 | 17 | 18 | BSD 19 | 20 | 21 | 22 | 23 | 24 | Sebastián Serra Landete 25 | José Fernández Mola 26 | David Vives Colom 27 | Gabriel Felez Palacios 28 | Sergi Fabrego i Serrat 29 | Xavier Ortiz-Quintana Escardivol 30 | 31 | 32 | catkin 33 | 34 | roscpp 35 | rospy 36 | cv_bridge 37 | delta_robot_kinematics 38 | 39 | roscpp 40 | rospy 41 | cv_bridge 42 | delta_robot_kinematics 43 | 44 | 45 | -------------------------------------------------------------------------------- /delta_robot_drivers/src/delta_angles_streamer.cpp: -------------------------------------------------------------------------------- 1 | #include "delta_robot_drivers/delta_angles_streamer.h" 2 | #include "delta_robot_kinematics/delta_kinematics.h" 3 | 4 | #include "ros/ros.h" 5 | #include "std_msgs/UInt16MultiArray.h" 6 | #include "std_msgs/Float64.h" 7 | #include 8 | #include 9 | 10 | 11 | DeltaAnglesStreamer::DeltaAnglesStreamer(const double& steps) 12 | : steps_(steps), nh_(ros::this_node::getName()) 13 | /*: 14 | currentPosition_(cv::Mat(2,1) << 0.0, 0.0) 15 | , directionVector_(cv::Mat(2,1) << 0.0, 0.0) 16 | , uDirectionVector_(cv::Mat(2,1) << 0.0, 0.0) 17 | */ 18 | { 19 | angles_pub_ = nh_.advertise("trajectory_angles", 1); 20 | } 21 | DeltaAnglesStreamer::~DeltaAnglesStreamer() 22 | { 23 | } 24 | void DeltaAnglesStreamer::setCurrentTrajectory(const double &x, const double &y, double theta_values[]) 25 | { 26 | //double theta_values [3] = {0, 0, 0}; 27 | double z = -0.25; 28 | /* Aquí s'entra en matèria, es crea el vector "posició final" en base al topic subscrit */ 29 | cv::Mat goalPosition = (cv::Mat_(2,1) << x, y); 30 | /* El vector director serà, per tant, l'extrem menys la posició actual*/ 31 | directionVector_ = goalPosition ;//- currentPosition_; //TXEMA 32 | /* Aquest vector director no es pot aplicar a saco, cal fer-ho en trossos petits cada cert temps. 33 | Les següents línies miren si el vector director és prou petit. Si ho és, ja l'aplica tot. Per 34 | comprovar-ho, mira la seva distància i li aplica el valor absolut*/ 35 | if (fabs(cv::norm(directionVector_, cv::NORM_L2)) < steps_) 36 | { 37 | uDirectionVector_ = cv::Mat_::zeros(2, 1); 38 | //currentPosition_ = goalPosition; //TXEMA 39 | } 40 | /* Però la majoria de vegades caldrà anar partint la trajectòria. Per partir-la, es calcula 41 | primer el vector unitari que es correspon a la direcció del vector director. El vector unitari, 42 | és el mateix vector en direcció i sentit, però el seu mòdul val 1. Quan es multiplica les components 43 | d'aquest vector per un valor, el mòdul passa a ser aquest valor.*/ 44 | else 45 | { 46 | //Calcula el vector unitari 47 | uDirectionVector_ = directionVector_ / cv::norm(directionVector_, cv::NORM_L2); 48 | //Multiplica el vector unitari per un valor petit i el suma a la posició actual 49 | currentPosition_ = currentPosition_ + directionVector_ * steps_;// Kind of proportional controller 50 | } 51 | 52 | 53 | if (currentPosition_.at(0,0) > 0.175) currentPosition_.at(0,0) = 0.175; 54 | else if (currentPosition_.at(0,0) < -0.175) currentPosition_.at(0,0) = -0.175; 55 | 56 | if (currentPosition_.at(1,0) > 0.175) currentPosition_.at(1,0) = 0.175; 57 | else if (currentPosition_.at(1,0) < -0.175) currentPosition_.at(1,0) = -0.175; 58 | 59 | ROS_INFO("========================="); 60 | ROS_INFO("Current position: %f y: %f z:%f", currentPosition_.at(0,0), currentPosition_.at(1,0), z); 61 | // IK 62 | delta_kinematics::inversekinematics( 63 | currentPosition_.at(0,0), currentPosition_.at(1,0), z, theta_values); 64 | 65 | // Rad to degrees plus offset 66 | theta_values[0] = (conversio * theta_values[0]); 67 | theta_values[1] = (conversio * theta_values[1]); 68 | theta_values[2] = (conversio * theta_values[2]); 69 | 70 | if (theta_values[0] >= 360 71 | || theta_values[1] >= 360 72 | || theta_values[2] >= 360) 73 | { 74 | return; 75 | } 76 | 77 | std_msgs::UInt16MultiArray angles; 78 | angles.layout.dim.push_back(std_msgs::MultiArrayDimension()); 79 | angles.layout.dim[0].size = 3; 80 | angles.layout.dim[0].label = "thetas"; 81 | angles.data.clear(); 82 | 83 | angles.data.push_back(theta_values[0]); 84 | angles.data.push_back(theta_values[1]); 85 | angles.data.push_back(theta_values[2]); 86 | 87 | angles_pub_.publish(angles); 88 | } 89 | -------------------------------------------------------------------------------- /delta_robot_drivers/src/delta_angles_streamer_node.cpp: -------------------------------------------------------------------------------- 1 | #include "delta_robot_drivers/delta_angles_streamer.h" 2 | 3 | #include 4 | #include "std_msgs/Float64.h" 5 | #include 6 | #include 7 | #include "geometry_msgs/Vector3.h" 8 | 9 | void setCurrentTrajectory(const geometry_msgs::Vector3::ConstPtr& circle_center, 10 | DeltaAnglesStreamer& streamer) 11 | { 12 | 13 | //ROS_INFO("Setting new trajectory x: %d y:%d", circle_center->x, circle_center->y); 14 | geometry_msgs::Vector3 new_circle_center = *circle_center; 15 | // ROS_INFO("========================="); 16 | // ROS_INFO("Setting new trajectory x: %f y: %f z:%f", new_circle_center.x, new_circle_center.y, new_circle_center.z); 17 | 18 | /*Pot passar que si el node de la càmera no existeix, el valor subscrit valdrà 19 | un Not A Number. Això és un problema si es fa càlcul numèric. Les següents linies 20 | eviten que això passi i li donen un valor conegut. 21 | */ 22 | double theta_values [3] = {0, 0, 0}; 23 | 24 | streamer.setCurrentTrajectory(new_circle_center.x, new_circle_center.y, theta_values); 25 | 26 | //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 27 | // si thetas = 360 valor fora de rang.!!!!!! NO s'han d'aplicar. 28 | //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 29 | // Guard against bad trajectory 30 | /* 31 | if (theta_values[0] == 360 32 | || theta_values[1] == 360 33 | || theta_values[2] == 360) 34 | { 35 | return; 36 | } 37 | */ 38 | ROS_INFO("Trajectory angles Theta1: %f Theta2: %f Theta3: %f" 39 | , theta_values[0], theta_values[1], theta_values[2]); 40 | 41 | ROS_INFO("========================="); 42 | } 43 | 44 | /* Els següents defines haurien d'acabar sent paràmetres al ROS launch */ 45 | #define DEF_MAX_STEP_SIZE 0.05 46 | #define DEF_EXECUTION_FREQUENCY 10 /* Hz */ 47 | 48 | int main(int argc, char** argv) 49 | { 50 | ros::init(argc, argv, "delta_angles_streamer_node"); 51 | ros::NodeHandle nh; 52 | ros::NodeHandle pnh("~"); 53 | 54 | double max_steps, exec_freq; 55 | std::string ip_addr; 56 | pnh.param("max_steps", max_steps, DEF_MAX_STEP_SIZE); 57 | pnh.param("exec_freq", exec_freq, DEF_EXECUTION_FREQUENCY); 58 | 59 | ros::AsyncSpinner spinner(1); 60 | 61 | spinner.start(); 62 | 63 | ros::Rate loop_rate(exec_freq); 64 | 65 | DeltaAnglesStreamer streamer(max_steps); 66 | 67 | ros::Subscriber coord = 68 | nh.subscribe("/delta_img_processor/center_ray_direction", 1000, 69 | boost::bind(setCurrentTrajectory, _1, boost::ref(streamer))); 70 | 71 | while (ros::ok()) 72 | { 73 | //execute pending callbacks 74 | //ros::spinOnce(); 75 | loop_rate.sleep(); 76 | } 77 | 78 | //ros::waitForShutdown(); 79 | //ros::spin(); 80 | return 0; 81 | } 82 | -------------------------------------------------------------------------------- /delta_robot_drivers/src/delta_hw_driver.cpp: -------------------------------------------------------------------------------- 1 | // SYSTEM 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // ROS headers 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // ROS controls 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "std_msgs/UInt16MultiArray.h" 31 | 32 | 33 | namespace fan_hwiface 34 | { 35 | // For simulation only - determines how fast a trajectory is followed 36 | static const double POSITION_STEP_FACTOR = 10; 37 | 38 | class DeltaHWDriver : public hardware_interface::RobotHW 39 | { 40 | public: 41 | 42 | DeltaHWDriver() {} 43 | virtual ~DeltaHWDriver() {} 44 | 45 | std::string robot_namespace_; 46 | 47 | std::string urdf_string_; 48 | urdf::Model urdf_model_; 49 | 50 | 51 | realtime_tools::RealtimePublisher *pub_commands_; 52 | ros::Subscriber sub_measures_; 53 | 54 | ros::Subscriber angles_sub_; 55 | ros::Subscriber arduino_feedback_angles_sub_; 56 | 57 | double thetas_[3] = {0, 0, 0}; 58 | double feed_back_thetas_[3] = {0, 0, 0}; 59 | 60 | bool init(ros::NodeHandle &nh, std::string robot_name) 61 | { 62 | n_joints_ = 3; 63 | 64 | loop_hz_ = 0.1; 65 | 66 | pub_commands_ = new realtime_tools::RealtimePublisher(nh, "arduino_cmd", 1); 67 | //sub_measures_ = nh.subscribe("potentiometer_msr", 1000, &DeltaHWDriver::updateMsr, this) ; 68 | 69 | angles_sub_ = nh.subscribe("/delta_robot_drivers/trajectory_angles", 1000, &DeltaHWDriver::updateAngles, this) ; 70 | 71 | arduino_feedback_angles_sub_= nh.subscribe("/arduino/angles", 1000, &DeltaHWDriver::updateArduinoAngles, this) ; 72 | 73 | 74 | nh.getParam("gain", gain_); 75 | nh.getParam("bias", bias_); 76 | 77 | robot_namespace_ = robot_name; 78 | joint_names_.push_back( std::string("link_0_JOINT_1") ); 79 | joint_names_.push_back( std::string("link_0_JOINT_2") ); 80 | joint_names_.push_back( std::string("link_0_JOINT_3") ); 81 | 82 | joint_position_.resize(n_joints_); 83 | joint_position_prev_.resize(n_joints_); 84 | joint_velocity_.resize(n_joints_); 85 | joint_velocity_command_.resize(n_joints_); 86 | 87 | joint_lower_limits_.resize(n_joints_); 88 | joint_upper_limits_.resize(n_joints_); 89 | 90 | joint_position_command_.resize(n_joints_); 91 | 92 | for (int j = 0; j < n_joints_; ++j) 93 | { 94 | joint_position_[j] = 0.0; 95 | joint_position_prev_[j] = 0.0; 96 | joint_velocity_[j] = 0.0; 97 | joint_velocity_command_[j] = 0.0; 98 | joint_position_command_[j] = 0.0; 99 | } 100 | 101 | for(int j=0; j < n_joints_; j++) 102 | { 103 | state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_velocity_[j])); 104 | 105 | // Create position joint interface 106 | position_joint_interface_.registerHandle(hardware_interface::JointHandle(state_interface_.getHandle(joint_names_[j]),&joint_position_command_[j])); 107 | 108 | 109 | //hardware_interface::JointHandle joint_handle_velocity; 110 | //joint_handle_velocity = hardware_interface::JointHandle(state_interface_.getHandle(joint_names_[j]), &joint_velocity_command_[j]); 111 | //velocity_interface_.registerHandle(joint_handle_velocity); 112 | } 113 | 114 | registerInterface(&state_interface_); 115 | registerInterface(&position_joint_interface_); 116 | //registerInterface(&velocity_interface_); 117 | 118 | return true; 119 | }; 120 | 121 | void read(ros::Time time, ros::Duration period) 122 | { 123 | //ROS_INFO("Read from arduino"); 124 | 125 | joint_position_prev_[0] = joint_position_[0]; 126 | joint_position_command_[0] = joint_position_[0]; 127 | joint_position_[0] = thetas_[0];// -10; 128 | 129 | joint_position_prev_[1] = joint_position_[1]; 130 | joint_position_command_[1] = joint_position_[1]; 131 | joint_position_[1] = thetas_[1];// -11; 132 | 133 | joint_position_prev_[2] = joint_position_[2]; 134 | joint_position_command_[2] = joint_position_[2]; 135 | joint_position_[2] = thetas_[2];// -12; 136 | /* 137 | joint_position_command_[0] = thetas_[0]; 138 | joint_position_command_[1] = thetas_[1]; 139 | joint_position_command_[2] = thetas_[2]; 140 | */ 141 | return; 142 | }; 143 | 144 | void write(ros::Time time, ros::Duration period) 145 | { 146 | //ROS_INFO("Write to arduino"); 147 | if (pub_commands_->trylock()) 148 | { 149 | std_msgs::UInt16MultiArray angles; 150 | angles.layout.dim.push_back(std_msgs::MultiArrayDimension()); 151 | angles.layout.dim[0].size = 3; 152 | angles.layout.dim[0].label = "thetas"; 153 | angles.data.clear(); 154 | 155 | pub_commands_->msg_.data.clear(); 156 | // Move all the states to the commanded set points slowly 157 | for (std::size_t i = 0; i < n_joints_; ++i) 158 | { 159 | //ROS_INFO("Joint joint_position_command_ %d = %f", i, joint_position_command_[i]); 160 | //ROS_INFO("Joint joint_position_ %d = %f", i, joint_position_[i]); 161 | // Position 162 | p_error_ = joint_position_command_[i] - joint_position_[i]; 163 | // scale the rate it takes to achieve position by a factor that is invariant to the feedback loop 164 | joint_position_[i] += p_error_ * POSITION_STEP_FACTOR / loop_hz_; 165 | 166 | //ROS_INFO("Joint joint_position_command_ %d = %f", i, joint_position_command_[i]); 167 | //ROS_INFO("Joint joint_position_ %d = %f", i, joint_position_[i]); 168 | //ROS_INFO("Joint joint_position_ ERROR %d = %f", i, p_error_); 169 | 170 | } 171 | //ROS_INFO("Joint joint_position_ %d = %f", 0, joint_position_command_[0]); 172 | angles.data.push_back(joint_position_command_[0]); 173 | angles.data.push_back(joint_position_command_[1]); 174 | angles.data.push_back(joint_position_command_[2]); 175 | pub_commands_->msg_ = angles; 176 | 177 | /* 178 | // only actuate in the correct direction 179 | double fan_speed = ( ( joint_velocity_command_.at(1) ) ); 180 | if( fan_speed > 0.0 ) 181 | fan_speed = 0; 182 | fan_speed = std::abs(fan_speed); 183 | pub_commands_->msg_.data = static_cast(1023*fan_speed/5); 184 | if( pub_commands_->msg_.data > 255 ) 185 | pub_commands_->msg_.data = 255; 186 | */ 187 | //ROS_INFO("pub_command %f", pub_commands_->msg_.data); 188 | 189 | pub_commands_->unlockAndPublish(); 190 | } 191 | return; 192 | }; 193 | 194 | // hardware interfaces 195 | hardware_interface::JointStateInterface state_interface_; 196 | hardware_interface::VelocityJointInterface velocity_interface_; 197 | hardware_interface::PositionJointInterface position_joint_interface_; 198 | 199 | 200 | // joint limits interfaces 201 | joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; 202 | joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; 203 | joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; 204 | joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; 205 | 206 | // Before write, you can use this function to enforce limits for all values 207 | void enforceLimits(ros::Duration period); 208 | 209 | // configuration 210 | int n_joints_; 211 | std::vector joint_names_; 212 | 213 | // limits 214 | std::vector 215 | joint_lower_limits_, 216 | joint_upper_limits_; 217 | 218 | // state and commands 219 | std::vector 220 | joint_position_, 221 | joint_position_prev_, 222 | joint_velocity_, 223 | joint_velocity_command_, 224 | joint_position_command_; 225 | 226 | // aux var to update the angle 227 | double angle_; 228 | 229 | // sensor calibration 230 | double gain_; 231 | double bias_; 232 | 233 | // set all members to default values 234 | void reset(); 235 | 236 | // transmissions in this plugin's scope 237 | std::vector transmissions_; 238 | 239 | double p_error_, v_error_, e_error_; 240 | double loop_hz_; 241 | 242 | private: 243 | 244 | void updateMsr(const std_msgs::UInt16::ConstPtr& msg) 245 | { 246 | angle_ = gain_* msg->data + bias_; 247 | } 248 | void updateAngles(const std_msgs::UInt16MultiArray::ConstPtr& angles_msg_ptr) 249 | { 250 | std_msgs::UInt16MultiArray angles_msg = *angles_msg_ptr; 251 | 252 | ROS_INFO("Received new angles to send: %d, %d, %d", angles_msg.data[0], angles_msg.data[1], angles_msg.data[2]); 253 | 254 | thetas_[0] = angles_msg.data[0]; 255 | thetas_[1] = angles_msg.data[1]; 256 | thetas_[2] = angles_msg.data[2]; 257 | 258 | 259 | } 260 | void updateArduinoAngles(const std_msgs::UInt16MultiArray::ConstPtr& angles_msg_ptr) 261 | { 262 | std_msgs::UInt16MultiArray angles_msg = *angles_msg_ptr; 263 | 264 | ROS_INFO("Received Arduino angles: %d, %d, %d", angles_msg.data[0], angles_msg.data[1], angles_msg.data[2]); 265 | 266 | feed_back_thetas_[0] = angles_msg.data[0]; 267 | feed_back_thetas_[1] = angles_msg.data[1]; 268 | feed_back_thetas_[2] = angles_msg.data[2]; 269 | } 270 | 271 | 272 | }; // class 273 | 274 | } // namespace 275 | 276 | bool g_quit = false; 277 | 278 | void quitRequested(int sig) 279 | { 280 | g_quit = true; 281 | } 282 | 283 | int main( int argc, char** argv ) 284 | { 285 | // initialize ROS 286 | ros::init(argc, argv, "delta_robot_driver_node", ros::init_options::NoSigintHandler); 287 | 288 | // ros spinner 289 | ros::AsyncSpinner spinner(1); 290 | spinner.start(); 291 | 292 | // custom signal handlers 293 | signal(SIGTERM, quitRequested); 294 | signal(SIGINT, quitRequested); 295 | signal(SIGHUP, quitRequested); 296 | 297 | // create a node 298 | ros::NodeHandle copter_arm_nh; 299 | 300 | /* 301 | ros::NodeHandle nh; 302 | 303 | ros::Subscriber angles = 304 | nh.subscribe("/delta_img_processor/center_ray_direction", 1, 305 | boost::bind(setCurrentTrajectory, _1, boost::ref(streamer))); 306 | */ 307 | 308 | // get params or give default values 309 | std::string name; 310 | copter_arm_nh.param("name", name, std::string("delta_robot")); 311 | 312 | // advertise the e-stop topic 313 | // ros::Subscriber estop_sub = copter_arm_nh.subscribe(copter_arm_nh.resolveName("emergency_stop"), 1, eStopCB); 314 | 315 | // get the general robot description, the lwr class will take care of parsing what's useful to itself 316 | // std::string urdf_string = getURDF(copter_arm_nh, "/robot_description"); 317 | 318 | // construct and start the real lwr 319 | fan_hwiface::DeltaHWDriver copter_arm_hwiface; 320 | // copter_arm_hwiface.create(name, urdf_string); 321 | if(!copter_arm_hwiface.init(copter_arm_nh, "delta_robot"/*name*/)) 322 | { 323 | ROS_FATAL_NAMED("lwr_hw","Could not initialize robot real interface"); 324 | return -1; 325 | } 326 | 327 | // timer variables 328 | struct timespec ts = {0, 0}; 329 | ros::Time last(ts.tv_sec, ts.tv_nsec), now(ts.tv_sec, ts.tv_nsec); 330 | ros::Duration period(1.0); 331 | 332 | //the controller manager 333 | controller_manager::ControllerManager manager(&copter_arm_hwiface, copter_arm_nh); 334 | 335 | ros::Rate rate(100); 336 | while( !g_quit ) 337 | { 338 | // get the time / period 339 | if (!clock_gettime(CLOCK_REALTIME, &ts)) 340 | { 341 | now.sec = ts.tv_sec; 342 | now.nsec = ts.tv_nsec; 343 | period = now - last; 344 | last = now; 345 | } 346 | else 347 | { 348 | ROS_FATAL("Failed to poll realtime clock!"); 349 | break; 350 | } 351 | 352 | // read the state from the lwr 353 | copter_arm_hwiface.read(ros::Time::now(), period); 354 | 355 | // Compute the controller commands 356 | bool resetControllers = true; 357 | /*if(!wasStopHandled && !resetControllers) 358 | { 359 | ROS_WARN("E-STOP HAS BEEN PRESSED: Controllers will be restarted, but the robot won't move until you release the E-Stop"); 360 | ROS_WARN("HOW TO RELEASE E-STOP: rostopic pub -r 10 /NAMESPACE/emergency_stop std_msgs/Bool 'data: false'"); 361 | resetControllers = true; 362 | wasStopHandled = true; 363 | } 364 | 365 | if( isStopPressed ) 366 | { 367 | wasStopHandled = false; 368 | } 369 | else 370 | { 371 | resetControllers = false; 372 | wasStopHandled = true; 373 | } */ 374 | 375 | // update the controllers 376 | manager.update(ros::Time::now(), period, resetControllers); 377 | 378 | // write the command to the lwr 379 | copter_arm_hwiface.write(ros::Time::now(), period); 380 | 381 | rate.sleep(); 382 | } 383 | 384 | std::cerr<<"Stopping spinner..."< 2 | 3 | delta_robot_firmware 4 | 1.0.0 5 | The delta_robot_arduino_firmware package 6 | 7 | 8 | Sebastián Serra Landete 9 | José Fernández Mola 10 | David Vives Colom 11 | Gabriel Felez Palacios 12 | Sergi Fabrego i Serrat 13 | Xavier Ortiz-Quintana Escardivol 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | Sebastián Serra Landete 23 | José Fernández Mola 24 | David Vives Colom 25 | Gabriel Felez Palacios 26 | Sergi Fabrego i Serrat 27 | Xavier Ortiz-Quintana Escardivol 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | catkin 48 | roscpp 49 | rospy 50 | std_msgs 51 | roscpp 52 | rospy 53 | std_msgs 54 | trajectory_msgs 55 | pr2_controllers_msgs 56 | actionlib 57 | joint_state_controller 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /delta_robot_firmware/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | include_directories( 4 | ${ROS_LIB_DIR} 5 | ) 6 | 7 | # add_definitions(-DUSB_CON) 8 | 9 | print_board_list() 10 | print_programmer_list() 11 | 12 | # Something is wrong that it requires the time.cpp for time conversions 13 | generate_arduino_firmware(delta_arduino 14 | SRCS firmware.cpp ${ROS_LIB_DIR}/time.cpp 15 | BOARD uno 16 | PORT /dev/ttyACM0 17 | PROGRAMMER avrispmkii 18 | ) 19 | -------------------------------------------------------------------------------- /delta_robot_firmware/src/firmware.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * rosserial Servo Control Example 3 | * 4 | * This sketch demonstrates the control of hobby R/C servos 5 | * using ROS and the arduiono 6 | * 7 | * For the full tutorial write up, visit 8 | * www.ros.org/wiki/rosserial_arduino_demos 9 | * 10 | * For more information on the Arduino Servo Library 11 | * Checkout : 12 | * http://www.arduino.cc/en/Reference/Servo 13 | */ 14 | 15 | #if (ARDUINO >= 100) 16 | #include 17 | #else 18 | #include 19 | #endif 20 | 21 | #include 22 | #include 23 | #include 24 | #include "std_msgs/UInt16MultiArray.h" 25 | 26 | ros::NodeHandle nh; 27 | 28 | Servo servo_j1; 29 | Servo servo_j2; 30 | Servo servo_j3; 31 | 32 | std_msgs::UInt16MultiArray angles_; 33 | 34 | void servo_cb( const std_msgs::UInt16MultiArray& cmd_msg){ 35 | 36 | servo_j1.write(cmd_msg.data[0]); //set servo angle, should be from 0-180 37 | servo_j2.write(cmd_msg.data[1]); //set servo angle, should be from 0-180 38 | servo_j3.write(cmd_msg.data[2]); //set servo angle, should be from 0-180 39 | 40 | digitalWrite(13, HIGH-digitalRead(13)); //toggle led 41 | } 42 | 43 | 44 | ros::Subscriber sub("/delta_robot/arduino_cmd", servo_cb); 45 | //ros::Publisher pub_angle_("/arduino/angles", &angles_); 46 | 47 | void setup(){ 48 | pinMode(13, OUTPUT); 49 | 50 | nh.initNode(); 51 | nh.subscribe(sub); 52 | //nh.advertise(pub_angle_); 53 | 54 | servo_j1.attach(9); //attach it to pin 9 55 | servo_j2.attach(10); //attach it to pin 10 56 | servo_j3.attach(11); //attach it to pin 11 57 | } 58 | 59 | void loop(){ 60 | //pub_angle_.publish(&angles_); 61 | nh.spinOnce(); 62 | delay(1); 63 | } 64 | -------------------------------------------------------------------------------- /delta_robot_img_processor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(delta_robot_img_processor) 3 | 4 | #set C++11 support 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 6 | 7 | ## Find catkin macros and libraries 8 | find_package( catkin REQUIRED COMPONENTS 9 | roscpp 10 | rospy 11 | image_transport 12 | cv_bridge) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | find_package(OpenCV REQUIRED) 16 | 17 | #source files 18 | SET(SRCS 19 | src/ros_img_processor.cpp 20 | src/delta_img_processor_node.cpp) 21 | 22 | ## The catkin_package macro generates cmake config files for your package 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | rospy 27 | image_transport 28 | cv_bridge) 29 | 30 | add_definitions(-DROS=1) #build using ROS libraries 31 | 32 | ## Specify additional locations of header files 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ) 37 | include_directories(${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 38 | 39 | ## Declare a cpp executable for pipol_tracker_node 40 | add_executable(delta_robot_img_processor_node 41 | ${SRCS}) 42 | #add_dependencies(pipol_tracker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 43 | target_link_libraries(delta_robot_img_processor_node ${OpenCV_LIBS} ${catkin_LIBRARIES}) 44 | 45 | #install binaries 46 | install(TARGETS delta_robot_img_processor_node 47 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 48 | 49 | #Share launch and configs 50 | foreach(dir launch config) 51 | install(DIRECTORY ${dir}/ 52 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 53 | endforeach(dir) 54 | -------------------------------------------------------------------------------- /delta_robot_img_processor/README.md: -------------------------------------------------------------------------------- 1 | ## Delta robot image processor 2 | -------------------------------------------------------------------------------- /delta_robot_img_processor/include/delta_robot_img_processor/camera.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_H 2 | #define CAMERA_H 3 | 4 | //OpenCV 5 | #include "opencv2/opencv.hpp" 6 | #include "opencv2/core.hpp" 7 | 8 | //TODO: In a future it could be a class for now with an inlined function it's enough. 9 | namespace Camera 10 | { 11 | // calculate rady direction from Hough detected ciercle using the camera K (Intrinsic calibration matrix) 12 | // d = K^-1 * u. 13 | // where u is the center of the circle. 14 | void get_ray_direction(const cv::Mat &matrixK, const cv::Point ¢er, cv::Mat &ray_direction) 15 | { 16 | // Make room for the inverted K matrix. 17 | cv::Mat kinverted; 18 | // Inver the K matrix. 19 | cv::invert(matrixK, kinverted, cv::DECOMP_LU); 20 | // Make room and initialize ray direction value. 21 | ray_direction = (cv::Mat_(3,1) << 0, 0, 0); 22 | // Homogenous point (give it a third coordinate equal to 1). 23 | cv::Mat homogenous_point = (cv::Mat_(3,1) << center.x, center.y, 1.0); 24 | // Put in world coordinates the circle center. 25 | ray_direction = kinverted * homogenous_point; 26 | 27 | }; 28 | } // Camera 29 | #endif 30 | -------------------------------------------------------------------------------- /delta_robot_img_processor/include/delta_robot_img_processor/circle_detector.h: -------------------------------------------------------------------------------- 1 | #ifndef CIRCLE_DETECTOR_H 2 | #define CIRCLE_DETECTOR_H 3 | 4 | //OpenCV 5 | #include "opencv2/opencv.hpp" 6 | #include "opencv2/core.hpp" 7 | 8 | //TODO: In a future it could be a class for now with an inlined function it's enough. 9 | namespace Hough_Transform 10 | { 11 | //constants 12 | const int DEF_GAUSSIAN_BLUR_SIZE = 11; 13 | const double DEF_GAUSSIAN_BLUR_SIGMA = 2; 14 | const double DEF_CANNY_EDGE_TH = 150; 15 | const double DEF_HOUGH_ACCUM_RESOLUTION = 2; 16 | const double DEF_MIN_CIRCLE_DIST = 30; 17 | const double DEF_HOUGH_ACCUM_TH = 70; 18 | const int DEF_MIN_RADIUS = 20; 19 | const int DEF_MAX_RADIUS = 100; 20 | 21 | void calculate(cv::Mat &image, std::vector & circles) 22 | { 23 | cv::Point center; 24 | cv::Mat gray_image; 25 | 26 | //clear previous circles 27 | circles.clear(); 28 | // If input image is RGB, convert it to gray 29 | cv::cvtColor(image, gray_image, CV_BGR2GRAY); 30 | //Reduce the noise so we avoid false circle detection 31 | cv::GaussianBlur( gray_image, gray_image, cv::Size(DEF_GAUSSIAN_BLUR_SIZE, DEF_GAUSSIAN_BLUR_SIZE), DEF_GAUSSIAN_BLUR_SIGMA ); 32 | //Apply the Hough Transform to find the circles 33 | cv::HoughCircles( gray_image, circles, CV_HOUGH_GRADIENT, DEF_HOUGH_ACCUM_RESOLUTION, DEF_MIN_CIRCLE_DIST, DEF_CANNY_EDGE_TH, DEF_HOUGH_ACCUM_TH, DEF_MIN_RADIUS, DEF_MAX_RADIUS ); 34 | 35 | }; 36 | 37 | void calculate(cv::Mat &image, std::vector & circles, int gaussian_blur_size, double gaussian_blur_sigma, double accum_resolution, double cirlce_dist, double canny, double accum_th, int min_rad, int max_rad) 38 | { 39 | cv::Point center; 40 | cv::Mat gray_image; 41 | 42 | //clear previous circles 43 | circles.clear(); 44 | // If input image is RGB, convert it to gray 45 | cv::cvtColor(image, gray_image, CV_BGR2GRAY); 46 | //Reduce the noise so we avoid false circle detection 47 | cv::GaussianBlur( gray_image, gray_image, cv::Size(gaussian_blur_size, gaussian_blur_size), gaussian_blur_sigma ); 48 | //Apply the Hough Transform to find the circles 49 | cv::HoughCircles( gray_image, circles, CV_HOUGH_GRADIENT, accum_resolution, cirlce_dist, canny, accum_th, min_rad, max_rad ); 50 | 51 | }; 52 | } // Hough 53 | namespace Hough_Circle 54 | { 55 | // Reads center coordinates from a hough detected circle. 56 | void get_center_coordinates(const cv::Vec3f &circle, cv::Point ¢er, int &radius) 57 | { 58 | //Read center of the circle from Vec3f 59 | center = cv::Point(cvRound(circle[0]), cvRound(circle[1])); 60 | //Read radius of the center from Vec3f. 61 | radius = cvRound(circle[2]); 62 | }; 63 | } // Hough_Circle 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /delta_robot_img_processor/include/delta_robot_img_processor/ros_img_processor.h: -------------------------------------------------------------------------------- 1 | #ifndef ROS_IMG_PROCESSOR_H 2 | #define ROS_IMG_PROCESSOR_H 3 | 4 | //std C++ 5 | #include 6 | 7 | //ROS headers for image I/O 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "geometry_msgs/Vector3.h" 14 | 15 | /** \brief Simple Image Processor 16 | * 17 | * Simple Image Processor with opencv calls 18 | * 19 | */ 20 | class RosImgProcessorNode 21 | { 22 | protected: 23 | //ros node handle 24 | ros::NodeHandle nh_; 25 | 26 | //image transport 27 | image_transport::ImageTransport img_tp_; 28 | 29 | // subscribers to the image and camera info topics 30 | image_transport::Subscriber image_subs_; 31 | ros::Subscriber camera_info_subs_; 32 | 33 | //publishers 34 | image_transport::Publisher image_pub_; 35 | ros::Publisher ray_direction_circle_pub; 36 | 37 | //pointer to received (in) and published (out) images 38 | cv_bridge::CvImagePtr cv_img_ptr_in_; 39 | cv_bridge::CvImage cv_img_out_; 40 | // ray direction in camera frame. 41 | cv::Mat ray_direction_; 42 | 43 | //Camera matrix 44 | cv::Mat matrixP_; 45 | cv::Mat matrixK_; 46 | 47 | //image encoding label 48 | std::string img_encoding_; 49 | 50 | //wished process rate, [hz] 51 | double rate_; 52 | 53 | protected: 54 | // callbacks 55 | void imageCallback(const sensor_msgs::ImageConstPtr& _msg); 56 | void cameraInfoCallback(const sensor_msgs::CameraInfo & _msg); 57 | void draw_clircle(const cv::Point & center, const int radius, bool draw_center_coordinates); 58 | void draw_ray_direction_vector(const cv::Point & center); 59 | 60 | public: 61 | /** \brief Constructor 62 | * 63 | * Constructor 64 | * 65 | */ 66 | RosImgProcessorNode(); 67 | 68 | /** \brief Destructor 69 | * 70 | * Destructor 71 | * 72 | */ 73 | ~RosImgProcessorNode(); 74 | 75 | /** \brief Process input image 76 | * 77 | * Process input image 78 | * 79 | **/ 80 | void process(); 81 | 82 | /** \brief Publish output image 83 | * 84 | * Publish output image 85 | * 86 | */ 87 | void publish(); 88 | 89 | /** \brief Returns rate_ 90 | * 91 | * Returns rate_ 92 | * 93 | **/ 94 | double getRate() const; 95 | }; 96 | #endif 97 | -------------------------------------------------------------------------------- /delta_robot_img_processor/launch/delta_robot_img_processor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 30 | 31 | 32 | 33 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 50 | 51 | 52 | 53 | 59 | 60 | 61 | 62 | 63 | 64 | 67 | 68 | -------------------------------------------------------------------------------- /delta_robot_img_processor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | delta_robot_img_processor 4 | 1.0.0 5 | 6 | Delta robot node for receiving image to detect circels for delta trajectory planning. 7 | 8 | 9 | 10 | Sebastián Serra Landete 11 | José Fernández Mola 12 | David Vives Colom 13 | Gabriel Felez Palacios 14 | Sergi Fabrego i Serrat 15 | Xavier Ortiz-Quintana Escardivol 16 | 17 | 18 | BSD 19 | 20 | 21 | 22 | 23 | 24 | Sebastián Serra Landete 25 | José Fernández Mola 26 | David Vives Colom 27 | Gabriel Felez Palacios 28 | Sergi Fabrego i Serrat 29 | Xavier Ortiz-Quintana Escardivol 30 | 31 | 32 | catkin 33 | 34 | roscpp 35 | rospy 36 | image_transport 37 | cv_bridge 38 | 39 | roscpp 40 | rospy 41 | image_transport 42 | cv_bridge 43 | 44 | 45 | -------------------------------------------------------------------------------- /delta_robot_img_processor/src/delta_img_processor_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | //ros dependencies 3 | #include "delta_robot_img_processor/ros_img_processor.h" 4 | 5 | //node main 6 | int main(int argc, char **argv) 7 | { 8 | //init ros 9 | ros::init(argc, argv, "delta_img_processor"); 10 | 11 | //create ros wrapper object 12 | RosImgProcessorNode imgp; 13 | 14 | //set node loop rate 15 | ros::Rate loopRate(imgp.getRate()); 16 | 17 | //node loop 18 | while ( ros::ok() ) 19 | { 20 | //execute pending callbacks 21 | ros::spinOnce(); 22 | 23 | //do things 24 | imgp.process(); 25 | 26 | //publish things 27 | imgp.publish(); 28 | 29 | //relax to fit output rate 30 | loopRate.sleep(); 31 | } 32 | 33 | //exit program 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /delta_robot_img_processor/src/ros_img_processor.cpp: -------------------------------------------------------------------------------- 1 | #include "delta_robot_img_processor/ros_img_processor.h" 2 | #include "delta_robot_img_processor/circle_detector.h" 3 | #include "delta_robot_img_processor/camera.h" 4 | 5 | //OpenCV 6 | #include "opencv2/opencv.hpp" 7 | #include "opencv2/core.hpp" 8 | #include "opencv2/imgproc.hpp" 9 | 10 | RosImgProcessorNode::RosImgProcessorNode() : 11 | nh_(ros::this_node::getName()), 12 | img_tp_(nh_) 13 | { 14 | //loop rate [hz], Could be set from a yaml file 15 | rate_=10; 16 | 17 | //sets publishers 18 | image_pub_ = img_tp_.advertise("image_out", 100); 19 | ray_direction_circle_pub = nh_.advertise("center_ray_direction", 1); 20 | 21 | ray_direction_ = (cv::Mat_(3,1) << 0, 0, 0) ; 22 | 23 | //sets subscribers 24 | image_subs_ = img_tp_.subscribe("image_in", 1, &RosImgProcessorNode::imageCallback, this); 25 | camera_info_subs_ = nh_.subscribe("camera_info_in", 100, &RosImgProcessorNode::cameraInfoCallback, this); 26 | } 27 | 28 | RosImgProcessorNode::~RosImgProcessorNode() 29 | { 30 | // 31 | } 32 | 33 | void RosImgProcessorNode::process() 34 | { 35 | cv::Rect_ box; 36 | 37 | //check if new image is there 38 | if ( cv_img_ptr_in_ != nullptr ) 39 | { 40 | //copy the input image to the out one 41 | cv_img_out_.image = cv_img_ptr_in_->image; 42 | // detected circles- 43 | std::vector circles; 44 | 45 | // detect circles in the image. 46 | Hough_Transform::calculate(cv_img_out_.image, circles); 47 | 48 | //draw circles on the image 49 | for(unsigned int ii = 0; ii < circles.size(); ii++ ) 50 | { 51 | // if valid circle. 52 | if ( circles[ii][0] != -1 ) 53 | { 54 | // center of the circle. 55 | cv::Point center; 56 | // radius of the circle. 57 | int radius; 58 | 59 | // get cirlce coodinates, center point and radius. 60 | Hough_Circle::get_center_coordinates(circles[ii], center, radius); 61 | // draw circle. 62 | draw_clircle(center, radius, true/*draw circle center coordinates*/); 63 | // calculate center circle ray direction from camera frame persepctive, 64 | // put the circle center point in the real world. 65 | Camera::get_ray_direction(matrixK_, center, ray_direction_); 66 | // draw vector. 67 | //draw_ray_direction_vector(center); 68 | } 69 | } 70 | 71 | //sets and draw a bounding box around the ball 72 | //box.x = (cv_img_ptr_in_->image.cols/2)-10; 73 | //box.y = (cv_img_ptr_in_->image.rows/2)-10; 74 | //box.width = 20; 75 | //box.height = 20; 76 | //cv::rectangle(cv_img_out_.image, box, cv::Scalar(0,255,255), 3); 77 | } 78 | 79 | //reset input image 80 | cv_img_ptr_in_ = nullptr; 81 | } 82 | void RosImgProcessorNode::draw_clircle(const cv::Point & center, int radius, bool draw_center_coordinates) 83 | { 84 | // circle center in yellow 85 | cv::circle(cv_img_out_.image, center, 5, cv::Scalar(255, 255, 0), -1, 8, 0 ); 86 | // circle perimeter in purple. 87 | cv::circle(cv_img_out_.image, center, radius, cv::Scalar(255, 0, 255), 3, cv::LINE_8, 0 ); 88 | 89 | if (draw_center_coordinates) 90 | { 91 | // circle center point x and y coordinates. 92 | std::ostringstream stringStream; 93 | stringStream << " x:" << center.x << "\n" << " y:" << center.y; 94 | // print circle center coordinates 95 | cv::putText(cv_img_out_.image, stringStream.str(), center, cv::FONT_HERSHEY_PLAIN, 0.8, cv::Scalar(255, 153, 51), 2, 0.5); 96 | } 97 | } 98 | void RosImgProcessorNode::draw_ray_direction_vector(const cv::Point & center) 99 | { 100 | // line from center circle 101 | cv::line( cv_img_out_.image, center, cv::Point( ray_direction_.at(0, 0), ray_direction_.at(1, 0) ), cv::Scalar( 110, 220, 0 ), 2, 8 ); 102 | } 103 | void RosImgProcessorNode::publish() 104 | { 105 | //image_raw topic 106 | if(cv_img_out_.image.data) 107 | { 108 | cv_img_out_.header.seq ++; 109 | cv_img_out_.header.stamp = ros::Time::now(); 110 | cv_img_out_.header.frame_id = "camera"; 111 | cv_img_out_.encoding = img_encoding_; 112 | image_pub_.publish(cv_img_out_.toImageMsg()); 113 | 114 | // publish center ray direction. 115 | geometry_msgs::Vector3 direction; 116 | direction.x = ray_direction_.at(0, 0); //Minus added for matching the robot mounting bracket 117 | direction.y = -ray_direction_.at(1, 0); 118 | direction.z = ray_direction_.at(2, 0); 119 | ray_direction_circle_pub.publish(direction); 120 | } 121 | } 122 | 123 | double RosImgProcessorNode::getRate() const 124 | { 125 | return rate_; 126 | } 127 | 128 | void RosImgProcessorNode::imageCallback(const sensor_msgs::ImageConstPtr& _msg) 129 | { 130 | try 131 | { 132 | img_encoding_ = _msg->encoding;//get image encodings 133 | cv_img_ptr_in_ = cv_bridge::toCvCopy(_msg, _msg->encoding);//get image 134 | } 135 | catch (cv_bridge::Exception& e) 136 | { 137 | ROS_ERROR("RosImgProcessorNode::image_callback(): cv_bridge exception: %s", e.what()); 138 | return; 139 | } 140 | } 141 | 142 | void RosImgProcessorNode::cameraInfoCallback(const sensor_msgs::CameraInfo & _msg) 143 | { 144 | matrixP_ = (cv::Mat_(3,3) << _msg.P[0],_msg.P[1],_msg.P[2], 145 | _msg.P[3],_msg.P[4],_msg.P[5], 146 | _msg.P[6],_msg.P[7],_msg.P[8]); 147 | //std::cout << matrixP_ << std::endl; 148 | 149 | matrixK_ = (cv::Mat_(3,3) << _msg.K[0],_msg.K[1],_msg.K[2], 150 | _msg.K[3],_msg.K[4],_msg.K[5], 151 | _msg.K[6],_msg.K[7],_msg.K[8]); 152 | //std::cout << matrixK_ << std::endl; 153 | } 154 | -------------------------------------------------------------------------------- /delta_robot_kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(delta_robot_kinematics) 3 | 4 | #set C++11 support 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 6 | 7 | ## Find catkin macros and libraries 8 | find_package( catkin REQUIRED COMPONENTS 9 | roscpp 10 | rospy) 11 | 12 | ## The catkin_package macro generates cmake config files for your package 13 | catkin_package( 14 | INCLUDE_DIRS 15 | include 16 | LIBRARIES 17 | delta_kinematics 18 | CATKIN_DEPENDS 19 | roscpp 20 | rospy) 21 | 22 | ## Specify additional locations of header files 23 | include_directories( 24 | include 25 | ) 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | add_library(delta_kinematics 31 | src/delta_kinematics.cpp 32 | ) 33 | -------------------------------------------------------------------------------- /delta_robot_kinematics/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/serrauvic/ros_delta_robot/5940f173e6c5ca05aa4bb1cbf7bc135b6618412b/delta_robot_kinematics/README.md -------------------------------------------------------------------------------- /delta_robot_kinematics/include/delta_robot_kinematics/delta_kinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef DELTA_KINEMATICS_H 2 | #define DELTA_KINEMATICS_H 3 | 4 | #include "ros/ros.h" 5 | 6 | /********************Contants***********************************************/ 7 | const double pi = 3.14159265358979323846; 8 | const double sb = 0.2; 9 | const double sp = 0.08; 10 | const double lsup = 0.095; 11 | const double linf = 0.315;//0.46; 12 | const double conversio = 180/pi; 13 | 14 | //densitats 15 | const double densitylsup = 0.2;// Kg/m 16 | const double densitylinf = 0.15*2;//Kg/m 17 | const double weighteff = 0.1;//Kg 18 | 19 | // distancies al punt de càlcul 20 | const double g = 9.81; 21 | const double wb = sb*sqrt(3)/6; 22 | const double ub = sb*sqrt(3)/3; 23 | const double wp = sp*sqrt(3)/6; 24 | const double up = sp*sqrt(3)/3; 25 | 26 | const double a = wb-up; 27 | const double b = (sp/2)-(wb*sqrt(3)/2); 28 | const double c = wp-(0.5*wb); 29 | // pes dels components 30 | const double peslsup = lsup*densitylsup*g; 31 | const double peslinf = linf*densitylinf*g; 32 | const double peseff = weighteff*g; 33 | const double offset = pi/2; 34 | 35 | const double angleTrim = 0.8; 36 | 37 | namespace delta_kinematics 38 | { 39 | /* 40 | Inverse kinematics 41 | càlcul de l'angle a partir de la posició del endeffector 42 | posicio x i y de l'end effector seràn variables, z cte 43 | retorna un array que conté els angles theta1, theta2, theta3 respectivament. 44 | */ 45 | void inversekinematics (double x, double y, double z, double thetas[]); 46 | 47 | } // delta_kinematics 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /delta_robot_kinematics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | delta_robot_kinematics 4 | 1.0.0 5 | 6 | Delta robot kinematics node. 7 | 8 | 9 | 10 | Sebastián Serra Landete 11 | José Fernández Mola 12 | David Vives Colom 13 | Gabriel Felez Palacios 14 | Sergi Fabrego i Serrat 15 | Xavier Ortiz-Quintana Escardivol 16 | 17 | 18 | BSD 19 | 20 | 21 | 22 | 23 | 24 | Sebastián Serra Landete 25 | José Fernández Mola 26 | David Vives Colom 27 | Gabriel Felez Palacios 28 | Sergi Fabrego i Serrat 29 | Xavier Ortiz-Quintana Escardivol 30 | 31 | 32 | catkin 33 | 34 | roscpp 35 | rospy 36 | 37 | roscpp 38 | rospy 39 | 40 | 41 | -------------------------------------------------------------------------------- /delta_robot_kinematics/src/delta_kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "delta_robot_kinematics/delta_kinematics.h" 2 | 3 | 4 | /* 5 | Inverse kinematics 6 | càlcul de l'angle a partir de la posició del endeffector 7 | posicio x i y de l'end effector seràn variables, z cte 8 | retorna un array que conté els angles theta1, theta2, theta3 respectivament. 9 | */ 10 | void delta_kinematics::inversekinematics (double x, double y, double z, double thetas[]) 11 | { 12 | /*float64 <-> double c++*/ 13 | //double thetas [3] = {0, 0, 0}; 14 | 15 | double e1 = 2*lsup*(y+a); 16 | double f1 = 2*z*lsup; 17 | double g1 = (x*x) + (y*y)+ (z*z) + (lsup*lsup) + (a*a) + 2*y*a - (linf*linf); 18 | 19 | double e2 = -lsup*(sqrt(3)*(x+b)+y+c); 20 | double f2 = 2*z*lsup; 21 | double g2 = (x*x) + (y*y) + (z*z) + (b*b) + (c*c) + (lsup*lsup) +2*(x*b+y*c)-(linf*linf); 22 | 23 | double e3 = lsup*(sqrt(3)*(x-b)-y-c); 24 | double f3 = 2*z*lsup; 25 | double g3 = (x*x) + (y*y) + (z*z) + (b*b) + (c*c) + (lsup*lsup) +2*(-x*b+y*c)-(linf*linf); 26 | 27 | double sol1plus = 2 * atan((-f1+sqrt((e1*e1) + (f1*f1)-(g1*g1)))/(g1-e1)); 28 | double sol1min = 2 * atan((-f1-sqrt((e1*e1) + (f1*f1)-(g1*g1)))/(g1-e1)); 29 | double sol2plus = 2 * atan((-f2+sqrt((e2*e2) + (f2*f2)-(g2*g2)))/(g2-e2)); 30 | double sol2min = 2 * atan((-f2-sqrt((e2*e2) + (f2*f2)-(g2*g2)))/(g2-e2)); 31 | double sol3plus = 2 * atan((-f3+sqrt((e3*e3) + (f3*f3)-(g3*g3)))/(g3-e3)); 32 | double sol3min = 2 * atan((-f3-sqrt((e3*e3) + (f3*f3)-(g3*g3)))/(g3-e3)); 33 | 34 | //isreal? 35 | // comprobació de si els valors són reals 36 | if ((sol1min < pi/2) && (sol1min > -pi/2)) 37 | { 38 | thetas[0] = -sol1min*angleTrim + offset; 39 | } 40 | else if ((sol1plus < pi/2) && (sol1plus > -pi/2)) 41 | { 42 | thetas[0] = -sol1plus*angleTrim + offset; 43 | } 44 | else 45 | { 46 | thetas[0] = offset; //plus offset error sing 47 | } 48 | if ((sol2min < pi/2) && (sol2min > -pi/2)) 49 | { 50 | thetas[1] = -sol2min*angleTrim + offset; 51 | } 52 | else if ((sol2plus < pi/2) && (sol2plus > -pi/2)) 53 | { 54 | thetas[1] = -sol2plus*angleTrim + offset; 55 | } 56 | else 57 | { 58 | thetas[1] = offset; //plus offset error sing 59 | } 60 | if ((sol3min < pi/2) && (sol3min > -pi/2)) 61 | { 62 | thetas[2] = -sol3min*angleTrim + offset; 63 | } 64 | else if ((sol3plus < pi/2) && (sol3plus > -pi/2)) 65 | { 66 | thetas[2] = -sol3plus*angleTrim + offset; 67 | } 68 | else 69 | { 70 | thetas[2] = offset; //plus offset error sing 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /delta_robot_support/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(delta_robot_support) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED 6 | urdf 7 | std_msgs 8 | # joint_trajectory_action 9 | trajectory_msgs 10 | # pr2_controllers_msgs 11 | position_controllers 12 | joint_state_controller 13 | roscpp 14 | urdf 15 | rosserial_arduino 16 | rosserial_client 17 | std_msgs) 18 | 19 | catkin_package( 20 | ) 21 | 22 | ########### 23 | ## Build ## 24 | ########### 25 | include_directories(include 26 | ${catkin_INCLUDE_DIRS} 27 | ) 28 | -------------------------------------------------------------------------------- /delta_robot_support/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/serrauvic/ros_delta_robot/5940f173e6c5ca05aa4bb1cbf7bc135b6618412b/delta_robot_support/README.md -------------------------------------------------------------------------------- /delta_robot_support/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | delta_robot: 2 | # Publish all joint states ---------------------------------- 3 | delta_joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Joint Trajectory Controller ------------------------------- 8 | delta_position_trajectory_controller: 9 | type: position_controllers/JointTrajectoryController 10 | # CR: Here is my point, can the urdf/transmission specification be used for joint names? 11 | joints: 12 | - link_0_JOINT_1 13 | - link_0_JOINT_2 14 | - link_0_JOINT_3 15 | constraints: 16 | goal_time: 5.0 17 | stopped_position_tolerance: 0.4 # Defaults to 0.01 18 | joint1: 19 | trajectory: 0.60 20 | goal: 0.15 21 | joint2: 22 | trajectory: 0.60 23 | goal: 0.15 24 | joint3: 25 | trajectory: 0.60 26 | goal: 0.15 27 | -------------------------------------------------------------------------------- /delta_robot_support/config/delta.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 535 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | con1: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | con13: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | con2: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | con23: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | con3: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | con33: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | link_0: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | lleg_1: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | lleg_12: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | lleg_2: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | Value: true 111 | lleg_22: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Value: true 116 | lleg_3: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | lleg_32: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | low_base: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Value: true 131 | uleg_1: 132 | Alpha: 1 133 | Show Axes: false 134 | Show Trail: false 135 | Value: true 136 | uleg_2: 137 | Alpha: 1 138 | Show Axes: false 139 | Show Trail: false 140 | Value: true 141 | uleg_3: 142 | Alpha: 1 143 | Show Axes: false 144 | Show Trail: false 145 | Value: true 146 | world: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | Name: RobotModel 151 | Robot Description: robot_description 152 | TF Prefix: "" 153 | Update Interval: 0 154 | Value: true 155 | Visual Enabled: true 156 | Enabled: true 157 | Global Options: 158 | Background Color: 48; 48; 48 159 | Default Light: true 160 | Fixed Frame: world 161 | Frame Rate: 30 162 | Name: root 163 | Tools: 164 | - Class: rviz/Interact 165 | Hide Inactive Objects: true 166 | - Class: rviz/MoveCamera 167 | - Class: rviz/Select 168 | - Class: rviz/FocusCamera 169 | - Class: rviz/Measure 170 | - Class: rviz/SetInitialPose 171 | Topic: /initialpose 172 | - Class: rviz/SetGoal 173 | Topic: /move_base_simple/goal 174 | - Class: rviz/PublishPoint 175 | Single click: true 176 | Topic: /clicked_point 177 | Value: true 178 | Views: 179 | Current: 180 | Class: rviz/Orbit 181 | Distance: 10 182 | Enable Stereo Rendering: 183 | Stereo Eye Separation: 0.0599999987 184 | Stereo Focal Distance: 1 185 | Swap Stereo Eyes: false 186 | Value: false 187 | Focal Point: 188 | X: 0 189 | Y: 0 190 | Z: 0 191 | Focal Shape Fixed Size: true 192 | Focal Shape Size: 0.0500000007 193 | Invert Z Axis: false 194 | Name: Current View 195 | Near Clip Distance: 0.00999999978 196 | Pitch: 0.785398006 197 | Target Frame: 198 | Value: Orbit (rviz) 199 | Yaw: 0.785398006 200 | Saved: ~ 201 | Window Geometry: 202 | Displays: 203 | collapsed: false 204 | Height: 846 205 | Hide Left Dock: false 206 | Hide Right Dock: false 207 | QMainWindow State: 000000ff00000000fd00000004000000000000016b000002aafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002aa000000db00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002aa000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003ffc0100000002fb0000000800540069006d00650100000000000004b00000031700fffffffb0000000800540069006d006501000000000000045000000000000000000000022a000002aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 208 | Selection: 209 | collapsed: false 210 | Time: 211 | collapsed: false 212 | Tool Properties: 213 | collapsed: false 214 | Views: 215 | collapsed: false 216 | Width: 1200 217 | X: 59 218 | Y: 34 219 | -------------------------------------------------------------------------------- /delta_robot_support/launch/delta_robot_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 45 | 46 | 47 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 66 | 67 | 68 | 69 | 74 | 75 | 76 | 77 | 78 | 79 | 84 | 85 | 86 | 91 | 92 | 93 | 94 | 100 | 101 | 102 | 103 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 139 | 142 | 145 | 146 | -------------------------------------------------------------------------------- /delta_robot_support/launch/delta_robot_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 45 | 46 | 47 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 66 | 67 | 68 | 69 | 74 | 75 | 76 | 77 | 78 | 79 | 84 | 85 | 86 | 91 | 92 | 93 | 94 | 100 | 101 | 102 | 103 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 139 | 142 | 145 | 146 | -------------------------------------------------------------------------------- /delta_robot_support/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | delta_robot_support 4 | 1.0.0 5 | The delta_robot_support package 6 | 7 | 8 | Sebastián Serra Landete 9 | José Fernández Mola 10 | David Vives Colom 11 | Gabriel Felez Palacios 12 | Sergi Fabrego i Serrat 13 | Xavier Ortiz-Quintana Escardivol 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | Sebastián Serra Landete 23 | José Fernández Mola 24 | David Vives Colom 25 | Gabriel Felez Palacios 26 | Sergi Fabrego i Serrat 27 | Xavier Ortiz-Quintana Escardivol 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | catkin 48 | roscpp 49 | rospy 50 | std_msgs 51 | roscpp 52 | rospy 53 | std_msgs 54 | trajectory_msgs 55 | pr2_controllers_msgs 56 | actionlib 57 | joint_state_controller 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /delta_robot_support/urdf/delta_robot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 30 | 31 | 32 | 35 | 36 | 37 | 38 | 39 | 40 | /${namespace} 41 | 42 | true 43 | false 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | true 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 572 | 573 | 574 | 575 | 576 | 577 | 578 | 579 | 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 590 | 591 | 592 | 593 | 594 | 595 | 596 | 597 | 598 | 599 | 600 | 601 | 602 | 603 | 604 | 605 | 606 | 607 | 608 | 609 | 610 | 611 | 612 | 613 | con23 614 | low_base 615 | 0 0 0 0 0 0 616 | 617 | 1 0 0 618 | 619 | -100 620 | 100 621 | 622 | 623 | 0 624 | 0 625 | 0.000000 626 | 0.00030000 627 | 628 | 0 629 | 630 | 631 | 632 | 633 | con33 634 | low_base 635 | 0 0 0 0 0 0 636 | 637 | 1 0 0 638 | 639 | -100 640 | 100 641 | 642 | 643 | 0 644 | 0 645 | 0.000000 646 | 0.00030000 647 | 648 | 0 649 | 650 | 651 | 652 | 653 | 654 | con13 655 | lleg_1 656 | -0.05 0 0 0 0 0 657 | 658 | 0 1 0 659 | 660 | -100 661 | 100 662 | 663 | 664 | 0 665 | 0 666 | 0.000000 667 | 0.00030000 668 | 669 | 0 670 | 671 | 672 | 673 | 674 | 675 | con23 676 | lleg_2 677 | -0.05 0 0 0 0 0 678 | 679 | 0 1 0 680 | 681 | -100 682 | 100 683 | 684 | 685 | 0 686 | 0 687 | 0.000000 688 | 0.00030000 689 | 690 | 0 691 | 692 | 693 | 694 | 695 | 696 | con33 697 | lleg_3 698 | -0.05 0 0 0 0 0 699 | 700 | 0 1 0 701 | 702 | -100 703 | 100 704 | 705 | 706 | 0 707 | 0 708 | 0.000000 709 | 0.00030000 710 | 711 | 0 712 | 713 | 714 | 715 | 716 | 717 | 718 | 719 | 720 | transmission_interface/SimpleTransmission 721 | 722 | hardware_interface/PositionJointInterface 723 | 724 | 725 | PositionJointInterface 726 | 1.0 727 | 1.0 728 | 729 | 730 | 731 | 0.1 732 | 733 | 734 | 735 | 736 | 737 | 738 | transmission_interface/SimpleTransmission 739 | 740 | hardware_interface/PositionJointInterface 741 | 742 | 743 | PositionJointInterface 744 | 1.0 745 | 1.0 746 | 747 | 748 | 749 | 0.1 750 | 751 | 752 | 753 | 754 | 755 | 756 | transmission_interface/SimpleTransmission 757 | 758 | hardware_interface/PositionJointInterface 759 | 760 | 761 | PositionJointInterface 762 | 1.0 763 | 1.0 764 | 765 | 766 | 767 | 0.1 768 | 769 | 770 | 771 | 772 | 773 | 774 | 775 | true 776 | 1000.0 777 | 778 | robot_description 779 | 781 | 782 | 787 | 788 | 789 | 790 | true 791 | 100.0 792 | low_base 793 | base_link/f3d 794 | ${namespace}/low_base 795 | 796 | 797 | 798 | 799 | -------------------------------------------------------------------------------- /delta_robot_support/urdf/meshes/Platform.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/serrauvic/ros_delta_robot/5940f173e6c5ca05aa4bb1cbf7bc135b6618412b/delta_robot_support/urdf/meshes/Platform.STL -------------------------------------------------------------------------------- /delta_robot_support/urdf/meshes/fixed_base2.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | VCGLab 6 | VCGLib | MeshLab 7 | 8 | Y_UP 9 | Τρί Δεκ 13 19:47:12 2016 10 | Τρί Δεκ 13 19:47:12 2016 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 150 259.808 0 160 0 0 160 0 10 113.301 96.2436 10 140 0 10 140 50 10 160 50 10 176.699 113.564 10 186.699 96.2436 0 220 138.564 0 140 50 0 113.301 96.2436 0 123.301 113.564 0 176.699 113.564 0 70 121.244 0 70 121.244 10 0 0 0 300 0 10 230 121.244 0 300 0 0 150 259.808 10 80 138.564 0 0 0 10 70 121.244 -3.46945e-15 113.301 96.2436 -3.46945e-15 123.301 113.564 -3.46945e-15 123.301 113.564 10 80 138.564 10 80 138.564 -3.46945e-15 160 50 0 140 0 0 220 138.564 10 186.699 96.2436 10 176.699 113.564 -3.46945e-15 186.699 96.2436 -3.46945e-15 230 121.244 10 230 121.244 -3.46945e-15 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 0.866025 0.5 0 0.866025 0.5 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.866025 0.5 0 -0.866025 0.5 0 0.866025 0.5 0 0.866025 0.5 0 -0.866025 0.5 0 -0.866025 0.5 0 0 -1 0 0 -1 0 0.5 0.866025 0 0.5 0.866025 0 -0.866025 0.5 0 -0.866025 0.5 0 -0.5 -0.866025 0 -0.5 -0.866025 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 0.5 -0.866025 0 0.5 -0.866025 0 0.866025 0.5 0 0.866025 0.5 0 -0.5 0.866025 0 -0.5 0.866025 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 |

9 9 0 0 0 0 31 31 0 31 31 1 0 0 1 20 20 1 19 19 2 17 17 2 1 1 2 1 1 3 17 17 3 2 2 3 3 3 4 15 15 4 22 22 4 20 20 5 27 27 5 26 26 5 22 22 6 4 4 6 3 3 6 3 3 7 4 4 7 5 5 7 3 3 8 5 5 8 26 26 8 26 26 9 5 5 9 6 6 9 26 26 10 6 6 10 20 20 10 17 17 11 35 35 11 2 2 11 2 2 12 35 35 12 32 32 12 2 2 13 32 32 13 6 6 13 6 6 14 32 32 14 7 7 14 6 6 15 7 7 15 20 20 15 20 20 16 7 7 16 31 31 16 8 8 17 18 18 17 19 19 17 10 10 18 30 30 18 16 16 18 19 19 19 1 1 19 8 8 19 8 8 20 1 1 20 29 29 20 8 8 21 29 29 21 13 13 21 21 21 22 0 0 22 12 12 22 12 12 23 0 0 23 9 9 23 16 16 24 14 14 24 10 10 24 10 10 25 14 14 25 11 11 25 10 10 26 11 11 26 29 29 26 29 29 27 11 11 27 12 12 27 29 29 28 12 12 28 13 13 28 13 13 29 12 12 29 9 9 29 14 14 30 16 16 30 15 15 30 15 15 31 16 16 31 22 22 31 35 35 32 17 17 32 18 18 32 18 18 33 17 17 33 19 19 33 27 27 34 20 20 34 21 21 34 21 21 35 20 20 35 0 0 35 4 4 36 22 22 36 30 30 36 30 30 37 22 22 37 16 16 37 15 15 38 3 3 38 23 23 38 23 23 39 3 3 39 24 24 39 3 3 40 26 26 40 24 24 40 24 24 41 26 26 41 25 25 41 26 26 42 27 27 42 25 25 42 25 25 43 27 27 43 28 28 43 2 2 44 6 6 44 1 1 44 1 1 45 6 6 45 29 29 45 6 6 46 5 5 46 29 29 46 29 29 47 5 5 47 10 10 47 5 5 48 4 4 48 10 10 48 10 10 49 4 4 49 30 30 49 31 31 50 7 7 50 9 9 50 9 9 51 7 7 51 13 13 51 7 7 52 32 32 52 33 33 52 33 33 53 32 32 53 34 34 53 32 32 54 35 35 54 34 34 54 34 34 55 35 35 55 36 36 55

56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /delta_robot_support/urdf/meshes/upper_leg.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/serrauvic/ros_delta_robot/5940f173e6c5ca05aa4bb1cbf7bc135b6618412b/delta_robot_support/urdf/meshes/upper_leg.STL -------------------------------------------------------------------------------- /delta_robot_support/world/delta_robot.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 11 | 12 | model://sun 13 | 14 | 15 | 16 | 17 | 18 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 19 | orbit 20 | 21 | 22 | 23 | 24 | 25 | --------------------------------------------------------------------------------