Funções para a Cinemática de Robôs

Transcrição

1 Funções para a Cinemática de Robôs 19 de novembro de Tarefa funções Construir as no Scilab as seguintes funções: 1. Rot_x(theta) Recebe um ângulo θ e entrega a matriz de rotação x para o giro de θ radianos em torno de X 2. Rot_y(theta) Recebe um ângulo θ e entrega a matriz de rotação x para o giro de θ radianos em torno de Y. Rot_z(theta) Recebe um ângulo θ e entrega a matriz de rotação x para o giro de θ radianos em torno de Z 4. TH(matrizx, vectorx1) Recebe uma matriz de rotação x e um vetor de translação e entrega matriz de transformação homogênea 4x4 5. TH_inv(matriz4x4) Recebe uma matriz de transformação homogênea 4x4 e entrega a matriz que realiza a função inversa 6. Transforma_vec(matriz4x4, vecx1) Recebe uma matriz de transformação homogênea 4x4 (que leva do sist. A ao B) e un vector x1 representado no sistema B e entrega o vetor representado no sistema A 7. Cil_a_Cart(raio, ângulo, altura) Transforma as coordenadas de um ponto dada em coordenadas cilíndricas a coordenas cartesianas (x,y,z) 8. Esf_a_Cart(raio, ângulo, ângulo) Transforma as coordenadas de um ponto dada em coordenadas esféricas a coordenas cartesianas (x,y,z) 1

2 9. Rot_Euler_ZYZ(fi, theta, psi) Constrói a matriz de rotação apartir das três ângulos de Euler Rot_RPY(fi, theta, psi) Constrói a matriz de rotação apartir das três ângulos Roll, Pitch e Yaw 11. Euler_param(theta, vec x1) Entrega os quatro parâmetros de Euler apartir apartir de um vector (eixo de giro) e um ângulo de rotação em torno do eixo de giro 12. Rot_Euler_param(4 parâmetros de Euler vec4x1) Constrói a matriz de rotação apartir dos quatro parâmtros de Euler 1. Angulos_Euler_ZYZ(Matrizx) Obtém os ângulos de Euler apartir da matriz de rotação x 14. Angulos_RPY(Matrizx) Obtém os ângulos de Roll, Pitch e Yaw apartir da matriz de rotação x 15. Param_Euler_matriz(Matrizx) Obtém os quatro parâmetros de Euler apartir da matriz de rotação x 16. Eixo_ang_Param_Euler(4 parametros Euler 4x1) Apartir dos quatro parâmetros de Euler entrega o eixo de giro unitario e o ângulo de rotação em torno deste eixo 17. TH_DH(4 parametros de Denavit-Hartemberg) Apartir dos quatro parâmetros de Denavit-Hartemberg entrega a matriz de transformação homogênea. 18. TH_screw(4 parametros: dois vetores s, s_0 e dois escalares: theta e d) Apartir dos quatro parâmetros para denir um movimento helicoidal entrega a matriz de transformação homogênea. 19. desenha_d( vetores de tamanho n) Apartir de três vetores contendo as coordenadas x, y e z de n pontos plotar os pontos no espaço unidos por retas (elos) 20. trajeto_reto(2 vetores x1 e dois escalares) Apartir de dois vetores contendo as coordenadas x, y e z do ponto inicial e do ponto nal da trajetória reta; um escalar representa o número de passos 2

3 (divisões) a serem feitos na reta para discretizar o problema e um escalar que representa o passo atual. Esta função entrega a coordenadas x,y,z do ponto referente ao passo atual, ou seja a posição do efetuador nal no espaço operacional. 21. cin_inv_rrr(dois vetores x1 um escalar) Apartir de um vetor contendo as coordenadas x, y e φ do efetuador nal do robô plano com três juntas rotativas e outro vetor contendo os comprimentos dos elos a 1, a 2 e a do robô e um escalar igual a 1 para conguração de cotovelo acima e zero para cotovelo abaixo do punho, deve a função entregar os valores dos ângulos nas juntas que levem o efetuador nal a conguração desejada. 22. cin_inv_rrp(um vetor x1 e um escalar) Apartir de um vetor contendo as coordenadas x, y e z do efetuador nal do robô com duas juntas rotativas e uma prismática (ver xerox) e um escalar representando o comprimento L 1 do primeiro elo, deve a função entregar os valores dos ângulos nas juntas rotativas (θ 1 e θ 2 ) e o deslocamento na junta prismática (d ) que levem o efetuador nal a conguração desejada. 2. programa_1_rrr(este é um programa "main" e não uma função) Para um robô RRR, dada uma trajetória reta entre dois pontos (no espaço 2D) com uma orientação de π/2, usando TH_DH para calcular as matrizes de transformação homogênea: plotar o valor das juntas θ 1, θ 2,θ durante o movimento 24. programa_2_rrp(este é um programa "main" e não uma função) Para um robô RRP, dada uma trajetória reta entre dois pontos (espaço D), usando TH_screw para calcular as matrizes de transformação homogênea plotar o valor das juntas (θ 1,θ 2,d ) durante o movimento 25. cin_dir_punho(vector x1) Apartir de um vetor x1 contendo os valores dos ângulos nas juntas rotativas (θ 4, θ 5 e θ 6 ), deve a função entregar a matriz de rotação que representa a orientação do efetuador nal de un punho esférico (dof rotativos ortogonais)

4 26. cin_inv_punho(matriz x) Apartir de uma matriz x ( R 6 ) contendo a orientação desejada para o último elo de um punho esférico, deve a função entregar os valores dos ângulos nas juntas rotativas (θ 4, θ 5 e θ 6 ) que levem o efetuador nal a conguração desejada. 27. programa puma_sem_punho(este é um programa "main" e não uma função) Para um robô antropomórco dof (PUMA sem punho esférico) con três juntas rotativas (sendo o eixo da primeira junta perpendicular as eixos das juntas 2 e os quais são paralelos entre ), dada uma trajetória reta entre dois pontos (espaço D), usando TH_screw para calcular as matrizes de transformação homogênea plotar o valor das juntas (θ 1,θ 2,θ ) durante o movimento obs. ver Leccion_11_2007.pdf 28. programa_4_puma_com_punho(este é um programa "main" e não uma função) Para um robô antropomórco com punho esférico 6dof (PUMA com punho esférico), dada uma trajetória reta entre dois pontos (espaço D) e uma orientação do efetuador nal constante de π em torno do eixo z do sist. de ref. da base (sist. zero), usando TH_screw plotar o valor das juntas (θ 1,θ 2,θ,θ 4,θ 5,θ 6 ) durante o movimento obs. ver Leccion_11_2007.pdf 29. Primerira Prova Robô SCARA Todos os arquivos correspondentes Jacobiano_stanford(n matrizes 4x4) Apartir das matrizes de transformação homogênea (4x4) do robô Stanford constrói a sua matriz jacobiana. 4

5 0. Spline_numerico(7 escalares) Apartir de:q inicial, q inicial,t inicial (posição, velocidade e tiempo iniciais), q final, q final,t final (posição, velocidade e tiempo nais) e o parâmetro de tempo atual t calcular q(t). 1. Spline_analitico(7 escalares) Apartir de:q inicial, q inicial,t inicial (posição, velocidade e tiempo iniciais), q final, q final,t final (posição, velocidade e tiempo nais) e o parâmetro de tempo atual t calcular q(t), q(t) e q(t), (posição, velocidade e aceleração na junta). Para isto resolva primeiro no WXmaxima e copie as equações analíticas dentro desta função. Por exemplo para t inicial =, t final = 10, q inicial =, q final = 15, qp inicial = 1, qp final = 1, compile no Wxmaxima o seguinte script: kill(all); q:a+b*t+c*t^2+d*t^; qp:diff(q,t); eq1:ev(q,t=t1)=q1; eq2:ev(q,t=t2)=q2; eq:ev(qp,t=t1)=qp1; eq4:ev(qp,t=t2)=qp2; res:solve([eq1,eq2,eq,eq4],[a,b,c,d]); th:ev(q,res); vel:diff(th,t); acc:diff(vel,t); constante:[q1=,q2=15,qp1=1,qp2=-1,t1=,t2=10]; wxplot2d(ev(th,constante),[t,ev(t1,constante),ev(t2,constante)],[xlabel,"t"], [ylabel,"theta"],[style,impulses],[gnuplot_preamble,"set grid"]); wxplot2d(ev(vel,constante),[t,ev(t1,constante),ev(t2,constante)],[xlabel,"t"], [ylabel,"velocidade"],[style,impulses],[gnuplot_preamble,"set grid"]); wxplot2d(ev(acc,constante),[t,ev(t1,constante),ev(t2,constante)],[xlabel,"t"], [ylabel,"aceleracao"],[style,impulses],[gnuplot_preamble,"set grid"]); e use as equações th, vel (do script acima no wxmaxima) para comparar com q(t) (obtida com a cinemática inversa) e q(t) (Obtida usando o Jacobiano) Assim, do wxmaxima temos que q(t) = th é th = th A + th B + th C + th D onde th A = t 1 ( qp1 t 2 q 1 t 2 2 ) + q 1 t 2 + t 1 2 ( (qp 1 qp 2 ) t q 2 t 2 ) + t1 (qp 2 t 2 q 2 ) t 2 + t 1 t 2 2 t 1 2 t 2 + t 1 th B = t ( qp 1 t 2 + t 1 ( ( 2 qp2 qp 1 ) t q 2 t 2 6 q 1 t 2 ) + (qp2 + 2 qp 1 ) t 1 2 t 2 + qp 2 t 1 ) t 2 + t 1 t 2 2 t 1 2 t 2 + t 1 th C = t2 ( ( qp 2 2 qp 1 ) t t 1 ((qp 1 qp 2 ) t 2 + q 2 q 1 ) + q 2 t 2 q 1 t 2 + (2 qp 2 + qp 1 ) t 1 2 ) t 2 + t 1 t 2 2 t 1 2 t 2 + t 1 th D = t (( qp 2 qp 1 ) t 2 + (qp 2 + qp 1 ) t q 2 2 q 1 ) t 2 + t 1 t 2 2 t 1 2 t 2 + t 1 5

6 2. Dinamica_antropomorfico(2 vetores x1, dois escalares) Entra um vetor de posição inicial ([x inicial, y inicial, z inicial )), um vetor de posição nal ([x final, y final, z final )), o peso (um escalar) da carga na garra (efetuador nal) e o tempo total do trajeto (outro escalar). A função deve calcular os torques nos três motores (este cálculo deve ser feito para cada t ) para o movimento desejado (do ponto inicial ao nal), assumindo que a velocidade inicial e nal são nulas. As três juntas devem percorrer uma trajetória cúbica (use a função spline_analitico ). Assuma valores para comprimentos, massas, inércias etc. 6