Ensinamento por demonstração com controlo de força e inspeção por visão em células de polimento robotizadas

FACULDADE DE ENGENHARIA DA UNIVERSIDADE DO PORTO Ensinamento por demonstração com controlo de força e inspeção por visão em células de polimento robotizadas Nuno Jorge Pais Martins Rodrigues MESTRADO INTEGRADO EM ENGENHARIA ELETROTÉCNICA E DE COMPUTADORES Orientador: António Paulo Gomes Mendes Moreira Coorientador: Paulo José Cerqueira Gomes da Costa Julho de 2016 c Nuno Rodrigues, 2016 Resumo Nesta dissertação, estudou-se a viabilidade de um sistema de polimento robotizado programado por demonstração e desenvolveu-se um sistema de inspeção automática por visão artificial de peças com superfície altamente refletora e espelhada. No que diz respeito ao primeiro, desenvolveu-se uma ferramenta de polimento onde encaixava um marcador luminoso e um sensor de força, para que as trajetórias e as forças demonstradas por um utilizador, pudessem ser registadas através do recurso a uma framework de transferência de know-how chamada 6DMimic. Estudou-se o controlo de força do manipulador industrial UR5, da Universal Robots, para ser utilizado no polimento robotizado. Para além da implementação dos protocolos de comunicação com o sensor de força e o 6DMimic, desenvolveram-se algumas funções em C++ para que fosse possível ler o estado interno e controlar o manipulador referido, a partir de um PC. Foi desenvolvido um método de calibração, para transformar corretamente as trajetórias da zona de demonstração para a zona de atuação do UR5. Estas trajetórias foram suavizadas, através de um algoritmo implementado para diminuir o número de instruções de movimentação enviadas para o manipulador industrial. Relativamente ao sistema de inspeção automática por visão artificial, desenvolveu-se uma técnica de condicionamento do ambiente de aquisição e um algoritmo de deteção de defeitos. O condicionamento do ambiente de aquisição foi extremamente necessário devido à elevada refletividade das peças, o que provocava o aparecimento de reflexões especulares indesejáveis. Este foi feito através do isolamento das peças, relativamente ao ambiente exterior, e através da utilização de iluminação do tipo difusa frontal de campo escuro. Foram comparadas várias abordagens para segmentar os defeitos que tipicamente apareciam nas peças. Os melhores resultados foram obtidos através da utilização do gradiente morfológico, no realce das orlas (defeitos) presentes nas imagens adquiridas. A identificação dos defeitos foi feita com base na imagem previamente segmentada, através de funções da biblioteca OpenCV. Por último, integrou-se no sistema de inspeção por visão o mesmo manipulador industrial, para que fosse feito o varrimento de toda a superfície das peças. i ii Abstract In this dissertation, was studied the viability of a robotic polishing system programmed by demonstration and was developed an automatic vision inspection system for highly reflective and mirrored surface pieces. On what concerns the first topic, a polishing tool was developed, where a marker and a force sensor were added, in order to record the trajectories and the forces demonstrated by a user, using a framework for human-robot skill transfer called 6DMimic. UR5’s industrial robot force control, was tested to be used in robotic polishing. Were implemented the communication protocols with force sensor, 6Dmimic and UR5. For the latter, were developed some C++ functions, to make possible reading the internal state and control the industrial robot from a PC. A calibration method was developed to properly transform trajectories from demonstration’s zone to UR5’s actuation zone. This trajectories were filtered by an algorithm to decrease the number of instructions sent to the industrial robot. Concerning the automatic vision inspection system, was developed a technique for conditioning the image acquisition space and a defect detection algorithm. The conditioning of image acquisition space was required due to the extremely high reflectivity pieces, which caused the appearance of unwanted specular reflections. This was done by isolating the pieces from the external space, and by the use of diffuse front lighting illumination (dark field). Several approaches, about segmentation of piece’s considered defects, were compared. Best results were obtained using morphological gradient in enhancement of edges (defects). The identification of defects was done using previously segmented image, through OpenCV functions. Finally, was integrated the same industrial robot in the automatic vision inspection system, to be do the scanning of the entire piece’s surface. iii iv Agradecimentos Em primeiro lugar, quero agradecer à minha família, particularmente aos meus pais, ao meu irmão e à minha avó, por todo o apoio que me deram. Sem eles, hoje não seria a pessoa que sou. Depois, à minha Andreinha, por todo o carinho, boa disposição e paciência, ajudando-me sempre nos momentos em que as coisas não correram tão bem. Aos intervenientes diretos na realização deste projeto: •Em especial, ao meu orientador, Professor Doutor António Paulo Moreira, por me ter orientado neste projeto. A sua disponibilidade e prontidão para me ajudar nos problemas que surgiram, foram cruciais para que soubesse sempre qual era o caminho a seguir; •Ao meu coorientador, Professor Doutor Paulo Costa, e ao Doutorado em Engenharia Andry Pinto, por me terem orientado e ajudado na elaboração do algoritmo de inspeção por visão artificial; •Ao Doutorado em Engenharia Marcos Ferreira, por toda ajuda prestada com o sistema de demonstração de trajetórias – 6DMimic; •Ao Doutorado em Engenharia Luís Rocha, por todo o conhecimento partilhado sobre manipuladores industriais; •Ao Sr. Fernando Guedes e ao Jorge Barbosa, por todo o apoio técnico prestado ao longo da realização deste projeto; •Ao INESC TEC e à Lobo & Filhos LTD, pela disponibilização de todos os recursos necessários para a realização de testes. Por último, mas não menos importante, quero agradecer a todos os meus colegas de curso, por toda a motivação e entreajuda ao longo dos últimos anos. Nuno Jorge P. M. Rodrigues v vi “It always seems impossible until it’s done.” Nelson Mandela vii xiv LISTA DE FIGURAS 4.37 Comparação entre os defeitos reais presentes na superfície superior da peça e os segmentados - Peça 40 - Ângulo: 270◦....................... 80 4.38 Comparação entre os defeitos reais presentes na superfície lateral da peça e os segmentados - Peça 32 - Ângulo: 342◦....................... 81 4.39 Comparação entre os defeitos reais presentes na superfície lateral da peça e os segmentados - Peça 00 - Ângulo: 18◦....................... 82 4.40 Comparação entre os defeitos reais presentes na superfície lateral da peça e os segmentados - Peça 11 - Ângulo: 0◦........................ 83 4.41 Comparação entre os defeitos reais presentes na superfície lateral da peça e os segmentados - Peça 40 - Ângulo: 18◦....................... 84 4.42 Resultados obtidos na identificação dos defeitos, numa superfície lateral - Peça 11 - Ângulo: 0◦..................................... 86 4.43 Resultados obtidos na identificação dos defeitos, numa superfície superior - Peça 33 - Ângulo: 120◦.................................. 87 4.44 Diagrama de atividade do algoritmo de identificação dos defeitos . . . . . . . . . 88 4.45 Interface desenvolvida para especificar as configurações das trajetórias utilizadas para o varrimento completo das superfícies das peças . . . . . . . . . . . . . . . 91 4.46 Peça de teste utilizada para o estudo do passo a utilizar na inspeção de cada superfície ......................................... 91 4.47 Imagens inspecionas, antes de serem processadas pelo algoritmo de reconstrução 92 4.48 Peça 01 - Comparação entre os defeitos detetados e os reais . . . . . . . . . . . . 94 4.49 Defeitos detetados descontínuos na borda da superfície da superior, quando na realidadeeramcontínuos.............................. 94 4.50 Presença de sub-defeitos (assinalados a vermelho dentro do retângulo vermelho maior)........................................ 95 5.1 Espelhamento de um padrão em peças com vários estados de polimento . . . . . 100 D.1 Resultados finais obtidos - Peça 00 . . . . . . . . . . . . . . . . . . . . . . . . . 159 D.2 Resultados finais obtidos - Peça 01 . . . . . . . . . . . . . . . . . . . . . . . . . 160 D.3 Resultados finais obtidos - Peça 02 . . . . . . . . . . . . . . . . . . . . . . . . . 160 D.4 Resultados finais obtidos - Peça 03 . . . . . . . . . . . . . . . . . . . . . . . . . 161 D.5 Resultados finais obtidos - Peça 10 . . . . . . . . . . . . . . . . . . . . . . . . . 161 D.6 Resultados finais obtidos - Peça 11 . . . . . . . . . . . . . . . . . . . . . . . . . 162 D.7 Resultados finais obtidos - Peça 12 . . . . . . . . . . . . . . . . . . . . . . . . . 162 D.8 Resultados finais obtidos - Peça 13 . . . . . . . . . . . . . . . . . . . . . . . . . 163 D.9 Resultados finais obtidos - Peça 31 . . . . . . . . . . . . . . . . . . . . . . . . . 164 D.10 Resultados finais obtidos - Peça 32 . . . . . . . . . . . . . . . . . . . . . . . . . 165 D.11 Resultados finais obtidos - Peça 33 . . . . . . . . . . . . . . . . . . . . . . . . . 165 D.12 Resultados finais obtidos - Peça 40 . . . . . . . . . . . . . . . . . . . . . . . . . 166 D.13 Resultados finais obtidos - Peça 41 . . . . . . . . . . . . . . . . . . . . . . . . . 166 D.14 Resultados finais obtidos - Peça 42 . . . . . . . . . . . . . . . . . . . . . . . . . 167 D.15 Resultados finais obtidos - Peça 43 . . . . . . . . . . . . . . . . . . . . . . . . . 167 Lista de Tabelas 3.1 Manipulador industrial UR5 da Universal Robots - Especificações principais . . . 20 3.2 Sensor de força/binário Gamma da ATI Industrial Automation - Especificações principais ...................................... 21 3.3 Samsung RC530-S04PT - Especificações principais . . . . . . . . . . . . . . . . 22 3.4 Tipos de pacotes enviados na trama de estado do UR5 ............... 35 3.5 Algum conteúdo dentro de um pacote do tipo Cartesian Info Ordem da receção da informação no buffer:TCP/IP network byte order ................. 36 3.6 Formato da trama de pedido enviada para a Net Box ................ 36 3.7 Formato da trama de resposta enviada pela Net Box, quando se pretendeu ler as componentes das forças e binários . . . . . . . . . . . . . . . . . . . . . . . . . 37 3.8 6DMimic - Protocolo de comunicação - Mensagens enviadas pela aplicação Master Nota: Os parêntesis retos não fazem parte das mensagens . . . . . . . . . . . 38 3.9 6DMimic - Protocolo de comunicação - Mensagens enviadas pela aplicação Slave Nota: Os parêntesis retos não fazem parte das mensagens . . . . . . . . . . . . . 39 3.10 Formato de cada linha do ficheiro com os pontos da trajetória Nota: Os valores sãoseparadosporvírgulas.............................. 39 3.11 Referenciais considerados nas zonas de demonstração e de atuação . . . . . . . . 43 4.1 The Imaging Source DMK 31AU03.AS - Especificações . . . . . . . . . . . . . . 63 4.2 GOYO OPTICAL INC. GMHR31614MCN - Especificações . . . . . . . . . . . 64 4.3 Protocolo de comunicação implementado - Mensagens enviadas pelo cliente Nota: Os parêntesis retos não fazem parte das mensagens . . . . . . . . . . . . . . . . 89 4.4 Protocolo de comunicação implementado - Mensagens enviadas pelo servidor Nota: Os parêntesis retos não fazem parte das mensagens . . . . . . . . . . . . . 90 4.5 Tempos de processamento obtidos do algoritmo de deteção de defeitos (carregamento da imagem, segmentação e identificação de defeitos) . . . . . . . . . . . . 93 xv xvi LISTA DE TABELAS Abreviaturas e Símbolos API Application Programming Interface CAD Computer Aided Design CAM Computer Aided Manufacturing CCD Charge-coupled device FEUP Faculdade de Engenharia da Universidade do Porto FPS Frames por segundo FTP File Transfer Protocol IMU Inertial Measurement Unit INESC TEC Instituto de Engenharia de Sistemas e Computadores - Tecnologia e Ciência IP Internet Protocol LED Light Emitting Diode LTD Limitada NC Numerical Control OpenCV Open Source Computer Vision Library PID Proporcional integral derivativo PoE Power over Ethernet RAM Random Access Memory TCP Transmission Control Protocol UDP User Datagram Protocol USB Universal Serial Bus VDC Voltage Direct Current 3D Espaço tridimensional A Ampere dB Decibel GB Gigabyte g Grama ◦Grau ◦C Grau Celsius Hz Hertz m Metro µMicro mm Milímetro N Newton px Pixel kg Quilograma V Volt W Watt xvii Capítulo 1 Introdução Esta dissertação tinha como objetivo o estudo da viabilidade de um sistema de polimento robotizado programado por demonstração humana e o desenvolvimento de um sistema de inspeção automática por visão artificial de peças com superfície altamente refletora e espelhada. Neste capítulo, é apresentado o problema em causa, a motivação que levou à sua resolução, os objetivos propostos e, por último, a estrutura deste documento. 1.1 Apresentação do problema A utilização de manipuladores robóticos nas operações de polimento de pequenas peças, é ainda muito escassa devido à complexidade de programação envolvida. O tempo necessário para se desenvolver manualmente uma aplicação é muito elevado pois os movimentos são complexos, muito variados e é necessário o controlo de força. Este facto, coloca de fora os manipuladores no caso de séries médias e pequenas e com elevado grau de customização que, no mundo atual, são cada vez mais frequentes. Posto isto, pretendeu-se inovar no sentido de se desenvolverem novas técnicas de programação de manipuladores, baseadas na programação por demonstração e no registo das forças envolvidas. Esta parte do projeto, realizou-se em colaboração com a empresa TALUS Robotics, S.A., que já possuía um sistema similar para pintura com uma patente da FEUP licenciada. A inspeção de qualidade é um passo extremamente importante na produção industrial e a visão artificial é cada vez mais utilizada nesta área. Normalmente, esta inspeção é feita por indivíduos qualificados, no entanto, como depende do olho humano muitas vezes está sujeita a erros, como é o caso do efeito do cansaço. Testes mostraram que a inspeção por visão humana não é mais do que 80% eficaz [6]. A inspeção por visão de peças polidas ou pintadas com superfície altamente refletora e espelhada, é particularmente complicada devido ao aparecimento de reflexões especulares ao longo da superfícies. Estas reflexões provocam a saturação dos pixeis nas zonas afetadas, impossibilitando a recolha de qualquer informação nessas zonas da peça. Desta forma, nesta dissertação 1 2Introdução pretendeu-se ainda desenvolver, para além de um algoritmo de visão artificial, técnicas especiais de iluminação que permitam a inspeção destas peças. A empresa Lobo & Filhos LTD, acompanhou o projeto, pois tem bastante interesse nos resultados finais devido ao tipo de peças que produz. 1.2 Motivação A automatização de processos é a minha principal área de interesse neste ramo de engenharia. Ter tido a oportunidade de estudar soluções nesta área com a possibilidade de realizar testes em ambiente industrial, foi sem dúvida uma motivação extra. Agora terminado este projeto, espero ter colmatado algumas das necessidades de inovação apresentadas. Este projeto foi desenvolvido não só com a colaboração da TALUS Robotics, S.A. e da Lobo & Filhos LTD, como já foi referido, mas também com a colaboração do INESC TEC. Estas parcerias foram uma mais-valia, uma vez que contribuíram com vastos conhecimentos nestas áreas e estimularam o trabalho de equipa. A valorização profissional resultante da interação com as empresas referidas, foi outro aspeto bastante importante, sendo sempre um fator motivador ao longo da realização desta dissertação. 1.3 Objetivos Os objetivos propostos para o estudo da viabilidade de um sistema de polimento robotizado programado por demonstração humana, consistiram no desenvolvimento de uma aplicação capaz de: •Gravar numa base de dados as trajetórias e as forças envolvidas na demonstração humana da tarefa de polimento; •Permitir ligeiros ajustes nos movimentos, quer nas trajetórias como nas velocidades, e nas forças envolvidas, para acelerar o processo; •Gerar o código a descarregar para o manipulador industrial através dos pontos das trajetórias e das forças, adquiridos durante a demonstração; •Disponibilizar uma interface com o utilizador para sinalizar o início e o fim das demonstrações. Nesta parte do projeto, outro objetivo proposto foi averiguar se seria viável utilizar o controlo de força do manipulador industrial UR5 da Universal Robot, sem ser necessário o uso de um sensor de força externo. Relativamente ao desenvolvimento do sistema de inspeção automática por visão artificial de peças com superfície altamente refletora e espelhada, os objetivos propostos foram: 1.4 Estrutura do documento 3 •Desenvolver uma técnica de iluminação robusta que permitisse a captação correta de imagens para serem analisadas em ambiente industrial; •Elaborar um algoritmo de inspeção de peças com tempo de processamento pequeno, que fosse capaz de identificar defeitos e classificar o estado de polimento atual (no caso concreto das peças polidas); •Comparar as abordagens baseadas em software grátis (licença BSD), com abordagens baseadas em software comercial (HALCON). Por último, pretendia-se a integração dos sistema de polimento robotizado e de inspeção automática por visão, para que as peças ao fim de serem polidas, fossem imediatamente inspecionadas. 1.4 Estrutura do documento Este documento está divido segundo os seguintes capítulos: •Capítulo 2 - Revisão Bibliográfica: Estudo realizado sobre sistemas de manipulação robotizados com controlo de força, sobre o sistema de demonstração de trajetórias utilizado e sobre sistemas de visão aplicáveis à inspeção de peças; •Capítulo 3 - Ensinamento por demonstração com controlo de força: Numa primeira fase, são apresentados todos os periféricos utilizados pelo sistema de polimento robotizado programado por demonstração humana, assim como a explicação da comunicação estabelecida com eles. De seguida, é explicada a calibração do sistema de demonstração de trajetórias e a forma como estas foram suavizadas. Por último, são apresentados os resultados obtidos através dos testes realizados ao controlo de força do manipulador industrial UR5; •Capítulo 4 - Inspeção por visão artificial de peças altamente refletoras e espelhadas: Exposição de todo o sistema de inspeção automática por visão artificial desenvolvido. No inicio são apresentadas as peças e respetivos defeitos considerados. Posteriormente, é explicada a técnica de condicionamento do ambiente de aquisição adotada e o algoritmo de deteção de defeitos desenvolvido; •Capítulo 5 - Conclusões gerais e trabalho futuro: Exposição das conclusões finais, do cumprimento dos objetivos estabelecidos e do trabalho futuro dos sistemas apresentados neste projeto; •Anexo A - Peças da ferramenta de polimento - Desenho detalhado: Desenho detalhado das peças desenvolvidas no SOLIDWORKS para ferramenta de polimento; •Anexo B - Código C++ - Ensinamento por demonstração com controlo de força: Apresentação das funções principais desenvolvidas para o sistema de polimento robotizado programado por demonstração; 4Introdução •Anexo C - Código C++ - Inspeção por visão artificial: Apresentação das funções principais do algoritmo de deteção de defeitos desenvolvido; •Anexo D - Inspeção por visão artificial de peças altamente refletoras e espelhadas - Resultados finais: Apresentação de todos os resultados obtidos nos testes realizados ao sistema de inspeção automática por visão artificial. Capítulo 2 Revisão Bibliográfica Este capítulo serve para apresentar a revisão bibliográfica da dissertação presente. Serão explicitados sistemas de manipulação robotizados com controlo de força, o sistema de demonstração de trajetórias a ser utilizado neste trabalho e sistemas de visão aplicáveis à inspeção de peças. 2.1 Sistemas de manipulação robotizados com controlo de força Nesta secção, serão abordadas algumas estratégias de controlo de força que têm vindo a ser utilizadas em aplicações de polimento robotizadas. A automação de processos de polimento é importante para melhorar a eficiência do acabamento da superfície das peças. Ultimamente, os manipuladores robóticos têm mostrado ser uma solução eficiente e económica para as aplicações de polimento automatizado [9]. O polimento é um processo mecânico baseado na remoção de uma camada muito fina do contorno da superfície. Uma operação idêntica ao polimento de peças é a rebarbação. A diferença entre os dois está na profundidade de material a remover, que é constante no polimento e não é na rebarbação (figura 2.1). Figura 2.1: Processo de polimento (à esquerda) e processo de rebarbe (à direita) [1] Preston [10] descreveu a remoção de material, dz, em qualquer ponto entre dois materiais em contato através de uma força F, movendo-se a uma velocidade relativa vre aplicada uma pressão p, através da seguinte equação 5 12 Revisão Bibliográfica Figura 2.8: Marcador luminoso numa pistola de pintura [4] inspeção. Nas secções posteriores, serão considerados alguns sistemas de polimento com inspeção por visão, algoritmos de inspeção normalmente usados com este tipo de peças e técnicas de iluminação especiais. 2.3.1 Sistemas de polimento com inspeção por visão Têm vindo a ser desenvolvidos vários sistemas de visão aplicáveis a inspeção de peças polidas, como por exemplo: •Em 2004, Chiu-Chun Ngan e Hon-Yuen Tam em [21], desenvolveram um sistema de visão composto por uma câmara CCD, um computador e um algoritmo de análise da imagem, para inspeção de moldes e matrizes polidas. O algoritmo de análise é baseado nas direções das fendas na superfície da peça a ser polida, o que dita o estado atual de polimento da peça. Foram obtidos resultados com rugosidades de superfícies de 0,02µm; •Em 2010, foi desenvolvido um sistema complexo baseado em redes neuronais artificiais para polimento robotizado de superfícies. Em primeiro lugar, a superfície é aprendida pela rede neuronal que, posteriormente, dá instruções ao manipulador industrial para se mover. Os parâmetros de polimento desenvolvidos no sistema são o tempo e a força. Ao fim do ciclo de polimento, a peça é inspecionada pelo sistema de visão e, caso não esteja com as caraterísticas pretendidas, é novamente polida com os parâmetros tempo e força ajustados pela rede neuronal. A principal vantagem do uso de redes neuronais está no facto do sistema possuir aprendizagem e ter capacidade para se adaptar aos diversos tipos de defeitos que possam aparecer [22]. 2.3 Sistemas de visão aplicáveis à inspeção de peças 13 2.3.2 Algoritmos de visão para inspeção de peças Para se obter um bom desempenho, o sistema de inspeção tem de ser robusto a problemas como a presença de ruído nas imagens, reflexão especular não uniforme ao longo da superfície, vários tipos de defeitos, etc. Quando se tem como objetivo a instalação deste tipo de sistemas em linhas de produção, como o caso de estudo presente, é necessário ter-se tempos de processamento pequenos para não atrasar toda a produção. Portanto, na elaboração do algoritmo de inspeção, deve conseguir-se estabelecer um ponto de equilíbrio entre a eficiência da inspeção e o tempo que ela demora a ser feita [23]. Vários algoritmos têm vindo a ser estudados, como por exemplo através do uso de support vector machines [24], através de preservação de bordas, duplo threshold ethreshold adaptativo [23] e por comparação com uma imagem de referência [6]. No último, a imagem de deteção Dpode ser definida como D(i,j) =    1,se |I(i,j)−R(i,j)|>p(i.j) 0,se |I(i,j)−R(i,j)| ≤ p(i.j) (2.4) onde Ié a imagem capturada, Ré a imagem de referência e pé o threshold. 2.3.3 Técnicas de iluminação O ambiente de aquisição de imagem pode e deve ser condicionado através da iluminação, permitindo desta forma realçar as características da imagem que se querem analisar. As técnicas de iluminação são dependentes de vários fatores, como a direção da luz, a posição da fonte de luz relativamente ao objeto e à câmara e, da quantidade de luz incidente no objeto e captada pela câmara [5]. Quanto à direção da luz, esta pode ser difusa ou direta. O uso de iluminação difusa dá origem a reflexões homogéneas, facilitando a eliminação de sombras e a redução de reflexões especulares (figura 2.9). A iluminação direta permite evidenciar detalhes da superfície dos objetos (figura 2.10). As peças metálicas após polidas têm caraterísticas a altamente refletoras, pelo que a incidência de luz provocará a reflexões especulares não uniformes devido à forma não plana da superfície. Desta forma, o uso de iluminação difusa poderá ser vantajoso. Relativamente à posição da fonte de luz relativamente ao objeto e à câmara, pode usar-se iluminação frontal ou em contraluz. A iluminação em contraluz normalmente é usada quando se pretende evidenciar o contorno dos objetos, logo, como no caso de estudo não se pretende fazer isto, não será vantajoso o uso desta técnica (figura 2.11). No que diz respeito à quantidade de luz incidente no objeto e captada pela câmara, poderá usar-se iluminação de campo claro ou escuro. A iluminação de campo escuro carateriza-se pela baixa quantidade de luz incidente na câmara e, normalmente é usada quando se pretende realçar 14 Revisão Bibliográfica (a) Sistemas de iluminação (b) Imagem captada Figura 2.9: Iluminação difusa [5] saliências e concavidades nos objetos (figura 2.12). A iluminação de campo claro carateriza-se pela incidência de uma quantidade razoável de luz na câmara. A reflexão especular, referida anteriormente, é um problema inerente na inspeção deste tipo de superfícies uma vez que, quando é muito intensa, existe perda de informação sobre as zonas de reflexão uma vez que os pixels saturam em tons de branco (figura 2.13). Posto este problema, têm vindo a ser desenvolvidas técnicas de iluminação especiais para contornar este problema, tais como: •Iluminação uniforme do cenário com lâmpadas florescentes e instalação de uma caixa de plástico 70% transparente na zona onde a peça é inspecionada. Este método não se tornou completamente eficaz uma vez que as zonas mais escuras da imagem não são uniformizadas e, dado que no caso de estudo em causa a comparação das imagens é feita através do nível de cinzento, defeitos nestas zonas podem não ser detetados (figura 2.14) [6]. Desta forma, seria interessante testar outros algoritmos de inspeção para casos como este; •A desvantagem anterior é ultrapassada com o dispositivo criado em [7]. Trata-se de um anel de LEDs numa cúpula difusa. A luz difusa multi-direcional é refletida por quase todo o hemisfério, produzindo uma luz uniforme no centro da cúpula, onde as peças são bem iluminadas (figura 2.15). 2.3 Sistemas de visão aplicáveis à inspeção de peças 15 (a) Sistemas de iluminação (b) Imagem captada Figura 2.10: Iluminação direta [5] (a) Sistema de iluminação (b) Imagem captada Figura 2.11: Iluminação em contraluz [5] (a) Sistema de iluminação (b) Imagem captada Figura 2.12: Iluminação de campo escuro [5] 16 Revisão Bibliográfica Figura 2.13: Esfera metálica com reflexão especular [6] Figura 2.14: Esfera metálica com zonas escuras [6] (a) Sem defeito (b) Com deifeito Figura 2.15: Esfera iluminada uniformemente [7] Capítulo 3 Ensinamento por demonstração com controlo de força 3.1 Visão geral da solução proposta A solução proposta de polimento robotizado programado por demonstração humana consiste, numa primeira fase, na demonstração das trajetórias e das forças envolvidas nesta tarefa. A demonstração das trajetórias foi feita com recurso a uma framework de transferência de know-how chamada 6DMimic. Esta framework, através da tecnologia sincrovision, de um marcador luminoso e de um software dedicado, é capaz de gravar trajetórias demonstradas pelo operador. A gravação das forças é feita de forma sincronizada com as trajetórias. Na figura 3.1, é apresentada a zona de demonstração destas trajetórias e forças, assim como a polidora e a ferramenta de polimento utilizada. 17 18 Ensinamento por demonstração com controlo de força (a) Zona de demonstração (b) Ferramenta de demonstração do polimento Figura 3.1: Zona e ferramenta utilizadas na demonstração da tarefa de polimento por um operador A partir das trajetórias e das forças demonstradas, seria gerado o código para o manipulador industrial UR5, da Universal Robots, para que este executasse a tarefa de polimento robotizado (figura 3.2). Neste projeto, apenas foi gerado o código a enviar para o manipulador relacionado com as trajetórias. O controlo de força foi somente testado e, devido à falta de tempo, não se conseguiu integrar no sistema final. Figura 3.2: Manipulador industrial UR5, da Universal Robots, na execução da tarefa de polimento robotizado 3.2 Hardware 19 3.2 Hardware Nesta secção, será apresentado todo o hardware utilizado na solução proposta de polimento robotizado programado por demonstração. 3.2.1 Manipulador industrial – UR5 O manipulador industrial utilizado foi o UR5 da Universal Robots (figura 3.3). Optou-se por este manipulador, visto que já possuía o controlo de força necessário para a tarefa de polimento robotizado e ser apropriado para funcionar junto de operadores humanos, sem necessidade de segurança extra. No entanto, era sabido à partida que este controlo não seria muito preciso, uma vez que é feito com base na leitura das correntes elétricas nos motores das articulações. Assim sendo, um dos objetivos desta dissertação, foi efetuar testes ao controlo referido, como será visto posteriormente. Figura 3.3: Manipulador industrial UR5 da Universal Robots Na tabela seguinte, são apresentadas as especificações principais deste manipulador industrial. Especificações sobre as forças não são dadas pela marca. 20 Ensinamento por demonstração com controlo de força Node articulações 6 Raio de trabalho 850 mm Peso 18.4 kg Carga de trabalho máxima 5 kg Alcance das articulações +/- 360◦ Velocidades Todas as articulações: 180◦/s Ferramenta: Típico 1 m/s Repetibilidade +/- 0.1 mm Comunicação TCP/IP 100 Mbit: IEEE 802.3u, 100BASE-TX Ethernet socket & Modbus TCP Classificação IP IP54 Consumo de potência Aproximadamente 200 watts num programa típico Temperatura de funcionamento 0 - 50◦C Alimentação 100-240 VAC, 50-60 Hz Link para a folha de especificações Clique aqui Link para o manual de utilizador Clique aqui Link para o manual de programação (via URScript) Clique aqui Tabela 3.1: Manipulador industrial UR5 da Universal Robots - Especificações principais 3.2.2 Sensor de força/binário O sensor de força/binário utilizado para gravar as forças demonstradas na tarefa de polimento, foi o Gamma da ATI Industrial Automation (figura 3.4). Este sensor, também foi usado para medir as forças controladas pelo manipulador industrial, como será visto mais à frente. Figura 3.4: Sensor de força/binário Gamma da ATI Industrial Automation Na tabela seguinte, são apresentadas as especificações principais deste sensor, para a montagem através de uma Net Box (figura 3.5), que será, também, apresentada de seguida. A calibração 3.2 Hardware 21 utilizada foi a SI-65-5, dadas as forças envolvidas no polimento das peças. Este sensor era configurável através de uma página web disponibilizada pela marca. Calibração Eixos SI-32-2.5 SI-65-5 SI-130-10 Alcances de medição Fx, Fy (N) 32 65 130 Fz (N) 100 200 400 Tx, Ty (Nm) 2.5 5 10 Tz (Nm) 2.5 5 10 Resolução Fx, Fy (N) 1/160 1/80 1/40 Fz (N) 1/80 1/40 1/20 Tx, Ty (Nm) 1/2000 10/13333 1/800 Tz (Nm) 1/2000 10/13333 1/800 Peso 0.255 kg Diametro 75.4 mm Altura 33.3 mm Link para a folha de especifcações Clique aqui Tabela 3.2: Sensor de força/binário Gamma da ATI Industrial Automation - Especificações principais O dispositivo Net Box, uma vez ligado ao sensor, serve para o alimentar e medir as suas componentes da força e do binário. Fornece interfaces de comunicação Ethernet e CAN bus, sendo compatível com os protocolos Ethernet padrão, Ethernet/IPTM e DeviceNetTM (todas as informações sobre este dispositivo podem ser consultadas aqui). A interface de comunicação utilizada foi a Ethernet, tendo sido alimentado pela mesma interface através de um switch PoE (Power over Ethernet). Usou-se um computador pessoal com as caraterísticas apresentadas na tabela seguinte, para comunicar com a Net Box. 28 Ensinamento por demonstração com controlo de força (a) Montagem no SOLIDWORKS - Vista expandida (b) Montagem no SOLIDWORKS (c) Montagem real Figura 3.14: Montagem 2 - Ferramenta de demonstração da tarefa de polimento 3.3 Comunicação 29 (a) Montagem no SOLIDWORKS - Vista expandida (b) Montagem no SOLIDWORKS (c) Montagem real Figura 3.15: Montagem 3 - Ferramenta de polimento robotizado, utilizada pelo manipulador industrial, com sensor de força/binário 3.3 Comunicação Nesta secção, será explicada como foi estabelecida a comunicação entre o computador pessoal apresentado anteriormente, com o manipulador industrial UR5, o sensor de força/binário e o 6DMimic. 30 Ensinamento por demonstração com controlo de força 3.3.1 UR5 Este manipulador, poderia ser programado a três diferentes níveis: •Através do teach pendant; •Através do envio de instruções URScript a partir de um PC; •Através de uma API em C. Escolheu-se a segunda alternativa, devido à documentação disponibilizada e à possibilidade de o fazer a partir de uma aplicação cliente a correr num PC, ligado ao controlador do manipulador usando sockets TCP/IP. Esta conexão foi feita através de um cabo ethernet, usando a porta 30002 e o IP estático do UR5, configurado através do teach pendant. A leitura do estado do manipulador (p. ex. posição do ponto central da ferramenta, posições e velocidades das articulações e correntes elétricas nos motores), foi feita através da mesma comunicação especificada acima. De seguida, será explicado com maior detalhe as instruções utilizadas para controlar este manipulador, assim como a descodificação da trama enviada com o seu estado interno. 3.3.1.1 Envio de instruções URScript URScript é uma linguagem de programação, utilizada para programar o manipulador industrial em causa. Como qualquer outra linguagem de programação, esta tem variáveis, tipos, instruções de controlo de fluxo do programa, funções, etc. Toda a documentação sobre esta linguagem de programação pode ser consultada aqui. A frequência da comunicação na porta utilizada era 10 Hz (10 instruções por segundo), como tal, o envio de cada instrução seguiu-se de uma espera de 100 ms, para se ter a certeza que as instruções eram recebidas pelo controlador. Para a execução de trajetórias, foram enviadas instruções de configuração e longas listas de instruções de movimentação do ponto central da ferramenta. As instruções de movimentação, foram dadas para as poses da ferramenta. Estas poses, correspondem a uma posição (X, Y e Z) e a uma orientação (Rx, Ry e Rz) da ferramenta do manipulador em relação ao seu referencial base - hX Y Z Rx Ry Rzi. Os valores da posição foram usados em metros e os da orientação em radianos, segundo a notação de representação de rotações Eixo-Ângulo. Estas listas de movimentações, foram enviadas a partir dos pontos das trajetórias gravadas. Todas as instruções foram enviadas sobre a forma de programas completos, que corriam no controlador do manipulador logo após o seu envio. No entanto, estas instruções também poderiam ser enviadas de forma isolada, sem estarem dentro de programas, e também corriam logo após ser concluído o envio de cada uma. Contudo, esta não seria uma forma cómoda de controlar o manipulador, uma vez que o envio de cada movimentação teria de ser precedido de um teste para averiguar se a movimento anterior já estaria concluído. Caso isto não fosse feito, a última movimentação enviada sobrepunha-se a todas as outras enviadas anteriormente. Com o envio de 3.3 Comunicação 31 programas completos, este problema não acontece, uma vez que cada movimentação só é executada após a conclusão da sua precedente. A instrução de movimentação escolhida foi o movel (movimento linear no espaço da ferramenta), para que o movimento sobre o disco da polidora fosse linear. Para além da pose, esta função recebia uma aceleração, a, em m/s2, uma velocidade, v, em m/s, um tempo, t, em se um raio de tolerância para a paragem do ponto central da ferramenta, r, em m. O parâmetro t, sendo opcional, não foi usado uma vez que este tem prioridade sobre os parâmetros aceleração e velocidade. A aceleração e o raio de tolerância utilizados foram 4m/s2e 0.012m, respetivamente, uma vez que foi para estes valores que se obteve uma maior semelhança entre a trajetória demonstrada e a executada pelo manipulador. A velocidade da movimentação era retornada pelo 6DMimic, como será visto mais à frente. Resumindo, a instrução de movimentação enviada tinha sempre o seguinte formato, onde a pose enviada tinha o formato indicado em cima (com os parêntesis retos): •movel(pose, a=4.0, v=v_ponto, r=0.012) Para além das instruções de movimentação, foram também enviadas nos cabeçalhos dos programas, as seguintes instruções de configuração do manipulador: •set_gravity - Para especificar a montagem do manipulador. Esta especificação foi feita através de um vetor com magnitude e sentido inverso ao da aceleração da gravidade, expresso no referencial base do manipulador. Dada a montagem e o referencial base do UR5 apresentados na figura 3.16, o vetor especificado foi h9.82 0 0i; Figura 3.16: Montagem do UR5, numa estrutura dual-arm, e respetivo referencial base •set_tcp - Para especificar o ponto central da ferramenta, expresso no referencial base do elemento terminal do manipulador (figura 3.17), sobre o formato de pose. Para a montagem 32 Ensinamento por demonstração com controlo de força 1 da ferramenta de polimento, a pose especificada foi h000.138 0 0 0ie para a montagem 3, h000.221 0 0 0i; Figura 3.17: Referencial base do elemento terminal do UR5 •set_payload - Para especificar o peso das ferramentas, sendo 1 kg, para a montagem 1, e 2.3 kg, para a montagem 3. Na figura 3.18, é apresentado o diagrama de atividade da função execute_task, implementada para enviar uma trajetória demonstrada para o UR5. Todas as funções para controlar este manipulador, foram desenvolvidas em C++ e encontram-se disponíveis na secção B.1.1 dos anexos. Para dar início à execução da trajetória, tinha de se carregar o ficheiro de output do 6DMimic com extensão .segs (apresentado posteriormente) e carregar em "Ler pontos e executar movimentos", na interface de teste desenvolvida ilustrada na figura 3.19. 3.3 Comunicação 33 Figura 3.18: Diagrama de atividade do algoritmo implementado para enviar uma trajetória demonstrada para o UR5 34 Ensinamento por demonstração com controlo de força Figura 3.19: Interface de teste desenvolvida para carregar os pontos da trajetória demonstrada e iniciar a sua execução com o UR5 3.3.1.2 Leitura do estado A leitura do estado do UR5, foi feita através da descodificação de uma trama, cuja documentação pode ser consultada aqui. De forma geral, na porta 30002, o controlador deste manipulador vai atualizando uma trama com o formato apresentado na figura 3.20. Esta atualização é feita a uma velocidade de 10 Hz. 3.3 Comunicação 35 Figura 3.20: Formato da trama com a informação do estado do UR5 Quando o "Robot Message Type" tinha o valor 16, significava que a mensagem enviada era do tipo "Robot State" e, dessa forma, poderia ser percorrido o resto da trama à procura de informações sobre o estado do robô. A tabela seguinte apresenta os possíveis valores e respetivos significados que pode assumir o "Package-Type": Valor Significado 0Robot Mode Data 1Joint Data 2Tool Data 3Masterboard Data 4Cartesian Info 5Kinematics Info 6Configuration Data 7Force Mode Data 8Additional Info 9Calibration Data Tabela 3.4: Tipos de pacotes enviados na trama de estado do UR5 Onde, por exemplo, num pacote do tipo Cartesian Info, temos as seguintes informações sobre a posição e a orientação do referencial da ferramenta, em relação ao referencial base do manipulador: 36 Ensinamento por demonstração com controlo de força Tamanho (bytes) Tipo de dados Significado 8 double X (m) 8 double Y (m) 8 double Z (m) 8 double Rx (rad) 8 double Ry (rad) 8 double Rz (rad) ... Tabela 3.5: Algum conteúdo dentro de um pacote do tipo Cartesian Info Ordem da receção da informação no buffer:TCP/IP network byte order Posto isto, desenvolveram-se funções para ler as seguintes informações atuais: •Pose da ferramenta do manipulador em relação ao referencial base do manipulador - get_actual_tcp_pose; •Posições das articulações - get_actual_joints_pos; •Velocidades das articulações - get_actual_joints_speed. Estas funções, podem ser consultadas na secção B.1.2 dos anexos. O algoritmo desenvolvido resumiu-se à pesquisa das informações pretendidas na trama recebida. A leitura do estado do manipulador, embora não tenha sido utilizada diretamente nesta parte do projeto, foi bastante útil no desenvolvimento do programa final do sistema automático de inspeção de peças, que será apresentado no capítulo seguinte. 3.3.2 Sensor de força/binário A comunicação com o sensor de força foi feita através de sockets UDP/IP, segundo o protocolo cuja documentação pode ser consultada no capítulo 10 deste manual. A interface de comunicação disponibilizada usa RDT (Raw Data Transfer), o que possibilitou a transferência de dados até 7000 Hz. A trama de pedido enviada para a Net Box tinha o seguinte formato: Tipo de dados Signifiado Uint16 Cabeçalho estático (command_header = 0x1234) Uint16 Comando a executar (command) Uint32 Node amostras para ler (sample_count) Tabela 3.6: Formato da trama de pedido enviada para a Net Box 3.3 Comunicação 37 Tendo-se enviado os valores command = 0x0002 (Start high-speed real-time streaming) e sample_count = 1, quando se pretendeu ler as componentes das forças e binários atuadas no sensor, e o valor command = 0x0042, quando se pretendeu tarar o sensor. Quando se pretendeu ler as componentes das forças e binários, a trama de resposta tinha o seguinte formato: Tipo de dados Significado Uint32 Node sequência do pacote (rdt_sequence) Uint32 Noda sequência de registo interna (ft_sequence) Uint32 Código do estado do sistema (status) Int32 Força no eixo X (Fx) em N Int32 Força no eixo Y (Fy) em N Int32 Força no eixo Z (Fz) em N Int32 Binário no eixo X (Tx) em Nm Int32 Binário no eixo Y (Ty) em Nm Int32 Binário no eixo Z (Tz) em Nm Tabela 3.7: Formato da trama de resposta enviada pela Net Box, quando se pretendeu ler as componentes das forças e binários Quando se pretendeu pretendeu calibrar a tara do sistema, não havia trama de resposta. Com o envio deste pedido, as componentes das forças e dos binários ficavam todas muito próximas de zero. As forças e binários eram envidas sobre a forma de contagens. Como se configurou 10000000 contagens por N e por Nm (unidades utilizadas para a força e o binário, respetivamente), o valor real das forças foi obtido dividindo os valores recebidos por 10000000. Estabeleceu-se um período de amostragem de 50 ms (20 amostras por segundo). Todo o código desenvolvido para comunicar com o sensor, é apresentado na secção B.2 dos anexos. 3.3.3 6DMimic A comunicação com o 6DMimic foi feita através de sockets UDP/IP. Desenvolveu-se uma aplicação Master (servidor UDP), que enviava os comandos de início e fim da gravação das trajetórias, para a aplicação Slave (cliente UDP - 6DMimic). O IP e as portas da aplicação Master, eram configuráveis através da interface gráfica do 6DMimic (figura 3.21). 44 Ensinamento por demonstração com controlo de força •Do referencial base de demonstração (referencial 0), para o referencial base do UR5 (referencial 3) - H3 0. A figura seguinte, apresenta todas as transformações homogéneas tidas em conta nos cálculos. Figura 3.26: Transformações homogéneas envolvidas para transformar os pontos das trajetórias demonstradas para o referencial base do UR5 Na figura acima, para além das matrizes H1 2eH3 0, no processo de calibração as matrizes H0 1e H0 2têm o seguinte significado: •H0 1- Matriz de transformação homogénea formada pela matriz de rotação (R0 1) e pelo vetor deslocamento (v0 1), associados ao ponto do marcador luminoso dado pelo ficheiro de output do 6DMimic, no momento de calibração; •H0 2- Matriz de transformação homogénea formada pela matriz de rotação R0 2e pelo vetor deslocamento v0 2, associados ao ponto central da ferramenta dado pelo teach pendant do MOTOMAN (em relação ao referencial base do manipulador), no momento de calibração. Na aquisição deste ponto, foi necessário ter-se corretamente configurada a ferramenta do manipulador. Estes pontos de calibração foram adquiridos com a ferramenta orientada perpendicularmente ao disco da polidora, e com o ponto central da ferramenta encostado a um ponto de referência marcado no mesmo disco. Desta forma, do processo de calibração resultava a matriz H1 2, dada da seguinte forma: 3.4 Demonstração de trajetórias 45 H1 2= (H0 1)−1H0 2=H1 0H0 2(3.1) A matriz H3 0, foi obtida por inspeção dos referências de base do MOTOMAN e do UR5. Devido às montagem dos manipuladores, esta matriz é formada pela matriz de rotação (R3 0) correspondente a uma rotação de 90◦em torno do eixo y3, e pelo vetor de deslocamento nulo. Por último, uma vez que a distância da polidora à origem dos referenciais base dos manipuladores era diferente, o último passo da calibração consistiu no cálculo de um vetor deslocamento para corrigir este problema. Sendo v3 UR_tool o vetor deslocamento associado ao ponto central da ferramenta do UR5 em relação ao seu referencial base, adquirido no momento de calibração, o vetor deslocamento em causa, v, foi calculado da seguinte forma: v=R3 0v0 2−v3 UR_tool (3.2) A partir das matrizes e do vetor calculados, H1 2,H3 0e v, os pontos das trajetórias demonstrados foram corretamente transformados da seguinte forma: H3 2=H3 0H0 2−Hv=H3 0H0 1H1 2−Hv(3.3) Onde Hvé a matriz de transformação homogénea formada pela matriz de rotação nula e pelo vetor deslocamento v, e H3 2é a matriz de transformação homogénea formada pela matriz de rotação e o vetor deslocamento, associados ao ponto central da ferramenta em relação ao referencial base do UR5. Este último ponto considerado, era o enviado na instrução de movimentação do manipulador. 3.4.2 Suavização A suavização das trajetórias foi feita através da diminuição do número de instruções enviadas para o UR5. Devido ao facto dos pontos das trajetórias estarem espaçados por apenas 80 ms no tempo, para trajetórias lentas eram geradas demasiadas instruções de movimentação para espaços relativamente curtos. Para piorar a situação, verificou-se que a aquisição dos pontos da trajetória apresentavam um desvio padrão máximo no eixo x do referencial base da demonstração de 0.48 mm. Este facto provocava pequenas vibrações no ponto central da ferramenta, mesmo para uma demonstração parada auxiliada pelo MOTOMAN. Este problema era atenuado com a especificação da tolerância de paragem do ponto central da ferramenta, r, especificado na função movel. Contudo, para os movimentos de rotação da ferramenta muito utilizados no polimento, esta tolerância não funcionava pois tratava-se de uma tolerância linear e não angular. Desta forma, para rotações demonstradas de, por exemplo 90◦, em 46 Ensinamento por demonstração com controlo de força torno do eixo z da ferramenta executadas com alguma velocidade, o UR5 demorava muito mais tempo a executar essa rotação devido à sobrecarga de instruções enviadas. O diagrama de atividade seguinte, apresenta o algoritmo implementado para suavizar a trajetória demonstrada, ajustar a sua velocidade e realizar a transformação dos seus pontos expressos no referencial base da demonstração, para o referencial base do UR5. Na secção B.4 dos anexos, está presente o código da função transform_6DMimic_points, correspondente ao diagrama de atividade em causa. Figura 3.27: Diagrama de atividade do algoritmo implementado para transformar a trajetória demonstrada A ação 1 do diagrama de atividade acima, correspondente à suavização da trajetória, foi implementada através do seguinte algoritmo: 1. Para começar, é criado um vetor sem nenhum elemento para os pontos da trajetória suavizada; Para cada ponto associado à trajetória demonstrada, expresso em relação ao referencial base da demonstração, é feito o seguinte: 2. Se o ponto em causa corresponder ao primeiro ou ao último ponto da trajetória demonstrada, então esse ponto é imediatamente inserido no início ou no fim, respetivamente, do vetor dos pontos da trajetória suavizada. Se isto se verificar, o passo a seguir é ignorado; 3. É calculado o valor absoluto da rotação e do deslocamento entre o referencial associado ao ponto em causa e o referencial associado ao último ponto no vetor dos pontos da trajetória suavizada. Tendo em conta os valores da rotação e do deslocamento obtidos, um dos seguintes passos é executado: 3.4 Demonstração de trajetórias 47 3.1. Se a rotação e o deslocamento forem inferiores a uma tolerância angular e de deslocamento, respetivamente, são acumuladas em variáveis auxiliares as componentes do ponto em causa no que diz respeito à sua velocidade, posição e orientação, assim como é incrementado o número total de pontos acumulados; 3.2. Se a rotação ou o deslocamento forem iguais ou superiores a uma tolerância angular ou de deslocamento, respetivamente, primeiramente são acumuladas as componentes referidas no ponto anterior do ponto em causa, assim como é incrementado o número de pontos acumulados. Posteriormente, são calculadas as médias dessas componentes (referentes a todos os pontos acumulados) e é inserido o ponto médio resultante no vetor dos pontos da trajetória suavizada. Por fim, são colocadas a zero todas as variáveis auxiliares utilizadas. Para o cálculo do valor absoluto da rotação entre os referenciais, referido no ponto 3, usaramse quaterniões. Sendo q1o quaternião da rotação do referencial associado ao ponto em causa (expresso em relação ao referencial base da demonstração), e q2o quaternião da rotação do referencial associado ao último ponto no vetor dos pontos da trajetória suavizada (também expresso em relação ao referencial base da demonstração), o quaternião da rotação entre estes dois referenciais foi calculado da seguinte forma: q3=q2q1−1(3.4) Para se realizar a comparação dessa rotação com a tolerância angular especificada, utilizouse a biblioteca Eigen para converter o quaternião em notação Eixo-Ângulo (composta por um vetor unitário e um ângulo de rotação em torno desse vetor) e usou-se o ângulo de rotação obtido nesta notação. A acumulação das componentes da orientação dos pontos em causa, também foi realizada com recurso à notação Eixo-Ângulo, onde foram tidas em conta as componentes x,yez, dos vetores unitários, e os ângulos de rotação em torno desses vetores. A chamada da função transform_6DMimic_points tem o seguinte formato: 1void demo : : transform_6DMimic_points (double a,double r,double posToleration ,←- double angleToleration ,double vel_adjust ,universal_robot : : pose ←- p_calib_6DM ,universal_robot : : pose p_calib_MOTOMAN ,universal_robot : : pose←- p_calib_UR ) Onde: •aé a aceleração dos pontos em m/s2- valor usado igual a 4 m/s2; •ré a tolerância linear de paragem do ponto central da ferramenta em mvalor usado igual a 0.012 m; 48 Ensinamento por demonstração com controlo de força •posToleration é o valor absoluto da tolerância de deslocamento mínima (em m) referida anteriormente - valor usado igual a 0.015 m; •angleToleration é o valor absoluto da tolerância angular (em graus) referida anteriormente - valor usado igual a 7◦; •vel_ad just é um parâmetro para aumentar ou diminuir a velocidade de execução da trajetória. Usou-se, por exemplo, o valor 2 para duplicar esta velocidade; •p_calib6DM,p_calib_MOTOMAN ep_calib_UR, são as poses adquiridas no momento de calibração. Os valores acima usados, foram obtidos através da realização de vários testes, até que a trajetória executada pelo UR5 correspondesse o mais fidedignamente à trajetória demonstrada pelo operador. Nota: Os pontos das trajetórias demonstradas (expressos em relação ao referencial base da demonstração) e os das trajetórias a executar (expressos em relação ao referencial de atuação), estavam em vetores de poses associados à classe de demonstração, demo, implementada em C++. 3.5 Controlo de força Nesta secção, serão apresentados os modos de controlo de força do UR5 e os testes realizados ao modo escolhido. 3.5.1 Tipos Resumidamente, este manipulador pode ser configurado para controlar a força de quatro modos diferentes: •Simples: A força é controlada segundo o eixo z do referencial selecionado; •Complexo: A força é controlada até seis graus de liberdade (Fx, Fy, Fz, Tx, Ty e Tz) segundo o referencial selecionado; •Ponto: O referencial de trabalho tem sempre o eixo y a apontar na direção formada pelo vetor, que aponta desde o ponto central da ferramenta do manipulador, até à origem do referencial selecionado; •Movimento: A força é executada perpendicularmente à direção do movimento. O modo escolhido foi o complexo, uma vez que se pretendia especificar as componentes da força segundo o referencial da ferramenta. Para mais informações sobre os modos de controlo de força, deve ser consultado o manual de utilizador indicado na secção 3.2.1. 3.5 Controlo de força 49 3.5.2 Testes e resultados Nesta secção, serão apresentados os testes realizados ao controlo de força do UR5, no modo complexo. Os testes foram realizados colocando as articulações deste manipulador industrial segundo duas configurações distintas, ilustradas nas figuras 3.29 e3.30. A direção e o sentido das forças exercidas, é a indicada pelo vetor da força, ~ F. Utilizou-se o sensor de força/binário apresentado anteriormente, para medir as forças controladas pelo UR5 no ponto central da ferramenta. Os referenciais associados ao ponto central da ferramenta do manipulador e do sensor de força, foram configurados para coincidirem com o ponto central da peça polida (referenciais apresentados na figura 3.29 e3.30). Utilizou-se uma interface gráfica acessível através do teach pendant do UR5 (figura 3.28), para se realizarem os testes ao controlo de força. A partir desta interface era possível especificar: •O modo de controlo; •O referencial tido em conta; •Os eixos segundo os quais se pretendia aplicar a força; •Os valores das forças; •A velocidade de aproximação ao obstáculo. Esta foi uma maneira prática de testar o controlo de força, sem ser necessário escrever alguma linha de código. O referencial tido em conta, foi o da ferramenta apresentado anteriormente. Figura 3.28: Interface gráfica do teach pendant do UR5 para testar o controlo de força 50 Ensinamento por demonstração com controlo de força (a) Visão geral (b) Teste do eixo z do referencial da ferramenta (c) Teste do eixo y do referencial da ferramenta Figura 3.29: Configuração 1 - Testes efetuados contra o poste e a trave de uma baliza 3.5 Controlo de força 51 Figura 3.30: Configuração 2 - Testes efetuados contra uma mesa - Teste do eixo z do referencial da ferramenta Posto isto, com todas as componentes do binário a zero, assim como as da força, com exceção do eixo que se queria testar, e com uma velocidade de aproximação muito baixa para evitar forças muito elevadas no embate contra os obstáculos, obtiveram-se os resultados apresentados de seguida. Nota: Antes de se iniciar o movimento de aproximação aos obstáculos, orientou-se o eixo a testar perpendicularmente aos últimos e calibrou-se a tara do sensor de força para ignorar o peso da ferramenta. Configuração 1: Figura 3.31: Configuração de teste: 1 Eixo: y Força: 50N 52 Ensinamento por demonstração com controlo de força Figura 3.32: Configuração de teste: 1 Eixo: z Força: 30N Figura 3.33: Configuração de teste: 1 Eixo: z Força: 50N 3.5 Controlo de força 53 Figura 3.34: Configuração de teste: 1 Eixo: z Força: 60N Figura 3.35: Configuração de teste: 1 Eixo: z Força: 100N Configuração 2: 60 Inspeção por visão artificial de peças altamente refletoras e espelhadas Figura 4.3: Sistema de visão artificial - Exterior da caixa [8] Figura 4.4: Sistema de visão artificial - Interior da caixa [8] Figura 4.5: Sistema de visão artificial - Cúpula de iluminação difusa [8] 4.2 Sistema de visão artificial 61 Figura 4.6: Sistema de visão artificial - Placa reguladora da intensidade de iluminação [8] Figura 4.7: Sistema de visão artificial - Sistema de ventilação e respetiva placa de controlo manual [8] Figura 4.8: Sistema de visão artificial - Local de encaixe da câmara e respetiva lente [8] O sistema de visão artificial é alimentado a 12V e na sua intensidade máxima consome aproximadamente 1.17A (14,04W). Para alimentar o seu circuito, usou-se uma fonte de alimentação de bancada que preenchesse os requisitos anteriores. 62 Inspeção por visão artificial de peças altamente refletoras e espelhadas Nas secções seguintes serão justificadas as alterações efetuadas a este sistema. 4.2.1 Zona de inspeção A zona de inspeção da caixa teve necessariamente de ser adaptada para que fosse possível colocar as peças numa posição que permitisse a sua inspeção através do manipulador UR5. Desta forma, abriu-se a parte frontal da caixa, junto à zona de inspeção, e preencheu-se metade do seu fundo com uma placa de acrílico preto revestida com veludo preto. A última alteração está relacionada com a técnica de iluminação que foi usada e que será explicada posteriormente. As figuras 4.9a e4.9b ilustram as alterações referidas. (a) (b) Figura 4.9: Sistema de visão artificial – Adaptação da parte frontal da caixa 4.2.2 Câmara Utilizou-se a câmara monocromática DMK 31AU03.AS da The Imaging Source (figura 4.10), cujas especificações estão presentes na tabela 4.1. Esta era a câmara que vinha com o sistema de visão artificial e, uma vez que se obteve bons resultados com a mesma, não houve necessidade de alteração. 4.2 Sistema de visão artificial 63 Figura 4.10: The Imaging Source DMK 31AU03.AS Formatos de vídeo @ FPS 1024 x 768, Y800 @ 30, 15, 7.5, 3.75 FPS Gama dinâmica 8 bits Filtro de corte infravermelho Não CCD Sony ICX204AL Tipo Varrimento progressivo Formato 1/3" Resolução H: 1024 pixel, V: 768 pixel Montagem da lente C/CS Tipo de conetor USB Tensão de alimentação 4.5 VDC até 5.5 VDC (via cabo USB) Dimensões H: 50.6 mm, W: 50.6 mm, L: 50 mm Massa 265 g Tempo de exposição 1/10,000 segundos até 60 minutos Ganho 0 dB até 36 dB Temperatura máxima (operação) -5 ◦C até 45 ◦C Temperatura máxima (repouso) -20 ◦C até 60 ◦C Humidade máxima (operação) 20 % até 80 % sem condensação Humidade máxima (repouso) 20 % até 95 % sem condensação Folha de especificações Clique aqui Tabela 4.1: The Imaging Source DMK 31AU03.AS - Especificações 4.2.3 Lente No que diz respeito à lente, houve necessidade de se fazer uma adaptação e usar uma lente com maior distância de focagem. A lente que estava a ser usada era a LTC 3361/30 da PHILIPS (clique aqui para ver a folha de especificações), cuja distância de focagem era no máximo 8 mm, tendo sido feita a alteração para a lente GMHR31614MCN, da GOYO OPTICAL INC., com 16mm. Na tabela 4.2, encontram-se as especificações da última lente. As figuras 4.11a e4.11b correspondem a imagens retiradas em condições idênticas com a lente com distância de focagem de 8mm e com 64 Inspeção por visão artificial de peças altamente refletoras e espelhadas a de 16mm, respetivamente. Com esta alteração foi possível obter-se uma maior resolução da superfície da peça, realçando os defeitos nela contidos. (a) PHILIPS LTC 3361/30 (b) GOYO OPTICAL GMHR31614MCN Figura 4.11: Comparação de imagens captadas com as duas lentes Abertura (F) 1.4 Tipo Manual Rosca C Distância de focagem 16 mm Tamanho do sensor 2/3" Folha de especificações Clique aqui Tabela 4.2: GOYO OPTICAL INC. GMHR31614MCN - Especificações 4.3 Defeitos típicos As figuras seguintes apresentam os vários tipos de defeitos observados nas peças pintadas de teste. Estes deveram-se principalmente a erros no processo de pintura e secagem das peças, ou então a problemas ocorridos durante o transporte das mesmas. Como se pode reparar nas figuras, os defeitos estavam relacionados a saliências e concavidade, e/ou a perda significativa de refletividade e de espelhamento. Figura 4.12: Defeitos tipicamente observados na superfície superior da peça 4.4 Técnica de iluminação 65 Figura 4.13: Defeitos tipicamente observados na superfície lateral da peça 4.4 Técnica de iluminação A iluminação difusa é por definição a indicada para inspecionar peças altamente refletoras e espelhadas, como já foi explicado na secção 2.3.3. Esta foi a técnica de iluminação utilizada no que diz respeito à direção da luz, tendo sido usada para o efeito a cúpula de iluminação difusa referida anteriormente. 66 Inspeção por visão artificial de peças altamente refletoras e espelhadas No sentido de se obter uma imagem idêntica às apresentadas na figura 2.15, foram feitas algumas experiências variando a intensidade da iluminação (através da placa reguladora da intensidade de iluminação apresentada anteriormente), o tempo de exposição da câmara e a posição das peças na zona de inspeção. Note-se que, ao aumentar o tempo de exposição da câmara, maior é a quantidade de luz captada do ambiente, ficando desta forma a imagem mais clara. A figura 4.14 corresponde a uma imagem captada com a intensidade da iluminação no mínimo, o tempo de exposição da câmara de 1/25 s e a superfície superior da peça perpendicularmente à câmara e à fonte de iluminação. Pode observar-se que, devido ao facto da câmara ser concêntrica com a fonte de iluminação, a quantidade de luz incidente na peça e refletida para a câmara é tanta que os pixeis na zona da peça saturam no máximo (reflexão especular), impossibilitando a extração de qualquer caraterística. Figura 4.14: Imagem retirada com a intensidade da iluminação no mínimo, tempo de exposição de 1/25 s e a superfície superior da peça perpendicularmente à câmara e à fonte de iluminação Posto isto e uma vez que a intensidade da iluminação já estava no mínimo, diminui-se o tempo de exposição da câmara até os pixeis deixarem de estar saturados, tendo-se obtido a imagem da figura 4.15. Esta alternativa não se tornou viável devido ao seguintes motivos: •É espelhado o interior da cúpula difusa (apresentado na figura 4.5) que, ao ser irregular, provoca variações elevadas nas intensidades dos pixeis na superfície da peça; •A lente da câmara é espelhada, apresentando-se como uma mancha negra na superfície da peça; •O ruído gaussiano é acentuado, confundindo-se facilmente com os defeitos mais pequenos. Desta forma, conclui-se que a posição da peça apresentada não era viável. 4.4 Técnica de iluminação 67 Figura 4.15: Imagem retirada com a intensidade da iluminação no mínimo, tempo de exposição de 1/250 s e a superfície superior da peça perpendicularmente à câmara e à fonte de iluminação Posicionando a superfície superior da peça obliquamente à câmara e à iluminação, obteve-se o resultado da figura 4.16 (com a intensidade da iluminação no mínimo e um tempo de exposição da câmara de 1/120 s). Figura 4.16: Imagem retirada com a intensidade da iluminação no mínimo, tempo de exposição de 1/120 s e a superfície superior da peça obliquamente à câmara e à fonte de iluminação Esta opção mostrou-se viável devido aos seguintes factos: •Alguns defeitos típicos são saliências ou concavidades, tendo tornado a utilização de iluminação difusa de campo escuro viável. Recorde-se que, por definição, a iluminação de campo escuro é usada para detetar saliências e concavidades, como foi apresentado na secção 2.3.3; 68 Inspeção por visão artificial de peças altamente refletoras e espelhadas •A superfície uniforme do veludo de revestimento do interior da caixa do sistema de visão artificial (apresentado na figura 4.4 e reforçado na adaptação feita ilustrada na figura 4.9), mostrou-se viável para espelhar na superfície das peças, uma vez que não provoca variações elevadas e bruscas nas intensidades dos pixeis; •Alguns defeitos típicos estão relacionados com a perda da refletividade e do espelhamento nessas zonas, não sendo corretamente espelhada a superfície uniforme do veludo referido no tópico anterior, facilitando a deteção dos defeitos. Esta técnica, contudo, apresenta reflexões especulares em zonas problemáticas da peça, como a borda da superfície superior e uma parte da superfície lateral, devido à posição dessas zonas em relação à iluminação/câmara. Tentou eliminar-se este efeito, novamente, diminuindo o tempo de exposição da câmara, no entanto, observou-se que à medida que se diminuía esse tempo, os defeitos perdiam realce e passavam a ser indetetáveis (figura 4.17). Contrariamente, com o tempo de exposição em 1/120 s e aumentando a intensidade da luz, observou-se que a zona afetada pela reflexão especular não aumentou significativamente (em relação à zona obtida numa imagem com os defeitos suficientemente realçados) e que os defeitos foram realçados ainda mais(figuras 4.18a e4.18b). Figura 4.17: Imagem retirada com a intensidade da iluminação no mínimo, tempo de exposição de 1/250 s e a superfície superior da peça obliquamente à câmara e à cúpula de iluminação difusa Para se obter a imagem final, ajustou-se a posição e a inclinação da peça, o foco da lente e encontrou-se um ponto de equilíbrio entre a intensidade da iluminação e o tempo de exposição da câmara. Neste último ponto, teve-se em conta o facto de que as imagens seriam adquiridas com a peça em movimento e portanto, o tempo de exposição da câmara teria de ser baixo para evitar o rasto deixado pela peça na imagem. Encontrou-se o melhor resultado para um tempo de exposição de 1/120 s e uma intensidade de iluminação correspondente ao sistema de visão artificial a consumir 1.17A (14,04W/12V). No entanto, optou-se por usar uma intensidade mais baixa, o 4.5 Algoritmo de deteção de defeitos 69 correspondente a 0.40A (4.8W/12V), uma vez que uma intensidade superior a esta apenas realça defeitos muito pequenos que mais tarde seriam rejeitados pelo algoritmo de deteção. Desta forma, não se justificava estar a consumir uma potência superior a 4.8W. A figura 4.18 faz o comparativo entre estas duas situações, estando assinalada uma zona onde o caso referido acontece. (a) Imagem retirada com uma intensidade de iluminação correspondente ao sistema de visão artificial a consumir 0.40A (4,8W/12V) (b) Imagem retirada com uma intensidade de iluminação correspondente ao sistema de visão artificial a consumir 1.17A (14,04W/12V) Figura 4.18: Comparação de imagens adquiridas para um tempo de exposição da câmara de 1/120 s, a superfície superior da peça obliquamente à câmara e à fonte de iluminação e intensidades de iluminação distintas Conclui-se esta secção, apresentando na figura 4.19 duas imagens adquiridas nos testes finais do sistema de inspeção automática desenvolvido. A técnica de iluminação utilizada foi a difusa frontal de campo escuro com as especificações acima indicadas. (a) Imagem de uma peça a ser inspecionada na superfície superior (b) Imagem de uma peça a ser inspecionada na superfície lateral Figura 4.19 4.5 Algoritmo de deteção de defeitos Este algoritmo tinha como principal objetivo a identificação dos defeitos na superfície da peça. 76 Inspeção por visão artificial de peças altamente refletoras e espelhadas Figura 4.31: Diagrama de atividade do algoritmo de segmentação dos defeitos baseado na aplicação do operador Laplaciano 4.5.1.2 Baseada no gradiente morfológico O gradiente morfológico é normalmente usado na deteção de orlas. Este operador corresponde à diferença entre a dilatação e a erosão de uma imagem. Através da mesma estratégia adotada para a segmentação baseada no operador Laplaciano, obtiveram-se os resultados apresentados nas imagens seguintes, para a segmentação baseada no gradiente morfológico. O elemento estruturante considerado na aplicação deste operador, foi o apresentado anteriormente na figura 4.26. 4.5 Algoritmo de deteção de defeitos 77 (a) Superfície superior (b) Superfície lateral Figura 4.32: Resultado final da segmentação das superfícies da peça baseada no gradiente morfológico O diagrama de atividade apresentado de seguida, resume o algoritmo implementado para segmentar os defeitos usando o gradiente morfológico. O código implementado está disponível na secção C.2 dos anexos. 78 Inspeção por visão artificial de peças altamente refletoras e espelhadas Figura 4.33: Diagrama de atividade do algoritmo de segmentação dos defeitos baseado na aplicação do gradiente morfológico 4.5.1.3 Baseada noutros filtros Usando estratégias idênticas às apresentadas até agora, mas baseadas em operadores de 1a derivada, obtiveram-se os resultados da figura seguinte. Os operadores utilizados foram os filtros de Sobel e de Sharr, ambos com núcleos de dimensões 3x3. 4.5 Algoritmo de deteção de defeitos 79 (a) Filtro de Sobel (b) Filtro de Sharr Figura 4.34: Resultado final da segmentação baseada noutros filtros 4.5.1.4 Conclusões A partir das últimas imagens apresentadas, pôde concluir-se que abordagens baseadas nos filtros de Sobel e de Sharr, não seriam viáveis. Relativamente à aplicação do filtro de Sobel, pode observar-se a segmentação do contorno da superfície superior da peça, que não corresponde a um defeito. Esta abordagem poderia ser considerada caso se tivesse a certeza que a peça se posicionaria sempre no mesmo local e, desta forma, usava-se uma máscara de seleção mais estreita, para selecionar apenas a zona correspondente à superfície interior da peça. Como isto não acontecia, tal abordagem não pôde ser considerada. No decorrer dos testes realizados, verificou-se que a peça ao rodar na zona de inspeção sofria uma pequena oscilação, devido ao facto da pega de encaixe na peça não ser exatamente concêntrica com a rotação. As abordagens baseadas no operador Laplaciano e no gradiente morfológico, apresentaram bons resultados. Nas figuras abaixo, pode comparar-se os defeitos segmentados, através destas duas abordagens, e os visíveis nas imagens adquiridas com a câmara usada e com a policromática de um smartphone. Foram considerados todos os defeitos que tipicamente aparecem nestas peças. Pôde constatar-se que: •Os defeitos segmentados através da abordagem baseada no operador Laplaciano, seguem com maior rigor a forma e o tamanho dos defeitos reais, comparativamente aos segmentados através da abordagem baseada no gradiente morfológico (figura 4.35); •A abordagem baseada no gradiente morfológico tem uma resposta mais satisfatória para o tipo de defeitos apresentados nas figuras 4.37 e4.41. Posto isto, optou-se pela abordagem baseada no gradiente morfológico, uma vez que existe maior garantia na deteção do último tipo de defeito referido. 80 Inspeção por visão artificial de peças altamente refletoras e espelhadas (a) Defeitos adquiridos com a câmara monocromática usada (b) Defeitos adquiridos com a câmara policromática de um smartphone (c) Resultado da segmentação baseada no operador Laplaciano (d) Resultado da segmentação baseada no gradiente morfológico Figura 4.35: Comparação entre os defeitos reais presentes na superfície superior da peça e os segmentados - Peça 33 - Ângulo: 120◦ (a) Defeitos adquiridos com a câmara monocromática usada (b) Defeitos adquiridos com a câmara policromática de um smartphone (c) Resultado da segmentação baseada no operador Laplaciano (d) Resultado da segmentação baseada no gradiente morfológico Figura 4.36: Comparação entre os defeitos reais presentes na superfície superior da peça e os segmentados - Peça 31 - Ângulo: 0◦ (a) Defeitos adquiridos com a câmara monocromática usada (b) Defeitos adquiridos com a câmara policromática de um smartphone (c) Resultado da segmentação baseada no operador Laplaciano (d) Resultado da segmentação baseada no gradiente morfológico Figura 4.37: Comparação entre os defeitos reais presentes na superfície superior da peça e os segmentados - Peça 40 - Ângulo: 270◦ 4.5 Algoritmo de deteção de defeitos 81 (a) Defeitos adquiridos com a câmara monocromática usada (b) Defeitos adquiridos com a câmara policromática de um smartphone (c) Resultado da segmentação baseada no operador Laplaciano (d) Resultado da segmentação baseada no gradiente morfológico Figura 4.38: Comparação entre os defeitos reais presentes na superfície lateral da peça e os segmentados - Peça 32 - Ângulo: 342◦ 82 Inspeção por visão artificial de peças altamente refletoras e espelhadas (a) Defeitos adquiridos com a câmara monocromática usada (b) Defeitos adquiridos com a câmara policromática de um smartphone (c) Resultado da segmentação baseada no operador Laplaciano (d) Resultado da segmentação baseada no gradiente morfológico Figura 4.39: Comparação entre os defeitos reais presentes na superfície lateral da peça e os segmentados - Peça 00 - Ângulo: 18◦ 4.5 Algoritmo de deteção de defeitos 83 (a) Defeitos adquiridos com a câmara monocromática usada (b) Defeitos adquiridos com a câmara policromática de um smartphone (c) Resultado da segmentação baseada no operador Laplaciano (d) Resultado da segmentação baseada no gradiente morfológico Figura 4.40: Comparação entre os defeitos reais presentes na superfície lateral da peça e os segmentados - Peça 11 - Ângulo: 0◦ 84 Inspeção por visão artificial de peças altamente refletoras e espelhadas (a) Defeitos adquiridos com a câmara monocromática usada (b) Defeitos adquiridos com a câmara policromática de um smartphone (c) Resultado da segmentação baseada no operador Laplaciano (d) Resultado da segmentação baseada no gradiente morfológico Figura 4.41: Comparação entre os defeitos reais presentes na superfície lateral da peça e os segmentados - Peça 40 - Ângulo: 18◦ 4.5 Algoritmo de deteção de defeitos 85 4.5.2 Identificação dos defeitos A identificação dos defeitos foi feita com base na imagem previamente segmentada, usando o gradiente morfológico. Aplicou-se uma função disponibilizada pelo OpenCV, para encontrar os contornos dos defeitos segmentados (findContours). Através destes contornos e recorrendo a outra função disponibilizada (moments), extraíram-se os centros de massa e as áreas dos defeitos segmentados. A partir dos valores das áreas dos defeitos, rejeitaram-se aqueles que tinham uma área inferior a 60 px, para a superfície superior, e inferior a 70 px, para a superfície lateral. Estes valores foram obtidos através de comparações entre os defeitos visíveis a olho nu e a sua área correspondente obtida. Em trabalhos futuros, poderá ser desenvolvida uma interface gráfica, onde estes parâmetros são facilmente ajustáveis pelo utilizador. Utilizando as funções de desenho da mesma biblioteca, desenharam-se retângulos vermelhos em torno dos defeitos aprovados, e verdes em torno dos rejeitados. Nas figuras seguintes, é apresentado um caso de identificação dos defeitos para cada superfície. 92 Inspeção por visão artificial de peças altamente refletoras e espelhadas Com os passos especificados, para cada peça teve-se 24 (360◦/15◦) imagens para a superfície superior e 20 (360◦/18◦) imagens para a superfície lateral, 44 imagens no total. De forma a serem observados os resultados obtidos, foram reconstruídas as superfícies das peças, a partir das imagens com os defeitos assinalados. Para a superfície superior, a reconstrução consistiu nas três etapas seguintes: 1. Transformação de perspetiva da imagem cortada pela máscara indicada na figura 4.28a, para a posição correspondente aos 0◦do círculo correspondente a esta superfície (ver função transform_sup_zone na secção C.4 dos anexos). A transformação de perspetiva é calculada com base nas posições de 4 pixeis de partida e de chegada conhecidos; 2. Rotação da imagem obtida no ponto anterior para a sua posição relativa na peça (ver função rotate na secção C.4 dos anexos); 3. Aplicação de um bitwise OR entre todas as imagens da peça para esta superfície. Para a superfície lateral, a reconstrução consistiu nas duas etapas seguintes: 1. Transformação de perspetiva da imagem cortada pela máscara indicada na figura 4.28c, para a sua posição relativa num retângulo representativo desta superfície (ver função transform_lat_zone na secção C.5 dos anexos); 2. Aplicação de um bitwise OR entre todas as imagens da peça para esta superfície. Poderá reparar-se nas imagens posteriormente apresentadas nos resultados obtidos, que as reconstruções referidas levaram ao aparecimento de linhas orientadas radialmente, para a superfície superior, e de linhas verticais, para a superfície lateral. Contudo, estas linhas não afetaram a inspeção da peça, uma vez que são uma consequência da junção das imagens, após já terem sido inspecionadas. Na figura seguinte, são apresentadas duas imagens de cada superfície, antes de serem processadas pelo algoritmo de reconstrução. (a) Superfície superior (b) Superfície lateral Figura 4.47: Imagens inspecionas, antes de serem processadas pelo algoritmo de reconstrução 4.6 Testes e resultados 93 Todos os resultados obtidos encontram-se na secção Ddos anexos, sendo que a subsecção D.1 corresponde às peças que eram consideradas boas e a D.2 às que eram consideradas más. Os tempos máximos e típicos de processamento do algoritmo de deteção de defeitos (carregamento da imagem, segmentação e identificação de defeitos), foram os indicados na tabela 4.5. O computador pessoal utilizado na realização dos testes foi o apresentado na secção 4.1. O tempo de reconstrução das superfícies não foi considerado, uma vez que é um processo utilizado apenas para a observação de resultados. O tempo de varrimento das peças através das trajetórias executadas, também não foi considerado devido ao facto do manipulador parar e esperar que seja adquirida a imagem em todos os passos de inspeção, como sugere o protocolo de comunicação implementado. Este facto fez com que cada peça demorasse cerca de 30 segundos a ser varrida. Optou-se por esta abordagem para que fosse garantido, nesta fase, o sincronismo entre as imagens adquiridas e o ângulo de rotação da peça. Contudo, este processo poderá facilmente ser acelerado, colocando a peça a rodar a uma velocidade constante e adquirindo as imagens entre variações de tempo constantes. Superfície Tempo de processamento (ms) Node frames Tempo total máximo (ms)Típico Máximo Superior 150 288 24 6912 Lateral 150 195 20 3900 Total 10812 Tabela 4.5: Tempos de processamento obtidos do algoritmo de deteção de defeitos (carregamento da imagem, segmentação e identificação de defeitos) Relativamente aos resultados obtidos na deteção dos defeitos, pôde concluir que, de forma geral, foram obtidos bons resultados com esta abordagem, uma vez que os defeitos assinalados nas imagens se assemelham na sua maioria aos visíveis a olho nu. Comparando as superfícies superiores das peças boas e más, é bem percetível a diferença na quantidade e no tamanho dos defeitos detetados (exepto o caso da peça 01 (D.2a), que a sua classificação como peça boa deixa algumas dúvidas). Verificou-se também que a presença de defeitos nas superfícies laterais, não tem grande peso na avaliação das peças, caso estes sejam de pequena dimensão. Contudo, identificaram-se algumas limitações na deteção de alguns tipos de defeitos, como por exemplo: •No caso da peça 01, é observável um "arranhão"que, por ser de certa forma suave e não retirar toda a refletividade e espelhamento nessa zona, acabou por não ser detetado. A figura seguinte ilustra o caso referido. Para solucionar este problema, terão de ser estudados outros operadores usados na deteção de orlas e outras técnicas de condicionamento do ambiente de aquisição. 94 Inspeção por visão artificial de peças altamente refletoras e espelhadas (a) Defeitos detetados (b) Imagem a cores Nota: O defeito só se tornava percetível, quando espelhava nas suas imediações uma fonte de iluminação intensa Figura 4.48: Peça 01 - Comparação entre os defeitos detetados e os reais •A inclinação da peça em relação à fonte de iluminação e à câmara, o passo adotado no varrimento da superfície superior e a focagem da lente, deviam ter sido estudados tendo em conta os defeitos na borda da superfície superior. Como se pode observar na figura seguinte, o defeito na borda era contínuo, no entanto, como uma parte dessa zona deixou de ficar tão bem focada, os defeitos não foram detetados dessa forma. Na peça apresentada, este facto não impediria que a peça fosse rejeitada. No entanto, se os defeitos coincidissem apenas com a zona que não é tão bem focada, poderiam passar despercebidos. Figura 4.49: Defeitos detetados descontínuos na borda da superfície da superior, quando na realidade eram contínuos •Por último, devido a variações elevadas nos pixeis dentro de defeitos de grandes dimensões, são detetados sub-defeitos, tendo faltado rejeitar os mesmos. Esta rejeição poderia ser feita com base na posição e na área ocupada pelos defeitos. A figura seguinte apresenta a situação referida. 4.7 Conclusões 95 Figura 4.50: Presença de sub-defeitos (assinalados a vermelho dentro do retângulo vermelho maior) 4.7 Conclusões O objetivo principal desta parte do trabalho foi desenvolver um sistema o mais genérico possível, que permitisse a inspeção de qualquer tipo de peça com superfície altamente refletora e espelhada. Pôde concluir-se que, para a inspeção deste tipo de peças pintadas ou muito bem polidas, o conceito do sistema de inspeção apresentado é viável e necessitará apenas de pequenas adaptações no que diz respeito à posição das peças na zona de inspeção, às trajetórias executadas no varrimento das peças e às máscaras de seleção das zonas de interesse. O resultado obtido com o condicionamento do ambiente de aquisição, através da técnica de iluminação difusa frontal de campo escuro e do isolamento em relação ao ambiente exterior com caixa apresentada, foi bastante satisfatório pelos motivos de seguida expostos. Os defeitos foram realçados de forma conveniente para, posteriormente, serem detetados. O fundo da caixa revestido com veludo preto, mostrou ser uma solução ótima para cobrir o fundo das imagens. Isto porque, como não se tinha a certeza da posição exata onde a peça iria ser inspecionada, a máscara de seleção teve de ser alargada, acabando por selecionar um bocado do fundo em causa. No entanto, como o fundo escolhido não provoca uma variação elevada nas intensidades dos pixéis de fronteira com a peça, este facto acabou por não ser um problema. O algoritmo desenvolvido, com a segmentação dos defeitos através da aplicação do gradiente morfológico e, posteriormente, com a deteção dos defeitos através dos contornos dos defeitos segmentados, apresentou bons resultados, uma vez que se obteve uma grande semelhança entre os defeitos detetados e os visíveis a olho nu. Contudo, este algoritmo apresentou certas limitações, como a não deteção de defeitos mais suaves, o problema referido dos defeitos na borda da superfície superior e a não rejeição dos sub-defeitos, como foi demonstrado na secção anterior. Em trabalhos futuros, será necessário desenvolver um algoritmo de avaliação das peças, para que, com base nas posições, densidades e tamanhos dos defeitos, se determine se uma peça deve ser ou não rejeitada. Também terão de ser estudadas novas soluções, de forma a diminuir o número de imagens adquiridas, visto que a cada peça correspondem 44 imagens e isso fez com que este 96 Inspeção por visão artificial de peças altamente refletoras e espelhadas sistema levasse cerca de 11 segundos (no pior dos casos) para inspecionar uma peça, o que é demasiado tempo para uma aplicação de inspeção em tempo real. As soluções a estudar para responder ao último problema poderão passar pela introdução de um conjunto de câmaras estrategicamente posicionadas e pelo processamento paralelo das imagens. Capítulo 5 Conclusões gerais e trabalho futuro Neste capítulo, serão expostas as conclusões gerais e o trabalho futuro deste projeto. Relativamente à solução proposta de polimento robotizado programado por demonstração humana, o estudo realizado sobre sistemas de robotizados com controlo de força faziam prever algumas dificuldades neste âmbito. Contudo, este problema seria ultrapassado através da utilização do manipulador industrial UR5, que já possuía controlo de força. Contudo, uma vez que este controlo era feito recorrendo às correntes elétricas nos motores das articulações do manipulador, os testes realizados mostraram que o seu desempenho dependia fortemente da posição dessas articulações e, no melhor caso, a força controlada era pouco precisa. Desta forma, no trabalho futuro deste projeto poderá vir a ser testado, se para as peças em causa a polir, um controlo com pouca precisão e sensibilidade é suficiente. Caso não o seja, existem vários sensores de força/binário no mercado com alta repetibilidade e precisão, que podem ser usados pelo UR5, para que o controlo em causa seja executado com alta precisão. O sensor utilizado neste projeto para a gravação das forças, pode ser ligado a este manipulador através do protocolo de comunicação Modbus, muito utilizado em redes de automação industrial (clique aqui para ler uma demonstração de utilização do sensor de força/binário Gamma da ATI Industrial Automation com o UR5). A programação do manipulador em causa, foi feita através do envio de instruções URScript, como foi dito anteriormente. A comunicação com o manipulador foi realizada com sucesso, contudo, no final, veio a descobrir-se que esta deveria ter sido feita pela porta 30003, e não pela 30002, uma vez que a frequência da comunicação é de 125 Hz, e na utilizada é de apenas 10 Hz. Para o envio de instruções URScript, esta alteração é imediata (alterando apenas o número da porta), no entanto, não o é para a leitura do estado interno do manipulador, uma vez que a trama enviada nesta porta tem um formato ligeiramente diferente. Desta forma, terá de ser adaptado o algoritmo de leitura da trama referida. Futuramente, para evitar o envio de programas repetidos para o manipulador, poderá ser implementado um protocolo FTP (File Transfer Protocol), que guarda os programas na memória interna do controlador do manipulador. Desta forma, um operador será capaz de dar início à execução dos programas através do teach pendant, sem ser necessária a ligação a um PC. No que diz respeito à demonstração das forças e das trajetórias, conseguiu-se implementar 97 98 Conclusões gerais e trabalho futuro corretamente uma estratégia para que estas fossem gravadas sincronizadamente e, posteriormente, fossem geradas as instruções a enviar para o manipulador industrial, para que este executasse as trajetórias demonstradas. Também foi testado o envio das instruções de controlo de força (force_mode eend_force_mode), no entanto, estas instruções não foram geradas a partir das forças demonstradas. Ficou ainda por implementar a gravação num ficheiro de texto ou numa base de dados, dos pontos associados às trajetórias e às forças demonstradas. Concretamente em relação à framework 6DMimic, sugere-se que futuramente esta possa ser melhorada, através do acrescento das seguintes funcionalidades: •Ser possível configurar o ponto central da ferramenta, em relação ao referencial associado ao centro do marcador luminoso. Com esta melhoria, a lista retornada com pontos da trajetória demonstrada, já estaria associada ao referencial do ponto central da ferramenta a utilizar, o que tornaria menos complexo o processo de calibração dos referenciais apresentado anteriormente; •O protocolo de comunicação com esta framework, poderá fazer o streaming online dos pontos das trajetórias registados. Através desta melhoria, não se teriam de utilizar ficheiros de output para transferir os pontos das trajetórias para uma aplicação Master, o que tornaria esta operação impercetível para o utilizador; •Deverá ser acrescentada alguma redundância ao sistema, para se eliminar o problema do marcador oculto. Como o sistema apresentado tem apenas duas câmaras, posicionadas lado a lado, e um marcador luminoso, ao serem demonstradas rotações, este marcador corria o risco de ficar oculto quando se posicionava atrás da ferramenta (em relação ao ponto de vista das câmaras). Esta situação poderá ser resolvida através da utilização de mais do que um marcador, posicionados de forma a que pelo menos um deles esteja sempre visível pelas câmaras, através da inserção de mais câmaras posicionadas de forma diferente, ou então, através da inserção de uma IMU (Inertial Measurement Unit) no marcador. No que diz respeito ao sistema de inspeção automática por visão artificial de peças com superfície altamente refletora e espelhada, a revisão bibliográfica fazia prever alguma dificuldade na aquisição de imagens com qualidade suficiente para serem inspecionadas, devido às reflexões especulares provocadas pela iluminação. Para contornar este problema, desenvolveu-se uma técnica eficaz de condicionamento do ambiente de aquisição das imagens, através da utilização de uma caixa de isolamento com o ambiente exterior e da utilização do tipo de iluminação difusa frontal de campo escuro. O algoritmo desenvolvido para identificar defeitos presentes neste tipo de peças, apresentou bons resultados, na medida em que os defeitos identificados correspondiam aos visíveis através do olho humano. Futuramente, deverá ser desenvolvido um algoritmo de classificação das peças, com base nas posições e áreas dos defeitos identificados, capaz de rejeitar peças defeituosas. Também terá de ser estudada uma estratégia para diminuir o número total de imagens adquiridas para Conclusões gerais e trabalho futuro 99 inspecionar uma única peça. Isto porque, para as peças de teste consideradas, tinham de ser adquiridas 44 imagens, o que tornava demoroso o seu processamento. No pior dos casos, estimou-se um tempo de processamento total de aproximadamente 11 s. Este problema poderá ser corrigido, estudando soluções que utilizem um conjunto de câmaras estrategicamente posicionadas (e não apenas uma) e que processem as imagens adquiridas paralelamente. A comparação dos algoritmos de visão baseadas em software grátis (licença BSD), como por exemplo a biblioteca do OpenCV, com abordagens baseadas em software comercial, como por exemplo o HALCON, proposta nos objetivos desta dissertação não foi feita, pois como se conseguiu obter bons resultados com o OpenCV, sem que para isso tenha sido despendido muito tempo, não faria sentido testar abordagens baseadas em software comercial. Para terminar, nos objetivos deste projeto, também foi proposto o desenvolvimento de um algoritmo capaz de classificar o estado de polimento de peças metálicas. Já não houve tempo suficiente para se abordar este problema, no entanto, pensou-se numa possível solução a implementar, apresentada de seguida. As peças metálicas bem polidas distinguem-se claramente das não polidas através da refletividade e do espelhamento da sua superfície, como foi possível observar na figura 3.11, apresentada anteriormente. Desta forma, pensou-se que o estado de polimento possa vir a ser detetado através do espelhamento de um padrão previamente conhecido, na superfície da peça a inspecionar. Caso esse padrão não fosse detetado, ou as linhas não fossem bem definidas, em certas zonas da superfície, significaria que essas zonas estariam mal polidas. A figura seguinte ilustra o caso em que esse padrão seria corretamente espelhado. 100 Conclusões gerais e trabalho futuro (a) Por polir (b) Zonas mal polidas (c) Zona mal polida ao centro Figura 5.1: Espelhamento de um padrão em peças com vários estados de polimento Anexos 101 108 Peças da ferramenta de polimento - Desenho detalhado A.4 Peça 6 - Adaptador para encaixar o sensor de força na peça 31,50 63 6 50 90° Furo vazante Parafuso M6 7 CIMA FRENTE C D E B F A 23 14 C F E A B D 2 14 3 DRAWN CHK'D APPV'D MFG Q.A UNLESS OTHERWISE SPECIFIED: DIMENSIONS ARE IN MILLIMETERS SURFACE FINISH: TOLERANCES: LINEAR: ANGULAR: FINISH: DEBURR AND BREAK SHARP EDGES NAME SIGNATURE DATE MATERIAL: DO NOT SCALE DRAWING REVISION TITLE: DWG NO. SCALE:1:2 SHEET 1 OF 1 A4 WEIGHT: Peça 6 A.5 Peça 7 - Adaptador para encaixar a peça 5 no manipulador industrial 109 A.5 Peça 7 - Adaptador para encaixar a peça 5 no manipulador industrial 103 6,60 12,60 6 50 89,20 90° CC Furo vazante e escareado Parafuso M6 Furo roscado Parafuso M6 63 31,50 A A BAIXO 7 15 SECTION A-A 6 SECTION C-C CIMA C D E B F A 23 14 C F E A B D 2 14 3 DRAWN CHK'D APPV'D MFG Q.A UNLESS OTHERWISE SPECIFIED: DIMENSIONS ARE IN MILLIMETERS SURFACE FINISH: TOLERANCES: LINEAR: ANGULAR: FINISH: DEBURR AND BREAK SHARP EDGES NAME SIGNATURE DATE MATERIAL: DO NOT SCALE DRAWING REVISION TITLE: DWG NO. SCALE:1:2 SHEET 1 OF 1 A4 WEIGHT: Peça 7 110 Peças da ferramenta de polimento - Desenho detalhado Anexo B Código C++ - Ensinamento por demonstração com controlo de força B.1 Manipulador industrial UR5 da Universal Robots - Comunicação B.1.1 Controlo 1/∗sends a message to u n i v e r s a l ro bot ∗/ 2string universal_robot : : send_to_ur (char ∗send_buffer ,int buffer_len ) { 3 4string msg =this −>conn .send_msg (send_buffer ,buffer_len ) ; 5i f (msg .compare (0 , 4 , " Erro " ) == 0) return msg ; 6 7return "ok " ; 8} 9 10 /∗r e c i e v e s a message from u n i v e r s a l r ob ot ∗/ 11 string universal_robot : : recv_from_ur (char ∗recv_buffer ,int buffer_len ) { 12 13 string msg =this −>conn .recv_msg (recv_buffer ,buffer_len ) ; 14 i f (msg .compare (0 , 4 , " Erro " ) == 0) return msg ; 15 16 return "ok " ; 17 18 } 19 20 /∗ 21 Parameters 22 q : j o i n t p o s i t i o n s −b= t r u e 23 ( q can a ls o be s p e c i f i e d as a pose , p ) −b= f a l s e 24 a : j o i n t a c c e l e r a t i o n of lead in g a xis [ rad /^s2 ] 25 v : j o i n t speed of l e a ding ax i s [ rad / s ] 26 t : time [S ] 111 112 Código C++ - Ensinamento por demonstração com controlo de força 27 r : blend r a d i u s [m] 28 ∗/ 29 string universal_robot : : movej (bool b,double q[ 6 ] , universal_robot : : pose p ,←- double a,double v,double t,double r) { 30 31 ostringstream s ; 32 char ∗send_buffer ; 33 34 i f (b== f a l s e ) { 35 36 s.str (std : : string ( ) ) ; 37 38 s<< " movej ( p [ " << p.X<< " , " << p.Y<< " , " << p.Z<< " , " << 39 p.Rx << " , " << p.Ry << " , " << p.Rz << 40 " ] , a=" << a<< " , v=" << v<< " , r=" << r<< " ) \ n" ; 41 42 send_buffer =_strdup (s.str ( ) . c_str ( ) ) ; 43 44 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 45 46 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffer + 0) ; 47 48 Sleep (UR_COMM_DELAY ) ; 49 } 50 51 else { 52 53 s.str (std : : string ( ) ) ; 54 s<< " movej ( [ " << q[ 0 ] << " , " << q[ 1 ] << " , " << q[ 2 ] << " , " << 55 q[3] << " , " << q[4] << " , " << q[5] << 56 " ] , a=" << a<< " , v=" << v<< " , t =" << t<< " , r=" << r←- << " ) \ n" ; 57 58 send_buffer =_strdup (s.str ( ) . c_str ( ) ) ; 59 60 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 61 62 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffer + 0) ; 63 64 Sleep (UR_COMM_DELAY ) ; 65 66 } 67 68 return "ok " ; 69 } 70 71 string universal_robot : : movel (universal_robot : : pose p ) { 72 73 ostringstream s ; B.1 Manipulador industrial UR5 da Universal Robots - Comunicação 113 74 char ∗send_buffer ; 75 76 s.precision (7) ; 77 78 s.str (std : : string ( ) ) ; 79 80 s<< " movel ( p [ " << p.X<< " , " << p.Y<< " , " << p.Z<< " , " << 81 p.Rx << " , " << p.Ry << " , " << p.Rz << 82 " ] , a=" << p.a<< " , v=" << p.v<< " , r=" << p.r<< " ) \ n" ; 83 84 send_buffer =_strdup (s.str ( ) . c_str ( ) ) ; 85 86 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 87 88 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffer + 0) ; 89 90 Sleep (UR_COMM_DELAY ) ; 91 92 return "ok " ; 93 94 } 95 96 string universal_robot : : force_mode (pose task_frame ,int selection_vector [6 ] , ←- double wrench [ 6 ] , int type ,double limits [ 6 ] ) { 97 98 ostringstream s ; 99 char ∗send_buffer ; 100 101 s.precision (7) ; 102 103 s.str (std : : string ( ) ) ; 104 105 s<< " force_mode ( p [ " << task_frame .X<< " , " << task_frame .Y<< " , " <<←- task_frame .Z<< " , " << 106 task_frame .Rx << " , " << task_frame .Ry << " , " << task_frame .Rz << 107 " ] , [ " << selection_vector [ 0] << " , " << selection_vector [1] << " , " <<←- selection_vector [2 ] << " , " 108 << selection_vector [3] << " , " << selection_vector [4] << " , " << ←- selection_vector [5 ] << " ] " << 109 " , [ " << wrench [0] << " , " << wrench [1] << " , " << wrench [2] << " , " 110 << wrench [ 3 ] << " , " << wrench [ 4 ] << " , " << wrench [ 5 ] << " ] " << 111 " , " << type << 112 " , [ " << limits [0] << " , " << limits [1] << " , " << limits [2] << " , " 113 << limits [ 3 ] << " , " << limits [ 4 ] << " , " << limits [ 5 ] << " ] ) \ n" ; 114 /∗ 115 s << " force_mode ( to ol_ pos e ( ) , [ " << s e l e c t i o n _ v e c t o r [ 0] << " , " << ←- s e l e c t i o n _ v e c t o r [ 1] << " , " << s e l e c t i o n _ v e c t o r [ 2] << " , " 116 << s e l e c t i o n _ v e c t o r [ 3] << " , " << s e l e c t i o n _ v e c t o r [ 4] << " , " << ←- s e l e c t i o n _ v e c t o r [ 5] << " ]" << 114 Código C++ - Ensinamento por demonstração com controlo de força 117 " , [" << wrench [ 0 ] << " , " << wrench [ 1 ] << " , " << wrench [2] << " , " 118 << wrench [ 3 ] << " , " << wrench [4 ] << " , " << wrench [5 ] << " ] " << 119 " , " << type << 120 " , [" << l i m i t s [0] << " , " << l i m i t s [1 ] << " , " << l i m i t s [ 2] << " , " 121 << l i m i t s [3 ] << " , " << l i m i t s [ 4] << " , " << l i m i t s [ 5] << " ]) \ n " ; 122 ∗/ 123 send_buffer =_strdup (s.str ( ) . c_str ( ) ) ; 124 125 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 126 127 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffe r + 0) ; 128 129 Sleep (UR_COMM_DELAY ) ; 130 131 return "ok " ; 132 } 133 134 /∗Resets the robot mode from force mode to normal op e ra t i o n . ∗/ 135 string universal_robot : : end_force_mode ( ) { 136 137 ostringstream s ; 138 char ∗send_buffer ; 139 140 s.precision (7) ; 141 142 s.str (std : : string ( ) ) ; 143 144 s<< " end_force_mode () \n" ; 145 146 send_buffer =_strdup (s.str ( ) . c_str ( ) ) ; 147 148 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 149 150 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffe r + 0) ; 151 152 Sleep (UR_COMM_DELAY ) ; 153 154 return "ok " ; 155 156 } 157 158 string universal_robot : : end_teach_mode ( ) { 159 160 char ∗send_buffer =" def end_teach ( ) : \ n" ; 161 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 162 Sleep (UR_COMM_DELAY ) ; 163 164 send_buffer =" end_teach_mode ( ) \ n" ; 165 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; B.1 Manipulador industrial UR5 da Universal Robots - Comunicação 115 166 Sleep (UR_COMM_DELAY ) ; 167 168 send_buffer =" end \ n" ; 169 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 170 Sleep (UR_COMM_DELAY ) ; 171 172 return "success"; 173 174 } 175 176 string universal_robot : : teach_mode ( ) { 177 178 char ∗send_buffer =" def s t a r t _ t e a c h ( ) : \ n" ; 179 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 180 Sleep (UR_COMM_DELAY ) ; 181 182 send_buffer =" def f r e e d r i v i n g ( ) : \ n" ; 183 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 184 Sleep (UR_COMM_DELAY ) ; 185 186 send_buffer =" teach_mode ( ) \ n" ; 187 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 188 Sleep (UR_COMM_DELAY ) ; 189 190 send_buffer =" get_actual_tcp_pose () \n"; 191 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 192 Sleep (UR_COMM_DELAY ) ; 193 194 send_buffer =" s l e e p (500) \ n" ; 195 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 196 Sleep (UR_COMM_DELAY ) ; 197 198 send_buffer =" end_teach_mode ( ) \ n" ; 199 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 200 Sleep (UR_COMM_DELAY ) ; 201 202 send_buffer =" end \ n" ; 203 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 204 Sleep (UR_COMM_DELAY ) ; 205 206 send_buffer =" while True : \ n" ; 207 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 208 Sleep (UR_COMM_DELAY ) ; 209 210 send_buffer =" f r e e d r i v i n g ( ) \ n" ; 211 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 212 Sleep (UR_COMM_DELAY ) ; 213 214 send_buffer =" end \ n" ; 116 Código C++ - Ensinamento por demonstração com controlo de força 215 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 216 Sleep (UR_COMM_DELAY ) ; 217 218 send_buffer =" end \ n" ; 219 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 220 Sleep (UR_COMM_DELAY ) ; 221 222 return "success"; 223 } 224 225 /∗Sends URscrip program header −configuration∗/ 226 string universal_robot : : send_prog_header (double gravity [3 ] , universal_robot : : ←- pose tcp ,double payload ) { 227 228 ostringstream s ; 229 s.precision (16) ; 230 231 char ∗send_buffer =" def prog ( ) : \ n " ; 232 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 233 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffe r + 0) ; 234 235 Sleep (UR_COMM_DELAY ) ; 236 237 s.str (std : : string ( ) ) ; 238 s<< " s e t _ g r a v i t y ( [ " << gravity [ 0 ] << " , " << gravity [ 1 ] << " , " << ←- gravity [2 ] << " ] ) \ n" ; 239 send_buffer =_strdup (s.str ( ) . c_str ( ) ) ; 240 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffe r + 0) ; 241 / / s p r i n t f ( send_ bu ffe r , " s e t _ g r a v i t y ([% f ,%f ,% f ] ) \ n " , g r a v i t y [ 0 ] , g r a v i t y ←- [ 1 ] , g r a v i t y [ 2 ] ) ; 242 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 243 244 Sleep (UR_COMM_DELAY ) ; 245 246 s.str (std : : string ( ) ) ; 247 s<< " tcp =p [ " << tcp .X<< " , " << tcp .Y<< " , " << tcp .Z<< " , " << tcp←- .Rx << " , " << tcp .Ry << " , " << tcp .Rz << " ] \ n" ; 248 send_buffer =_strdup (s.str ( ) . c_str ( ) ) ; 249 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffe r + 0) ; 250 / / s p r i n t f ( send_buffer , " tcp=p[%f ,%f ,%f ,%f ,%f ,% f ] \ n " , tcp .X, tcp .Y, tcp . ←- Z , tcp . Rx , tcp . Ry , tcp . Rz ) ; 251 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 252 253 Sleep (UR_COMM_DELAY ) ; 254 255 send_buffer =" s e t _ t c p ( tcp ) \ n " ; 256 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 257 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffe r + 0) ; 258 B.1 Manipulador industrial UR5 da Universal Robots - Comunicação 117 259 Sleep (UR_COMM_DELAY ) ; 260 261 s.str (std : : string ( ) ) ; 262 s<< " set_payload(" << payload << " ) \ n" ; 263 send_buffer =_strdup (s.str ( ) . c_str ( ) ) ; 264 / / s p r i n t f ( send_buffer , " set_payload (%f ) \ n " , payload ) ; 265 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 266 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffer + 0) ; 267 268 Sleep (UR_COMM_DELAY ) ; 269 270 return "ok " ; 271 } 272 273 /∗Sends URscrip program f o o t e r −end∗/ 274 string universal_robot : : send_prog_footer ( ) { 275 276 char ∗send_buffer =" end \ n" ; 277 send_to_ur (send_buffer ,strlen (send_buffer ) ) ; 278 / / p r i n t f (" program l i n e : %.∗s \ n " , send_buffer , se nd_bu ffer + 0) ; 279 280 Sleep (UR_COMM_DELAY ) ; 281 282 return "ok " ; 283 284 } 285 286 /∗Performs a ta s k from the given po i nt s ∗/ 287 string universal_robot : : execute_task (vector<universal_robot : : pose>points ,←- universal_robot : : pose tcp ,double payload ) { 288 289 double gravity [ 3 ] = { 9.82 , 0 , 0 }; 290 this −>send_prog_header (gravity ,tcp ,payload ) ; 291 292 double q[ 6 ] ; 293 int i= 0; 294 for (std : : vector<pose >::iterator it =points .begin ( ) ; it != points .end ( ) ; ←- ++it ) { 295 this −>movel (points [i] ) ; 296 cout << endl << ( ( points .size ( ) −(i+ 1) ) ∗UR_COMM_DELAY ) / 1000 << "←- more seconds to go . . . " << endl << endl ; 297 / / t h i s −>movej ( f a l s e , q , p o i n t s [ i ] , poi n t s [ i ] . a , po i n t s [ i ] . v , p o i n t s [ i ] . ←- t , po i nt s [ i ] . r ) ; 298 i++; 299 } 300 301 this −>send_prog_footer ( ) ; 302 303 return "success"; 124 Código C++ - Ensinamento por demonstração com controlo de força 237 } 238 239 } 240 241 } 242 243 244 245 Sleep (time_joint_adjust ) ; 246 247 } B.2 Sensor de força/torque - Comunicação 1string ft_sensor : : read_ft ( ) { 2 3byte request [RESQUEST_ARRAY_LEN ] ; /∗The re q u e s t data se n t to the ←- Net F / T . ∗/ 4char resquest_char [RESQUEST_ARRAY_LEN ] ; 5RESPONSE resp ;/∗The s t r u c t u r e d response re ce ived from the Net F←- / T . ∗/ 6byte response [RESPONSE_ARRAY_LEN ] ; /∗The raw response data ←- r ec e iv ed from t he Net F / T . ∗/ 7char response_char [RESPONSE_ARRAY_LEN ] ; 8int i;/∗Generic loop / a r r a y index . ∗/ 9int err ;/∗E rro r s t a t u s of o p e r a t i o n s . ∗/ 10 char ∗AXES [ ] = { "Fx" ,"Fy" ," Fz " ,"Tx" ,"Ty" ,"Tz" }; /∗The names of ←- the fo r c e and torque axes . ∗/ 11 12 ∗(uint16∗)&request [0] = htons (0x1234 ) ; /∗standar d header . ∗/ 13 ∗(uint16∗)&request [2] = htons (COMMAND_Start ) ; /∗per t a b l e 9.1 in Net F / T ←- user manual . ∗/ 14 ∗(uint32∗)&request [4] = htonl (NUM_SAMPLES ) ; /∗see s e c t i o n 9.1 in Net F /T ←- user manual . ∗/ 15 16 for (unsigned int i= 0; i<RESQUEST_ARRAY_LEN ;i++) 17 resquest_char [i] = ( char)request [i] ; / / Byte to char conversion 18 19 string msg =this −>conn .send_msg (resquest_char ,RESQUEST_ARRAY_LEN ) ; 20 i f (msg .compare (0 , 4 , " Erro " ) == 0) return msg ; 21 22 msg =this −>conn .recv_msg (response_char ,RESPONSE_ARRAY_LEN ) ; 23 i f (msg .compare (0 , 4 , " Erro " ) == 0) { 24 return msg ; 25 } 26 B.3 6DMimic - Comunicação 125 27 for (unsigned int i= 0; i<RESPONSE_ARRAY_LEN ;i++) 28 response [i] = (byte )response_char [i] ; / / Char to byte conversion 29 30 resp .rdt_sequence =ntohl (∗(uint32 ∗)&response [ 0 ] ) ; 31 resp .ft_sequence =ntohl (∗(uint32 ∗)&response [ 4 ] ) ; 32 resp .status =ntohl (∗(uint32 ∗)&response [ 8 ] ) ; 33 34 for (i= 0; i< 6; i++) { 35 resp .FTData [i] = ntohl (∗(int32∗)&response[12 + i∗4 ] ) ; 36 } 37 38 /∗Output the response data . ∗/ 39 printf (" St a t u s : 0x%08x \ n" ,resp .status ) ; 40 for (i= 0; i< 6; i++) 41 { 42 this −>ft_values [i] = resp .FTData [i] / COUNTS_PER_FORCE ; 43 printf ("%s : %.4 f %s \ n" ,AXES [i] , this−>ft_values [i] , FORCE_UNIT ) ; 44 } 45 46 return msg ; 47 } 48 string ft_sensor : : remove_bias ( ) { 49 50 byte request [RESQUEST_ARRAY_LEN ] ; /∗The r e q u es t dat a s en t to t he Net F / T . ←- ∗/ 51 char resquest_char [RESQUEST_ARRAY_LEN ] ; 52 53 ∗(uint16∗)&request [0] = htons (0x1234 ) ; /∗standa r d header . ∗/ 54 ∗(uint16∗)&request [2] = htons (COMMAND_Bias ) ; /∗per t a b l e 9.1 i n Net F / T ←- user manual . ∗/ 55 56 for (unsigned int i= 0; i<RESQUEST_ARRAY_LEN ;i++) 57 resquest_char [i] = ( char)request [i] ; / / Byte to char conversion 58 59 string msg=t h i s −>conn .send_msg (resquest_char ,RESQUEST_ARRAY_LEN ) ; 60 61 return msg ; 62 } B.3 6DMimic - Comunicação 1void sixDmimic : : start_and_sync (const char ∗file_name ) { 2 3char buf [BUFLEN ] ; 4int npos ; 5double a,x,y,b,delay ; 126 Código C++ - Ensinamento por demonstração com controlo de força 6double first_frame ,offset ; 7 8string s_buf ="START6DM " ; 9s_buf .append (string (file_name ) ) ; 10 cout << " Sent to 6D−Mimic : " << s_buf << endl ; 11 12 npos =s_buf .length ( ) ; 13 strncpy (buf ,s_buf .c_str ( ) , sizeof(buf ) ) ; 14 int sizedest =sizeof(dest ) ; 15 16 int ret =sendto (s,buf ,npos , 0 , (sockaddr∗)&dest ,sizedest ) ; 17 i f (ret == −1) { 18 printf (" \ nSend Error Code : \ n" ) ; 19 } 20 21 22 fflush (stdout ) ; 23 24 / / limpar o b u f f e r com o memset −Acrescentar o ócdigo 25 26 / / t ry to rec e iv e some data , t h i s i s a blocking c a l l 27 i f ( ( recvfrom (s,buf ,BUFLEN , 0 , ( sockaddr ∗)&dest , &slen ) ) == SOCKET_ERROR←- ) 28 { 29 printf (" recvfrom ( ) f a i l e d with e r r o r code : %d" ,WSAGetLastError ( ) ) ; 30 exit (EXIT_FAILURE ) ; 31 } 32 33 / / t1 34 a=std : : chrono : : duration_cast<std : : chrono : : microseconds >(std : : chrono : : ←- system_clock : : now ( ) . time_since_epoch ( ) ) . count ( ) ; 35 36 s_buf =string (buf ) ; 37 38 / / t0 39 x=stod (s_buf .substr (10) ) ; 40 41 offset =a−x; 42 43 fflush (stdout ) ; 44 / / limpar o b u f f e r com o memset −Acrescentar o ócdigo 45 / / t ry to rec e iv e some data , t h i s i s a blocking c a l l 46 i f ( ( recv_len =recvfrom (s,buf ,BUFLEN , 0 , ( sockaddr ∗)&dest , &slen ) ) == ←- SOCKET_ERROR ) 47 { 48 printf (" recvfrom ( ) f a i l e d with e r r o r code : %d" ,WSAGetLastError ( ) ) ; 49 exit (EXIT_FAILURE ) ; 50 } 51 B.3 6DMimic - Comunicação 127 52 s_buf =string (buf ) ; 53 54 first_frame =stod (s_buf .substr (15) ) ; 55 56 cout << "∗S t a r t and sync in fo \ n \ n" ; 57 cout << "∗∗ S t a r t " << endl ; 58 printf ("6Dmimic time x : %f \ n" ,x) ; 59 printf (" Local time a : %f \ n" ,a) ; 60 printf ("6DMimic clock o f f s e t (ms) : %f \ n" ,offset / 1000) ; 61 cout << "∗∗ 1sFrame " << endl ; 62 printf ("6Dmimic time f i r s t frame : %f \ n" ,first_frame ) ; 63 64 fstream fs ; 65 fs .open ("6DM_start_sync_info . txt" ,ios : : out |ios : : trunc ) ; 66 i f ( ! fs ) 67 { 68 std : : cerr << " Cannot open the out pu t f i l e : 6 DM_start_sync_info " << std←- : : endl ; 69 return; 70 } 71 fs << " c l o c k _ o f f s e t : " << offset << endl ; 72 fs << "first_frame : " << first_frame << endl ; 73 fs .close ( ) ; 74 75 / / Sync with NTP 76 77 /∗ 78 char buf [BUFLEN ] ; 79 i n t npos ; 80 double a , x , y , b , delay ; 81 82 s t r i n g s_buf = "START6DM " ; 83 s_buf . append ( s t r i n g ( file_name ) ) ; 84 cout << " Sent to 6D−Mimic : " << s_buf << endl ; 85 86 npos = s_buf . l en gt h ( ) ; 87 s tr nc py ( buf , s_buf . c _ s t r ( ) , s i z e o f ( buf ) ) ; 88 i n t s i z e d e s t = s i z e o f ( d es t ) ; 89 90 a = s t d : : chrono : : d urat i o n _ c ast < s t d : : chrono : : microseconds >( std : : chrono : : ←- system_clock : : now ( ) . time_since_epoch ( ) ) . count ( ) ; 91 92 i n t r e t = sendto ( s , buf , npos , 0 , ( sockaddr ∗)&dest , s i z e d e s t ) ; 93 i f ( r e t == −1) { 94 p r i n t f ( " \ nSend Error Code : \ n " ) ; 95 } 96 97 f f l u s h ( s td ou t ) ; 98 / / áNecessrio limpar o b u f f e r com o memset −Acrecentar o ócdigo 128 Código C++ - Ensinamento por demonstração com controlo de força 99 100 / / t ry to rec e iv e some data , t h i s i s a blocking c a l l 101 i f ( ( recvfrom ( s , buf , BUFLEN, 0 , ( sockaddr ∗)&dest , &s l e n ) ) == SOCKET_ERROR←- ) 102 { 103 p r i n t f (" recvfrom ( ) f a i l e d with e r ro r code : %d " , WSAGetLastError ( ) ) ; 104 e x i t ( EXIT_FAILURE) ; 105 } 106 107 s_buf = s t r i n g ( buf ) ; 108 109 x = sto d ( s_buf . s u b s t r (10) ) ; 110 111 f f l u s h ( s td ou t ) ; 112 / / c l e a r the b u f fe r by f i l l i n g null , i t might have pre v i o u sly receiv e d data 113 / / limpar o b u f f e r com o memset −Acrescentar o ócdigo 114 115 / / t ry to rec e iv e some data , t h i s i s a blocking c a l l 116 i f ( ( re cv_ len = recvfrom ( s , buf , BUFLEN, 0 , ( sockaddr ∗)&dest , &sl e n ) ) == ←- SOCKET_ERROR) 117 { 118 p r i n t f (" recvfrom ( ) f a i l e d with e r ro r code : %d " , WSAGetLastError ( ) ) ; 119 e x i t ( EXIT_FAILURE) ; 120 } 121 122 b = s t d : : chrono : : du r atio n _ c a s t < std : : chrono : : microseconds >( std : : chrono : : ←- system_clock : : now ( ) . time_since_epoch ( ) ) . count ( ) ; 123 124 s_buf = s t r i n g ( buf ) ; 125 126 y = sto d ( s_buf . s u b s t r (15) ) ; 127 128 cout << " ∗S t a r t and sync i nf o \ n \ n " ; 129 cout << " ∗∗ S t a r t " << endl ; 130 p r i n t f ( " Local time a : %f \ n " , a ) ; 131 p r i n t f ("6 Dmimic time x : %f \ n " , x ) ; 132 cout << " ∗∗ 1sFrame " << endl ; 133 p r i n t f ("6 Dmimic time y : %f \ n " , y ) ; 134 p r i n t f ( " Local time b : %f \ n " , b ) ; 135 136 delay = ( b −a ) −( y −x ) ; 137 offset = x −( a + ( delay / 2) ) ; 138 139 p r i n t f ( " b−a (ms) : %f \ n " , ( b −a ) / 1000) ; 140 p r i n t f ( " y−x (ms) : %f \ n " , ( y −x ) /1000) ; 141 142 cout << " Communication delay (ms) : " << delay /1000 << endl ; 143 p r i n t f ("6DMimic clock o f f s e t (ms) : %f \ n " , o f f s e t /1000) ; 144 B.3 6DMimic - Comunicação 129 145 std : : f stream fs ; 146 fs . open ( " sync_log . t x t " , std : : fstre am : : in | st d : : fstream : : out | s t d : : ←- fstrea m : : app ) ; 147 fs << delay / 1000 << " " << o f f s e t / 1000 << "\ n " ; 148 fs . c lo se ( ) ; 149 150 return; 151 ∗/ 152 } 153 154 155 int sixDmimic : : stop ( ) { 156 157 char buf [BUFLEN ] ; 158 string s_buf ; 159 int npos ; 160 161 printf (" Sent to 6D−Mimic : STOP6DM\ n" ) ; 162 163 int ret =sendto (s,"STOP6DM" , 7 , 0 , ( sockaddr ∗)&dest ,sizeof(dest ) ) ; 164 i f (ret == −1) { 165 printf (" \ nSend Erro r Code : \ n" ) ; 166 } 167 168 cout << " Recieved from 6DM: " ; 169 170 fflush (stdout ) ; 171 172 / / t ry to rec e iv e some data , t h i s i s a blocking c a l l 173 i f ( ( recv_len =recvfrom (s,buf ,BUFLEN , 0 , ( sockaddr ∗)&dest , &slen ) ) == ←- SOCKET_ERROR ) 174 { 175 printf (" recvfrom ( ) f a i l e d with e r r o r code : %d" ,WSAGetLastError ( ) ) ; 176 exit (EXIT_FAILURE ) ; 177 } 178 179 s_buf =string (buf ) ; 180 181 i f (s_buf .compare ("6DM_END" ) == 0) { 182 cout << "END" << endl ; 183 return 0; 184 } 185 186 e l s e i f (s_buf .compare ("6DM_END_NOK" ) == 0) { 187 cout << "END NOK" << endl ; 188 return −1; 189 } 190 191 cout << " Trash " << endl ; 130 Código C++ - Ensinamento por demonstração com controlo de força 192 return −2; 193 194 } B.4 Função desenvolvida para transformar os pontos da trajetória da zona de demonstração para a zona de atuação e suavizá-la antes de ser enviada para o UR5 1void demo : : transform_6DMimic_points (double a,double r,double posToleration ,←- double angleToleration ,double vel_adjust ,universal_robot : : pose ←- p_calib_6DM ,universal_robot : : pose p_calib_MOTOMAN ,universal_robot : : pose←- p_calib_UR ) { 2 3 4/ / Calculo da t ran sfor mac ao homogenea da e s f e r a para o TCP −H_1_2 5Matrix4d H_0_1_calib ,H_0_2_calib ,H_1_2 ; 6Matrix3d R_0_1_calib ,R_0_2_calib ; 7AngleAxisd aa ; 8 9R_0_1_calib =AngleAxisd (p_calib_6DM .Rz ,Vector3d : : UnitZ ( ) ) 10 ∗AngleAxisd (p_calib_6DM .Ry ,Vector3d : : UnitY ( ) ) 11 ∗AngleAxisd (p_calib_6DM .Rx ,Vector3d : : UnitX ( ) ) ; 12 aa .fromRotationMatrix (R_0_1_calib ) ; 13 14 H_0_1_calib =demo : : get_HomeogeneousTransform (aa , 15 p_calib_6DM .X, 16 p_calib_6DM .Y, 17 p_calib_6DM .Z) ; 18 19 R_0_2_calib =AngleAxisd (p_calib_MOTOMAN .Rz ,Vector3d : : UnitZ ( ) ) 20 ∗AngleAxisd (p_calib_MOTOMAN .Ry ,Vector3d : : UnitY ( ) ) 21 ∗AngleAxisd (p_calib_MOTOMAN .Rx ,Vector3d : : UnitX ( ) ) ; 22 aa .fromRotationMatrix (R_0_2_calib ) ; 23 24 H_0_2_calib =demo : : get_HomeogeneousTransform (aa , 25 p_calib_MOTOMAN .X, 26 p_calib_MOTOMAN .Y, 27 p_calib_MOTOMAN .Z) ; 28 29 H_1_2 =H_0_1_calib .inverse ( ) ∗H_0_2_calib ; 30 31 32 / / Calculo da çãtransformao homogenea do TCP do MOTOMAN para o TCP do UR − ←- H_2_4 33 Matrix4d H_3_0 ,H_3_4_calib ,H_2_4 ; B.4 Função desenvolvida para transformar os pontos da trajetória da zona de demonstração para a zona de atuação e suavizá-la antes de ser enviada para o UR5 131 34 Matrix3d R_3_0 ,R_3_4_calib ,R_2_4 ; 35 36 R_3_0 =AngleAxisd (universal_robot : : degree_to_rad (0) , Vector3d : : UnitZ ( ) ) 37 ∗AngleAxisd (universal_robot : : degree_to_rad (90) , Vector3d : : UnitY ( ) ) 38 ∗AngleAxisd (universal_robot : : degree_to_rad (0) , Vector3d : : UnitX ( ) ) ; 39 aa .fromRotationMatrix (R_3_0 ) ; 40 H_3_0 =demo : : get_HomeogeneousTransform (aa , 41 0 , 42 0 , 43 0) ; 44 45 /∗Vector3d vr_3_4 = Vector3d ( p_calib_UR . Rx , p_calib_UR . Ry , p_calib_UR . Rz ) ; 46 double norm = vr_3_4 . norm ( ) ; 47 Vector3d vr_3_4_normalized = vr_3_4 . normalized ( ) ; 48 AngleAxisd aa_3_4 ( norm , vr_3_4_normalized ) ; 49 50 H_3_4_calib = demo : : get_HomeogeneousTransform ( aa_3_4 , 51 p_calib_UR .X, 52 p_calib_UR .Y, 53 p_calib_UR .Z) ; 54 55 H_2_4 = H_0_2_calib . in v er s e ( ) ∗H_3_0 . i nv er se ( ) ∗H_3_4_calib ; ∗/ 56 57 Vector3d v_motoman_calib (p_calib_MOTOMAN .X,p_calib_MOTOMAN .Y,←- p_calib_MOTOMAN .Z) ; 58 v_motoman_calib =R_3_0 ∗v_motoman_calib ; 59 60 Vector3d translacao =v_motoman_calib −Vector3d (p_calib_UR .X,p_calib_UR .←- Y,p_calib_UR .Z) ; 61 62 / / Transformacao de todos os pontos 63 Matrix4d H_0_1 ,H_3_4 ; 64 Matrix3d R_0_1 ,R_0_1_ant ,R_3_4 ; 65 Quaterniond q1 ,q2 ,q3 ; 66 AngleAxisd ang_var ; 67 Vector3d v ,deslocamento ; 68 vector<universal_robot : : pose>filtred ; 69 double norm ,x_sum ,y_sum ,z_sum ,Rx_sum ,Ry_sum ,Rz_sum ,v_sum ,ang_sum ; 70 int acumul_points = 0; 71 x_sum = 0; y_sum = 0; z_sum = 0; Rx_sum = 0; Ry_sum = 0; v_sum = 0; ←- ang_sum = 0; 72 Rz_sum = 0; 73 AngleAxisd ang_avg ; 74 75 / / c i c l o de a j u s te da t r a j e t o r i a 76 for (int c= 0; c<p_segs_file .size ( ) ; c++) { 77 78 R_0_1 =AngleAxisd (p_segs_file [c] . Rz ,Vector3d : : UnitZ ( ) ) 79 ∗AngleAxisd (p_segs_file [c] . Ry ,Vector3d : : UnitY ( ) ) 132 Código C++ - Ensinamento por demonstração com controlo de força 80 ∗AngleAxisd (p_segs_file [c] . Rx ,Vector3d : : UnitX ( ) ) ; 81 aa .fromRotationMatrix (R_0_1 ) ; 82 83 i f (c== 0) { 84 filtred .push_back (p_segs_file [c] ) ; 85 } 86 87 e l s e i f (c> 0 && c<p_segs_file .size ( ) −1) { 88 89 R_0_1_ant =AngleAxisd (filtred [filtred .size ( ) −1].Rz ,Vector3d : : ←- UnitZ ( ) ) 90 ∗AngleAxisd (filtred [filtred .size ( ) −1 ] . Ry ,Vector3d : : UnitY ( ) ) 91 ∗AngleAxisd (filtred [filtred .size ( ) −1 ] . Rx ,Vector3d : : UnitX ( ) ) ; 92 93 q1 =Quaterniond (R_0_1_ant ) ; 94 q2 =Quaterniond (R_0_1 ) ; 95 q3 =q1∗q2 .inverse ( ) ; 96 ang_var =AngleAxisd (q3 ) ; 97 98 deslocamento =Vector3d (p_segs_file [c] . X,p_segs_file [c] . Y,←- p_segs_file [c] . Z)− 99 Vector3d (filtred [filtred .size ( ) −1].X,filtred [filtred .←- size ( ) −1 ] . Y,filtred [filtred .size ( ) −1 ] . Z) ; 100 101 norm =sqrt (deslocamento .x( ) ∗deslocamento .x( ) + deslocamento .y( ) ∗←- deslocamento .y( ) + deslocamento .z( ) ∗deslocamento .z( ) ) ; 102 103 acumul_points++; 104 105 / / Angle a x i s tem uma unica çãsoluo para o eixo ( excepto para o s i n a l ←- ) , sendo o s i n a l do angulo determinado pelo s i n a l do eixo 106 i f (universal_robot : : rad_to_degree (std : : abs (ang_var .angle ( ) ) ) >= ←- angleToleration | | norm >= posToleration ) { 107 108 / / p ri n t_ p o se ( p _ s e g s _ f i l e [ c ] ) ; 109 110 x_sum += p_segs_file [c] . X;y_sum += p_segs_file [c] . Y;z_sum += ←- p_segs_file [c] . Z; 111 v_sum += p_segs_file [c] . v; 112 113 114 i f (aa .angle ( ) < 0) { 115 print_AngleAxis (aa ) ; 116 aa .axis ( ) = −1∗aa .axis ( ) ; 117 aa .angle ( ) = −1∗aa .angle ( ) ; 118 print_AngleAxis (aa ) ; 119 system (" pause " ) ; 120 } 121 B.4 Função desenvolvida para transformar os pontos da trajetória da zona de demonstração para a zona de atuação e suavizá-la antes de ser enviada para o UR5 133 122 Rx_sum += aa .axis ( ) . x( ) ; Ry_sum += aa .axis ( ) . y( ) ; Rz_sum += aa .←- axis ( ) . z( ) ; 123 ang_sum += aa .angle ( ) ; 124 125 126 ang_avg =AngleAxisd (ang_sum /acumul_points ,Vector3d (Rx_sum /←- acumul_points ,Ry_sum /acumul_points ,Rz_sum /acumul_points←- ) . normalized ( ) ) ; 127 Matrix3d ang_avg_mat =ang_avg .toRotationMatrix ( ) ; 128 129 p_segs_file [c] . X=x_sum /acumul_points ; 130 p_segs_file [c] . Y=y_sum /acumul_points ; 131 p_segs_file [c] . Z=z_sum /acumul_points ; 132 p_segs_file [c] . Rx =ang_avg_mat .eulerAngles (2 , 1 , 0) . z( ) ; 133 p_segs_file [c] . Ry =ang_avg_mat .eulerAngles (2 , 1 , 0) . y( ) ; 134 p_segs_file [c] . Rz =ang_avg_mat .eulerAngles (2 , 1 , 0) . x( ) ; 135 p_segs_file [c] . v=v_sum /acumul_points ; 136 137 / / cout << " Resu l t a nte da media : " << endl ; 138 / / p ri n t_ p o se ( p _ s e g s _ f i l e [ c ] ) ; 139 / / print_AngleAxis ( AngleAxisd ( averageRot ) ) ; 140 / / system (" pause " ) ; 141 142 filtred .push_back (p_segs_file [c] ) ; 143 144 acumul_points = 0; x_sum = 0; y_sum = 0; z_sum = 0; v_sum = 0; 145 Rx_sum = 0; Ry_sum = 0; Rz_sum = 0; ang_sum = 0; 146 } 147 else { 148 149 x_sum += p_segs_file [c] . X;y_sum += p_segs_file [c] . Y;z_sum += ←- p_segs_file [c] . Z; 150 v_sum += p_segs_file [c] . v; 151 152 i f (aa .angle ( ) < 0) { 153 print_AngleAxis (aa ) ; 154 aa .axis ( ) = −1∗aa .axis ( ) ; 155 aa .angle ( ) = −1∗aa .angle ( ) ; 156 print_AngleAxis (aa ) ; 157 system (" pause " ) ; 158 } 159 160 Rx_sum += aa .axis ( ) . x( ) ; Ry_sum += aa .axis ( ) . y( ) ; Rz_sum += aa .←- axis ( ) . z( ) ; 161 ang_sum += aa .angle ( ) ; 162 163 / / print_AngleAxis ( AngleAxisd ( R_0_1 ) ) ; 164 / / p ri n t_ p o se ( p _ s e g s _ f i l e [ c ] ) ; 165 }