Projeto, Controle E Análise De Um Manipulador Robótico Modular

Transcript

UNIVERSIDADE DE MOGI DAS CRUZES Diego Varalda de Almeida PROJETO, CONTROLE E ANÁLISE DE UM MANIPULADOR ROBÓTICO MODULAR Mogi das Cruzes Junho de 2016 UNIVERSIDADE DE MOGI DAS CRUZES Diego Varalda de Almeida  11121502987 PROJETO, CONTROLE E ANÁLISE DE UM MANIPULADOR ROBÓTICO MODULAR Projeto de Graduação apresentado ao curso de Engenharia Mecânica da Universidade de Mogi das Cruzes, como parte dos requisitos necessários à obtenção do título de Engenheiro. Orientador: Prof. Ms. Gilberto Tomáz Junior Universidade de Mogi das Cruzes – UMC Engenharia Mecânica Programa de Graduação Mogi das Cruzes Junho de 2016 Almeida, Diego Varalda de PROJETO, CONTROLE E ANÁLISE DE UM MANIPULADOR ROBÓTICO MODULAR/ Diego Varalda de Almeida. – Mogi das Cruzes, Junho de 2016236 p. : il. (algumas color.) ; 30 cm. Orientador: Prof. Ms. Gilberto Tomáz Junior Monografia – Universidade de Mogi das Cruzes – UMC Engenharia Mecânica Programa de Graduação, Junho de 2016. 1. Manipulador Robótico. 2. Controle. 2. Análise Dinâmica. I. Gilberto Tomaz Junior. II. Universidade de Mogi das Cruzes. III. Curso de Engenharia Mecânica IV. Projeto, Controle e Análise de um Manipulador Robótico Modular Dedico este trabalho aos meus pais, Suleine e Arlindo e à todos os moradores do flat 33 Agradecimentos No decorrer de minha formação algumas pessoas e instituições foram de vital importância e merecem os meus agradecimentos. Primeiramente, gostaria de expressar minha gratidão ao meu orientador, prof. Ms. Gilberto Tomáz Junior, pelas orientações proporcionadas que definitivamente contribuíram para a realização dessa monografia. Além do meu orientador, meus sincero agradecimento vai também aos professores ph.D. Jan Vorstius e ph.D. William Lewinger, da Universidade de Dundee na Escócia, pelo auxílio e orientações durante o desenvolvimento do software no estágio de verão no Laboratório de Robótica. Seus conselhos e ensinamentos me proporcionaram uma maior compreensão sobre este fascinante campo que é a robótica. Agradeço também ao Governo Brasileiro, em especial, ao Ministério da Educação e CNPq que criaram programas de incentivo à educação tais como o PROUNI e o Ciências sem Fronteiras, o que me possibilitou concluir o curso de engenharia e participar de um intercâmbio. Por fim gostaria de agradecer aos meus amigos de classe, tanto aos que conheci na UMC quanto aos que conheci na Universidade de Dundee, pelas conversas estimulantes, pela companhia e pela diversão que compartilhamos durante estes anos de graduação, tanto em sala de aula quanto fora dela. “Jamais considere seus estudos como uma obrigação, mas como uma oportunidade invejável (. . . ) para aprender a conhecer a influência libertadora da beleza do reino do espírito, para seu próprio prazer pessoal e para proveito da comunidade à qual seu futuro trabalho pertencer.” (Albert Einstein) Resumo Manipuladores robóticos são amplamente utilizados na indústria por serem máquinas de grande versatilidade. Este tipo de robô pode realizar tarefas como: pintura, soldagem, identificação e movimentação de objetos além de montagem de componentes em uma linha de montagem entre outras. Contudo, para se realizar tais tarefas o robô necessita de uma série de instruções fornecidas pelo usuário. Estas instruções são geralmente linhas de código em uma linguagem própria da máquina, que são armazenadas na memória do controlador e posteriormente lidas pelo robô. Com o intuito de agilizar a fase de programação de uma tarefa, um software de controle baseado em interface gráfica se faz necessário. Esta interface gráfica possibilita o usuário a passar instruções para o manipulador robótico através de simples opções pré-programadas podendo, inclusive, combinar estas opções para criar tarefas mais complexas. Neste trabalho, será proposto um projeto de manipulador robótico modular com seis graus de liberdade e é apresentado um programa baseado em interface gráfica desenvolvido em MATLAB para controle e simulação do manipulador. Palavras-chave: manipulador. robô. projeto. controle. software. análise. interface gráfica. Abstract Robotic Manipulators are widely used in the industry for being machines of high versatility. This type of robot can perform tasks such as painting, welding, identification and objects handling as well as component assembly on an assembly line among others. However, to perform such tasks the robot requires a series of instructions provided by the user. These instructions are usually lines of code in its own machine language, which are stored in the controller memory and later read by the robot. In order to speed up the programming phase of a task, GUI-based control software is required. This GUI allows the user to pass instructions to the robotic manipulator through simple preprogrammed options, with the possibility to combine these options to create more complex tasks. In this paper, it will be presented the design of a six degrees of freedom modular robotic manipulator in addition to a GUI-based program developed in MATLAB for robot control and simulation. Keywords: manipulator. robot. design. control. software. analysis. graphical user interface Lista de ilustrações Figura 1 – Manipuladores modular: a)Robotnik modular robot, b)Kuka LBR iiwa 7R800, c)Universal Robot UR10 . . . . . . . . . . . . . . . . . . . . . Figura 2 – Estrutura geral do robô integrado com seu ambiente . . . . . . . . . . Figura 3 – Os três ângulos de rotação de um punho esférico . . . . . . . . . . . . . Figura 4 – Configurações básicas de um manipulador robótico: (a: cartesiano; b: cilíndrico; c: polar; d: articulado). . . . . . . . . . . . . . . . . . . . . . Figura 5 – Tipos de juntas empregadas em robôs . . . . . . . . . . . . . . . . . . Figura 6 – Representação esquemática das juntas . . . . . . . . . . . . . . . . . . Figura 7 – Manipulador planar de 2 GDL com suporte em O . . . . . . . . . . . . Figura 8 – Típico volume de trabalho para configurações comuns de robôs . . . . . Figura 9 – Cinemática: a)direta; b)inversa . . . . . . . . . . . . . . . . . . . . . . Figura 10 – Movimento descoordenado (esquerda: deslocamento de cada junta; direita: trajetória do manipulador no plano) . . . . . . . . . . . . . . . . Figura 11 – Movimento sequencial (esquerda: deslocamento de cada junta; direita: trajetória do manipulador no plano) . . . . . . . . . . . . . . . . . . . Figura 12 – Movimento coordenado (esquerda: deslocamento de cada junta; direita: trajetória do manipulador no plano) . . . . . . . . . . . . . . . . . . . Figura 13 – Aproximação numérica de derivada . . . . . . . . . . . . . . . . . . . . Figura 14 – Exemplo de trajetória desenhada em CAD . . . . . . . . . . . . . . . . Figura 15 – Gráfcos da trajetória linear: a) trajetória em linha reta; b) aproximação da linha reta por pontos de passagem; c) curvas de coordenadas das juntas em função da distância linear; d) velocidade angular; e) aceleração angular . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Figura 16 – Problemas geométricos com trajetória cartesiana: a)tipo 1; b)tipo 2; c)tipo 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Figura 17 – Parâmetros de Denavit-Hartenberg . . . . . . . . . . . . . . . . . . . . Figura 18 – Manipulador do tipo articulado com os parâmetros D-H . . . . . . . . Figura 19 – Divisão da matriz de transformação homogênea . . . . . . . . . . . . . Figura 20 – Representação geométrica do Jacobiano . . . . . . . . . . . . . . . . . Figura 21 – Fluxograma de solução da Cinemática Reversa por Jacobiano . . . . . Figura 22 – Trajetória com restrição de orientação . . . . . . . . . . . . . . . . . . Figura 23 – Diagrama de blocos para o problema de controle do robô. . . . . . . . Figura 24 – Sistema de simulação do robô e controle . . . . . . . . . . . . . . . . . Figura 25 – Esquema do controle antecipatório . . . . . . . . . . . . . . . . . . . . 18 22 23 25 26 26 27 28 30 35 36 36 40 41 42 43 46 47 47 48 50 53 62 63 64 Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura Figura 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 – – – – – – – – – – – – – – – – – – – – – – Figura 48 – Figura 49 – Figura 50 – Figura 51 – Figura 52 – Figura 53 – Figura 54 – Figura 55 – Compensação de distúrbio por torque computado . . . . . . . . . . . . Diagrama de blocos do método de torque computado . . . . . . . . . . Manipulador proposto em uma configuração arbitrária . . . . . . . . . Junta do cotovelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Frameless brushless DC motor . . . . . . . . . . . . . . . . . . . . . . . Harmonic drive escolhido para a junta do cotovelo . . . . . . . . . . . . Freio permanente eletromagnético, modelo Combiperm . . . . . . . . . Trava mecânica proposta . . . . . . . . . . . . . . . . . . . . . . . . . . Sensor de torque . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Eixo principal da junta do cotovelo . . . . . . . . . . . . . . . . . . . . Sensor de posição angular (encoder) . . . . . . . . . . . . . . . . . . . Conjunto do elo 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Vista em corte do punho . . . . . . . . . . . . . . . . . . . . . . . . . . Resultado da análise feita no elo 2 . . . . . . . . . . . . . . . . . . . . Resultado da análise feita no elo 3 . . . . . . . . . . . . . . . . . . . . Resultado da análise feita no eixo principal da junta da base . . . . . . Resultado da análise feita no eixo principal da junta do cotovelo . . . . Resultado da análise feita no eixo principal da junta do punho . . . . . Manipulador proposto com os eixos de referência de cada elo . . . . . . Diagrama de blocos do controle torque computado . . . . . . . . . . . Diagrama de blocos do controle antecipatório . . . . . . . . . . . . . . Pontos de passagem com as velocidades desejadas nos pontos indicados pelas tangentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Exemplo 1 - coordenado; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . . . . . Exemplo 2 - descoordenado; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . Exemplo 3 - sequencial; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . . . . . Exemplo 4 - linear; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . . . . . Exemplo 5 - circulo; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . . . . . Exemplo 6 - elipse; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . . . . . Exemplo 7 - curva de Lissajous; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . Exemplo 8 - hipotrocoide; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . 65 65 67 69 70 71 71 72 73 73 74 75 76 77 78 79 80 81 82 85 86 88 91 92 93 94 95 96 97 98 Figura 56 – Exemplo 9 - hélice cônica; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas . . . . . . 99 Figura 57 – Vetores do centro de gravidade de cada elo . . . . . . . . . . . . . . . . 100 Figura 58 – Curvas de torque da trajetória 1 - coordenado . . . . . . . . . . . . . . 102 Figura 59 – Curvas de torque da trajetória 2 - descoordenado . . . . . . . . . . . . 102 Figura 60 – Curvas de torque da trajetória 3 - sequencial . . . . . . . . . . . . . . . 103 Figura 61 – Curvas de torque da trajetória 4 - linear . . . . . . . . . . . . . . . . . 103 Figura 62 – Curvas de torque da trajetória 5 - círculo . . . . . . . . . . . . . . . . . 104 Figura 63 – Curvas de torque da trajetória 6 - elipse . . . . . . . . . . . . . . . . . 104 Figura 64 – Curvas de torque da trajetória 7 - curva de Lissajous . . . . . . . . . . 105 Figura 65 – Curvas de torque da trajetória 8 - hipotrocoide . . . . . . . . . . . . . 105 Figura 66 – Curvas de torque da trajetória 9 - hélice . . . . . . . . . . . . . . . . . 106 Figura 67 – Posição do manipulador em diferentes momentos da simulação . . . . . 107 Figura 68 – Resultados da simulação dinâmica . . . . . . . . . . . . . . . . . . . . 108 Figura 69 – Estrutura e princípio de funcionamento do SJM III . . . . . . . . . . . 111 Figura 70 – Mensagem exibida ao iniciar o programa . . . . . . . . . . . . . . . . . 118 Figura 71 – Aba Configurações . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 Figura 72 – Conjuntos de ângulos - (a)conjunto da junta, (b)conjunto global, (c)conjunto do motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 Figura 73 – Tabela de parâmetros do robô . . . . . . . . . . . . . . . . . . . . . . . 126 Figura 74 – Propriedades de massa do desenho . . . . . . . . . . . . . . . . . . . . 127 Figura 75 – Alinhamento da modelo no software . . . . . . . . . . . . . . . . . . . 127 Figura 76 – Aba Ferramentas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129 Figura 77 – Mover efetuador com pontos de passagem . . . . . . . . . . . . . . . . 130 Figura 78 – Percurso através de funções paramétricas . . . . . . . . . . . . . . . . . 130 Figura 79 – Aba Programas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132 Figura 80 – Aba Simulação Dinâmica . . . . . . . . . . . . . . . . . . . . . . . . . . 133 Figura 81 – Janela de Animação . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134 Lista de tabelas Tabela 1 – Comparação entre diferentes métodos de cinemática inversa; modelo com um efetuador, 18 GDL, e metas dentro do volume de trabalho . Tabela 2 – Comparação entre diferentes métodos de cinemática inversa; modelo com um efetuador, 18 GDL, e metas fora do volume de trabalho . . . Tabela 3 – Capacidade de carga do manipulador . . . . . . . . . . . . . . . . . . Tabela 4 – Especificações técnicas estimadas para o manipulador . . . . . . . . . Tabela 5 – Especificações técnicas das juntas . . . . . . . . . . . . . . . . . . . . Tabela 6 – Parâmetros de Denavit–Hartenberg do manipulador proposto . . . . Tabela 7 – Tipos e características de trajetórias . . . . . . . . . . . . . . . . . . Tabela 8 – Propriedades dos elos . . . . . . . . . . . . . . . . . . . . . . . . . . Tabela 9 – Variáveis da estrutura Histórico (H) . . . . . . . . . . . . . . . . . . Tabela 10 – Variáveis da estrutura Mensagens (M) . . . . . . . . . . . . . . . . . Tabela 11 – Variáveis da estrutura Configurações Adicionais (AS) . . . . . . . . . . 31 . . . . . . . . . . 31 67 68 70 83 89 101 137 138 138 Lista de abreviaturas e siglas CAD Desenho assistido por computador. CNC Comando Numérico Computadorizado DC Corrente contínua. FEA Análise de Elementos Finitos GDL Graus de liberdade GUI Interface Gráfica do Usuário GUIDE Graphical User Interface Developer Environment NASA Administração Nacional da Aeronáutica e Espaço PCI Placa de circuito impresso PID Sistema de controle do tipo Proporcional, Integral e Derivado. PWM Modulação por Largura de Pulso RIA Robotics Institute of America RPM Rotações por minuto SI Sistema Internacional de Unidades SISO Única Entrada - Única Saída Lista de símbolos J Matrix Jacobiana t Tempo [s] θ Coordenada de uma junta de revolução, [rad] θ˙ Velocidade da variável de junta, [rad/s] θ¨ Aceleração da variável da junta, [rad/s2 ] λ Constante de amortecimento ai Coeficiente i da função polinomial L Lagrangiano qi Coordenada generalizada T Matriz de transformação homogênea Qi Forças externas generalizadas K Energia cinética, [kg.m2 s−2 ] U Energia potencial, [kg.m2 s−2 ] I Tensor de inércia, [kg.m2 ] r Vetor distância ρ Densidade do corpo, [kg.m−3 ] L Transformada de Laplace Sumário 1 1.1 1.1.1 1.2 1.3 1.4 1.5 INTRODUÇÃO . . . . . . . . . . . . . . . . . . . . . Histórico dos manipuladores robóticos industriais Manipuladores modulares . . . . . . . . . . . . . . . . . . Motivação . . . . . . . . . . . . . . . . . . . . . . . . . Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . Metodologia . . . . . . . . . . . . . . . . . . . . . . . . Organização e escopo do trabalho . . . . . . . . . . . 2 2.1 2.1.1 2.1.2 2.1.3 2.1.4 2.1.5 2.1.6 2.2 2.3 2.3.1 2.3.2 REVISÃO BIBLIOGRÁFICA . . . . . . . . . . Manipuladores robóticos . . . . . . . . . . . . . . Estrutura e componentes de um robô . . . . . . . . . Sistemas de acionamento e congurações do robô . . . Esquema de notação de juntas . . . . . . . . . . . . . Movimentos do robô e graus de liberdade . . . . . . . Volume de trabalho . . . . . . . . . . . . . . . . . . . Ciclo de funcionamento de um manipulador . . . . . Cinemática e geração de trajetória . . . . . . . . Dinâmica e controle . . . . . . . . . . . . . . . . . Modelagem dinâmica . . . . . . . . . . . . . . . . . . Controle do manipulador . . . . . . . . . . . . . . . . . . . . . . . . . 3 3.1 3.1.1 3.1.2 3.1.3 3.1.4 3.1.5 3.2 3.2.1 3.2.2 3.2.3 METODOLOGIA . . . . . . . . . . . . . . . . . . Geração de trajetórias . . . . . . . . . . . . . . . Trajetória ponto-a-ponto . . . . . . . . . . . . . . . . Trajetórias parametrizadas . . . . . . . . . . . . . . . Trajetória modelada em AutoCad . . . . . . . . . . . Trajetória linear . . . . . . . . . . . . . . . . . . . . Algoritmo anticolisão . . . . . . . . . . . . . . . . . . Cinemática do manipulador . . . . . . . . . . . . Cinemática direta . . . . . . . . . . . . . . . . . . . . Cinemática inversa: método pseudo-inverso Jacobiano Mínimos quadrados amortecido. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 17 18 19 19 19 20 . . . . . . . . . . . 21 21 21 24 25 25 27 28 29 32 32 33 . . . . . . . . . . 34 34 34 39 40 40 43 44 44 46 49 3.2.4 3.2.5 3.2.6 3.3 3.3.1 3.3.2 3.3.3 3.4 3.4.1 3.4.2 Redundância e Singularidades . . . . . . . . Velocidade e aceleração . . . . . . . . . . . . Restrições na conguração do manipulador . Dinâmica do Manipulador . . . . . . . . Conceitos preliminares . . . . . . . . . . . . Equações de movimento do manipulador . . Extendendo para um manipulador de n GDL Sistemas de controle . . . . . . . . . . . . Rastreio de um ponto de referência . . . . . Controle antecipatório e torque computado . 4 4.1 4.1.1 4.1.2 4.1.3 4.2 4.2.1 4.2.2 4.2.3 4.3 MODELAMENTO DO MANIPULADOR ROBÓTICO Requisitos iniciais . . . . . . . . . . . . . . . . . . . . . . . . Capacidade de carga . . . . . . . . . . . . . . . . . . . . . . . . Número de graus de liberdade . . . . . . . . . . . . . . . . . . Velocidade linear . . . . . . . . . . . . . . . . . . . . . . . . . Detalhamento do mecanismo . . . . . . . . . . . . . . . . . Módulo da junta . . . . . . . . . . . . . . . . . . . . . . . . . . Elos do manipulador . . . . . . . . . . . . . . . . . . . . . . . . Punho . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Análise estrutural por Elementos Finitos . . . . . . . . . 5 5.1 5.1.1 5.2 5.3 5.3.1 5.4 5.4.1 5.4.2 ANÁLISE E RESULTADOS Cinemática . . . . . . . . . . . Jacobiano . . . . . . . . . . . . . Controle . . . . . . . . . . . . . Trajetória . . . . . . . . . . . . Exemplo de trajetórias . . . . . Dinâmica . . . . . . . . . . . . Curvas de torque dos exemplos . Exemplo de simulação dinâmica 6 6.1 6.1.1 6.1.2 6.1.3 CONSIDERAÇÕES FINAIS . . . . . . . . . . . . . . . . . Sugestões para trabalhos futuros . . . . . . . . . . . . . . . Implementar um sistema de controle de força no software . . . . Reprogramação do software em outra linguagem de programação Implementação de mecanismo de segurança . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 52 52 54 54 56 58 61 62 62 . . . . . . . . . 66 66 66 66 66 67 68 75 75 76 . . . . . . . . 82 82 84 85 86 90 99 101 106 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109 . . . . 110 . . . . 110 . . . . 110 . . . . 111 REFERÊNCIAS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112 APÊNDICES 116 APÊNDICE A  SOFTWARE DE CONTROLE . A.1 Leiaute e funcionalidades do programa . . . . . A.1.1 Caixa de Mensagens . . . . . . . . . . . . . . . . . . . A.1.2 Aba Congurações . . . . . . . . . . . . . . . . . . . . A.1.2.1 Obtendo a matriz de inércia do elo A.1.3 A.1.4 A.1.5 A.1.6 A.2 A.2.1 A.2.2 A.2.3 A.3 A.3.1 A.3.2 A.3.3 A.3.4 Aba Comandos . . . . . . Aba Programas . . . . . Aba Simulação Dinâmica Janela de Animação . . . Arquivos do programa SMART-GUI.m . . . . . SMART-GUI.g . . . . . Funções . . . . . . . . . . Estruturas de dados . HISTORY.mat . . . . . . MESSAGES.mat . . . . . ROBOT-DATA.mat . . . SETTINGS.mat . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117 . . . . . . . . . . 117 . . . . . . . . . . 118 . . . . . . . . . . 118 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 128 132 133 134 135 135 135 136 136 136 136 137 138 APÊNDICE B  R DO SOFTWARE ROTINAS EM MATLAB DE CONTROLE . . . . . . . . . . . . . . . . . . . 139 APÊNDICE C  PROJETO DO MANIPULADOR . . . . . . . . 223 17 Capítulo 1 Introdução 1.1 Histórico dos manipuladores robóticos industriais A palavra robô surgiu por volta de 1920 pelo autor tcheco K. Capek em sua obra teatral Robô universal de Rossum, e é derivada da palavra robota, que significa trabalhador. Em 1941 Isaac Asimov propôs as quatro leis da robótica em sua curta história de ficção científica Runaround, com o intuito de proteger a humanidade de gerações inteligentes de robôs. As quatro leis são: Lei zero: um robô não pode causar mal à humanidade ou, por omissão, permitir que a humanidade sofra algum mal. Lei um: Um robô não pode ferir um ser humano ou, por inação, permitir que um ser humano sofra algum mal. Lei dois: Um robô deve obedecer as ordens que lhe sejam dadas por seres humanos exceto nos casos em que tais ordens entrem em conflito com a Primeira Lei. Lei três: Um robô deve proteger sua própria existência desde que tal proteção não entre em conflito com a Primeira ou Segunda Leis (JAZAR, 2007). Posteriormente, o robô industrial foi definido pelo Instituto de Robótica da América (RIA), como um manipulador multifuncional reprogramável, projetado para mover materiais, peças, ferramentas, ou outros dispositivos especializados através de movimentos programáveis e realizar uma série de outras atividades (DUYSINX; GERADIN, 2004). O primeiro robô moderno foi o Unimates, produzido por J. Engelberger no início dos anos 60. A empresa Unimation foi a primeira a comercializar robôs. Por este motivo Engelberger é conhecido como o pai da robótica. Nos anos 80 a indústria de robôs cresceu muito rápido principalmente devido ao grande investimento pela indústria automotiva. Robôs apareceram como um resultado da combinação de duas tecnologias: teleoperadores e controle numérico computadorizado (CNC) de máquinas de usinagem. Teleoperadores foram desenvolvidos durante a Guerra Mundia II para manipulagem de materiais radioativos, e CNC foi desenvolvido para aumentar a precisão que era requisitada para a manufatura de peças com novas tecnologias. Portanto, os primeiros robôs não passavam de estruturas mecânicas com comando numérico que basicamente transferiam um material de um ponto A para B (JAZAR, 2007). Os robôs tradicionais possuem manipuladores de precisão controlados por PID com altos ganhos que corrigem distúrbios rapidamente. O manipulador são movidos geralmente Capítulo 1. Introdução 18 por motores de passo ou DC conectados a uma redução enquanto que encoders rastreiam as juntas permitindo o controle da posição. Atuadores dinâmicos (incluindo massas, refletores inerciais e rigidez) influenciam drasticamente no sistema de controle e performance geral. Eles transmitem alta (idealmente infinita) impedância mecânica, o que significa que o robô resiste ao movimento quando sujeito à uma força. Os robôs também possuem alta largura de banda e movem para a posição comandada não importa a força externa agindo em suas juntas. Esta característica é perfeita para automação industrial por que permite que os robôs rastreiam trajetórias em ambientes estáticos ou mapeados, como em selecionar e levantar, por exemplo. 1.1.1 Manipuladores modulares Manipuladores robóticos são cinematicamente compostos de elos conectados por juntas para formar uma cadeia cinemática. Contudo, o robô como um sistema, consiste de um manipulador, um punho, um efetuador, atuadores, sensores, controladores, processadores, e software (JAZAR, 2007). Manipuladores modulares são geralmente, formados por juntas rotativas e ligados por elos. Possuem duas juntas na região do ombro, uma na parte do cotovelo e três juntas compondo o punho. A maioria dos manipuladores robóticos moudulares são colaborativos, isto é, permitem que o operador trabalhe no mesmo ambiente em que o robô está sem a necessidade de grades de proteção, característica esta vantajosa para tarefas complexas que o robô não consegue executar, sendo este utilizado para aumentar a velocidade do operador. A Figura 1 mostra alguns modelos existentes no mercado de manipuladores modulares. Figura 1 – Manipuladores modular: a)Robotnik modular robot, b)Kuka LBR iiwa 7R800, c)Universal Robot UR10 Fonte: Robotnik (2016), Corp. (2016), Robots (2016) Capítulo 1. Introdução 1.2 19 Motivação Uma das motivações para a realização desse trabalho se baseia na escassez de robôs modulares na indústria. A maioria dos modelos presentes no mercado atualmente são de grande porte e possuem configuração de elos e juntas fixa, o que dificulta sua aplicação em determinados ambientes. Outro motivo é o fato de que os software controladores de robôs e os próprios robôs que possuem certa precisão terem preços muito elevados, dificultando a aquisição dessas máquinas em pequenas e médias empresas. Além disso, os softwares de controle presentes atualmente são de pouco ou quase nenhuma versatilidade, isto é, são produzidos para um único robô. Por este motivo, o software de controle foi desenvolvido de maneira que possa ser empregado em vários manipuladores com configuração de juntas de revolução, precisando apenas de fazer uma modificação dos parâmetros passados ao software tais como: quantidade de juntas, massa, comprimento e centro de massa de cada elo. Por último, a interface gráfica foi projetada de maneira a apresentar ferramentas didáticas para o aprendizado da disciplina de robótica e automação nas salas de aula. O modo de simulação permite que o software simule movimentos do manipulador sem que um manipulador físico esteja conectado ao programa. 1.3 Objetivos Este trabalho tem como objetivo principal desenvolver o projeto de um protótipo de manipulador robótico modular na qual seja possível acoplar um determinado número de juntas. Também será desenvolvido neste trabalho um software de controle com interface gráfica utilizando o software Matlab capaz de controlar o manipulador robótico em questão. 1.4 Metodologia O trabalho começa com o projeto de um manipulador robótico modular utilizando o software Solidworks. Durante o desenvolvimento do manipulador, o software ANSYS será empregado para análise de elementos finitos (FEA) de suas partes. Em seguida, as equações e métodos para a análise comportamental do manipulador são estudados. Como o manipulador é modular e pode possuir diversas juntas, métodos algébricos para se calcular a cinemática e dinâmica do robô não são apropriados, devendo então fazer uso de métodos numéricos para obtenção dos resultados. Em seguida, um sistema de controle usando as equações da dinâmica é criado para o manipulador e simulado no software MATLAB. Este sistema de controle será utilizado no software desenvolvido permitindo um controle preciso do manipulador. Capítulo 1. Introdução 20 Por fim a interface gráfica é desenvolvida, utilizando a toolbox GUIDE do MATLAB. A interface gráfica apresentará uma opção de trabalhar apenas em modo de simulação, onde simulará os movimentos do robô quando este não estiver conectado ao computador, ou em modo real, que efetivamente manda o sinal para o manipulador, movimentando-o. 1.5 Organização e escopo do trabalho Este trabalho está dividido em 6 capítulos. O primeiro consistiu de uma breve introdução ao tema abordado e histórico dos manipuladores robóticos. No Capítulo 2 é apresentado uma revisão bibliográfica dos principais temas relacionados a este trabalho. Nesta revisão é mostrado os métodos existentes para modelagem cinemática e dinâmica para manipuladores robóticos, além dos diferentes tipos de sistemas de controle. É mostrado nesse capítulo quais os métodos que serão empregados e a justificativa de se usar tais métodos. O Capítulo 3 apresenta toda a metodologia para o trabalho. Neste capítulo a definição e modelagem matemática dos métodos escolhidos para a cinemática, dinâmica e sistema de controle utilizados em manipuladores robóticos são apresentados e discutidos. No Capítulo 4, a modelagem do manipulador modular proposto é apresentada. Será mostrado neste capítulo os elementos de máquinas utilizado, tais como: redutor de velocidade, transmissor de potência, sensores elétricos e outros componentes que compõem as juntas. Ao final é realizada uma análise de elementos finitos dos componentes estruturais do robô. O Capítulo 5 apresenta a análise e resultados para o manipulador de seis graus-deliberdade apresentado no Capítulo 4. É aplicado neste capítulo as equações e conceitos visto no Capítulo 3. É feita também uma discussão sobre o comportamento cinemático e dinâmico para diferentes tipos de trajetórias. No Apêndice A, o software de controle baseado em interface gráfica é apresentado. É mostrado como o programa foi desenvolvido, além do papel de cada função dentro do programa. É mostrado ao final deste apêndice como a interface gráfica é utilizada para o controle do manipulador robótico. Por fim, no Capítulo 6, são apresentadas as conclusões do trabalho e as propostas de trabalhos futuros. 21 Capítulo 2 Revisão Bibliográfica Spong e Vidyasagar (1989) sugerem que existem seis problemas que devem ser estudados para se controlar efetivamente um manipulador robótico, são eles: cinemática direta, cinemática inversa, velocidade cinemática, dinâmica, controle da posição e controle da força. Existem inúmeros métodos para se tratar cada problema citado, o modelo dinâmico do manipulador, por exemplo, pode ser desenvolvido usando-se a segunda lei de Newton ou o método de energia de Euler-Lagrange. Neste capítulo é feita uma revisão bibliográfica em que procura-se apresentar os métodos existentes na literatura e justificar a escolha dos métodos escolhidos que serão empregados no software de controle do manipulador. 2.1 Manipuladores robóticos Na indústria os robôs do tipo manipulador são empregados em uma variedade de aplicações, sendo a maioria delas para a movimentação de materiais, peças e ferramentas de diversos tipos (GROOVER, 1988). As aplicações industriais podem ser dividas nas áreas: aplicações de manuseio de materiais e carregamento e descarregamento de máquinas; aplicações de processamento (soldagem a ponto, soldagem a arco; pintura a pistola, entre outras); montagem e inspeção (GROOVER, 1988). 2.1.1 Estrutura e componentes de um robô Um manipulador robótico tem uma estrutura composta de cinco componentes que interagem juntos para garantir que o manipulador consiga completar a função passada a ele (DUYSINX; GERADIN, 2004). A estrutura mecânica, ou mecanismo articulado, é feito idealmente de membros rígidos ou elos que articula junto com juntas mecânicas. Esta estrutura carrega o efetuador que pode ser uma ferramenta, garra, ou outro dispositivo específico. Os atuadores proporcionam a potência mecânica de forma a mover a estrutura mecânica contra a ação de forças externas, gravidade, inércia para modificar a configuração e, portanto, a posição geométrica da ferramenta. Estes podem ser elétricos, hidráulicos, ou pneumáticos e devem ser controlados apropriadamente. A transmissão mecânica, conecta e adapta os atuadores à estrutura mecânica. Esta transmissão adapta os atuadores para carga aplicada e ao mesmo tempo transmitem os Capítulo 2. Revisão Bibliográfica 22 esforços do atuador para as juntas mecânicas. Os sensores proporcionam os sentidos para o robô. Eles podem, por exemplo, providenciar informações de tato, visão, e temperatura. Eles são divididos em dois grupos: sensores que fornece informação sobre o próprio robô ou sobre o ambiente externo, na qual o manipulador se encontra. E por fim, tem-se a unidade de controle, que assume algumas funções, como: coletar e processar as informações dadas pelos sensores; tomar decisões sobre o plano de movimento do manipulador; e organizar o fluxo de informações para se comunicar com as juntas do manipulador e o ambiente (DUYSINX; GERADIN, 2004). Figura 2 – Estrutura geral do robô integrado com seu ambiente Fonte: (DUYSINX; GERADIN, 2004) A estrutura mecânica é detalhada a seguir. Elos Os corpos rígidos individuais que compõem o robô são chamados elos. Em robótica, geralmente quando dizemos braço, nos referimos aos elos. Uma braço robótico é um membro rígido que pode ter movimento relativo com respeito de todos os outros elos. Do ponto de vista cinemático, dois ou mais membros conectados juntos de forma que não é possível ter movimento relativo entre eles são considerados um único elo (JAZAR, 2007). Capítulo 2. Revisão Bibliográfica 23 Juntas Dois elos são conectados por contato na junta onde seus movimentos relativos podem ser expressos por uma única coordenada. Juntas são tipicamente de revolução ou prismática. A junta de revolução permite rotação relativa entre os elos, já a junta prismática permite movimento de translação relativo entre dois elos. Rotação relativa de elos conectados por uma junta de revolução ocorre sobre um linha chamada de eixo da junta, esse também é o caso para movimento linear que ocorre em elos conectados por uma junta prismática. O valor da coordenada que descreve a posição relativa de dois elos conectados a uma junta é chamado de coordenada da junta ou variável da junta (JAZAR, 2007). Punhos As juntas na cadeia cinemática entre o antebraço e o efetuador são referidos como punho. Enquanto que a estrutura do manipulador é responsável por posicionar o efetuador em uma posição do espaço, o punho é responsável em orientar o efetuador nos três ângulos desejados. A maioria dos punhos são projetados como esféricos, o que significa que três eixos de juntas rotativas se intersectam em um ponto chamado de ponto do punho. Este tipo de configuração, simplifica a análise cinemática, permitindo a separação da posição e orientação do efetuador (DUYSINX; GERADIN, 2004; JAZAR, 2007). Figura 3 – Os três ângulos de rotação de um punho esférico Fonte: PLW (2013) Capítulo 2. Revisão Bibliográfica 24 Efetuadores O efetuador é a parte montada no último elo para fazer o trabalho necessário do robô. O tipo mais simples do efetuador é a garra, que é geralmente capaz de executar duas ações: abrir e fechar. A montagem do punho e braço do robô são usados essencialmente para posicionar e orientar o efetuador e a possível ferramenta que ele pode carregar. É o efetuador ou a ferramenta que irá realmente executar a tarefa (JAZAR, 2007). Por ser um item muito específico de cada tarefa, o projeto do efetuador não faz parte do escopo desse trabalho, mas é apontado nas sugestões de trabalhos futuros. 2.1.2 Sistemas de acionamento e congurações do robô Robôs industriais são acionados por um dos três tipos de sistema de acionamento (GROOVER, 1988): 1 Acionamento hidráulico; 2 Acionamento elétrico; 3 Acionamento pneumático. Robôs industriais estão disponíveis em uma variedade de tamanhos, formas e configurações físicas. As quatro configurações básicas são (GROOVER, 1988): 1 Configuração polar; 2 Configuração cilíndrica; 3 Configuração de coordenadas cartesianas; 4 Configuração articulada. A configuração adotada para o projeto do manipulador apresentado neste trabalho, foi a configuração articulada. Sua configuração é semelhante à de um braço humano. É composta por juntas rotacionais e elos em sequência alternada. Um punho pode ser unido à extremidade do antebraço, proporcionando, assim, um maior número de graus-de-liberdade. Manipulador articulado De acordo com Craig (1989), um manipulador antropormófico, ou articulado, geralmente consiste de duas juntas no ombro, sendo uma para elevação para fora do plano horizontal e outro para rotação em torno de um eixo vertical (geralmente o eixo z), uma junta no cotovelo, que é muitas vezes paralelo à junta de elevação do ombro e duas ou três juntas no punho. Capítulo 2. Revisão Bibliográfica 25 Figura 4 – Configurações básicas de um manipulador robótico: (a: cartesiano; b: cilíndrico; c: polar; d: articulado). Fonte: Duysinx e Geradin (2004) Os robôs articulados têm a vantagem de serem capazes de alcançar espaço confinados, como a soldagem dentro de um automóvel, por exemplo, o que minimiza a intrusão da estrutura do robô no espaço de trabalho. Outra vantagem é que eles geralmente requerem uma estrutura muito menor do que a dos robôs cartesianos, o que os tornam mais baratos e mais comuns em aplicações industriais (CRAIG, 1989). 2.1.3 Esquema de notação de juntas A configuração de um manipulador robótico pode ser descrita em termos de tipos de juntas. Começa-se pela junta mais próxima a base e prosseguindo para as outras 2 juntas seguintes. Para um manipulador do tipo configuração articulada, a denominação se torna: TRR ou VVR. Onde T é uma junta de torção, R junta de revolução e V é uma junta revolvente, como mostrado na figura abaixo. A notação do robô pode incluir os movimentos do punho, mediante designação de dois ou três tipos de juntas de punho. A importância de se designar um manipulador em relação ao tipo de suas juntas se deve ao fato que a configuração do manipulador vai determinar o seu volume de trabalho. 2.1.4 Movimentos do robô e graus de liberdade As juntas e elos permitem que o robô mova seu corpo a fim de se realizar um trabalho produtivo. O efetuador localizado ao final do último elo, possibilita a realização de alguma tarefa específica, como por exemplo, o efetuador pode ser uma ferramenta ou uma garra, de maneira a movimentar ou usinar uma peça. Os movimentos de juntas individuais e do Capítulo 2. Revisão Bibliográfica 26 Figura 5 – Tipos de juntas empregadas em robôs Fonte: Duysinx e Geradin (2004) Figura 6 – Representação esquemática das juntas Fonte: Groover (1988) punho são chamados de graus de liberdade, sendo um robô industrial típico equipado com 4 a 6 graus de liberdade (GROOVER, 1988). Em um corpo rígido, grau de liberdade é definido como o número de movimentos independentes que ele possui. Pode-se calcular o número de graus de liberdade de um sistema utilizando a equação Gruebler (YI FINGER SUSAN, 2010). GDL = 3 (e − 1) − 2n − h Onde: GDL é o total de graus de liberdade de um mecanismo e é o número de elos (incluindo suporte) (2.1) Capítulo 2. Revisão Bibliográfica 27 n é o número de pares inferiores (um grau de liberdade) h equivale ao número de pares superiores (dois graus de liberdade) Os pares inferiores são considerados as juntas (tanto de rotação como de translação), já os pares superiores são considerados as engrenagens ou cames. Para um manipulador com duas juntas e dois elos como mostrado na Figura 7, o número de graus-de-liberdade se torna: GDL = 3 (3 − 1) − 2.2 − 0 = 2 Neste caso e = 3 pois o sistema possui 2 elos e mais 1 suporte, e h = 0 pois o sistema não possui engrenagens e/ou cames. Se fizermos e = 4 e n = 3, então GDL = 3 e para e = 5 e n = 4, GDL = 4. É possível perceber que, para um manipulador com juntas de revolução, a cada par de junta e elo inserido, o número de graus de liberdade aumenta em 1. Sendo assim, um manipulador com n juntas, possuirá n graus de liberdade. Figura 7 – Manipulador planar de 2 GDL com suporte em O Fonte: Produzido pelo autor. 2.1.5 Volume de trabalho De acordo com Groover (1988), volume de trabalho se refere ao espaço que um dado manipulador consegue posicionar seu efetuador. É, em geral, definido pelo tipo de juntas usado na estrutura do robô e do pulso. Para um braço esférico (TRL), por exemplo, seu volume de trabalho seria, teoricamente, uma esfera cujo raio corresponde a extensão máxima do braço esticado. A figura abaixo mostra o volume de trabalho para cada configuração do robô. Capítulo 2. Revisão Bibliográfica 28 Figura 8 – Típico volume de trabalho para configurações comuns de robôs Fonte: Groover (1988) 2.1.6 Ciclo de funcionamento de um manipulador Fazendo uso de um software de controle baseado em interface gráfica, o usuário especifica uma posição para o efetuador, esta posição pode ser uma coordenada final apenas ou uma trajetória no espaço ; o usuário então define a velocidade do efetuador (esta pode ser constante ou uma função do tempo) ou o tempo desejado para finalizar o trajeto, o computador então precisa converter as coordenadas definidas pelo usuário (estas coordenadas são cartesianas: xyz) em coordenadas das juntas (ângulo θ), esta conversão é feita usando as equações da cinemática e é chamada de cinemática inversa. Quando o usuário especifica apenas o ponto final (meta) para o efetuador, o computador calcula os ângulos das juntas apenas para este ponto e faz uma interpolação polinomial utilizando o tempo do trajeto para fazer um movimento suave, evitando picos de velocidade e aceleração. Para o caso de ser definido uma trajetória, então o computador deve calcular os ângulos das juntas para cada ponto da trajetória, neste caso a velocidade e aceleração das juntas podem apresentar descontinuidades (DUYSINX; GERADIN, 2004). Uma vez obtidos a posição, velocidade e aceleração de cada junta em qualquer instante t, o computador calcula o torque em cada junta necessário para efetuar este movimento. Este torque é calculado usando as equações de movimento do manipulador. As equações da dinâmica podem apresentar imprecisões no valor do torque devido a forças que são difíceis de quantificar, como atritos presentes na transmissão da junta entre outros fatores. Por este motivo utiliza-se sensores que medem a posição (ângulo) da junta a cada período de tempo, esta informação é utilizada em um sistema de controle, que Capítulo 2. Revisão Bibliográfica 29 vai calcular a quantidade de torque que deve ser adicionado ao valor inicial para garantir que o manipulador chegue à sua meta nas condições (velocidade, ou tempo) impostas pelo usuário. Este ciclo é conhecido como inteligência comportamental do robô, onde cada etapa é fundamental para a realização do movimento do manipulador. Este ciclo é repetido cada vez que o usuário define uma nova meta para o efetuador. 2.2 Cinemática e geração de tra jetória Em robótica, o movimento do efetuador de um manipulador deve ser o mais suave possível, evitando assim variações abruptas de posição, velocidade e aceleração. Movimentos abruptos necessitam de uma quantidade ilimitada de potência para ser implementado (CRAIG, 1989). Por este motivo, implementar um algoritmo eficaz para geração de trajetória é essencial para um bom funcionamento do manipulador. Como visto na subseção 2.1.1, um manipulador é composto por elo e juntas. Estas juntas podem ser de quatro formas: junta de revolução, junta prismática, de rosca e esférica, cada uma possuindo um número diferente de graus de liberdade. O problema da cinemática direta faz uso das equações da cinemática e dos parâmetros de cada junta para determinar a posição e orientação do efetuador. O ângulo entre os elos é a variável da equação para uma junta de revolução, assim como a extensão do elo é a variável para uma junta prismática. Na cinemática direta, as variáveis de cada junta são conhecidas e deseja-se saber a posição e orientação de cada junta. Cinemática inversa se refere ao uso das equações da cinemática para a determinação dos parâmetros das juntas que providenciam uma posição desejada do efetuador (CRAIG, 1989). Na maioria das aplicações de um manipulador, deseja-se que o efetuador vá para uma certa posição no espaço sem se ter conhecimento das variáveis das juntas (ângulo ou deslocamento). O usuário então programa a posição final ou a trajetória do efetuador e as equações da cinemática inversa transformam essas coordenadas em ângulos ou deslocamentos para os atuadores. O problema da cinemática inversa é considerado mais complexo que a cinemática direta (BUSS, 2009), por se ter várias possíveis soluções (ou em alguns casos nenhuma solução é encontrada) de variável da junta para um determinado conjunto de coordenada (x, y e z). Os métodos para se resolver cinemática inversa são divididos em dois grandes grupos: soluções analítica e numérica, sendo o primeiro utilizado para manipuladores com seis juntas ou menos em que o punho é esférico. Neste, obtém-se uma equação fechada para cada junta. Capítulo 2. Revisão Bibliográfica 30 Figura 9 – Cinemática: a)direta; b)inversa Fonte: Buss (2009) O segundo grupo de soluções, os métodos numéricos são dividos em pela forma como são abordados, sendo os principais: uso da matriz Jacobiana, uso de um sistema de controle, e uso de métodos de otimização que minimizam uma função erro (objetiva). Ao longo dos anos, diversos modelos numéricos para resolver o problema de cinemática inversa foram implementados (ARISTIDOU; LASENBY, 2009). De acordo com Fëdor (2003), a maioria dos sistemas usam o método da peseudoinversa Jacobiana ou sua transposta para solucionar o problema contudo este método sofre de singularidades. Zhao e Badler (1994) resolve a cinemática inversa como um problema de otimização, em que se minimiza um conjunto de equações não-lineares, definidas no espaço Cartesiano com restrições. A solução por método Jacobiano lineariza o movimento do efetuador relativo à mudanças instantâneas de posição das variáveis das juntas aristidou. Muitos métodos tem side desenvolvidos para calcular or aproximar a inversa do Jacobiano, tais como: Transposta Jacobiano, Mínimos quadrados amortecido (DLS), mínimos quadrados amortecido com decomposição de valor único (SVD-DLS) e mínimos quadrados amortecido seletivo (SDLS) e outras extensões (BUSS, 2009; NAKAMURA; HANAFUSA, 1986; W.WAMPLER, 1986; BAILLIEUL, 1985; WOLOVICH; ELLIOTT, 1984; BALESTRINO; SCIAVICCO, 1984). Outros tipo de solução é baseado no método de Newton. Estes algoritmos procuram uma posição e orientação do efetuador que irá minimizar a função objetiva, retornando portanto, um movimento suave sem descontinuidades (ARISTIDOU; LASENBY, 2009). Os mais conhecidos são o metódo de Powell, o método de Broyden e o método de Broyden, Fletcher, Goldfarb e Shanno (BFGS)(FLETCHER, 1987), utilizados extensivamente em otimização. O método cíclico de coordenadas descendente(CCD), apresentado por Wang e Chen (1991), é um método iterativo heurístico bastante popular por ser considerado um dos mais Capítulo 2. Revisão Bibliográfica 31 rápidos a convergir para a proximidade da resposta, contudo é relativamente lento para alcançar alta precisão e frequentemente produz movimentos erráticos e descontínuos. Por este motivo, CCD é geralmente empregado em conjunto com outros métodos de otimização, como o BFGS, que assume quando o manipulador já está na proximidade da meta desejada, como apresentado por Wang e Chen (1991). Estes métodos são extensivamente empregados em animações gráficas e em manipuladores robóticos com elevado número de graus de liberdade. Podem ser usados como soluções em tempo real onde a posição da junta do manipulador é atualizada a cada iteração do algoritmo ou pode-se computar todas as iterações previamente e obter os valores de ângulos de cada junta para só então depois atualizar a posição do manipulador. Tabela 1 – Comparação entre diferentes métodos de cinemática inversa; modelo com um efetuador, 18 GDL, e metas dentro do volume de trabalho Método Núm. de iterações Tempo de cada iteração (µs) Pseudoinversa 32 96,4 Pseudoinversa’ 46 104,1 Mínimos quadrados amortecido 35 89,2 Transposta 59 83,3 Transposta’ 163 84,8 Fonte – (NILSSON, 2009) A Tabela 1 mostra alguns dos métods que utiliza a matriz Jacobiana. Nesta tabela precebe-se que o método de pseudoinversa tem o menor número de iterações, contudo o tempo para cada iteração é um pouco maior que o método dos mínimos quadrados amortecido (NILSSON, 2009) Tabela 2 – Comparação entre diferentes métodos de cinemática inversa; modelo com um efetuador, 18 GDL, e metas fora do volume de trabalho Método Núm. de iterações Tempo de cada iteração (µs) Pseudoinversa - - Pseudoinversa’ - - Mínimos quadrados amortecido 74 81,5 Transposta 117 73,8 Transposta’ 98 77,8 Fonte – (NILSSON, 2009) A Tabela 2 mostra que ambos os métodos da pseudoinversa não puderam produzir nenhum resultado quando as metas estão fora de alcance (NILSSON, 2009). O metódo da Capítulo 2. Revisão Bibliográfica 32 pseudoinversa puro fica instável próximos a singularidades (ASADA, 2008), já os outros métodos necessitam de um maior número de iterações para alcançar o ponto mais próximo possível da meta. Conclui-se que o método dos mínimos quadrados amortecido que utiliza a matriz Jacobiana do sistema é o método ideal para ser empregado em manipuladores robóticos, uma vez que se mostra estável e consegue alcançar a meta em menos iterações. Este método será empregado no software de controle apresentado no Apêndice A. 2.3 Dinâmica e controle A dinâmica e sistemas de controle são partes fundamentais para o controle do manipulador. A simulação do movimento só pode ser feita se as equações de movimento do manipulador forem obtidas, assim como o movimento real no robô físico, só obterá precisão se for implementado um sistema de controle adequado para rastreio de trajetória. 2.3.1 Modelagem dinâmica A descrição do movimento do robô quando há considerações das forças agindo nele é dada pelas equações de movimento, que são obtidas através da modelagem dinâmica do manipulador (CRAIG, 1989). Assim como na cinemática, em dinâmica de manipuladores, existem dois problemas a resolver: no primeiro, tem-se a trajetória desejada e deve-se encontrar o torque necessário em cada junta para mover o manipulador, no outro tem-se uma curva de torque e deseja-se saber como o manipulador vai responder à isto (CRAIG, 1989). O comportamento dinâmico do robô é descrito em termos de taxa de variação de tempo da configuração do robô em relação aos torques aplicados por cada atuador. Esta relação pode ser expressa por um conjunto de equações diferenciais, chamadas equações de movimento. Existem várias técnicas para se determinar tais equações (FU et al., 1987; SHAHINPOOR, 1987; SPONG; VIDYASAGAR, 1989; MURRAY et al., 1994; CRAIG, 1989). A formulação por Newton-Euler, por exemplo, implica em determinar as equações de movimento baseando-se no balanceamento de forças agindo em cada elo. Este método resulta em equações para cada corpo, ou elo do robô, o que faz o sistema volumoso e difícil de se trabalhar para robôs com grande número de elos. Fang A. Basu e Wang (1994), apresenta um método recursivo baseado no princípio de D’Alambert e trabalho virtual para se determinar o modelo dinâmico de um manipulador genérico. O método de Lagrange-Euler usa a conservação de energia no sistema (princípio da ação mínima de Hamilton) com um Lagrangiano, para determinar as equações de movimento (ASADA, Capítulo 2. Revisão Bibliográfica 33 2008). O problema será abordado neste texto fazendo uso do método de Euler-Lagrange para equações de movimento. Este método foi preferido por apresentar algumas vantagens sobre o método de Newton-Euler, algumas delas são: o método de Lagrange fornece automaticamente tantas equações quanto há graus de liberdade; as equações de Lagrange utilizam naturalmente as coordenadas generalizadas do sistema1 sem a necessidade de converter tudo para Cartesiano como é o caso do método de Newton-Euler; o método de Lagrange também elimina as forças de suportes, o que torna o método de certa forma mais direto e menos laborioso (VANDIVER, 2011). A seção 3.3 mostra o equacionamento para obter-se as equações de movimento de um manipulador com n graus de liberdade. 2.3.2 Controle do manipulador O problema de controle para manipuladores robóticos consiste em determinar as entradas das juntas em função do tempo que faça o efetuador executar o movimento comandado (sequência de posições e orientações ou um percurso contínuo). Estas entradas podem ser forças e torques nas juntas, ou podem ser entradas do atuadores, como por exemplo, voltagem (SPONG; VIDYASAGAR, 1989) Existem diferentes técnicas e metodologias para controle de manipuladores, o método escolhido tem impacto significante na performance do manipulador. Rastreio de percursos contínuos requer uma arquitetura de controle diferente que requer o controle para ponto-a-ponto (SPONG; VIDYASAGAR, 1989). Um dos mais simples tipos de estratégia de controle é o controle independente de junta. Neste tipo de controle cada eixo do manipulador é controlado como um sistema de uma única-entrada/única-saída. Qualquer efeito de acoplamento devido ao movimento dos outros elos é tratado como uma perturbação (SPONG; VIDYASAGAR, 1989). O usuário ou o algorítmo do software determina a trajetória desejada para o movimento do manipulador. O sistema de controle deve ser projetado para que o manipulador consiga se aproximar o melhor possível da trajetória desejada (KOIVO, 1989). Os valores da trajetória real são retroalimentados através de retroação para determinar possíveis erros de trajetória, o sistema de controle vai então trabalhar para compensar os erros observados (KOIVO, 1989). Na realidade, as equações dinâmicas para um manipulador robótico formam um sistema de multi-variáveis complexo e não-linear (SPONG; VIDYASAGAR, 1989). A análise de controle no contexto não-linear permite uma análise rigorosa da performance do sistema de controle, e também permite o projeto de leis de controle robustas e adaptivas que garantem a estabilidade e rastreio de trajetórias arbitrárias (SPONG; VIDYASAGAR, 1989). 1 para manipuladores articulados, a coordenada generalizada é o ângulo da junta θ. 34 Capítulo 3 Metodologia Neste capítulo são apresentados os conceitos teóricos que constituem as bases para a análise e controle de um manipulador robótico. São tratados neste capítulo os seis problemas apresentados no capítulo anterior que, de acordo com Spong e Vidyasagar (1989), são fundamentais para o desenvolvimento do software de controle do manipulador e para a análise apresentados no Capítulo 5 3.1 Geração de trajetórias A trajetória consiste de um histórico de posição, velocidade e aceleração da coordenada da junta em função do tempo, para cada grau de liberdade, e descreve o movimento desejado de um manipulador no espaço multidimensional. A intenção de se criar uma interface para geração de trajetória para um sistema robótico, é que o usuário não deve ter a necessidade de escrever funções complicadas de tempo e espaço para especificar a tarefa, ao invés disso, o usuário especificaria apenas uma posição meta desejada e a orientação do efetuador, e o sistema decide a forma exata do percurso para chegar lá, a duração, o perfil de velocidade e outros detalhes (CRAIG, 1989). 3.1.1 Trajetória ponto-a-ponto O tipo de movimento mais simples do robô é o movimento ponto-a-ponto. Neste tipo de movimento o robô encontra-se em uma determinada posição e orientação e ordena-se que ele vá para uma posição e orientação final sem se importar com o caminho intermediário entre esses dois pontos. O computador converte essa meta descrita no espaço cartesiano em espaço das juntas usando a cinemática inversa e define como será a curva de posição da junta com base na velocidade ou tempo estipulado pelo usuário. Este tipo de movimento é adequado para tarefas de transferência quando a área de trabalho está livre de obstáculos (SPONG; VIDYASAGAR, 1989). Com relação ao movimento das juntas neste tipo de movimento, existem algumas possíveis maneiras de acionamento, mostrados a seguir. Capítulo 3. Metodologia 35 Movimento Descoordenado Em um manipulador com todas as juntas rotativas, o efetuador terá um movimento descoordenado se todas as juntas se moverem com a mesma velocidade. Uma vez que, para um determinado movimento, cada junta terá uma distância diferente a percorrer, uma vez que todas estão a uma velocidade constante igual, elas terminarão seu movimento em momentos diferentes. O caminho criado por este tipo de movimento é circular com descontinuidades, como mostrado na Figura 10. Este tipo de trajetória foi implementado na interface gráfica, embora seja desaconselhado utilizá-lo quando o robô estiver carregando objetos pesados. O programa envia o ângulo de destino para cada junta com uma velocidade constante e igual para todas as juntas conectadas. Figura 10 – Movimento descoordenado (esquerda: deslocamento de cada junta; direita: trajetória do manipulador no plano) Fonte: Marion e Thornton (1981) Movimento sequencial O movimento sequencial consiste em mover uma junta por vez até o efetuar alcançar seu destino. A junta mais próxima da base geralmente é movida primeiro, e apenas quando seu movimento for concluído é que o movimento da próxima junta se inicia, a velocidade para cada junta pode ser diferente e não necessariamente constante. Uma vantagem deste tipo de controle de junta, é que, uma vez que apenas uma junta está sendo movida por vez, a corrente total consumida por cada atuador é menor se comparado com os outros tipos de movimento. A desvantagem, contudo, é que este método apresenta várias descontinuidades na trajetória do efetuador e em muitos casos, um controle adicional é necessário para evitar que o efetuador colida com objetos ou superfícies. A figura a seguir mostra o movimento de um manipulador com 2 graus-de-liberdade com movimento sequencial. Capítulo 3. Metodologia 36 Figura 11 – Movimento sequencial (esquerda: deslocamento de cada junta; direita: trajetória do manipulador no plano) Fonte: Marion e Thornton (1981) Junta interpolada O método de junta interpolada é um dos mais usados em sistemas robóticos por ser um movimento suave. Neste caso, todas as juntas possuem velocidades diferentes o que faz com que todas as juntas terminem ao mesmo tempo, ou seja, não há grandes variações em velocidade e aceleração. O caminho descrito pelo efetuador, por ser curvo, não apresenta descontinuidades e pode ser descrito por uma interpolação polinomial. Movimento suave é considerado como uma função contínua que possui, no mínimo, duas derivadas contínuas. Isto evita movimentos bruscos, com solavancos diminuindo, assim, o desgaste do mecanismo e possíveis vibrações que podem provocar ressonâncias no manipulador (CRAIG, 1989). Figura 12 – Movimento coordenado (esquerda: deslocamento de cada junta; direita: trajetória do manipulador no plano) Fonte: Marion e Thornton (1981) Para um percurso suave, a velocidade inicial e e velocidade final são iguais à zero, Capítulo 3. Metodologia 37 e os ângulos das juntas inicial e final correspondem ao ângulo em que a junta está e o ângulo calculado final respectivamente, portanto: θ (0) = θ0 , θ (tf ) = θf , θ˙ (0) = 0, θ˙ (tf ) = 0. (3.1) As quatro restrições podem ser satisfeitas por um polinômio de terceiro grau. Este polinômio tem a forma θ (t) = a0 + a1 t + a2 t2 + a3 t3 (3.2) tomando-se a derivada desse polinômio para calcular a velocidade e aceleração, obtém-se θ˙ (t) = a1 + 2a2 t + 3a3 t2 θ¨ (t) = 2a2 + 6a3 t (3.3) Combinando as equações (3.2) e (3.3) com as restrições desejadas (3.1), obtém-se quatro equações com quatro incógnitas: θ0 = a0 θf = a0 + a1 tf + a2 t2f + a3 t3f (3.4) 0 = a1 0 = a1 + 2a2 tf + 3a3 t2f Encontrando os coeficientes ai dessas equações, obtém-se: a0 = θ0 a1 = 0 3 a2 = 2 (θf − θ0 ) tf 2 a3 = − 3 (θf − θ0 ) tf (3.5) Com esses coeficientes é possível calcular o polinômio cúbico que conecta qualquer posição inicial de ângulo de junta com qualquer posição final, de forma que as velocidades inicial e final sejam zero, formando portanto, um movimento suave. As funções θ (t), θ˙ (t) e θ¨ (t), tornam-se, portanto: θ (t) = − 2 3 (θf − θ0 ) t3 + 2 (θf − θ0 ) t2 + θ0 3 tf tf (3.6) 6 6 θ˙ (t) = − 3 (θf − θ0 ) t2 + 2 (θf − θ0 ) t tf tf (3.7) 12 6 θ¨ (t) = − 3 (θf − θ0 ) t + 2 (θf − θ0 ) tf tf (3.8) Capítulo 3. Metodologia 38 É possível também descrever alguns pontos de passagem para o efetuador1 , de forma que o efetuador passará por esses pontos sem parar. Cada um desses pontos são convertidos para o espaço das juntas pelo uso da cinemática inversa. As restrições para os pontos inicial e final permanecem os mesmos, contudo para os pontos de passagem as velocidades se torna uma velocidade conhecida. θ˙ (0) = θ˙0 θ˙ (tf ) = θ˙f (3.9) As equações que descrevem o polinômio cúbico são: θ0 = a0 θf = a0 + a1 tf + a2 t2f + a3 t3f θ˙0 = a1 θ˙f = a1 + 2a2 tf + 3a3 t2 (3.10) f Obtém-se, portanto, os coeficientes da função: a0 = θ 0 a1 = θ˙0 2 1 3 (θf − θ0 ) − θ˙0 − θ˙f 2 tf tf tf   3 1 a3 = − 3 (θf − θ0 ) + 2 θ˙f − θ˙0 tf tf (3.11) a2 = Substituindo os coeficientes nas equações (3.2) e (3.3), para obter: ! ! ! !  3 1  3 2 1 θ (t) = − 3 (θf − θ0 ) + 2 θ˙f − θ˙0 t3 + 2 (θf − θ0 ) − θ˙0 − θ˙f t2 + θ˙0 t + θ0 tf tf tf tf tf (3.12)  9 3  6 4 2 θ˙ (t) = − 3 (θf − θ0 ) + 2 θ˙f − θ˙0 t2 + 2 (θf − θ0 ) − θ˙0 − θ˙f t + θ˙0 (3.13) tf tf tf tf tf !  18 6  6 4 2 θ¨ (t) = − 3 (θf − θ0 ) + 2 θ˙f − θ˙0 t + 2 (θf − θ0 ) − θ˙0 − θ˙f tf tf tf tf tf ! (3.14) As velocidades nos pontos de passagem devem ser conhecidas para se calcular o polinômio para cada junta. Essa velocidade pode ser especificada pelas seguintes formas: pelo usuário, como uma velocidade cartesiana linear e angular para o efetuador naquele instante; o sistema escolhe automaticamente as velocidades aplicando uma heusterística adequada no espaço das juntas ou no espaço cartesiano; ou o sistema escolhe as velocidades de forma que a aceleração no ponto de passagem seja contínua (CRAIG, 1989). 1 A expressão ponto se refere à sistemas de referência que fornecem tanto a posição quanto a orientação do efetuador. Capítulo 3. Metodologia 39 3.1.2 Trajetórias parametrizadas Em certas ocasiões o usuário necessita que o efetuador percorra um caminho específico, ao invés de apenas especificar um ponto inicial e uma meta. Esta trajetória pode ser descrita no espaço cartesiano de forma paramétrica, onde as coordenadas x, y, z são funções de tempo. Para este caso, o usuário especifica um tempo desejado para se completar o trajeto, ou uma velocidade linear constante. Para cada ponto da trajetória, é necessário calcular o valor da variável de junta utilizando o método de cinemática inversa apresentado na subseção 3.2.3. Para manipuladores com até três juntas, pode-se utilizar o método geométrico, e substituindo as variáveis cartesianas pela função paramétrica se tem a função fechada de variável de junta em função do tempo. Nos casos em que o manipulador possui mais de três juntas, ou graus de liberdade, um método numérico é usado para calcular a variável de junta para certos ponto da trajetória, com espaçamento ∆s entre cada ponto, uma tabela é então formada para cada junta. Para a implementação da interface gráfica, o método numérico de cinemática inversa escolhido foi o método dos mínimos quadrados amortecido , por se mostrar ser o mais rápido a convergir os valores. Este método reduz o número total de iterações para se obter a tabela de variáveis de junta para cada ponto, tornando o processo mais rápido e estável. Em trajetórias parametrizadas, os pontos estão muito próximos um do outro, o que torna inviável computar um polinômio cúbico para cada ponto. Por este motivo não é possível determinar a derivada de forma algébrica, uma derivada numérica é então empregada de forma a obter a velocidade e aceleração do espaço de junta. Uma derivada numérica é obtida utilizando a definição de derivada: f (x + h) − f (x) h→0 h f 0 = lim (3.15) Onde a aproximação numérica se torna: f0 ≈ f (x0 + h) − f (x0 ) h (3.16) A aceleração é obtida pela segunda derivada: f 00 ≈ f (x0 + h) − 2f (x0 ) + f (x0 − h) h2 (3.17) Onde f (x0 ) é o valor do ângulo θ da junta i para um dado momento t. O valor de h deve ser pequeno o suficiente de maneira que se tenha uma boa aproximação, mas não tão pequeno que se carregue muito o processamento do algoritmo. Capítulo 3. Metodologia 40 Figura 13 – Aproximação numérica de derivada Fonte: Produzido pelo autor 3.1.3 Trajetória modelada em AutoCad Quando a obtenção das equações parametrizadas se torna complexa, pode-se recorrer a um software de desenho como o AutoCAD, para obter uma tabela de coordenadas cartesianas para o efetuador. No software, o usuário desenha a trajetória desejada para o efetuador a converte em uma tabela de pontos relativamente próximos e uniformemente espaçados. Com o AutoCAD é possível exportar uma tabela de pontos presentes em um arquivo de desenho para o Microsoft Excel, que posteriormente pode ser incorporado na interface gráfica do usuário. Contudo, a tabela de pontos nem sempre está em ordem, e é necessário utilizar um algoritmo para reorganização dos pontos (a ordem dos pontos de uma trajetória é fundamental). Este algoritmo não será explicado aqui, mas é descrito brevemente na descrição do programa feito em Matlab para este fim. Neste programa, contido nos anexos, também descreve o método de se converter uma curva ou até mesmo caracteres de texto, como no exemplo abaixo, em tabela de pontos utilizando o AutoCAD. O exemplo abaixo mostra a trajetória desenhada em AutoCAD correspondente às três letras que compõem o logo da universidade, isto é, ‘UMC’. A Figura 14 mostra os pontos distribuídos no espaço de trabalho do manipulador. 3.1.4 Trajetória linear Em um manipulador de juntas de revolução, o movimento do efetuador nunca é linear (BUSS, 2009; CRAIG, 1989). Porém, é possível aproximar o trajeto em um percurso linear, Capítulo 3. Metodologia 41 Figura 14 – Exemplo de trajetória desenhada em CAD Fonte: Produzido pelo autor informando pontos de passagens relativamente próximos uns dos outros. A trajetória retilínea pode ser entendida como um caso específico da trajetória parametrizada. Para este caso, também é necessário calcular os ângulos das juntas para pontos igualmente espaçados do percurso da linha. O efetuador terá sua velocidade linear constante durante o percurso, fazendo com que a curva de variável de junta se torne complexa. Em percursos de linha reta, as derivadas primeira e segunda das coordenadas de juntas, são quase sempre curvas com descontinuidades, por isso é aconselhável evitar os percursos de linha reta em tarefas com elevação de cargas. Um dos métodos mais simples para se obter uma trajetória em linha reta é o algoritmo de Taylor, que utiliza pontos de passagem igualmente espaçados entre o ponto inicial e final. O algoritmo pode ser descrito como segue: 1o Calcule as coordenadas das juntas para o ponto inicial e final (meta); 2o Calcule um ponto médio, no espaço das juntas e Cartesiano; 3o Se o erro no trajeto planejado for maior que o permitido, então divida o trecho em dois e insira mais um ponto de passagem entre as extremidades; 4o Cheque o erro em ambos os lados do trajeto, caso o erro ainda permanecer maior que o permitido, adicione mais pontos de passagem até que o erro seja reduzido. Quanto maior o número de pontos de passagem, mais próximo será a aproximação da trajetória necessária. Capítulo 3. Metodologia 42 Figura 15 – Gráfcos da trajetória linear: a) trajetória em linha reta; b) aproximação da linha reta por pontos de passagem; c) curvas de coordenadas das juntas em função da distância linear; d) velocidade angular; e) aceleração angular Fonte: Marion e Thornton (1981) Alguns problemas podem aparecer quando é utilizado trajetórias lineares. O primeiro tipo é o problema da trajetória possuir pontos intermediários inalcançáveis. Neste caso, embora a localização inicial do manipulador e a meta estejam dentro do volume de trabalho, é possível que alguns pontos da reta sejam inalcancáveis, um exemplo disso seria uma linha reta em que seu meio passe muito próximo à base do manipulador, não sendo possível o efetuador alcançar esses pontos sem que haja uma colisão do efetuador com algum elo. O segundo problema ocorre quando os pontos da trajetória são próximos à singularidades. Se um manipulador está seguindo uma trajetória cartesiana em linha reta e se aproxima de uma configuração singular do mecanismo, a velocidade de uma ou mais juntas pode aumentar ao infinito. Sendo as velocidades de mecanismos limitadas, o que ocorre na prática é que existirá um desvio do manipulador do curso desejado (CRAIG, 1989). O problema do tipo três surge quando um manipulador tem os pontos inicial e alvo atingíveis por diferentes soluções. Neste caso o manipulador consegue alcançar todos os pontos da trajetória com algumas soluções ao invés de uma única. Neste caso, o sistema de planejamento pode detectar o problema antes de mover o robô pelo percurso (CRAIG, 1989). Como será mostrado no Apêndice A, existe uma função específica que irá checar a trajetória gerada antes de iniciar a simulação ou enviar o movimento ao robô. A Figura 16 ilustra os três tipos de problemas. Capítulo 3. Metodologia 43 Figura 16 – Problemas geométricos com trajetória cartesiana: a)tipo 1; b)tipo 2; c)tipo 3 Fonte: Craig (1989) 3.1.5 Algoritmo anticolisão Em uma ocasião ideal, o usuário iria entrar com um ponto meta desejado e o sistema determinaria quantos pontos de passagem seria necessário para que a trajetória gerada fosse capaz de se livrar de qualquer obstáculo. Contudo, para se alcançar tal estado, o sistema precisaria ter modelos do manipulador, da área de trabalho e de todos os possíveis obstáculos que nela se encontram(através de sistema de visão) (CRAIG, 1989). Atualmente, não existem sistemas que conseguem planejar trajetórias que sejam totalmente livres de colisão (CRAIG, 1989). As técnicas existentes conseguem evitar algumas das possíveis colisões buscando em uma representação gráfica do ambiente uma trajetória livre de colisão. Contudo esses métodos são exponencialmente complexos se considerar um obstáculo móvel (como no caso de um segundo manipulador estar trabalhando na mesma área) ou se o manipulador possuir muitos graus de liberdade (CRAIG, 1989). Uma dos algoritmos mais simples para se evitar colisão consiste em descrever o objeto presente na área de trabalho do manipulador como uma aproximação do formato usando uma função parametrizada, como a da esfera por exemplo. O sistema iria então verificar se algum ponto da trajetória estaria dentro do volume da esfera e caso estivesse, este iria procurar através de uma análise vetorial, a menor distância fora da esfera em que não haveria trajetória. Este método é laborioso, pois o usuário teria que descrever o objeto toda vez que esse mudasse de lugar. Como o sistema de visão não faz parte do escopo deste trabalho, não será implementado no software de controle o algoritmo anticolisão. É apresentado no Capítulo 6 como sugestão para trabalho futuro, a implementação de um sistema de visão e algoritmo anticolisão no software de controle. Também é sugerido uma atualização para a junta Capítulo 3. Metodologia 44 do manipulador para que esta consiga detectar quando o manipulador colide com algum objeto acionando, assim, um sistema de segurança que desliga o torque dos atuadores das juntas. 3.2 Cinemática do manipulador Cinemática é o campo da Mecânica clássica que descreve os movimentos de pontos, corpos de sistemas de corpos sem consideração às causas do movimento (forças e torques). Cinemática é de grande importância no estudo de manipuladores robóticos pois permite a determinação da posição de cada junta e do efetuador para qualquer instante. 3.2.1 Cinemática direta Em um manipulador, juntas e elos são conectados de forma alternada. Assumindo que cada junta tenha um único grau de liberdade, o movimento de cada junta pode ser descrito por uma única variável: ângulo para juntas de revolução e deslocamento para juntas prismáticas. As equações da cinemática direta determinam o efeito acumulativo do conjunto inteiro de variáveis. Para um manipulador de n juntas, ele apresentará n + 1 elo, uma vez que cada junta se conecta com dois elos. Por convenção, as juntas são numeradas de 1 até n e os elos de 0 até n , partindo da base. É considerado que cada junta i é fixa com respeito o elo i − 1. Desta forma, quando uma junta i é acionada, o elo i se move. Para cada junta, uma variável qi é associada a esta, sendo que para uma junta de revolução, a variável qi é o ângulo entre as juntas, portanto: qi = θi . Permitindo que Ai seja a matriz homogênea de transformação que descreve a posição e orientação do link i com respeito as coordenadas x, y, z a partir da origem. Para um manipulador com uma matriz única Ai , esta matriz é função de uma única variável, isto é, Ai (qi ). Para um manipulador com duas ou mais juntas, a matriz de transformação das juntas j até a junta i é obtida a partir da multiplicação de todas as matrizes A de i até j: Tij = Ai+1 Ai+2 ...Aj−1 Aj (3.18) Para resolver o problema da cinemática direta, é necessária determinar a matriz de transformação Ai , uma vez determinada, a posição e orientação pode ser obtida diretamente da matriz final Tij . A convenção e método de obtenção de Ai é apresentado a seguir. Critério de notação de Denavit-Hartenberg Jacques Denavit e Richard Hartenberg introduziram uma convenção em 1995 para a definição de matrizes de juntas e matrizes de elo com o intuito de padronizar a estrutura Capítulo 3. Metodologia 45 de coordenadas de uma cadeia cinemática. Na convenção de D-H, cada matriz homogênea de transformação é representada com um produto de quatro outras transformações básicas: Ai = Rz,θi Transx,di Transz,ai Rx,αi Onde:  Rz,θi c −sθi  θi  sθi cθi =  0 0  0 0  Transx,di 1   0 =  0  0  Transz,ai  Rx,αi 1   0 =  0  0  0 0 1 0 0  0   0  1 0 1 0 0 0 0 1 0 0  0   di   1 0 1 0 0 0 0 1 0 ai   0   0  1 1 0 0   0 cαi −sαi =  0 −sαi cα i  0 0 0 (3.19) (3.20)  (3.21)  (3.22)  0  0   0  1 (3.23) As matrizes de rotação geralmente são apresentadas com dimensão 3 × 3. A quarta linha e coluna da matriz são adicionados para permitir a descrição não apenas da rotação em torno de um eixo, mas também de uma translação d. Portanto, a matriz Ai que é produto de duas matrizes de rotação e duas de translação, possui dimensão 4 × 4. Efetuando a multiplicação das quatro matrizes, a matriz Ai se torna:   c −sθi cαi sθi cαi ai cθi   θi   sθi cθi cαi −cθi sαi ai sθi    Ai =   0 sαi cα i di    0 0 0 1 (3.24) Onde c representa o cosseno do ângulo θ ou α, s representa o seno do ângulo e os parâmetros ai , αi , di e θi são: comprimento do elo, giro do elo, deslocamento do elo e ângulo da junta, respectivamente. Com estes parâmetros estabelecidos, a posição e orientação do efetuador com respeito a origem global do robô O pode ser encontrada, multiplicando-se n matrizes de Capítulo 3. Metodologia 46 Figura 17 – Parâmetros de Denavit-Hartenberg Fonte: Craig (1989) transformação homogêneas A, obtendo-se a matriz de transformação Tn0 , ou seja: n Y   n o a p  T0n = Aii−1 (qi ) =  0 0 0 1 i=1 (3.25) A matriz Tij é uma matriz quadrada 4 × 4 onde as primeiras três linhas e colunas representam a orientação da junta j e a quarta coluna fornece a coordenada (x, y e z) com respeito ao sistema de coordenada do elo i. A Figura 18 mostra os parâmetros D-H para um manipulador do tipo articulado e a Figura 19 mostra como é divida a matriz de transformação homogênea. 3.2.2 Cinemática inversa: método pseudo-inverso Jacobiano A cinemática direta pode ser interpretada com uma função da variável da junta qi . A posição do efetuador pode, então, ser escrita como: e = f (θ) (3.26) Onde e é a posição do efetuador, isto é: e = (e1 , e2 , ..., em )T , (em espaço tridimensional: m = 3), e θ é um vetor-coluna composto por todos as n variáveis de juntas, isto é: θ = (θ1 , θ2 , ..., θn )T . Para a cinemática inversa, deseja-se encontrar uma função que retorna um vetor de variáveis de juntas (ângulos ou deslocamentos) que vai fazer com que o efetuador alcance Capítulo 3. Metodologia 47 Figura 18 – Manipulador do tipo articulado com os parâmetros D-H Fonte: Produzido pelo autor Figura 19 – Divisão da matriz de transformação homogênea Fonte: Produzido pelo autor alguma posição desejada. Neste caso e é conhecido e queremos encontrar o valor de θ, então computa-se a função inversa de (3.26). θ = f −1 (e) (3.27) Esta função inversa nem sempre tem solução única, e é muito mais complexa quando o manipulador possui múltiplos graus-de-liberdade ou múltiplos efetuadores, o que faz o processo de encontrar as soluções mais exigente computacionalmente. Para manipuladores com múltiplos efetuadores (k), a posição no espaço é denotada por p1 , ..., pk , onde a posição de cada efetuador é uma função da variável da junta. Pode-se também escrever o vetor p como um vetor-coluna: (p1 , p2 , ..., pk )T que pode ser visto como um vetor com k entradas pertencentes ao R3 . Permita que o vetor t seja a posição desejada para cada efetuador, isto pode ser representado como um vetor coluna com k entradas: t = (t1 , ..., tk )T . A mudança em posição de cada efetuador pode ser calculada usando o vetor de posição atual s e a posição de destino t, considere e como o vetor de diferença entre os dois vetores, pode-se escrever para cada efetuador: e = t − p. Uma vez que a posição atual do efetuador é uma função Capítulo 3. Metodologia 48 de θ, a equação (3.26) para múltiplos efetuadores, se torna: ti = pi (θ) para todos k efetuador (3.28) Os valores para θ que satisfazem as equações (3.28) pode, em alguns casos, ter múltiplas soluções ou nenhuma solução. Contudo, é possível aproximar a função usando uma matriz Jacobiana, que é definida por: J (θ) = ∂pi ∂θj ! (3.29) i,j Onde J é a matriz k × n cujos elementos são vetores do R3 . A justificativa de se usar uma matriz Jacobiana é que a função de posição de cada efetuador pi (θ) é sempre diferenciável, portanto, a matriz Jacobiana apresentará a melhor aproximação linear da função ao redor do ponto escolhido, como mostrado na figura abaixo. As equações de velocidade e aceleração do efetuador também podem ser obtidas a partir da matriz Jacobiana. O algoritmo para se obter as soluções da cinemática inversa, pode ser descrito Figura 20 – Representação geométrica do Jacobiano Fonte: Produzido pelo autor como segue: Capítulo 3. Metodologia 49 Suponha que se conheça o valor da variável da junta θ e posição atual da junta (vetores p e t) em um dado instante. A partir desses valores, calcula-se a matriz Jacobiana usando a equação (3.29). Procuramos um valor de atualização para ∆θ que irá incrementar o ângulo da junta: θ := θ + ∆θ (3.30) Esta mudança da variável vai causar uma mudança da posição atual do efetuador p, e pode ser estimado: ∆p ≈ J∆θ (3.31) O valor ∆θ é escolhido de forma que ∆s é aproximadamente igual a diferença entre a posição final e a posição inicial (vetor e). Podemos encontra um valor de ∆θ que vá satisfazer a equação: e = J∆θ (3.32) O valor de ∆θ é encontrado calculando-se a inversa de matriz Jacobiana: ∆θ = J −1 e (3.33) Contudo, dependendo do número de juntas e do número de efetuadores do manipulador, a matriz Jacobiana pode não ser quadrada, da qual não se pode calcular a inversa. Por isso, é empregado alguns métodos para se contornar este problema, sendo um deles o uso do método da Transposta Jacobiana, que consiste de usar a transposta da matriz Jacobiana ao invés da inversa de J para encontrar ∆θ. Outro método utilizado é conhecido como método pseudoinverso, que é usado quando a matriz Jacobiana for retangular, a equação (3.33) se torna: ∆θ = J † e (3.34) Onde a matriz J † que possui dimensão n × m é o pseudo-inverso de J, também conhecida como Moore-Penrose inversa de J e fornece a melhor solução possível para a equação (3.32), uma vez que este método calcula esta equação usando o método dos quadrados mínimos. O pseudo-inverso de J pode ser calculada usando a equação:  J† = JT J −1 JT (3.35) Onde J T é a transposta da Jacobiana. 3.2.3 Mínimos quadrados amortecido. O método pseudo-inverso Jacobiano apresenta alguns problemas quando a meta está fora de alcance do manipulador ou quando está muito próxima da distância máxima de alcance. Estes problemas fazem o método numericamente instável quando o programa Capítulo 3. Metodologia 50 Figura 21 – Fluxograma de solução da Cinemática Reversa por Jacobiano Fonte: Joubert (2008) estiver tentando calcular o valor de ∆θ. Para evitar estes problemas com singularidades, o método de mínimos quadrados amortecido é utilizado. Este método se difere do pseudo-inverso Jacobiano no sentido que ao invés de encontrar um mínimo vetor ∆θ que fornece a melhor solução para a equação (3.32), o algoritmo vai procurar o valor de ∆θ que irá minimizar a quantidade kJ∆θ − ek2 + λ2 k∆θk2 (3.36) Onde λ ∈ R é uma constante de amortecimento não nulo. Pode-se reescrever a expressão acima como:   J T J + λ2 I ∆θ = J T e (3.37) O valor de J T J + λ2 I é não-linear (BUSS, 2009), e portanto, a solução dos mínimos Capítulo 3. Metodologia 51 quadrados é dada por:  ∆θ = J T J + λ2 I −1 JT e (3.38) O produto J T J é uma matriz quadrada n × n, em que n é o número de variáveis de juntas (graus-de-liberdade), considerando que  J T J + λ2 I −1  J T = J T JJ T + λ2 I −1 (3.39) Então:  ∆θ = J T JJ T + λ2 I −1 e (3.40)  −1 A vantagem de se usar (3.40) ao invés de (3.38) é que JJ T + λ2 I é mais fácil T de se calcular sua inversa, uma vez que JJ é uma matriz m × m , em que m é o espaço da meta (para uma espaço tridimensional: m = 3) e é na maioria dos casos menor que n. A constante de amortecimento λ depende dos detalhes do manipulador e da meta e deve ser escolhido com cuidado de maneira à fazer a equação (3.40) numericamente estável. A constante deve ser grande o suficiente de maneira que as soluções para ∆θ tenha bom comportamento próximo de singularidades, mas não tão grandes que a taxa de convergência se torne muito lenta (BUSS, 2009). 3.2.4 Redundância e Singularidades Um manipulador deve possuir pelo menos seis graus de liberdade de forma a poder alocar o efetuador em uma ponto e orientação arbitrária no espaço. Os manipuladores com uma número menor que 6 graus de liberdade não são capazes de executar o posicionamento de tais pontos arbitrários. Contudo, se o manipulador tiver mias que 6 graus de liberdade, irá existir um infinito número de soluções para as equações da cinemática. São, portanto, referidos como manipuladores redundantes (ASADA, 2008). A matriz jacobiana 6 × n, J (q) define um mapeamento: X˙ = J (q) q˙ (3.41) ˙ := (v, ω)T de velocidades do efetuador. entre o vetor q˙ das velocidades da junta e do vetor X Infinitesimalmente, isso define uma transformação linear (SPONG; VIDYASAGAR, 1989) dX = J (q) dq (3.42) entre os diferenciais dq e dX. Uma vez que a jacobiana é uma função da configuração q, aquelas configurações em que o ranque de J diminui são de significância especial. Tais configurações são chamadas singularidades ou configurações singular (SPONG; VIDYASAGAR, 1989). Capítulo 3. Metodologia 52 3.2.5 Velocidade e aceleração Como mostrado acima, as velocidades da junta e as velocidades do efetuador são relacionadas pela matriz jacobiana: ˙ = J (q) q˙ X (3.43) Portanto, a velocidade inversa se torna um problema de resolver um sistema de equações lineares. Para encontrar a aceleração, pode-se diferenciar a equação acima. Usando a regra do produto: ! d ¨ = J (q) q X ¨+ J (q) q˙ dt (3.44) ¨ de acelerações do efetuador, o vetor de aceleração Portanto, dado um vetor X instantânea da junta q ¨ é dado como a solução de: b = J (q) q ¨ (3.45) onde ¨ − d J (q) q˙ (3.46) b=X dt Para um manipulador de 6 graus de liberdade, as equações da velocidade e aceleração inversa podem ser escritas como (SPONG; VIDYASAGAR, 1989): ˙ q˙ = J(q)−1 X (3.47) q ¨ = J(q)−1 b (3.48) e Considerando que det J (q) 6= 0. Para manipuladores redundantes ou manipuladores com menos que 6 elos a matriz jacobiana não pode ser invertida, por não ser quadrada. Nestes casos, apenas existirá uma solução para a equação se o vetor do lado esquerdo da equação possuir o mesmo intervalo de espaço que a matriz jacobiana (SPONG; VIDYASAGAR, 1989) ˙ é determinada pelo usuário no software de A velocidade linear do efetuado X, controle. Uma vez que a inversa da jacobina varia dependendo da configuração do braço, a matriz de velocidade das juntas também irão variar, mesmo que a velocidade linear do efetuador seja constante (ASADA, 2008). 3.2.6 Restrições na conguração do manipulador Em alguns casos é necessário que o efetuador se movimente com uma orientação restrita. Um exemplo disso seria o movimento de uma lata de refrigerante aberta, como mostra a Figura 22. Deseja-se movimentar essa lata do ponto A para o ponto B por um caminho Capítulo 3. Metodologia 53 qualquer de forma que o plano do fundo da lata fique, durante todo o trajeto, paralelo ao chão evitando que a bebida derrame. Isso é alcançado descrevendo o ângulo do punho em função dos outros ângulos das juntas do cotovelo e ombro. Berenson e Kuffner (2012) e Ratliff Matt Zucker e Srinivasa (), apresentam técnicas baseada em otimização para restringir o movimento do punho durante a geração de trajetória. Figura 22 – Trajetória com restrição de orientação Fonte: Berenson e Kuffner (2012) Uma das formas de se restringir a orientação do punho é especificar três ângulos fixos com relação aos planos da base do robô e utilizar-se de um algoritmo de otimização como o método de Newton, que é um dos mais simples, para minimizar a função custo. Como o método de Newton tende a não convergir quando o manipulador está longe da meta, é utilizado o método da cinemática inversa por mínimos quadrados amortecido, apresentado na 3.2.3 para posicionar o efetuador na posição da meta, e a partir deste ponto, o método de Newton assume para satisfazer a orientação desejada. Usando o método de Newton-Raphson , a solução para o sistema de equações é determinado usando, iterativamente, a seguinte aproximação (GOLDENBERG; FENTON, Capítulo 3. Metodologia 54 1985) q(k+1) = q(k) + δ (k) (3.49) onde δ (k) é a solução para o sistema linear e   rj q (k) + n X (k) (k) Jji δi = 0, j = 1, ..., 6 (3.50) i=1 em que Jji é definido como (k) Jji  = ∂rj ∂qi ! i = 1, ..., n; j = 1, ..., 6 (3.51) q=q (k)  e rj q (k) consiste nos termos do vetor de erro residual entre a orientação e posição atuais do efetuador e meta. Estes valores são obtidos das matrizes de transformação homogênea, Equação 3.25, usando a seguinte relação (GOLDENBERG; FENTON, 1985):       rx = na . pt − pa ry = oa . pt − pa rz = aa . pt − pa  1  rφ = . aa .ot − at .oa 2  1  a t rθ = . n .a − nt .aa 2  1  a t rψ = . o .n − ot .na 2 (3.52) em que (φ, θ, ψ) são os ângulos desejados ao redor dos eixos xyz, respectivamente. Este método, funciona razoavelmente bem, até mesmo próximos de singularidades, porém deixa a resolução da cinemática inversa um pouco lenta e deve ser aplicado apenas onde há realmente a necessidade de se restringir o movimento do robô, como em operações de soldagem e pintura. É importante observar que para manipuladores com número de juntas menor que seis, não é possível satisfazer a meta e orientação desejada nos três eixos, para estes casos determina-se um ou mais valores do vetor r para 0. 3.3 Dinâmica do Manipulador 3.3.1 Conceitos preliminares Tensor de Inércia Um momento de inércia é definido como a integral do segundo momento em relação a um eixo de todos os elementos de massa dm que compõe o corpo. Para um eixo qualquer, o momento de inércia é (HIBBELER, 1999): I= Z m r2 dm (3.53) Capítulo 3. Metodologia 55 Para um corpo com densidade constante, podemos reescrever a equação acima, de forma que seus termos fiquem puramente geométricos, como segue: I=ρ Z r2 dV (3.54) V ) Onde ρ é a densidade do corpo e r é o vetor distância perpendicular do eixo até o elemento arbitrário dm. Para um elemento diferencial descrito em coordenadas cartesianas o vetor √ r com relação ao eixo de rotação x pode ser descrito como: r = y 2 + z 2 , o momento de inércia então fica: Z   Ixx = y 2 + z 2 ρdV (3.55) V Para os eixos y e z : Iyy = Z   (3.56)  (3.57) x2 + z 2 ρdV V Izz = Z  x2 + y 2 ρdV V O produto de inércia é um conjunto de dois planos ortogonais é definido como o produto da massa do elemento e as distâncias perpendiculares dos planos até o elemento. Para o plano y − z essa distância é x e para o plano x − z, a distância é y. Os produtos de inércia do corpo em relação a cada combinação de planos ficam: (HIBBELER, 1999) Ixy = Iyx = Z (xy) ρdV V Iyz = Izy = Z (yz) ρdV (3.58) V Ixz = Izx = Z (xz) ρdV V O tensor de inércia é um conjunto único de valores para um corpo quando é determinado para cada localização da origem O e orientação dos eixos de coordenadas, este tensor é definido como:   I −I −I xy xz   xx   I = −Iyx Iyy −Iyz  (3.59)   −Izx −Izy Izz O tensor de inércia é uma matriz simétrica, visto que Ixy = Iyx , Iyz = Izy e Ixz = Izx . Existem três eixos de inércia, que quando I é calculada, os termos Ixy = Iyx = Iyz = Izy = Ixz = Izx = 0, desta forma I se torna uma matriz diagonal. Quando o corpo é rotacionado em torno de seus eixos principais de inércia, não existe forças resultantes de desbalanceamento do corpo pois a massa está igualmente distribuída. Capítulo 3. Metodologia 56 Energia Potencial Energia potencial (simbolizado por U ou Ep) é a forma de energia que está associada a um sistema onde ocorre interação entre diferentes corpos e está relacionada com a posição que o determinado corpo ocupa, e sua unidade no Sistema Internacional de Unidades (SI), assim como o trabalho, é joule (J). A energia potencial é o nome dado a forma de energia quando está “armazenada”, isto é, que pode a qualquer momento manifestar-se , por exemplo, sob a forma de movimento, e a energia potencial é derivada de forças conservativas, ou seja, a trajetória do corpo não interfere no trabalho realizado pela força, o que importa são a posição final e a inicial, então o percurso não interfere no valor final da variação da energia potencial. U = −mi gr0,ci (3.60) Energia Cinética A energia cinética é a energia que está relacionada com o estado de movimento de um corpo. Este tipo de energia é uma grandeza escalar que depende da massa e do módulo da velocidade do corpo em questão. Quanto maior o módulo da velocidade do corpo, maior é a energia cinética. Quando o corpo está em repouso, ou seja, o módulo da velocidade é nulo, a energia cinética é nula. Para um dado sistema que se move a uma velocidade vci (velocidade do centro de massa) e velocidade angular ωi sua energia cinética é dada por (ASADA, 2008): K= n  X 1 i=1 1 mi |vci | + Ii ωi2 2 2 2  (3.61) 3.3.2 Equações de movimento do manipulador As equações de Lagrange são derivadas do princípio da ação mínima. Este teorema denota que a ação - uma grandeza física com dimensão equivalente à de energia multiplicada pela de tempo (joule-segundo no S.I.) - possui um valor estacionário - é máximo, mínimo, ou um ponto de sela - para a trajetória que será efetivamente percorrida pelo sistema em seu espaço de configuração (MARION; THORNTON, 1995). Para se utilizar das equações de Lagrange o sistema deve atender à alguns requisitos, como: ser holonômico, possuir coordenadas generalizadas independentes, e ter o tantos graus-de-liberdade quanto coordenadas generalizadas. Entende-se como coordenadas generalizadas de um sistema, o grupo mínimo de parâmetros para descrever completamente todas as configurações do sistema a qualquer instante t (VANDIVER, 2011). Capítulo 3. Metodologia 57 Trabalho virtual Uma força aplica trabalho W , quando existe um deslocamento da partícula ou corpo na direção de aplicação da força (HIBBELER, 1999). Um trabalho virtual consiste do produto de uma força generalizada e o deslocamento virtual que ela causa. Para um corpo que sofre um deslocamento de sua coordenada generalizada qi , seu trabalho seria: δW nc = n X (3.62) Qi δqi i=1 Assim como no caso de trabalho virtual para sistemas em equilíbrio estático, a variação do conjunto de energia cinética e energia potencial para uma força não conservativa seria zero. Considerado um dado instante em tempo, há uma variação infinitesimal na posição do sistema partindo de sua posição natural de equilíbrio dinâmico, a equação de variação de energia cinética, potencial e trabalho seria (VANDIVER, 2011): δK + δU − δW nc = 0 (3.63) Onde o índice nc significa que o trabalho é feito por uma força não conservativa.2 A técnica usada para encontrar as forças generalizadas do sistema é assumir que o sistema experimenta uma pequena deflexão virtual, da qual computa-se o incremento de variação de trabalho resultante. Equações de Lagrange Para um sistema com n graus de liberdade e coordenadas generalizadas qj , é possível calcular a função Lagrangiana L = K − U , onde K é a energia cinética e U é a energia potencial do sistema. A Lagrangiana é uma função de coordenadas generalizadas qj e velocidades generalizadas q˙j (VANDIVER, 2011): L = L (q1 , ...qi ...qn , q˙1 , ...q˙i ....q˙n ) (3.64) Onde n é o número de graus-de-liberdade do sistema. Uma vez que a energia potencial U , é função da coordenada generalizada qi , e a energia cinética é função da coordenada generalizada e da velocidade q˙i , pode-se reescrever a equação (3.64), como segue: L (qi , q˙i ) = K (qi , q˙i ) − U (qi ) (3.65) Utilizando as equações de Lagrange, as equações de movimento são, portanto: d dt 2 ∂L ∂ q˙i ! − ∂L = Qi ∂qi i = 1, 2..., n (3.66) Uma força é considerada conservativa quando causa trabalho em um sistema que é independente da trajetória do corpo ou partícula. Exemplos de forças conservativas são o peso de uma partícula e o trabalho realizado por uma força de mola (HIBBELER, 1999). Todas as outras forças externas e momentos que aplicam ou retiram energia do sistema são considerados como forças não conservativas. Capítulo 3. Metodologia 58 Onde Qi são as forças externas generalizadas. Uma vez que i vai de 1 à n, as equações de Lagrange fornecem n equações de movimento, que é o mesmo número de graus-de-liberdade que o sistema apresenta (VANDIVER, 2011). Este conjunto de equações diferenciais para um dado sistema, fornecem uma relação entre aceleração, velocidade e coordenadas. Elas constituem um conjunto de n equações diferenciais de segunda ordem. A solução geral contém n constantes, das quais podem ser determinadas pelas condições de contorno do sistema, isto é, valores das velocidades e coordenadas (GANZ, 2008). Fazendo uso da equação (3.65), a equação (3.66) pode ser reescrita como: d dt ! ∂ (K − U ) ∂ (K − U ) d − = ∂ q˙i ∂qi dt ! ∂ (K) d − ∂ q˙i dt ! ∂ (U ) ∂ (K) ∂ (U ) − + = Qi (3.67) ∂ q˙i ∂qi ∂qi Em sistemas mecânicos, a energia potencial nunca é uma função da velocidade (VANDIVER,   ) 2011), logo o termo dtd ∂(U = 0, portanto: ∂ q˙i d dt ! ∂ (K) ∂ (K) ∂ (U ) + = Qi − ∂ q˙i ∂qi ∂qi (3.68) Multiplicando ambos os lados por δqi , obtém-se: " d dt ! # ∂ (K) ∂ (U ) ∂ (K) − δqi + δqi = Qi δqi ∂ q˙i ∂qi ∂qi (3.69) A equação acima fica, portanto, da forma da equação (3.63) (VANDIVER, 2011). 3.3.3 Extendendo para um manipulador de n GDL Matriz de inércia multi-corpo Como mostra a equação (3.61), a energia cinética armazenada em um elo é composta de dois termos, energia cinética de translação e o outro devido à rotação sobre o centróide, 1 1 T Ki = mi vci vci + ωiT Ii ωi , 2 2 i = 1, 2, ..., n (3.70) onde ωi e Ii são, respectivamente, o vetor 3 × 1 de velocidade angular e a matriz 3 × 3 de inércia do elo i considerado pelos eixos de coordenada da base (referência inercial). A energia cinética total é, portanto: K= n X Ki (3.71) i=1 A velocidade linear e angular não são variáveis independentes, não atendendo ao requisito para utilizar as equações de Lagrange como discutido no começo da subseção 3.3.2. Para isto, é necessário reescrever a equação de energia cinética em função das coordenadas generalizadas apenas vci = JLi q˙ (3.72) ω i = JA q ˙ i Capítulo 3. Metodologia 59 onde JLi e JA i , são respectivamente, as matriz 3 × n relacionando a velocidade linear no centróide, e a velocidade angular do elo i com as velocidades das juntas. O jacobiano pode ser definido como: JLi = ∂r0,ci ∂qi (3.73) Substituindo a equação (3.72) nas equações (3.70) e (3.71): n T T 1 1X A m1 q˙ T JLi JLi q˙ + q˙ T JA ˙ = q˙ T Mq˙ K= i Ii Ji q 2 i=1 2   (3.74) onde M é uma matriz n × n dada por M= n  X m1 JLi T JLi + JA i T I i JA i  (3.75) i=1 A matriz M incorpora todas as propriedades de massas do manipulador inteiro, e é referido como uma matriz de inércia multi-corpos (ASADA, 2008). Como essa matriz envolve matrizes jacobianas, e estas variam com a configuração do manipulador, portanto, a matriz de inércia multi-corpos é dependente da configuração e representa as propriedades de massa instantaneamente para o manipulador naquela configuração. Escrevemos a matriz de inércia como M(q), uma função da variável da junta q. Fazendo uso dos componentes da matriz de inércia multi-corpos M = Mij , escrevemos a energia cinética total na forma quadrática escalar: K= n X n 1X Mij q˙i q˙j 2 i=1 j=1 (3.76) Diferenciando, para obter o primeiro termo da equação (3.69),   n n n X X d 1 X dMij d ∂K = Mij q˙j  = Mij q¨j + q˙j dt ∂ q˙i dt 2 j=1 j=1 j=1 dt (3.77) Por ser uma matriz simétrica, M, os pares de juntas i e j, possuem os mesmos coeficientes de iteração dinâmica, portanto, Mij = Mji O segundo termo da equação (3.77), geralmente é não nulo (SPONG; VIDYASAGAR, 1989), uma vez que a matriz M é dependente da configuração, aplicando a regra da cadeia neste termo, n n X X dMij ∂Mij dqk ∂Mij = = q˙k dt k=1 ∂qk dt k=1 ∂qk (3.78) Capítulo 3. Metodologia 60 O segundo termo da equação de movimento (3.69), também requer a derivada parcial de Mij da equação (3.76),   n X n X n X n ∂ 1 ∂T 1X ∂Mjk  = q˙j q˙k Mjk q˙j q˙k = ∂qi ∂qi 2 j=1 k=1 2 j=1 k=1 ∂qi (3.79) Substituindo equação (3.78)no segundo termo da equação (3.77) e combinando os termos resultantes com a equação (3.79), escrevemos os termos não lineares como (ASADA, 2008) hi = n X n X Cijk q˙j q˙k (3.80) j=1 k=1 onde os coeficientes Cijk é dado por: Cijk = ∂Mij 1 ∂Mjk − ∂qk 2 ∂qi (3.81) O coeficiente Cijk é chamado de símbolo de Christoffel (de primeira ordem) (SPONG; VIDYASAGAR, 1989). A equação (3.81) é não linear, uma vez que possui produtos de velocidades das juntas. Equação (3.81) pode ser dividida em termos proporcionais ao quadrado da velocidade das juntas, isto é, quando j = k e quando j 6= k. O primeiro representa os torques gerados por aceleração centrífuga e o segundo, os torque por aceleração de Coriolis (ASADA, 2008). hi = n X Cijj q˙j2 j=1 + n X Cijk q˙j q˙k (3.82) k6=j Forças generalizadas Forças que agem no sistema de corpos rígidos, podem ser representados por forças conservativas e não conservativas (ASADA, 2008). Como visto em seção 3.3.1, a energia potencial armazenada em todos os n elos do manipulador é dado por U =− n X mi gT r0,ci (3.83) i=1 Onde r0,ci é vetor posição do centróide Ci que é dependente da coordenada da junta. Substituindo esta equação, na equação de movimento de Lagrange (3.69), obtém-se a equação de torque por gravidade, para cada junta: Gi = n n X X ∂U ∂r0,cj =− mj gT =− mj gT JLj,i ∂qi ∂q i j=1 j=1 (3.84) Capítulo 3. Metodologia 61 onde JLj,i é um vetor coluna 3 × 1 que relaciona a velocidade linear do centróide do elo j com a velocidade da junta. Para um manipulador articulado, a equação de movimento pode ser escrita da seguinte forma: τ = M (q) q ¨ + C (q, q) ˙ q˙ + F (q, q) ˙ + G (q) (3.85) Onde: τ é o vetor de torque associado com as coordenadas generalizadas q. M é a matriz de massa do manipulador, M (q) q¨ = n X Mij q¨j (3.86) j=1 C descreve os efeitos da aceleração de Coriolis e centrípeta, torques relacionados com a primeira aceleração são proporcioanis à q˙i q˙j , enquanto que torques relacionados ao segundo termo são proporcionais à q˙i2 , C (q, q) ˙ q˙ = = n X j=1 n X j=1 n X Cijj q˙j2 + Cijk q˙j q˙k j6=k 1 ∂Mjj ∂Mij − ∂qj 2 ∂qi ! q˙j2 + n X j6=k ! ∂Mij 1 ∂Mjk − q˙j q˙k ∂qk 2 ∂qi (3.87) F descreve os efeitos por atrito viscoso e de Coulomb, e geralmente não é considerado parte da dinâmica de corpos rígidos (CORKE, 2011), G é o termo de carga pela gravidade, onde G = [G1 , G2 , ..., Gn ]T . 3.4 Sistemas de controle Quando considerado em um nível mais alto, o objetivo do sistema de controle do robô é permitir que este conclua uma tarefa especificada (DUYSINX; GERADIN, 2004). Existem vários tipos de controle para robôs (CORKE, 2011), sendo o tipo mais simples deles conhecido como controle de junta independente. Neste tipo de controle cada eixo do manipulador é controlado com um sistema de entrada-única e saída-única (do ingês, SISO: Single-input / single-output). Qualquer efeito de acoplamento devido ao movimento de outras juntas é tratado como distúrbio (SPONG; VIDYASAGAR, 1989). O objetivo deste tipo de sistema de controle é escolher um compensador de tal forma que a saída da (plant) consiga rastrear ou seguir uma saída desejada (meta, velocidade, aceleração ou torque), dado um sinal de referência. O sinal de controle, contudo, não é a única entrada agindo no sistema (SPONG; VIDYASAGAR, 1989)). O controlador é projetado, portanto, de forma que os efeitos dos distúrbios são reduzidos na saída. Capítulo 3. Metodologia 62 Figura 23 – Diagrama de blocos para o problema de controle do robô. Fonte: Spong e Vidyasagar (1989) A equação (3.85) mostrada na subseção 3.3.3, representa a dinâmica de uma cadeia de elos e juntas de um corpo rígido ideal, supondo que exista uma força agindo nas juntas. Embora esta equação seja complexa, ela é uma idealização e possui um número de efeitos dinâmicos que não são incluidos, ou são difíceis de se estimar com precisão como o atrito. Também, nenhum corpo é completamente rígido. Uma análise mais completa iria incluir várias fontes de flexibilidade, como deformações elásticas, engrenagens e rolamentos, deflexões dos elos sob carga e vibrações (SPONG; VIDYASAGAR, 1989). 3.4.1 Rastreio de um ponto de referência Um dos métodos de se rastrear uma meta é utilizando um compensador PID (do inglês: Proportional, Integral, Derivative). Este tipo de controle é adequado para as aplicações que não envolvam movimentos muito rápidos, especialmente em robôs com altas reduções de engrenagem entre o atuador e o elo. É para rastreio de uma meta apenas (não variante no tempo), uma vez que é limitado quando efeitos adicionais como entrada de saturação, distúrbios e dinâmica não-modelada devem ser consideradas (SPONG; VIDYASAGAR, 1989). Por este motivo, métodos de controle mais robustos são empregados. 3.4.2 Controle antecipatório e torque computado Controle antecipatório (Feedforward) é um método para rastrear uma trajetória variante no tempo e ao mesmo tempo rejeitar distúrbios variantes no tempo. Um esquema de controle feedforward consiste em adicionar um bloco antecipatório com a função de transferência como mostrado (SPONG; VIDYASAGAR, 1989). Permita que cada uma das três funções de transferência seja representada por razões polinomiais: G (s) = q (s) c (s) a (s) H (s) = F (s) = p (s) d (s) b (s) (3.88) Capítulo 3. Metodologia 63 Figura 24 – Sistema de simulação do robô e controle Fonte: Lilly (2012) Uma simples multiplicação entre os blocos do diagrama mostra que a função de transferência (s) T (s) = YR(s) é dada por: T (s) = q (s) + (c (s) b (s) + a (s) d (s)) b (s) + (p (s) d (s) + q (s) c (s)) (3.89) Para alcançar estabilidade no sistema de malha fechada, é necessário que o compensador H (s) e a função de transferência feedfoward F (s) possa ser escolhida de forma que o polinômio p (s) d (s) + q (s) c (s) e b (s) sejam estáveis pelo critério de Routh–Hurwitz, isso significa que além da função de transferência da malha fechada ser estável a função Capítulo 3. Metodologia 64 Figura 25 – Esquema do controle antecipatório Fonte: Spong e Vidyasagar (1989) de transferência feedfoward F (s), precisa também ser estável (SPONG; VIDYASAGAR, 1989). Se existe um distúrbio entrando no sistema como mostrado abaixo, então é possível mostrar que o erro de rastreio E (s) é dado por: E (s) = q (s) d (s) D (s) p (s) d (s) + q (s) c (s) (3.90) No caso de não haver distúrbios o sistema fechado irá rastrear qualquer trajetória desde que o sistema seja estável. O erro é, portanto, devido apenas aos distúrbios (SPONG; VIDYASAGAR, 1989). Cancelamento de distúrbios por Torque computado O sinal feedforward resulta em um rastreio assintótico de qulquer trajetória com ausência de distúrbios mas não melhora as propriedades de rejeição de distúrbios do sistema. Portanto, adicionamos para o sinal feedfoward, um termo que antecipa o efeito de distúrbio. Para uma trajetória desejada, é sobreimposto o termo: dd := X   djk q d q¨jd + X    cijk q d q˙id q˙jd + gk q d  (3.91) Uma vez que dd tem unidade de torque, a equação (3.91) é chamada de método do torque computado. A equação acima compensa de uma maneira feedforward o termo não linear do acoplamento inercial, aceleração de coriolis, centrípeta, e forças gravitacionais que aparecem devido ao movimento do manipulador. Esta equação é de extrema complexidade, se tornando uma preocupação no tempo de processamento. Uma vez que a trajetória desejada precisa ser conhecida, muitos dos termos podem ser pré-calculados e armazenados off-line (SPONG; VIDYASAGAR, 1989). Capítulo 3. Metodologia 65 Figura 26 – Compensação de distúrbio por torque computado Fonte: Spong e Vidyasagar (1989) Figura 27 – Diagrama de blocos do método de torque computado Fonte: Spong e Vidyasagar (1989) 66 Capítulo 4 Modelamento do Manipulador Robótico 4.1 Requisitos iniciais Para que o projeto do manipulador pudesse ser iniciado, teve-se que estabelecer alguns requisitos iniciais. Estes valores são descritos a seguir. 4.1.1 Capacidade de carga O manipulador robótico projetado neste trabalho não possui uma missão específica, ele pode ser empregado em uma série de tarefas que vão desde soldagem à separação e montagem de componentes em uma linha de montagem, cada tarefa irá requerer um efetuador específico, por este motivo o projeto do efetuador não faz parte do escopo deste trabalho. Para fins de se limitar a dimensão do manipulador, determina-se que o manipulador tenha uma carga útil (do inglês: payload) de no máximo 15kg quando estiver em uma configuração composta de seis juntas (6 GDL). 4.1.2 Número de graus de liberdade O manipulador proposto é composto de 6 juntas e 5 elos, contudo, por ser um manipulador modular, pode-se alterar o número de juntas e elos do robô com certa facilidade. O robô foi projeto de maneira a ser possível trabalhar com o mínimo de 4 juntas, e no máximo 8 juntas. A carga útil do manipulador depende da quantidade de GDL do manipulador e do comprimento e peso de cada elo/junta, a tabela abaixo mostra a carga útil de acordo com o número de juntas montados no manipulador. 4.1.3 Velocidade linear Não se faz aqui um requisito mínimo ou máximo de velocidade linear, pois este irá depender de uma série de fatores, como: número de juntas, comprimento dos elos, tipo de atuador utilizado, sistema de controle e tipo de trajetória adotada (KOIVO, 1989). Capítulo 4. Modelamento do Manipulador Robótico 67 Tabela 3 – Capacidade de carga do manipulador Número de juntas Carga útil máxima (kg) 4 17 5 16 6 15 7 12 8 9 Fonte – Produzido pelo autor Nota – Os valores apresentados na tabela foram obtidos aplicando-se um fator de segurança igual a 3. 4.2 Detalhamento do mecanismo Depois de considerados os requisitos iniciais, partiu-se para o projeto do manipulador. O modelo é descrito a seguir, a Tabela 4 mostra as especificações estimadas do manipulador proposto, os valores foram estimados a partir dos dados apresentados por Schuler et al. (2006). Os desenhos do projeto podem ser consultados no Apêndice C. Figura 28 – Manipulador proposto em uma configuração arbitrária Fonte: Produzido pelo autor Capítulo 4. Modelamento do Manipulador Robótico 68 Tabela 4 – Especificações técnicas estimadas para o manipulador Parâmetro Valor Unidade Graus de liberdade 6 - Faixa de trabalho (alcance máx.) 1,2 m Força máxima (estático) 350 N Força máxima (em movimento) 200 N Força contínua em movimento 150 N Precisão unidirecional +/-1,0 mm Repetibilidade unidirecional +/-0,4 mm Velocidade linear máxima do efetuador 1,65 m/s Frequência do sistema de controle 50 Hz Massa total 26,8 kg Fonte – Produzido pelo autor 4.2.1 Módulo da junta A parte mecânica mais complexa do manipulador é a junta. Ela deve conter inúmeros componentes para alta capacidade de carga ocupando o mínimo de espaço possível. Além disso, ela deve possuir funções adicionais como: freio de emergência, cabeamento para conduzir sinal e potência de uma junta para outra, e em alguns casos possuir atuação manual. Com o intuito de minimizar a complexidade da unidade central de controle, controle local do motor e sensores são inevitáveis. O projeto da junta foi baseado em um modelo proposto por Schuler et al. (2006) em um projeto desenvolvido para a NASA (National Aeronautics and Space Administration), este modelo usa um eixo vazado para transmitir torque do motor (do tipo frameless brushless DC motor) até o redutor de velocidade (Harmonic Drive). Ainda de acordo com Schuler et al. (2006), utilizou-se do conceito modular, com o intuito de se reduzir o custo de desenvolvimento, produção e montagem, portanto, a parte interna contém os mesmos componentes para todas as juntas, porém com escalas diferentes. Cada junta é integrada dentro de uma carcaça própria, o que permite que ela possa ser montada e testada fora do robô. As juntas que compõem o ombro exigem torques e rigidez maiores, por este motivo possuem motores de alto torque. Cada junta possui um motor DC sem escovas e sem carcaça, um freio eletromagnético, um harmonic drive para redução da velocidade, um encoder elétrico na saída do eixo, um sensor de torque e um sistema de trava contra rotação excessiva. A Figura 29 mostra a junta do cotovelo e a Tabela 5, mostra as especificações técnicas para cada junta. Capítulo 4. Modelamento do Manipulador Robótico 69 Figura 29 – Junta do cotovelo Fonte: Produzido pelo autor Atuador: motor DC sem escova e sem carcaça O conjunto da junta deve ser extremamente compacto, por este motivo, elas foram projetadas usando-se um motor especial que não possui carcaça, nem eixo próprio. O motor, conhecido como Direct Drive, foi projetado para ser incorporado diretamente na máquina, usando os próprios rolamentos da máquina para suportar o rotor. Esse motor é montado dentro da carcaça da junta, e possui sensores de efeito Hall 1 para monitorar a posição do rotor. A Figura 30 mostra o motor da empresa Kollmorgen, escolhido a junta do cotovelo. Harmonic drive Em manipuladores robóticos é comum o uso de harmonic drives para a redução da velocidade do motor, uma vez que este tipo de redutor é extremamente compacto e oferece 1 Um Sensor de Efeito Hall é um transdutor que, quando sob a aplicação de um campo magnético, responde com uma variação em sua tensão de saída. Capítulo 4. Modelamento do Manipulador Robótico 70 Tabela 5 – Especificações técnicas das juntas Junta Diâm. externo (mm) Comprimento (mm) Torque máx. (N m) Variação angular (◦ ) Veloc. máx. (RPM) 1 - Ombro 150 194,7 627,2 -200/+200 23,75 2 - Ombro 150 194,7 627,2 -200/+200 23,75 3 - Cotovelo 125 221 263,68 -200/+200 29,06 4 - Punho (pitch) 90 143,35 62,34 -200/+200 95 5 - Punho (yall) 90 143,35 62,34 -200/+200 95 6 - Punho (roll) 90 143,35 62,34 -200/+200 95 Fonte – Produzido pelo autor Nota – Os valores apresentados na tabela foram obtidos com base nos catálogos dos fabricantes de cada componente. Figura 30 – Frameless brushless DC motor Fonte: Kollmorgen reduções entre 50:1 até 200:1, valores ideias para um manipulador (SCHULER et al., 2006). Para a junta do cotovelo mostrado na Figura 29, o redutor escolhido tem redução de 160:1. Os harmonic drives utilizados no projeto são produzidos pela empresa Harmonic Drive AG, a Figura 31 mostra o redutor aplicado na junta 3 (cotovelo). Freio eletromagnético Um freio permanente para alta rotação é usado na junta como sistema de segurança e para parada rápida do movimento. No caso de haver corte da tensão, o freio é ativado automaticamente, uma vez que este utiliza a tensão para desmagnetizar o disco. Como é possível perceber na Figura 29, o freio é montado antes do motor (lado oposto do harmonic drive), isto acontece pois nessa posição a redução de velocidade ainda não ocorreu, o que significa que o freio só precisa ter capacidade de parar o torque proveniente do motor (aprox. Capítulo 4. Modelamento do Manipulador Robótico 71 Figura 31 – Harmonic drive escolhido para a junta do cotovelo Fonte: Harmonic Drive UG. 4Nm), e não o torque ampliado 160 vezes presente no eixo de saída. O freio escolhido, mostrado na Figura 32 foi projetado para ambientes secos, sem lubrificação, e é produzido pela empresa KEB. Figura 32 – Freio permanente eletromagnético, modelo Combiperm Fonte: KEB. Trava mecânica A trava limita mecanicamente a variação angular de operação da junta. Foi projetado de maneira a permitir uma variação de rotação de aproximadamente 400◦ . Uma rotação de Capítulo 4. Modelamento do Manipulador Robótico 72 junta acima de 360◦ é considerado útil para diminuir singularidades (SCHULER et al., 2006). A Figura 33 mostra o modelo proposto de trava para a junta do cotovelo. Figura 33 – Trava mecânica proposta Fonte: Produzido pelo autor. Sensor de torque A junta contém um sensor de torque integrado baseado em extensômetros elétricos (strain gauges). Uma configuração usando uma ponte Wheatston (ponte completa), compensa variações na temperatura tanto quanto variações no eixo do sensor, como deflexão, cargas axiais e radiais. O volume e massa requeridos são baixos, de acordo com Schuler et al. (2006), este tipo de sensor é preciso e linear para variações de torque estático de até 400Nm. O sensor é feito de um eixo vazado de parede fina assim como o eixo de saída do harmonic drive, a deformação devido a torção causada em ambos são compensadas pela unidade de controle com base na leitura dos sensores. A Figura 34 mostra o sensor de torque proposto. Para alcançar alta qualidade na medição do torque, os sinais do strain gauges devem ser ampliados antes de serem enviados para o controlador da junta. Para isto é necessário um circuito eletrônico específico para este fim, que é acoplado na junta junto com as placas de circuito impresso de controle do atuador e sensor de rotação. Os fios que saem do strain gauges passam pelo eixo vazado até a PCI. Capítulo 4. Modelamento do Manipulador Robótico 73 Figura 34 – Sensor de torque Fonte: Produzido pelo autor Eixo vazado A junta possui um eixo vazado por onde o cabeamento de alimentação dos sensores e atuadores podem passar pela junta sem necessidade de ficarem exposto externamente. É possível também passar neste eixo, mangueiras de ar comprimido, caso o robô esteja usando um efetuador pneumático (como garras com fechamento pneumático), mangueiras de até 6mm podem ser passados por dentro do eixo de uma junta à outra. A Figura 35 mostra o eixo proposto. Figura 35 – Eixo principal da junta do cotovelo Fonte: Produzido pelo autor Capítulo 4. Modelamento do Manipulador Robótico 74 Encoder O encoder utilizado é produzido pela empresa Netzerprecision. Ele funciona com 5V de tensão, tem baixo consumo de energia, baixo valor de inércia e quase nenhum atrito interno. Este modelo é ideal para a junta, pois além de ser vazado, requisito para permitir a passagem dos cabos, ele também permite rotação contínua, o que é necessário pois a trava mecânica só age na junta após 400◦ . O sinal de saída do encoder é uma curva senoidal ou coseinodal que representa o ângulo do eixo. O sinal digital deve ser obtido através de processamento adicional usando a PCI de controle. A Figura 36 mostra o encoder utilizado. Figura 36 – Sensor de posição angular (encoder) Fonte: Netzerprecision Controlador do atuador Um controlador de motor é um dispositivo eletrônico que atua como um dispositivo intermediário entre a unidade central de controle, uma fonte de alimentação e os motores. Embora a unidade central (computador) decide a velocidade e direção dos motores, não pode movê-los diretamente por causa de sua potência limitada (corrente e tensão). O controlador do motor, por outro lado, pode fornecer a corrente na tensão necessária, mas não pode decidir o quão rápido o motor deve girar. Assim, o computador e o controlador do motor tem que trabalhar em conjunto a fim de fazer mover os motores de forma adequada. Normalmente, a unidade central de controle pode instruir o controlador do motor sobre como alimentar os motores através de um método de comunicação padrão e simples, como UART (conhecido como serial) ou PWM. Além disso, alguns controladores de motor pode ser controlado manualmente por uma tensão analógica (geralmente criados com um potenciômetro) (SCLATER; CHIRONIS, Capítulo 4. Modelamento do Manipulador Robótico 75 1991). Na Figura 29 percebe-se que existe três discos paralelos montados externamente ao freio. Estas placas representam as PCIs de controle do motor. Não faz parte do escopo deste trabalho o desenvolvimento eletrônico do controlador dos atuadores, mas fica como sugestão para trabalho futuro. 4.2.2 Elos do manipulador O módulo do elo tem a função de aumentar o alcance do manipulador e fazer o ligamento entre duas juntas. É a parte mais simples de todo o robô, uma vez que todas os componentes principais como motor e sensores, encontram-se na junta. Todos os fios elétricos que ligam um motor a outro e os cabos do controlador passam por dentro do elo, já que este é feito a partir de um tubo. Figura 37 – Conjunto do elo 2 Fonte: Produzido pelo autor 4.2.3 Punho A Figura 38 mostra o conjunto do punho desenvolvido para o robô. Percebe-se que ele não é esférico, uma vez que os três eixos não se cruzam em um ponto comum. Decidiu-se por usar esta configuração de punho por ser possível o uso do mesmo conjunto de junta nos três eixos que o compõe. Ao final do punho existe uma flange com furos roscados em que é possível afixar uma garra, ferramenta, arco de solda, etc. Capítulo 4. Modelamento do Manipulador Robótico 76 Figura 38 – Vista em corte do punho Fonte: Produzido pelo autor 4.3 Análise estrutural por Elementos Finitos Para verificação e validação do modelo projetado foi feita uma análise de elementos finitos (FEA) utilizando o software ANSYS, em que se buscou averiguar os pontos nas peças que concentram as maiores tensões, qual o fator de segurança para cada modelo e se vão ou não falhar devido as cargas às quais são submetidos. A análise foi feita nas peças estruturais principais do manipulador: elos 2 e 3, e o eixo principal de cada junta. O material empregado no robô é uma liga de alumínio que possui uma tensão de escoamento de 280MPa e tensão última de tração de 310MPa. A análise foi feita fixando-se uma extremidade do elo e aplicando uma carga na outra. Para o elo 2 foi aplicado uma força de 300N, já para o elo 3, a força aplicada foi de 150N, simulando um corpo com massa de 15kg. Para os eixos principais, foi inserido um suporte fixo nos furos onde o eixo é preso no harmonic drive e porteriormente foi aplicado um momento na face do eixo onde existe o rasgo da chaveta. Para o eixo da junta da base, esse momento foi de 630Nm, para o eixo da junta do cotovelo, o momento foi de 265Nm e 65Nm para o eixo da junta do punho. Capítulo 4. Modelamento do Manipulador Robótico 77 Analisando os resultados percebe-se que a tensão equivalente de von-Mises é menor que a tensão de escoamento do material para todos os casos. Pode-se concluir, portanto, que os componentes analisados não irão falhar. As deformações resultantes nos elos são consideradas altas para um manipulador robótico, sendo este o principal motivo pela baixa precisão unidirecional do manipulador, +/ − 1, 0mm como mostrado na Tabela 4. As figuras abaixo mostram os resultados das simulações: deformação máxima, tensão de von-Mises e fator de segurança distribuído de cada componente. Figura 39 – Resultado da análise feita no elo 2 Capítulo 4. Modelamento do Manipulador Robótico Figura 40 – Resultado da análise feita no elo 3 78 Capítulo 4. Modelamento do Manipulador Robótico Figura 41 – Resultado da análise feita no eixo principal da junta da base 79 Capítulo 4. Modelamento do Manipulador Robótico Figura 42 – Resultado da análise feita no eixo principal da junta do cotovelo 80 Capítulo 4. Modelamento do Manipulador Robótico Figura 43 – Resultado da análise feita no eixo principal da junta do punho 81 82 Capítulo 5 Análise e Resultados 5.1 Cinemática A figura Figura 44 mostra a representação cinemática do manipulador proposto, é inserido nessa figura o alinhamento dos eixos de referência de cada junta. É possível perceber que os eixos da junta 1 estão sobrepostos com os eixos de referência inercial do robô (referência global), isto é feito com o intuito de simplificar as matrizes de cinemática direta, nota-se também que os eixos z1 , ..., zn são alinhados com o eixo de rotação das juntas. Usando o procedimento mostrado na subseção 3.2.1 obtém-se os parâmetro D-H que são mostrados na Tabela 6. Figura 44 – Manipulador proposto com os eixos de referência de cada elo Fonte: Produzido pelo autor. Capítulo 5. Análise e Resultados 83 Tabela 6 – Parâmetros de Denavit–Hartenberg do manipulador proposto Parâmetro Elo 1 Elo 2 Elo 3 Elo 4 Elo 5 Elo 6 ai [mm] 0 620 580 0 0 0 αi [ ] 90 0 0 90 -90 0 di [mm] 154 0 0 -225 -150 -113,5 θ1 θ2 θ3 θ4 θ5 θ6 o o θi [ ] Fonte – Produzido pelo autor. Procura-se alinhar os eixos de forma a obter o máximo de zeros nos parâmetros di e αi , por isso os eixos de coordenada x das juntas 2 e 3 são colineares. Com base nos parâmetros acima, pode-se estabelecer as matrizes de transformação homogênea para cada junta. Para auxiliar na formatação do texto, será utilizado a letra C com subíndice para denotar a função cosseno do ângulo, o mesmo para o seno, que será representado pela letra S. Assim os valores C1 , C12 , S1 , S12 , correspondem à cos(θ1 ), cos(θ1 + θ2 ), sin(θ1 + θ2 ), sin(θ1 + θ2 ), respectivamente.  T01 = A1 =         T12 = A2 =         T23 = A3 =         T34 = A4 =        cθ1 −sθ1 cα1 sθ1 sα1 a1 cθ1 sθ1 cθ1 cα1 −cθ1 sα1 a1 sθ1 0 sα1 cα 1 d1 0 0 0 1                 = cθ2 −sθ2 cα2 sθ2 sα2 a2 cθ2 sθ2 cθ2 cα2 −cθ2 sα2 a2 sθ2 0 sα2 cα 2 d2 0 0 0 1                 cθ3 −sθ3 cα3 sθ3 sα3 a3 cθ3 sθ3 cθ3 cα3 −cθ3 sα3 a3 sθ3 0 sα3 cα 3 d3 0 0 0 1                 cθ4 −sθ4 cα4 sθ4 sα4 a4 cθ4 sθ4 cθ4 cα4 −cθ4 sα4 a4 sθ4 0 sα4 cα 4 d4 0 0 0 1                 = = = cθ1 sθ1 0 0  0 sθ1 0   0 −cθ1 0    1 0 154   0 0 1 cθ2 −sθ2 sθ2 cθ2 0 0 0 0 0 620cθ2 0 620sθ2 1 0 0 1  cθ3 −sθ3 sθ3 cθ3 0 0 0 0 0 580cθ3 0 580sθ3 1 0 0 1  cθ4 sθ4 0 0                0 sθ4 0   0 −cθ4 0    1 0 −225   0 0 1 (5.1) Capítulo 5. Análise e Resultados  T45 = A5 =         T56 = A6 =        84 cθ5 −sθ5 cα5 sθ5 sα5 a5 cθ5 sθ5 cθ5 cα5 −cθ5 sα5 a5 sθ5 0 sα5 cα 5 d5 0 0 0 1 cθ6 −sθ6 cα6 sθ6 sα6 a6 cθ6 sθ6 cθ6 cα6 −cθ6 sα6 a6 sθ6 0 sα6 cα 6 d6 0 0 0 1                 =                 = cθ 5 sθ5 0 0  0 sθ5 0   0 −cθ5 0    1 0 −150   0 0 1 cθ6 −sθ6 sθ6 cθ6 0 0 0 0 0 0 0 0 1 −113, 5 0 1         A transformação da base para o efetuador é dada pela multiplicação das seis matrizes Ai , como segue:  T06 = A1 × A2 × A3 × A4 × A5 × A6 =         n x s x ax p x     ny sy ay py  n s a p =   nz sz az pz  0 0 0 1  0 0 0 1 (5.2) Em que p é o vetor posição da base do robô ao efetuador, n é o vetor de orientação do ângulo de guinada, s é a orientação do ângulo de inclinação e a é o vetor de orientação do ângulo de rolagem. 5.1.1 Jacobiano Para que seja possível implementar a cinemática inversa no software a matriz Jacobiana deve ser calculada de forma numérica eliminando, assim, a necessidade de se trabalhar com as derivadas parciais quem compõem a matriz. Isto é necessário porque os cálculos com variáveis simbólicas no MATLAB são consideravelmente mais lentos em comparação com manipulação de matrizes de números apenas. De acordo com Spong e Vidyasagar (1989), a matriz Jacobiana pode ser calculada recursivamente da seguinte forma:   zi−1 (on − oi−1 )  J = zi−1 (5.3) em que zi−1 = R0i−1 k, e on − oi−1 = R0i−1 dni−1 das quais k é o vetor unitário na direção do eixo z, portanto, k = (0, 0, 1)T , R0i−1 é a matriz de rotação da junta 0 a i − 1, e d é o vetor formado pela junta i − 1 até o efetuador: dni−1 = pn − pi−1 . O vetor p e a matriz R0i−1 são obtidos através das respectivas matrizes de transformação homogênea T . Por haver exatamente seis GDL, a matriz jacobiana é quadrada, isso significa que o manipulador não é redundante. Capítulo 5. Análise e Resultados 5.2 85 Controle Os sistemas de controle foram implementados utilizando o MATLAB Control System Toolbox (Simulink), e são baseados nos sistemas apresentado por Corke (2011) com algumas adaptações para a integração com o software de controle. A Figura 45 e Figura 46 mostram os diagramas de bloco dos dois sistemas de controle. Figura 45 – Diagrama de blocos do controle torque computado Fonte: Produzido pelo autor Capítulo 5. Análise e Resultados 86 Figura 46 – Diagrama de blocos do controle antecipatório Fonte: Produzido pelo autor 5.3 Tra jetória Existem sete possíveis tipos de trajetórias que o usuário pode escolher para descrever um movimento, sendo a mais utilizada a trajetória ponto-a-ponto coordenada, que faz a interpolação polinomial dos pontos inicial e final. Para se obter um movimento ainda mais Capítulo 5. Análise e Resultados 87 suave, optou-se por usar um polinômio de quinto grau, apresentado abaixo, com isto, é possível se determinar também uma velocidade e aceleração nos pontos inicial e final. θ(i) = a0 + a1 t + a2 t2 + a3 t3 + a4 t4 + a5 t5 (5.4) ˙ = a1 + 2a2 t + 3a3 t2 + 4a4 t3 + 5a5 t4 θ(i) (5.5) ¨ = 2a2 + 6a3 t + 12a4 t2 + 20a5 t3 θ(i) (5.6) onde: a0 = θ0 a1 = θ˙0 a3 = a4 = θ¨0 2     20θf − 20θ0 − 8θ˙f + 12θ˙0 tf − 3θ¨0 − θ¨f t2f 2t3f  a5 =   (5.7)  30θ0 − 30θf − 14θ˙f + 16θ˙0 tf − 3θ¨0 − 2θ¨f t2f 2t4f    12θf − 12θ0 − 6θ˙f + 6θ˙0 tf − θ¨0 − θ¨f t2f  a6 = 2t5f A velocidade nos pontos de passagem devem ser informados pelo usuário, quando não informados, é usado uma heurística para se determinar. Para cada junta um possível gráfico de posição angular por tempo é mostrado na Figura 47. Para trajetória em linha reta, o comprimento L da trajetória é dado por L = kpm − p0 k = q (xm − x0 )2 + (ym − y0 )2 + (zm − z0 )2 (5.8) Para trajetória parametrizada, o comprimento da curva no espaço é dado pela integral das funções que parametrizam a curva: S= v Ztf u u t 0 dx dt !2 dy + dt !2 dz + dt !2 dt (5.9) Para trajetória modelada em CAD, o comprimento da curva é aproximado por: S∼ = np −1 X i=1 kpi+1 − pi k = np −1 q X (xi+1 − xi )2 + (yi+1 − yi )2 + (zi+1 − zi )2 i=1 onde np é o número de pontos da tabela (gerados pelo programa CAD) (5.10) Capítulo 5. Análise e Resultados 88 Figura 47 – Pontos de passagem com as velocidades desejadas nos pontos indicados pelas tangentes Fonte: Craig (1989) Se a velocidade linear é dada, a velocidade angular pode ser calculada usando o ˙ portanto: Jacobiano, v = J θ,   J −1 v, n = 6 θ˙ =  † J v, n 6= 6 (5.11) onde J † é a pseudoinversa da matriz jacobiana, para os casos em que J não for quadrada, e o vetor v é obtido à partir da velocidade linear escalar v. v = kvk = q vx2 + vy2 + vz2 vx = vy = vz ∴v= (5.12) T √1 [v, v, v] 3 A Tabela 7 mostra os sete tipos de trajetórias que foram implementadas no software e o método de resolução de cada uma delas. Capítulo 5. Análise e Resultados 89 Tabela 7 – Tipos e características de trajetórias Núm (1) Trajetória (Tipo) Descoordenado (ponto-a-ponto) Entradas do usuário meta cartesiana; θ˙ Método de resolução θ0 e θf calculados por cinemática inversa; Duração do trajeto ti = (θ0 −θf ) θ˙ ti = (θ0 −θf ) θ˙ ˙ função: θ(t) = θ0 + θt (2.1) Sequencial (ponto-a-ponto) meta cartesiana; θ˙ θ0 e θf calculados por cinemática inversa; ˙ função: θ(t) = θ0 + θt (2.2) (3) (4) (5.1) (5.2) Sequencial (ponto-a-ponto) meta cartesiana; tempo de trajeto Coordenado (ponto-a-ponto) meta cartesiana; tempo de trajeto Coordenado com pontos de passagem (ponto-a-ponto) meta cartesiana; tempo de trajeto; velicidade linear e angular do efetuador em cada ponto de passagem2 Linha reta (ponto-a-ponto) Linha reta (ponto-a-ponto) meta cartesiana; tempo de trajeto meta cartesiana; velocidade linear constante, (v) θ0 e θf calculados por cinemática inversa; θ(t): Equação 5.4 ˙ θ(t): Equação 5.5 ¨ θ(t): Equação 5.6 θ0 e θf calculados por cinemática inversa; θ(t): Equação 5.4 ˙ θ(t): Equação 5.5 ¨ θ(t): Equação 5.6 θ0 e θf calculados por cinemática inversa; θ(t): Equação 5.4 ˙ θ(t): Equação 5.5 ¨ θ(t): Equação 5.6 θ0 e θf calculados por cinemática inversa; θ(t): algoritmo de Taylor ˙ θ(t): derivada numérica (3.16) ¨ θ(t): derivada numérica (3.17) Especificado pelo usuário Especificado pelo usuário Especificado pelo usuário para cada ponto de passagem Especificado pelo usuário θ0 e θf calculados por cinemática inversa; θ(t): algoritmo de Taylor ˙ θ(t): Equação 5.11 ¨ θ(t): derivada numérica ˙ (3.16) de θ, t= L v Capítulo 5. Análise e Resultados 90 Cont. Tipos e características de trajetórias Núm (6.1) (6.2) (7.1) (7.2) Trajetória (Tipo) Parametrizada (contínua) Parametrizada (contínua) Modelada em CAD (tabelada) Modelada em CAD (tabelada) Entradas do usuário funções paramétricas no espaço cartesiano; tempo de trajeto Método de resolução θ(t) : calculado pela cinemática inversa em vários pontos; Especificado pelo usuário ˙ θ(t): derivada numérica3.16 ¨ θ(t): derivada numérica3.17 θ(t) : calculado pela cinemática inversa em vários pontos; funções paramétricas no espaço cartesiano; velocidade linear constante, (v) tabela de pontos no espaço cartesianos; tempo de trajeto Duração do trajeto t= ˙ θ(t): Equação 5.11 ¨ θ(t): derivada numérica ˙ (3.16) de θ, θ(t) : calculado pela cinemática inversa em vários pontos; S v Especificado pelo usuário ˙ θ(t): derivada numérica3.16 ¨ θ(t): derivada numérica3.17 tabela de pontos no espaço cartesianos; velocidade linear constante, (v) θ(t) : calculado pela cinemática inversa em vários pontos; t= ˙ θ(t): Equação 5.11 ¨ θ(t): derivada numérica ˙ (3.16) de θ, S v Fonte – Produzido pelo autor. 5.3.1 Exemplo de trajetórias A seguir são apresentados alguns exemplos de trajetórias com o intuito de analisar o comportamento cinemático do manipulador. Ponto-a-ponto: junta interpolada (coordenado) O primeiro exemplo consiste de um movimento coordenado através de interpolação de quinto grau das juntas (Equação 5.4). O manipulador estava posicionado na origem: x = 1025, y = 215, z = 841 e foi movido para a posição: x = −100, y = −300, z = 670 com orientação arbitrária e tempo t = 10. A figura Figura 48 mostra o movimento e as curvas de posição, velocidade e aceleração. A trajetória foi determinada usando as definições do item (3) da Tabela 7. Capítulo 5. Análise e Resultados 91 Figura 48 – Exemplo 1 - coordenado; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor Percebe-se que as curvas são suaves e sem descontinuidades, por este motivo este é o método preferível para movimentações de cargas e afins. Ponto-a-ponto: descoordenado A Figura 49 mostra o trajeto feito para os mesmas coordenadas de partida e destino do exemplo anterior, mas dessa vez com trajetória descoordenada, a orientação é arbitrária e velocidade angular das juntas é constante θ˙ = 20[o /s]. Capítulo 5. Análise e Resultados 92 Figura 49 – Exemplo 2 - descoordenado; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor Ponto-a-ponto: sequencial Neste exemplo o manipulador também parte da posição de origem x = 1025, y = 215, z = 841 para a posição: x = −100, y = −300, z = 670 com orientação arbitrária e tempo t = 10, mas as juntas são acionadas de forma sequencial, começando a partir da junta da base. Capítulo 5. Análise e Resultados 93 Figura 50 – Exemplo 3 - sequencial; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor Ponto-a-ponto: linear Nete exemplo, partiu-se da posição de origem x = 1025, y = 215, z = 841 para a posição: x = 400, y = 550, z = 950 com orientação arbitrária e tempo t = 10 e trajetória linear, obtida através do algoritmo de Taylor. Capítulo 5. Análise e Resultados 94 Figura 51 – Exemplo 4 - linear; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor A seguir são apresentados os gráficos de algumas trajetórias parametrizadas em um manipulador tridimensional com 6 graus-de-liberdade: Parametrizada: círculo Um círculo de raio r e centro em (h, k), pode ser parametrizado pelas seguintes equações:    x    =a     z = r sin (t) + k y = r cos (t) + h t ∈ [0, 2π] Considerando os valores a = 600, r = 500, h = 100, k = 600 e 0 ≤ t ≥ 2π (5.13) Capítulo 5. Análise e Resultados 95 Figura 52 – Exemplo 5 - circulo; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor Parametrizada: elipse    x    =c     z = Zc + a cos (t) sin (ϕ) + b sin (t) sin (ϕ) y = Yc + a cos (t) cos (ϕ) − b sin (t) sin (ϕ) t ∈ [0, 2π] (5.14) Em que (Xc , Yc ) é o centro da elipse e ϕ é o ângulo entre o eixo Y e o eixo da elipse. Usando c = 800, Yc = 0, Zc = 600, a = 200, ϕ = 30 b = 400, d = 0, e = 700 e 0 ≤ t ≥ 2π Capítulo 5. Análise e Resultados 96 Figura 53 – Exemplo 6 - elipse; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor Parametrizada: curva de Lissajous    x    =c     z = b sin (kz t) + e y = a cos (ky t) + d t ∈ [0, 2π] (5.15) Onde ky e kz são constantes descrevendo o número de lóbulos na figura. Usando c = 600, a = 300, b = 400, ky = 1 kz = 3, d = 0, e = 700 e 0 ≤ t ≥ 2π com velocidade linear do manipulador v = 200[mm/s], obtém-se a curva mostrada na Figura 54. Capítulo 5. Análise e Resultados 97 Figura 54 – Exemplo 7 - curva de Lissajous; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor Parametrizada: hipotrocoide   x (θ)       =a R−r y (θ) = (R − r) cos θ + d cos θ +b r       R−r    z (θ) = (R − r) sin θ + d sin θ +c r   t ∈ [0, 8π] (5.16) Fazendo a = 700, R = 630, r = 280, b = 0, c = 600, d = 140 e 0 ≤ t ≥ 8π com velocidade linear constante do efetuador v = 200[mm/s] Capítulo 5. Análise e Resultados 98 Figura 55 – Exemplo 8 - hipotrocoide; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor Parametrizada: hélice cônica Um hélice pode ser parametrizada pelas seguintes equações:    x    = et + f     z = c + dt sin (t) y = a + bt cos (t) t ∈ [0, 8π] (5.17) Fazendo a = 0, b = d = 20, c = 700, e = 20, f = 500 e 0 ≤ t ≥ 8π, o seguinte trajeto é obtido. Capítulo 5. Análise e Resultados 99 Figura 56 – Exemplo 9 - hélice cônica; esquerda: percurso no espaço cartesiano; direita: curvas de posição, velocidade e aceleração das juntas Fonte: Produzido pelo autor 5.4 Dinâmica Revisitando a Equação 3.85: τ = M (q) q ¨ + C (q, q) ˙ q˙ + F (q, q) ˙ + G (q) Assim como no cálculo do Jacobiano, o segundo termo da equação de movimento (matriz C) necessita ser calculada recursivamente de forma a evitar o uso de derivadas parciais. Segundo Fang A. Basu e Wang (1994), é possível calcular este valor recursivamente da seguinte maneira: C (q, q) ˙ q˙ = Cmkj q˙j q˙k Cmkj   Hmkj + Dmkj , j < k = Emkj + Dmkj , j ≥ k (5.18) Capítulo 5. Análise e Resultados 100 onde, Hmkj = zm .J mk . (zj × zk ) Emkj = Dmkj = J mk = n P i=max(m,k,j) n P (zk × cji ).cmi mi zm .zj × (Ji × zk ) i=max(m,k,j) n P [Ji i=max(m,k) (5.19) − mi (dmi .dki )] cmi = zm × dmi Os demais termos da equação são calculados como mostrado na subseção 3.3.3. A Tabela 8, mostra o tensor de inércia para cada elo do manipulador. Os valores foram extraídos do software Solidworks e obtidos a partir de eixos paralelos aos eixos de referência do elo, porém posicionados no centro de gravidade do elo i(SPONG; VIDYASAGAR, 1989) 3 . O vetor rcg,i é o vetor formado partindo do sistema de referência da junta i − 1 até o centro de gravidade do elo i, como mostra a Figura 57. Figura 57 – Vetores do centro de gravidade de cada elo Fonte: Produzido pelo autor 3 consulte a subseção A.1.2.1 para mais detalhes Capítulo 5. Análise e Resultados 101 Tabela 8 – Propriedades dos elos Elo Elo 1 Elo 2 Elo 3 Elo 4 Elo 5 Efetuador Tensor de inércia[kg.m2 ]   0.05762 0.00003 −0.00003 I1,CG =  0.00003 0.02107 0.00721  −0.00003 0.00721 0.05602   0, 02862 0, 00013 0, 01413 I2,CG =  0, 00013 0, 76379 −0, 00001  0, 01413 −0, 00001 0, 75654   0, 01833 0, 00000 0, 03032 I3,CG =  0, 00000 0, 46232 0, 00000  0, 03032 0, 00000 0, 45439   0, 00845 0, 00001 0, 00000 I4,CG =  0, 00001 0, 00776 −0, 00056  0, 00000 −0, 00056 0, 00292   0, 00845 −0, 00001 0, 00000 I5,CG =  −0, 00001 0, 00777 0, 00057  0, 00000 0, 00057 0, 00292   0, 00163 −0, 00001 0, 0000 Ief et,CG =  −0, 00001 0, 00167 0, 00000  0, 00000 0, 00000 0, 00087 Vetor rcg,i [mm] Massa [kg] [0 69,12 139,70] 7,40 [293,86 -0,09 -153,91] 9,17 [217,86 0,11 -91,21] 6,736 [-0,04 -1,05 -203,37] 2,64 [-0,02 -9,02 -118,3] 2,64 [-0,13 0,1 -84,7] 0,94 Fonte – Produzido pelo autor. 5.4.1 Curvas de torque dos exemplos As figuras a seguir mostram as curvas de torque dos exemplos mostrados na subseção 5.3.1. Percebe-se que, para trajetórias com velocidade moderada e sem a aplicação de forças externas no manipulador, o torque da junta 1 e 2 não ultrapassa o valor de 180N m, valor este menor que o torque máximo da junta de 627, 2N m (Tabela 5), o mesmo ocorre para todas as outras juntas. Percebe-se também que quando a trajetória apresenta descontinuidades, a curva de torque também vai apresentar descontinuidades, por esta razão é desejável sempre o uso de trajetórias suaves por interpoladas polinomial. Capítulo 5. Análise e Resultados 102 Figura 58 – Curvas de torque da trajetória 1 - coordenado Fonte: Produzido pelo autor Figura 59 – Curvas de torque da trajetória 2 - descoordenado Fonte: Produzido pelo autor Capítulo 5. Análise e Resultados 103 Figura 60 – Curvas de torque da trajetória 3 - sequencial Fonte: Produzido pelo autor Figura 61 – Curvas de torque da trajetória 4 - linear Fonte: Produzido pelo autor Capítulo 5. Análise e Resultados 104 Figura 62 – Curvas de torque da trajetória 5 - círculo Fonte: Produzido pelo autor Figura 63 – Curvas de torque da trajetória 6 - elipse Fonte: Produzido pelo autor Capítulo 5. Análise e Resultados 105 Figura 64 – Curvas de torque da trajetória 7 - curva de Lissajous Fonte: Produzido pelo autor Figura 65 – Curvas de torque da trajetória 8 - hipotrocoide Fonte: Produzido pelo autor Capítulo 5. Análise e Resultados 106 Figura 66 – Curvas de torque da trajetória 9 - hélice Fonte: Produzido pelo autor 5.4.2 Exemplo de simulação dinâmica A seguir é apresentado um exemplo de dinâmica direta na qual o manipulador foi posicionado na posição horizontal (θ1 , ..., θ6 = 0[◦ ]) e em seguida foi desligado o torque dos motores(τ1 , ..., τ6 = 0[N m]). Esta simulação é feita a partir das equações de movimento(Equação 3.85) contudo, ao invés de se calcular o torque, é calculado as acelerações das juntas através de métodos numéricos de resolução de equações diferenciais como o Rungee-Kutta e posteriormente é aplicado integrais numéricas para se calcular a velocidade e posição das juntas. A simulação foi adaptada do código apresentado por Corke (2011) e usa a função ode45 do MATLAB para resolução das equações diferenciais. A Figura 67 mostra a posição do manipulador em diferentes instantes da simulação, as curvas de posição, velocidade e aceleração das juntas são mostradas na Figura 68. Capítulo 5. Análise e Resultados 107 Figura 67 – Posição do manipulador em diferentes momentos da simulação Fonte: Produzido pelo autor Capítulo 5. Análise e Resultados 108 Figura 68 – Resultados da simulação dinâmica Fonte: Produzido pelo autor É possível perceber claramente que o robô está entrando em colapso devido à gravidade, mas também nota-se que a velocidade rotacional dos elos está exercendo um torque centrípeto e de Coriolis na junta da base o que faz esta rotacionar, o movimento se assemelha ao de um pêndulo duplo. 109 Capítulo 6 Considerações Finais Neste trabalho foi proposto um projeto de manipulador modular do tipo articulado com seis graus de liberdade. Além disso, foi desenvolvido um software em MATLAB para controle do manipulador onde é possível simular o comportamento cinemático e dinâmico não apenas do manipulador proposto mas também de qualquer manipulador articulado, não importando o número de juntas. Este trabalho foi divido principalmente em duas fases: a primeira consistiu do detalhamento do modelamento do manipulador que foi elaborado no software Solidworks e usou-se de parâmetros pre-estabelecidos para seu desenvolvimento, como carga máxima útil, por exemplo. O projeto foi validade através de uma análise de elementos finitos feita no ANSYS. A segunda etapa consistiu de uma análise cinemática e dinâmica do manipulador proposto. Para desenvolvimento desta análise procurou-se estabelecer métodos para a solução de cinco dos seis problemas encontrados em controle de manipuladores: cinemática inversa, cinemática direta, velocidade e aceleração cinemática, equações de movimento e controle de posição do robô. O sexto problema, controle da força, não foi considerado neste trabalho. Para resolução da cinemática direta, foi utilizado as mastrizes de transformação homogêneas, empregadas em conjunto com os parâmetros de Denavit-Hartenberg, onde foi possível determinar a posição do efetuador conhecendo-se os valores das variáveis das juntas (ângulos). Para resolução da cinemática inversa foi utilizado dois métodos principais: o método dos mínimos quadrados amortecido que se utiliza da matriz Jacobiana para aproximar o efetuador da meta através de iterações das variáveis das juntas. Este método se mostrou eficaz para posicionar o manipulador em uma posição desejada, mesmo quando o manipulador está próximo de uma posição singular ou quando a coordenada está próxima do limite máximo alcançável. Contudo este método não permite atribuir uma orientação para o manipulador, para isto foi utilizado o método de Newton-Raphson para restringir a orientação do punho através de três ângulos ao redor dos eixos de referência da base. Este método também se mostrou razoavelmente estável e preciso nas aplicações realizadas. As equações de movimento do manipulador foram obtidas por meio do método Capítulo 6. Considerações Finais 110 de Euler-Lagrange na qual é possível determinar as curvas de torque necessárias para que o manipulador efetue uma determinada trajetória no espaço. Também foi possível determinar a dinâmica direta do manipulador na qual o usuário determina uma curva de torque, e através de integrais numéricas, obtém-se as curvas de posição, velocidade e aceleração das juntas para cada instante t. Por fim, foi implementado o sistema de controle de posição do robô, na qual usou-se de dois métodos clássicos: torque computado e controle antecipatório, na qual consistem em antecipar os valores de torque calculados e tratá-los como distúrbios para aumentar a eficiência e velocidade de convergência para os valores desejados, possibilitando um rastreio mais fiel da trajetória. Todos os métodos acima foram implementados em um software de controle baseado em interface gráfica desenvolvido em MATLAB, que possibilita ao usuário definir e analisar de maneira ágil diferentes trajetórias para o robô. Neste programa é possível simular escolher entre sete diferentes tipos de trajetórias, além de possibilitar a criação e armazenamento de um conjunto de comandos que podem ser reproduzidos automaticamente, o que torna esta ferramenta mais ágil de ser utilizada do que programas baseado em linhas de comando. 6.1 Sugestões para trabalhos futuros Alguns tópicos que estão intimamente relacionados ao título do trabalho mas são de complexidade muito elevadas foram omitidos do escopo deste trabalho, alguns deles são: sistemas de controle mais avançados que utilizam visão para determinação de trajetória; sistemas de controle do manipulador que usa sistemas de equações não lineares multivariável; controle de torque; e desenvolvimento eletrônico do controlador. A seguir é proposto outras sugestões para trabalhos futuros. 6.1.1 Implementar um sistema de controle de força no software Foi implementado apenas o sistema de controle de posição e velocidade, para atividades em que o robô não interage com o ambiente (como soldagem) para atividades em que o robô carrega carga ou monta uma peça em um conjunto é necessário um sistema de controle de força. 6.1.2 Reprogramação do software em outra linguagem de programação O Matlab é um software que proporciona uma linguagem de programação de alto nível. Por este motivo é relativamente simples e rápido implementar código nele, sendo este o motivo principal de se ter escolhido esta linguagem de programação para a execução do software de controle. Contudo, seria ideal não ter um software intermediário para um controle de Capítulo 6. Considerações Finais 111 robôs, sendo então indicado reprogramar o software de controle com a interface gráfica em uma outra linguagem de programação de forma que ele possa ser utilizado em outras plataformas e computadores que não possuem o Matlab instalado. JAVA, PYTHON, C++ e C# são linguagens de código aberto indicados para este fim. 6.1.3 Implementação de mecanismo de segurança A maioria dos robôs industriais atualmente não são colaborativos, ou seja, deve-se instalar uma cabine de proteção ao redor do robô caso o usuário for trabalhar próximo dele. Para transformar o manipulador em colaborativo, deve-se adicionar em cada junta um mecanismo de proteção que consiga detectar quando um certo torque é alcançado e parar o movimento do manipulador, protegendo, assim, o operador caso o manipulador colida com ele. Figura 69 – Estrutura e princípio de funcionamento do SJM III Fonte: Laboratory (2010) 112 Referências ABEL, E. Programming and Trajectory Planning – Lecture Notes. Dundee, Scotland: University of Dundee, 2015. ARISTIDOU, A.; LASENBY, J. Inverse Kinematics: a review of existing techniques and introduction of a new fast iterative solver. [S.l.]: University of Cambridge, Technical Report, 2009. ASADA, H. H. Introduction to Robotics, Lecture Notes. Massachusetts: MIT, 2008. BAILLIEUL, J. Kinematic programming alternatives for redundant manipulators. In Proceedings of the IEEE International Conference on Robotics and Automation, v. 2, p. 722–728, 1985. BALESTRINO, G. D. M. A.; SCIAVICCO, L. Robust control of robotic manipulators. In Proceedings of the 9th IFAC World Congress, v. 5, n. 9, p. 2435–2440, 1984. BERENSON, S. S. D.; KUFFNER, J. Task space regions: A framework for poseconstrained manipulation planning. The International Journal of Robotics Research, v. 30, p. 1435–1460, 2012. BUSS, S. R. Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods. San Diego, California: University of California, 2009. CHOSET, H. M. Principles of Robot Motion Theory, Algorithms, and Implementation. Massachusetts: MIT press, 2005. CORKE, P. I. Robotics, Vision & Control: Fundamental Algorithms in MATLAB. [S.l.]: Springer Berlin Heidelberg, 2011. (Springer Tracts in Advanced Robotics). CORP., K. LBR iiwa 7R800. 2016. Collaborative Kuka LBR iiwa 7R800. Disponível em: . Acesso em: 18 mar 2016. CRAIG, J. J. Introduction to Robotics Mechanics and Control. 2nd. ed. Boston, MA, USA: Addison Wesley Longman Publishing Co., Inc., 1989. DUYSINX, P.; GERADIN, M. An Introduction to Robotics: Mechanical Aspects. [S.l.]: University of Liège, 2004. FANG A. BASU, X. D. F. Y. J.; WANG, Z. Z. An efficient recursive approach for computer generation of manipulator dynamic model. Mathl. Comput. Modelling, v. 20, n. 9, p. 89–96, 1994. FëDOR, M. Application of inverse kinematics for skeleton manipulation in real-time. Proceedings of the 19th spring confer- ence on Computer graphics, ACM Press, n. 03, p. 203–212, 2003. Referências 113 FLETCHER, R. Practical methods of optimization. 2nd. ed. New York, NY, USA: Wiley-Interscience, 1987. FU, K.; GONZÁLEZ, R.; LEE, C. Robotics: control, sensing, vision, and intelligence. [S.l.]: McGraw-Hill, 1987. (McGraw-Hill series in CAD/CAM robotics and computer vision). GANZ, M. Introduction to Lagrangian and Hamiltonian Mechanics. [S.l.]: DIKU, 2008. GOLDENBERG, B. B. A. A.; FENTON, R. G. A complete generalized solution to the inverse kinematics of robots. IEEE Journal of Robotics and Automation, RA-1, n. 1, p. 14–20, 1985. GROOVER, e. a. M. P. Robótica, Tecnologia e Programação. São Paulo, SP: McGraw-Hill, 1988. HEATH, M. Scientific computing: an introductory survey. [S.l.]: McGraw-Hill, 2002. (McGraw-Hill Higher Education). HIBBELER, R. Mecânica: dinâmica. [S.l.]: LTC, 1999. JAZAR, R. N. Theory of Applied Robotics: Kinematics, Dynamics, and Control. Victoria, Australia: Springer Publishing Company, Incorporated, 2007. JOUBERT, N. Numerical Methods for Inverse Kinematics. [S.l.]: University of Berkeley, 2008. KAPLAN, W.; TSU, F. Cálculo avançado. [S.l.]: Edgard Bücher ltda., 1976. KOIVO, A. J. Fundamentals for Control of Robotics Manipulators. New York, NY, USA: John Wiley and Sons, 1989. LABORATORY, K. U. I. R. Safety Mechanisms. 2010. Safe Joint Mechanism. Disponível em: . Acesso em: 21 de dezembro de 2015. LEWIS, F. J.; DAWSON, D. M.; ABDALLAH, C. T. Robot Manipulator Control: Theory and Practice. New York, NY, USA: Marcel Dekker Inc., 2004. LILLY, K. Efficient Dynamic Simulation of Robotic Mechanisms. [S.l.]: Springer US, 2012. (The Springer International Series in Engineering and Computer Science). MARGHITU, D. B. Mechanisms and Robots Analysis with MATLAB. London: Springer, 2009. MARION, J. B.; THORNTON, S. T. Robot Manipulators: Mathematics, Programming, and Control. Massachusetts: MIT press, 1981. MARION, J. B.; THORNTON, S. T. Classical Dynamics of Particles and Systems. New York, NY, USA: Fort Worth: Saunders College Pub, 1995. MURRAY, R. et al. A Mathematical Introduction to Robotic Manipulation. [S.l.]: Taylor & Francis, 1994. Referências 114 NAKAMURA, Y.; HANAFUSA, H. Inverse kinematic solutions with singularity robustness for robot manipulator control. Trans. ASME, Journal of Dynamic Systems, Measurement, and Control, v. 3, n. 108, p. 163–171, 1986. NILSSON, R. Inverse Kinematics. [S.l.], 2009. PIRES, J. N. Industrial robots programming: Building applications for the factories of the future. New York, NY, USA: Springer, 2007. PLW. Robotic Systems. [S.l.]: Project Lead the way, 2013. RANKY, P. G.; HO, C. Y. Robot Modelling: Control and Applications with Software. Bedford, England: IFS, 1985. RATLIFF MATT ZUCKER, J. A. B. N.; SRINIVASA, S. Gradient optimization techniques for efficient motion planning. The Robotics Institute Carnegie Mellon University. ROBOTNIK. Modular manipulator Robotnik. 2016. Robotnik. Disponível em: . Acesso em: 18 mar 2016. ROBOTS, U. UR10 robot, a collaborative industrial robot. 2016. UR10. Disponível em: . Acesso em: 18 mar 2016. SANDIN, P. E. , Paul E. Robot Mechanisms and Mechanical Devices Illustrated. New York: McGraw-Hill, 2003. SANDLER, B. Z. Robotics: Designing the Mechanisms for Automated Machinery. San Diego: San Diego: Academic, 1999. SCHULER, S. et al. Design and development of a joint for the dextrous robot arm. ESA workshop on Advanced Space Technologies for Robotics and Automation, v. 1, n. 1, p. 1–8, 2006. SCLATER, N.; CHIRONIS, N. P. Mechanisms and Mechanical Devices Sourcebook. New York: McGraw-Hill, 1991. SHAHINPOOR, M. A robot engineering textbook. [S.l.]: Harper & Row, 1987. SICILIANO, B.; KHATIB, O. Springer Handbook of Robotics. Secaucus, NJ, USA: Springer-Verlag New York, Inc., 2007. SPONG, M. W.; VIDYASAGAR, M. Robot Dynamics and Control. [S.l.]: John Wiley & Sons, 1989. VANDIVER, J. K. An Introduction to Lagrange Equations. Massachusetts: Massachusetts Instute of Technology, 2011. VEPA, R. Biomimetic Robotics: Mechanisms and Control. Cambridge: Cambridge UP, 2009. VINOGRADOV, O. Fundamentals of Kinematics and Dynamics of Machines and Mechanisms. [S.l.]: Boca Raton: CRC, 2000. Referências 115 WANG, L.-C. T.; CHEN, C. C. A combined optimization method for solving the inverse kinematics problems of mechanical manipulators. IEEE Transactions on Robotics and Automation, v. 4, n. 7, p. 489–499, 1991. WILLIAMS, J. H. Fundamentals of Applied Dynamics. New York: J. Wiley, 1996. WOLOVICH, W. A.; ELLIOTT, H. A computational technique for inverse kinematics. The 23rd IEEE Conference on Decision and Control, n. 23, p. 1359–1363, 1984. W.WAMPLER, C. Manipulator inverse kinematic solutions based on vector formulations and damped least-squares methods. Proceeding of the IEEE Transactions on Systems, Man and Cybernetics, v. 16, p. 93–101, 1986. YI FINGER SUSAN, B. S. Z. Introduction to Mechanisms. [S.l.]: Carnegie Mellon University, 2010. ZHAO, J.; BADLER, N. I. Inverse kinematics positioning using nonlinear programming for highly articulated figures. ACM Transactions on Graphics, p. 313–336, 1994. Apêndices 117 APÊNDICE A Software de Controle Uma GUI (do inglês: Graphical User Interface), é um tipo de interface que permite o usuário interagir com um aparelho eletrônico através de ícones gráficos e indicações visuais. GUI foram introduzidas em reação à íngreme curva de aprendizado de interfaces de linhade-comando que requerem que comandos sejam digitados no teclado, como por exemplo usinagem em um torno CNC. GUIs fornecem controles de apontar-e-clicar, eliminando a necessidade do usuário de aprender uma linguagem de programação ou de digitar comandos para executar uma aplicação. Este capítulo tem como objetivo apresentar o software desenvolvido. A interface gráfica foi desenvolvida utilizando o software Matlab que possui uma caixa e ferramentas chamada GUIDE (Graphical User Interface Development Envirolment) que proporciona ferramentas de criação de interface gráfica para aplicações customizadas. O programa foi primeiramente desenvolvido na Universidade de Dundee, para controle dos robôs recém adquiridos AX-12A Smart Robotic Arm produzidos pela empresa CrustCrawler Robotics, por este motivo, foi dado o nome de SMART-GUI ao programa. Várias modificações foram feitas no programa com o intuito de ser possível controlar e simular movimentos não só do manipulador proposto, mas também, qualquer manipulador do tipo articulado, não importando a quantidade de graus de liberdade do mesmo 1 . O programa foi baseado nos conceitos e equações apresentados nos capítulos anteriores. É mostrado, neste capítulo, os algoritmos implementados no programa, podendo ser utilizado como referência para trabalhos futuros ou para aperfeiçoamento do programa. No capítulo seguinte será mostrado uma breve demonstração de como utilizar o programa para o controle do manipulador modular. A.1 Leiaute e funcionalidades do programa A interface gráfica foi projetada de forma a ser possível atribuir tarefas ao robô (quando este estiver conectado ao computador), e ao mesmo tempo, simular seus movimentos na área gráfica do programa. Para uma melhor disposição do layout, dividiu-se o programa em duas janelas, 1 Para um número de GDL maior que 10, o robô se torna super redundante e as equações da dinâmica ficam extremamente complexas, podendo tornar a simulação bastante lenta APÊNDICE A. Software de Controle 118 SMART-GUI e Simulation Windows, sendo a primeira dividida em três abas: Settings, Commands e Programs. Cada aba foi dividida em dois grupos de objetos: comandos e objeto handle. Um comando é composto por um conjunto de componentes que executam uma ação específica para o robô, como no exemplo mostrado na figura abaixo, o comando 1 tem a função de mover o efetuador para uma coordenada específica. Um objeto handle é um componente individual do comando e é armazenado na estrutura de handles do Matlab onde cada callback pode acessar suas informações. Cada comando é demarcado por um painel (contorno cinza), onde todos os objetos handles são inseridos. Abaixo é apresentada cada aba do software. A.1.1 Caixa de Mensagens Este componente é reservado para mostrar todas as mensagens relacionadas às ações e/ou configurações feitas no software. As mensagens tem o propósito de alertar o usuário sobre alguma mudança ou requisitar alguma ação dele. A figura abaixo mostra a mensagem exibida quando o usuário pressiona o botão ORIGEM. Figura 70 – Mensagem exibida ao iniciar o programa Fonte: Produzido pelo autor A.1.2 Aba Congurações Na aba Configurações todos os parâmetros do programa são mostrados em uma tabela. Ao total são 27 parâmetros diferentes que o usuário pode configurar no programa, abaixo é dado uma breve explicação sobre cada um deles. Os botões, do lado direito, permitem ao usuário salvar as configurações e abrir a tabela de parâmetros específicos do robô. Graus de liberdade do robô A primeira ação que o usuário deve tomar quando estiver utilizando o software é de configurar a quantidade de graus de liberdade que o robô terá. Esta ação é necessária tanto para quando o manipulador estiver conectado ao computador quanto para quando o APÊNDICE A. Software de Controle 119 Figura 71 – Aba Configurações Fonte: Produzido pelo autor usuário quiser apenas simular o movimento do manipulador no programa. Este campo aceita apenas números inteiros, um erro será exibido caso o usuário entre outro tipo de dado Posição inicial O botão ORIGEM vai mandar o robô para uma posição de segurança. Esta posição é estabelecida de forma que o robô possa repousar sem forçar os atuadores ou uma posição arbitrária escolhida pelo usuário, preferencialmente distante o suficiente da superfície onde a base está fixada, para não haver chances de colisão. O usuário, à seu critério, pode APÊNDICE A. Software de Controle 120 definir a coordenada Origem para a ponta do efetuador na linha Home Position, para isso é preciso entrar com o dimensão em mm para x, y, z dando um espaço simples para separar os números. A orientação do punho também pode ser inserido nesse campo em graus à partir dos eixos X, Y, Z, respectivamente portanto, este campo deve possuir um vetor de 6 valores [x, y, z, θx , θy , θz ]. Posição da junta na posição inicial Caso o usuário não queira especificar uma posição de origem cartesiana para o efetuador, é possível inserir os valores das juntas, o programa vai então calcular a posição e orientação através de cinemática direta. Distância máxima alcançável Neste campo o usuário insere a distância máxima que o manipulador pode alcançar quando este estiver estendido. Caso o usuário deixe este campo em branco o programa vai tentar calcular este valor usando os parâmetros de Denavit-Hartenberg e cinemática direta, porém estes valores podem não ser precisos em alguns casos. Modo de simulação apenas Modo de Simulação, que vai informar a GUI se o movimento é apenas para ser simulado na janela do programa ou, caso esta opção esteja desmarcada, o programa irá enviar a informação de posição para cada atuador, movimentando o robô. Habilitar simulação dinâmica Este é um valor booleano, quando ativado (verdadeiro) o software efetua os cálculos de dinâmica inversa para cada comando que o usuário der. Dependendo do número de graus de liberdade do manipulador, esta opção pode deixar a simulação do comando um pouco lenta, quando ativado. Caso desativado (falso) o usuário não poderá plotar as curvas de torque do manipulador e também não será possível utilizar dos comandos da quarta aba: Simulação Dinâmica e nem será possível ativar a opção Sistema de controle, uma vez que esta necessita dos valores de torque calculados. Habilitar sistema de controle Esta opção habilita o sistema de controle nos comandos. Quando habilitado, o software recalcula a trajetória baseado nas curvas de torque, a trajetória final é similar ao que um robô conectado no programa irá apresentar. Caso desativada esta opção, a trajetória mostrada na janela de animação será a ideal com base nos parâmetros passados pelo usuário. APÊNDICE A. Software de Controle 121 Taxa de atualização do sistema de controle Uma vez que o sistema de controle trabalha com funções discretas, a taxa de atualização do sistema de controle corresponde a diferença de tempo entre uma coleta de dados do sensor a outra no sistema de controle. Caso o robô não esteja conectado (modo de simulação apenas) esta taxa é usada para calcular a quantidade de pontos na trajetória, portanto para uma trajetória qualquer que leve t segundos para se concluir, os valores das juntas do manipulador serão calculados a cada T milisegundos, em que T é o valor da taxa de atualização determinado pelo usuário, resultando portanto, em T ∗ t passos. Habilitar efetuador O usuário deve marcar esta opção caso o manipulador a ser simulado ou controlado pelo programa possuir um efetuador. No caso do efetuador ser uma garra, o usuário poderá controlar o ângulo de abertura da garra na aba comandos e programas. Caso habilitado, o usuário deverá inserir os ângulos limites de trabalho na tabela de parâmetros do robô. Número do robô Para alguns tipos de controladores de robôs, é exigido um número de identificação único para o robô, este número pode ser inserido no software pelo campo Número do robô. Este campo aceita tanto número quanto caracteres de texto. Número da porta de comunicação Caso o robô esteja conectado, será necessário selecionar o número de porta para estabelecer a conexão. Este número pode ser obtido no Device Manager em computadores que o sistema operacional é o Microsoft Windows. Número Baud padrão Dependendo do controlador eletrônico utilizado pelo robô, a comunicação entre o software e robô é feita pela porta USB, e portanto, necessita do baud number, que consiste de uma taxa de modulação por segundo. Se o controlador for um Arduino, o baud padrão geralmente é 9600. Caso o usuário utilize o software apenas em modo de simulação, este parâmetro é irrelevante e pode ser deixado em branco. Mostrar rastro A opção mostrar rastro, quando habilitada exibe um rastro vermelho contínuo na ponta do efetuador quando este se movimenta. Esta opção não influencia na operação e resultado dos comandos, é usado apenas para melhor compreensão e visualização do movimento. APÊNDICE A. Software de Controle 122 Não é possível alterar a junta de origem do efetuador, o rastro será sempre mostrado na ponta da última junta, ou efetuador (quando houver). Limpar rastro a cada comando Esta opção quando ativada, limpa o rastro a cada movimento que o manipulador fizer. Isto é aconselhável quando o usuário estiver utilizando a aba programas, em que o manipulador vai executar uma série de comandos o que torna o área gráfica bastante poluída caso estiver desativado. Quadros por segundo Esta opção é equivalente ao número de quadros de um vídeo. Para um determinado movimento do manipulador, o número de pontos irá atualizar a representação gráfica n vezes. Este número depende da complexidade do manipulador carregado no software, se o manipulador tiver muitos detalhes, o tempo para atualizar cada quadro na janela de animação é longo, portanto o número de quadros por segundo deve ser reduzido. O usuário deve primeiro testar uma trajetória na aba comandos inserindo um tempo para a trajetória, se ao executar o comando a animação levar mais tempo que o tempo imposto, o número de quadros por segundo deve ser reduzido utilizando-se de uma regra de três simples. Este número não está associado à taxa de atualização do sistema de controle e nem à opção de mostrar rastro, ele é usado apenas para a animação do movimento. Salvar vídeo do comando Quando habilitado o software irá salvar um vídeo em formato .avi do comando que estiver sendo animado. A orientação da câmera e luz será igual ao mostrado durante a animação. O arquivo é salvo na pasta ativa (pasta que contém o software) no momento da animação. Mostrar eixos Mostra pequenos segmentos dos eixos x, y, z na ponta do efetuador. Este segmentos são exibidos tanto quando o manipulador está em movimento quanto em repouso. Esta opção funciona em associação com a ferramenta matriz de transformação (aba comandos). Os eixos serão exibidos na junta denotada pelo número escolhido na matriz, por exemplo, se a matriz de transformação escolhida for a T_01, os eixos irão ser mostrados na junta 1. Esta opção também pode ser ativada no menu de contexto: Tools>Display reference frames Erro permitido para a cinemática inversa Uma vez que o software utiliza um método iterativo para calcular a cinemática inversa, é necessário inserir um desvio máximo permitido para os valores calculados. O erro considerado é a diferença cartesiana entre a posição efetuador e a meta desejada. É APÊNDICE A. Software de Controle 123 aconselhável deixar este valor próximo de 0,01mm, para precisões maiores o software pode ficar mais lento para calcular a cinemática inversa e a taxa de atualização do sistema de controle pode não ser atendida. Número máximo de iterações na cinemática inversa Este se refere ao número máximo de iterações que o algoritmo de cinemática inversa vai ter para fazer com que o efetuador alcance a meta desejada. Caso este número seja alcançado, o algoritmo sai do loop, e retorna o valor das juntas calculados até este ponto. Valores muito baixos para este campo, fará com que o algoritmo saia da loop muito rápido sem alcançar uma boa aproximação da meta, enquanto que valores muito elevados fará com que se leve muito tempo para calcular os valores das juntas. Este número varia de acordo com o algoritmo utilizado e as dimensões do robô. Para o algoritmo de cinemática inversa implementado no software é aconselhável deixar este número próximo de 100. Vetor de gravidade Nesta opção o usuário pode escolher em que eixo a gravidade irá agir. Esta opção interfere no resultado da dinâmica inversa, uma vez que este valor é utilizado para calcular a parcela de torque relacionado à gravidade. Ao utilizar os parâmetros de Denavit-Hartenberg no manipulador, o eixo z da base geralmente fica na vertical, por isso é aconselhável deixar este vetor da seguinte forma: [0, 0, 9.08665], o valor deve estar em m.s−2 . Pausa entre atualizações no servo motor No caso de robôs que utilizam servo motores, os controladores necessitam de uma pausa entre o envio de informação do software para o controlador. Esta pause tem a finalidade de garantir que todos os pacotes de informação foram entregues antes de começar a enviar novos pacotes. Geralmente este valor é pequeno, por volta de 0.001s. Este parâmetro não interfere na simulação e pode ser deixada em branco caso o robô não esteja conectado. Passo de incremento cartesiano: grosseiro Na aba comandos, o comando Mover Efetuador utiliza do valor deste parâmetro para incrementar a posição do efetuador na direção do eixo clicado. O valor de incremento grosseiro é usado para aproximar o efetuador de uma posição desejada, quando este estiver longe da meta. Passo de incremento cartesiano: no Também é utilizado no mesmo comando: Mover Efetuador por incremento. O incremento fino é geralmente para posicionar o efetuador com maior precisão na meta desejada. APÊNDICE A. Software de Controle 124 Incremento linear para trajetória em linha reta O software utiliza o algoritmo de Taylor para criar trajetórias cartesiana lineares. O algoritmo consiste em inserir pontos de quebra na linha formada pela posição atual do efetuador e a meta desejada. Este parâmetro consiste na distância linear entre um ponto de quebra e outro (em mm). Conjunto dos ângulos O software utiliza os ângulos relativos das juntas para realizar os cálculos de cinemática direta, inversa e dinâmica. Porém, é possível modificar a forma como o programa mostra esses ângulos na aba comandos. Esta opção tem propósito didático apenas e serve para que o usuário possa rapidamente descobrir a posição das juntas em diferentes conjuntos, como global, relativo e motor. No conjunto global, os ângulos das juntas são medidos a partir dos planos de referência da base, já no conjunto relativo, os ângulos são medidos partindo da posição atual do elo que antecede a junta; e por fim no conjunto dos ângulos do motor, o valor do ângulo é medido a partir da posição zero do motor (para alguns tipos de robôs que utilizam servo motores). A figura abaixo mostra os diferentes conjuntos de ângulos. Figura 72 – Conjuntos de ângulos - (a)conjunto da junta, (b)conjunto global, (c)conjunto do motor Fonte: Produzido pelo autor Número do comando O número de comando consiste em uma identificação (valor inteiro) na estrutura do histórico na qual os resultados dos cálculos (posição das juntas, velocidade, aceleração e torque) são armazenados. É aconselhável deixar este parâmetro como 1, e a cada comando dado ao software este valor é automaticamente incrementado. Ao clicar no comando de APÊNDICE A. Software de Controle 125 plotar os gráficos, o programa checa o número de comando atual e acessa os valores do histórico salvos nesse endereço. Desta forma o usuário pode plotar os resultados de qualquer comando dado. Ao limpar o histórico o número do comando é automaticamente resetado para 1. Estado da interrupção de comando Este parâmetro é modificado automaticamente pelo software. Se um comando estiver em execução e o usuário clicar no botão Parar na aba comandos, este valor é modificado para verdadeiro, e as funções que estiverem em execução são interrompidas. Estado do comando Este comando também é modificado automaticamente pelo programa. Ao executar um comando este valor é mudado para falso, e ao terminar a execução do comando o parâmetro é mudado para verdadeiro. Este parâmetro informa algumas funções se o comando está em execução ou se já foi completado. Botão: Salvar congurações Salva as configurações atuais exibidas na tabela. A tabela é salva em um arquivo com extensão .mat (SETTINGS.mat). É aconselhável fazer uma cópia de segurança deste arquivo. Sempre que o software é aberto, a última versão da tabela configurações é carregado. Botão: Carregar congurações O usuário pode salvar uma cópia das configurações do programa para um determinado robô, e sempre que for ser utilizar esse robô carregar as configurações específicas para ele. Isso impede que o usuário tenha que modificar cada parâmetro das configurações toda vez que trocar de robô. Botão: Resetar congurações O usuário pode resetar a tabela de configurações para a versão ’de fábrica’. Nem todos os parêmtros padrões (de fábrica) funciona com todos os tipos de robôs. O usuário deve se ater a parâmetros mais fundamentais como número de graus de liberdade do robô e posição origem do manipulador, para não haver movimentos inesperados, como colisões. O usuário não estará livre de modificar a tabela de parâmetros do robô, pois não existem parâmetros de fábrica para este. APÊNDICE A. Software de Controle 126 Botão: Parâmetros do Robô Abre um formulário com a tabela de parâmetros do robô. Ao terminar de editar os parâmetros o usuário deve clicar no botão SALVAR. Toda vez que o usuário modificar os parâmetros do robô, o software vai atualizar os arquivos de desenho .STL (para a área gráfica - animação). O usuário deve se certificar que os desenhos estão salvos na pasta OTHER_FILES e que estão de acordo com os parâmetros inseridos. Os desenhos (arquivos STL) podem ser gerados em um software CAD, como Solidworks, por exemplo e devem ser salvos com os seguintes nomes: base.STL, link1.STL, link2.STL, etc. (número de arquivos de elos devem corresponder ao número de graus de liberdade - 1) e caso a opção ’Habilitar Efetuador’ tenha sido marcada (verdadeiro) na tabela de configurações, deve haver também um arquivo chamado end_effector.STL. Figura 73 – Tabela de parâmetros do robô Fonte: Produzido pelo autor A.1.2.1 Obtendo a matriz de inércia do elo Os valores Ixx , Ixy , Ixz , Iyy , Iyz , Izz podem ser obtidos através de cálculos ou de um software CAD. No Solidworks, é apresentado 3 opções de momento de Inércia, como mostrado na Figura 74. APÊNDICE A. Software de Controle 127 Figura 74 – Propriedades de massa do desenho Fonte: Produzido pelo autor O primeiro consiste nos momentos de inércia principais, ao redor dos eixos principais de inércia. O segundo é o momento de inércia obtido no centro de massa (CG) e alinhado com os eixos de coordenada do sistema, e por fim tem os momentos de inércia obtidos usando os eixos de coordenada do sistema. Os momentos de inércia utilizados no software é o segundo mostrado na figura. De acordo com Spong e Vidyasagar (1989), o equacionamento dinâmico é feito considerando a matriz de inércia ao redor de eixos paralelos aos eixos de referência do elo i e posicionados no centro de massa do elo. Portanto antes de obter os valores, o elo precisa ser alinhado no software CAD com os eixos de referência do elo, caso ainda não esteja, como mostrado na Figura 75. Na esquerda é mostrado a peça no Solidworks e os eixos do software, na direita é mostrado os eixos de referência de cada elo. Figura 75 – Alinhamento da modelo no software Fonte: Produzido pelo autor APÊNDICE A. Software de Controle 128 Os valores inseridos na tabela devem estar em kg.m2 . Os sinais dos produtos de inércia Ixy , Ixz , Iyz devem ser iguais aos apresentados pelo software CAD. Os sinais negativos são inseridos automaticamente que a matriz fique igual ao apresentado na Equação 3.59. Botão: Limpar histórico Este botão limpa o arquivo HISTORY.mat. Quando clicado, abre-se uma janela alertando que a opção não pode ser desfeita. Ao limpar o histórico o robô volta para sua posição de origem na janela de Animação. A partir desse ponto não é possível plotar os gráficos de posição, velocidade, aceleração e torque para os comandos feitos até o momento. A.1.3 Aba Comandos A aba Comandos apresenta os grupos de ferramentas que possibilita o usuário passar ações ao manipulador. Nesta aba é possível fazer apenas um único comando por vez. Mover para a coordenada Este comando move a ponta do efetuador (ou última junta, quando o efetuador não estiver presente) para uma posição arbitrária determinada pelo usuário. A forma com que as juntas se movimentarão e o caminho que o efetuador fará no espaço vai depender do tipo de trajetória escolhido, no menu Opções de trajetórias. Na maioria das aplicações industriais escolhe-se a trajetória interpolada que cria uma curva suave entre os posições iniciais e finais das juntas utilizando polinômios de quinto grau. As entradas nos campos X, Y, Z devem ser escalares se a trajetória for interpolada e vetores se as trajetória escolhida for Interpolada com pontos de passagem. O campo tempo, deve possuir o mesmo número de valores que os campos de coordenadas. Se o usuário desejar, por exemplo, mover o efetuador para a posição X = 1000, Y = 450, Z = 200 e precisar que o efetuador passe nas coordenadas X = 650, Y = 750, Z = 500 e X = 800, Y = 540, Z = 349, com velocidades 50mm/s e 30mm/s e tempo t1 = 10, t2 = 12, t3 = 15 respectivamente, os campos deverão ser preenchidos da seguinte maneira: O campo O (orientação) deve ter um vetor linha (1x3) para cada ponto de passagem e final. Se as velocidade dos pontos de passagem não for fornecido, o software vai tentar preencher esses valores aplicando uma heurística. O botão C tem a função apenas de limpar os campos. Mover efetuador Este comando move o efetuador na direção do eixo escolhido pelo usuário. A distância que o efetuador irá se mover depende do incremento escolhido, e dos valores destes passados APÊNDICE A. Software de Controle 129 Figura 76 – Aba Ferramentas Fonte: Produzido pelo autor na aba Configurações. É necessário informar o tempo de duração do movimento ou a velocidade linear do efetuador no espaço cartesiano. O caminho traçado pelo efetuador também depende do tipo de trajetória escolhida. Trajetória Parametrizada Neste comando o usuário descreve o trajeto do efetuador no espaço cartesiano através de funções paramétricas do tempo. O software permite que o usuário escreva expressões para designar uma função, esta expressão é então convertida para expressão simbólica utilizando os comandos do MATLAB, que vai criar um caminho determinado pelas funções. Este caminho é então convertidos em posição das juntas utilizando a cinemática inversa. Para uma trajetória em forma de círculo no plano Y − Z, os campos de trajetória paramétrica devem ser preenchidos da seguinte forma: APÊNDICE A. Software de Controle 130 Figura 77 – Mover efetuador com pontos de passagem Fonte: Produzido pelo autor Figura 78 – Percurso através de funções paramétricas Fonte: Produzido pelo autor O que resultaria em um círculo de raio 500. O tempo do trajeto também deve ser passado ao programa. Trajetória por tabela Neste comando o usuário é requisitado a escolher uma planilha do Microsoft Excel contendo as coordenadas cartesianas por onde deseja-se que o efetuador percorra. O tempo do trajeto é imposto pelo usuário no campo Tempo (em segundos). As coordenadas podem ser geradas por um programa CAD, e devem estar próximas uma das outras, uma vez que não é feito interpolação entre os pontos. APÊNDICE A. Software de Controle 131 Mover Juntas Este comando é similar ao comando Mover para coordenada porém, ao invés de se inserir a meta cartesiana para o efetuador, o usuário vai passar ao programa uma posição meta para a junta. Neste caso também haverá interpolação da curva, se a trajetória escolhida for Interpolada. Como nos outros comandos, o usuário é requisitado a passar um tempo total de trajeto. O software considera que a aceleração e velocidade inicial e final da junta são ambas iguais a zero. Garra Este comando é habilitado apenas se a opção Habilitar efetuador estiver selecionada na aba Configurações. Esta ferramenta é somente útil se o efetuador for uma garra. Com isto é possível mandar comandos de abrir e fechar completamente a garra ou abrir para uma posição específica. Quando a garra é aberta a posição da ponta da garra em relação à base muda, em comparação de quando a garra está fechada. O usuário deve ficar ciente de que o software sempre considera a distância da garra quando esta está fechada para seus cálculos de cinemática direta e inversa. Matrizes de transformação Esta opção exibe ao usuário a posição e orientação de uma junta específica dada pela matriz de transformação. Esta opção não influencia no movimento do robô, ela tem propósito didático apenas. O usuário pode escolher uma das n juntas do manipulador e a matriz de transformação homogênea daquela junta com relação à base se atualiza automaticamente. Botão Origem Este comando faz o efetuador se movimentar para a posição de Origem previamente estabelecido no aba Configurações. Não é necessário inserir um tempo de trajeto para este comando, pois o software vai utilizar a velocidade média das juntas inseridas na tabela de parâmetros do robô para determinar o tempo do percurso. A trajetória para este comando é sempre Interpolada, mesmo que outra opção esteja selecionada no menu. Movimento aleatório Este comando cria um conjunto de metas cartesianas aleatórias e posteriormente chama a função de movimentar o efetuador para coordenadas com pontos de passagem. O tempo e velocidade para cada ponto de passagem é determinado pelo programa. APÊNDICE A. Software de Controle 132 Botão Stop O comando Parar serve como um botão de emergência, quando pressionado ele para o robô, caso este esteja conectado no computador, e interrompe a animação na tela, caso esteja sendo executada. O tempo de parada é de no mínimo 1 segundo após o botão foi pressionado, isso se dá porque o programa não consegue interromper um loop que pode estar ocorrendo (dependendo da ação do robô) nas funções primárias. A.1.4 Aba Programas Na aba Programas é possível criar uma lista de comandos e fazer o manipulador executá-las de maneira automática. Figura 79 – Aba Programas Fonte: Produzido pelo autor APÊNDICE A. Software de Controle 133 Torque Umas das formas de se gerar uma trajetória para o manipulador é conhecida como teach pendent. Nesta forma, o usuário desliga o torque do manipulador de forma que este fica livre para ser movimentado manualmente pelo usuário, este então entra com um comando no computador para salvar a posição das juntas, que é feito a partir da leitura dos sensores. Para que isso seja possível, a junta tem que exercer no elo, apenas a força para sustentar o peso do manipulador, para que o usuário consiga movimentá-lo sem grande esforço. Este valor de torque é conhecido como compensação da gravidade e seu calculo é baseado nas forças nas juntas para manter equilíbrio estático do manipulador. A.1.5 Aba Simulação Dinâmica Figura 80 – Aba Simulação Dinâmica Fonte: Produzido pelo autor APÊNDICE A. Software de Controle 134 Nesta seção é possível inserir curvas paramétricas ou valores constantes de torque para cada junta e simular o comportamento dinâmico. Na parte: posição de início, o usuário pode posicionar o robô em uma configuração desejada para iniciar a simulação. Deve-se inserir um tempo para cada equação paramétrica, a simulação assumirá o maior valor de tempo inserido. A.1.6 Janela de Animação Nesta janela é exibida a animação de cada comando dado ao robô. A janela conta com uma barra de ferramentas nativa do MATLAB em que é possível alterar a orientação do robô na tela, dar zoom ou ainda colher valores dados como coordenadas das juntas, atrav[es do botão data cursor. Figura 81 – Janela de Animação Fonte: Produzido pelo autor Atualizar juntas O botão Atualizar Juntas irá atualizar os valores exibidos no Comando 2: Juntas, quando o robô for conectado e os atuadores (juntas) tiver sido movido manualmente para uma posição qualquer. Quando pressionado, este botão irá executar uma função que irá ler a posição de cada atuador e atualizar os controles do comando 2 e o texto. APÊNDICE A. Software de Controle 135 Limpar trilha A opção Limpar Rastro deleta todos os pontos azuis deixados no gráfico que traçam o caminho percorrido pelo efetuador do manipulador. Caso este botão seja pressionado enquanto o gráfico está animando algum movimento, os pontos deletados serão os pontos gerados até o momento que o botão foi pressionado, os pontos gerados depois disso serão exibidos na tela. Plotar grácos O botão plotar gráficos tem o propósito de gerar as curvas de velocidade e aceleração para um determinado movimento do manipulador. Mudar vista A qualquer momento durante ou depois do movimento do robô, o botão Mudar Vista pode ser pressionado, o que fará com que a vista de exibição do robô seja alterada para uma das vistas pré-definidas. Duas das vistas são isométricas e as outas são projeções ortogonais. A.2 Arquivos do programa A interface gráfica, desenvolvida em Matlab versão R2015a, é composta por diversos arquivos que trabalham interconectados, os principais deles são descritos a seguir. A.2.1 SMART-GUI.m O arquivo SMART_GUI.m reúne todas as ações para cada componente da interface gráfica. Este é o arquivo principal de todo o programa e contém a maior parte das funções do software. Cada vez que o usuário clica em algum botão da interface gráfica, o Matlab vai procurar a respectiva função daquele componente e executar as linhas de código dentro da função. A.2.2 SMART-GUI.g O arquivo SMART_GUI.fig é um arquivo gerado pelo Matlab que contém todas as informações dos componentes da GUI, tais como posição e tamanho de cada caixa de texto, tabelas, menus pop-up tamanho da janela do programa, etc. Antes do arquivo SMART_GUI.m ser lido, o Matlab lê o arquivo SMART_GUI.fig para juntar todos os componentes na tela. APÊNDICE A. Software de Controle 136 A.2.3 Funções As funções que compõem o programa são programadas em arquivos individuais com extensão .m A.3 Estruturas de dados Todas as configurações, parâmetros e valores resultados de cálculos feitos pelas funções primárias e secundárias são armazenados em estruturas de dados, para posteriormente serem acessados e processados por outras funções. Abaixo é dado uma breve apresentação de cada estrutura e como as funções acessam cada uma delas. Ao iniciar o software, as estruturas são carregadas pela função miscelânea MF_Load_Structures, que por sua vez, cria uma cópia das estruturas na área de trabalho base do MATLAB(base workspace). As funções quando necessitarem acessar valores de alguma estrutura, devem copiar a estrutura para a área de trabalho da função (usando a função evalin). Escolheu-se este método por se mostrar ser mais rápido do que carregar o arquivo .mat a cada chamada de função. A.3.1 HISTORY.mat O arquivo HISTORY.mat possui uma estrutura de dados chamada H, na qual todos os valores calculados da cinemática direta, inversa, dinâmica inversa, direta, e sistema de controle. Os arquivos são acessados através do nome da estrutura seguido pelo nome da variável (conhecido como field no MATLAB), usando notação de ponto. Cada comando dado pelo usuário cria uma nova ’camada’ na estrutura, sendo assim, para obter os resultados de um comando específico do histórico o usuário precisa do número de comando. As funções que que necessitarem acessar os valores das juntas de um determinado comando, utiliza a seguinte sintaxe: H(cn).q. A Tabela 9 apresenta as variáveis da estrutura H. Ao clicar no botão Limpar histórico na aba Configurações, todas as variáveis de todos os comandos dados (’camadas’ da estrutura) são resetadas (as variáveis permanecem, porém vazias). A.3.2 MESSAGES.mat O arquivo MESSAGES.mat, possui uma estrutura chamada M. Esta estrutura é responsável por armazenar todas as frases de avisos, instruções e mensagens que são exibidos no campo Messages, localizado na parte inferior do programa. Cada aba do programa é um field, da estrutura M e possui diversas mensagens que foram sendo adicionas durante o desenvolvimento do programa, com o intuito de informar o usuário sobre a utilidade específica de algum botão ou opção. A função que exibe as mensagens é a MF_Update_Message que carrega a estrutura M para sua área de trabalho e escolhe o texto da mensagem necessária APÊNDICE A. Software de Controle 137 Tabela 9 – Variáveis da estrutura Histórico (H) Variável Descrição q posição da variável da junta (matriz sp x n) dq velocidade da variável da junta ddq aceleração da variável da junta p vetor posição do efetuador lv velocidade linear do efetuador la velocidade angular do efetuador tq torque de cada junta time duração total do comando sp número de passos (step points) T matrizes de transformação homogênea de cada junta qc posição da variável da junta, resultado do sistema de controle dqc velocidade da variável da junta, resultado do sistema de controle ddqc aceleração da variável da junta, resultado do sistema de controle tqc torque de cada junta, resultado do sistema de controle Fonte – Produzido pelo autor usando o nome do campo e o número da mensagem. A Tabela 10 descreve cada variável da estrutura. A.3.3 ROBOT-DATA.mat O arquivo ROBOT_DATA.mat, possui as estruturas: D,LD,RP. A estrutura D, possui as equações da dinâmica inversa (torque) e Jacobiano de forma simbólica. Estas equações não são utilizadas nos cálculos, uma vez que o programa funções numéricas para calcular o torque e o Jacobiano (funções primárias PF_Inverse_Dynamics e PF_Jacobian), porém ter as funções de forma simbólica pode ser útil para o usuário para outras aplicações. O usuário pode copiar essas funções sempre que precisar. A estrutura LD, contém as informações sobre o desenho de cada elo do robô. Quando o usuário edita a tabela de parâmetros do robô, o programa converte os arquivos de extensão .STL em coordenadas de vértices, faces, e cores, possibilitando a criação de patches pelo MATLAB que por sua vez são impressos na janela de animação. A estrutura RP armazena os parâmetros do robô, preenchidos pelo usuário na aba configurações. Os campos dessa estrutura são idênticos aos da tabela mostrada na Figura 73. APÊNDICE A. Software de Controle 138 Tabela 10 – Variáveis da estrutura Mensagens (M) Variável Descrição settings mensagens da aba configurações commands mensagens da aba comandos programs mensagens da aba programas notice textos de anúncios sobre alguma ação. Ex: o robô foi movido para a posição de origem warnings textos de avisos (erros) Ex: um erro foi encontrado na linha x do programa animW mensagens sobre a janela de animação robotP mensagens sobre a tabela de parâmetros do robô simulation mensagens sobre a aba simulação dinâmica controlS textos das mensagens sobre o sistema de controle Fonte – Produzido pelo autor A.3.4 SETTINGS.mat O arquivo SETTINGS.mat, possui as estruturas S e AS. A estrutura S consiste de uma tabela do MATLAB, com os mesmos campos descritos na subseção A.1.2. Já a estrutura AS, armazena valores adicionais (additional settings) que não são editáveis pelo usuário, a Tabela 11 descreve cada variável contida em AS. Tabela 11 – Variáveis da estrutura Configurações Adicionais (AS) Variável Descrição traj_opts lista com os textos de todas as trajetórias disponíveis commands lista com os textos de todos os comandos disponíveis program_table nomes das variáveis usadas para formar uma tabela de comandos (na aba programas) robot_param lista com os textos dos parâmetros robô. Usado quando o programa carrega a tabela de parâmetros do robô na tela validation texto de validação para quando o usuário for carregar um arquivo de configurações externo. stop variável de controle para parar o robô cmd_state variável de controle sobre o comando co espeficação da orientação. Usado para informar à função da cinemátia inversa se deve tentar satisfazes a orientação imposta pelo usuário. cn variável de controle do número de comando Fonte – Produzido pelo autor 139 APÊNDICE B R do Software Rotinas em Matlab de Controle APÊNDICE B. Rotinas em Matlab® do Software de Controle 1 FUNÇÕES PRIMÁRIAS %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION PF-A: FORWARD KINEMATICS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will compute the homogeneous transformation % matrices T. The function receives the Denavit-Hartenberg parameters and % outputs a cell array formed by each joint T. The position vector of the % last joint is also outputted. The inputs that are angles must be in % degrees. Refer to section 4 of documentation for details. %========================================================================== function [P_XYZ, T] = T1_n = eye(4); PF_Forward_Kinematics(q, d, a, A) T{n} = zeros(4); %>>>>> FORWARD KINEMATICS BY DENAVITHARTENBERG REPRESENTATION <<<<<< for i=1:n %Theta, d, a, Alpha are lists (1 row T_i = ... matrix) with n inputs [cos(q(i)), -cos(A(i))*sin(q(i)), % Where n is the number of links sin(A(i))*sin(q(i)), a(i)*cos(q(i)); in the arm. sin(q(i)), cos(A(i))*cos(q(i)),%T: Joint Angles (theta) sin(A(i))*cos(q(i)), a(i)*sin(q(i)); %d: Link offset 0, sin(A(i)), %a: Link Length cos(A(i)), d(i); %A: Link Twist (alpha) 0, 0, %n: Robot degrees of freedom 0, 1]; n = length(q); %% Compute T %The homogeneous Transformation (Ai) is represented as the product of four % basic transformation (Rotation, Translation, Translation, Rotation): %Ai = Rz,Thetai*Transz,di*Transx,ai*Rx,alphai % Convert from degrees to radians (Computation in radians is faster) q = deg2rad(q); A = deg2rad(A); %Transformation matrix - T (from origin to respect to i), is obtained %by multiplying all A_i matrices. T1_n = T1_n * T_i; T{i} = T1_n; end %x, y and z are obtained from the last Transformation matrix (T_0_n) %i.e. the transformation matrix from the origin (0) to the end-effector (n) %x, y and z are in the 4th column of the T matrix. P_XYZ = T{n}((1:3),4)'; end % Preallocating matrices that will be used %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION PF-B: INVERSE KINEMATICS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function computes the inverse kinematics of the robotic % arm. The technique used is the Dumped Least Square together with Newton's % method for optimization used to achieve the desired orientation. If the % robot has fewer than 6 dof, then the orientation cannot be satisfied in % all axes. The output is a matrix (qm) of size:(sp x n => step points by % dof). Refer to section 4 of documentation for details. %========================================================================== function qm = % p_o = position and orientation vector PF_Inverse_Kinematics(p_o, q0, s) (x, y, z, rx, ry, rz)(step pts x 6) APÊNDICE B. Rotinas em Matlab® do Software de Controle % rx, ry, rz: rotation angle in degrees around the axis xyz % q0 = initial 'guess' or 'position' for the joints s = [Rx Ry Rz] % orientation specification: [1 1 1] all axes orientation will be % considered. [1 1 0]: only orientation around x and y will be considered, % and so on... %% GET THE NECESSARY VALUES RP = evalin('base', 'RP'); %Load Robot Parameters S = evalin('base', 'S'); % Load Settings (from base workspace) a = RP.a'; d = RP.d'; alpha = RP.alpha'; %D-H parameters n = length(q0); m_error = S.value{'ik_error'} * ones(1,n); %Maximum permitted error vector. max_iter = S.value{'ik_iternum'}; %Maximum number of iterations. %% ======================================= === % Pre-allocating the matrix that will store theta angles for each point qm = zeros(size(p_o, 1), length(q0)); Tmatrix(:,:,n) = zeros(4,4); nci = 0; %not computed itens if ~exist('s', 'var'); s = [0 0 0]; end q = DSL(q, J, lambda, we, n, error); q = Correcting_angles(q); %Update ee position and compute errors [~, T_m] = PF_Forward_Kinematics(q, d, a, alpha); error = error_f(T_m{n}, T_d, s); iter = iter + 1; %Increment of iteration number. if iter > max_iter % disp('REACHED MAXIMUM NUMBER OF ITERATIONS'); nci = [nci; i]; break end end iter = 0; % Method 2: Newton-Raphson used to find the final position and % orientation of the end-effector (uses optimization technique) while any(abs(error) > abs(m_error)) J = PF_Jacobian(T_m); %Computes the Jacobian q = NR(q, J, T_m{n}, T_d); q = Correcting_angles(q); %Update ee position and compute errors for i=1:length(p_o(:,1)); [~, T_m] = PF_Forward_Kinematics(q0, d, a, alpha); T_d = Goal_Transform(p_o(i, 4:end), p_o(i,1:3)); error = error_f(T_m{n}, T_d, s);%initial error (posit. and orientation) q = q0; lambda = 20; iter = 0; counter we = 50; %lambda factor; %iteration % Method 1: Dumped least square, used to approximate the end-effector % position to the neighbourhood of the goal position (position only, % orientation not considered) while any(abs(error(1:3)) > abs(m_error(1:3))) J = PF_Jacobian(T_m); %Computes the Jacobian [~, T_m] = PF_Forward_Kinematics(q, d, a, alpha); error = error_f(T_m{n}, T_d, s); iter = iter + 1; %Increment of iteration number. if iter > max_iter % disp('REACHED MAXIMUM NUMBER OF ITERATIONS'); nci = [nci; i]; break end end Tmatrix(:,:,i) = T_m{n}- T_d; %#delete this (for checking errors only) %Update initial theta angles q0 = q; qm(i,:) = q; end end %% METHODS function q = DSL(q, J, lambda, we, n, error) APÊNDICE B. Rotinas em Matlab® do Software de Controle dq = (J' * inv(J * J' + lambda ^ 2 eye(n))) * we * error'; q = q + dq'; %Update theta angle end * function q = NR(q, J, Ta, Td) r = residual(Ta, Td); if size(J) == size(J') Jinv = inv(J); else Jinv = pinv(J); end dq = Jinv * r'; q = q + dq'; %Update theta angle end %% Functions function goal_T = Goal_Transform(orient, pos) % Multiply the 3 rotation matrix for the 3 angles % Return a 3x3 rotation matrix; goal_T = eye(4); goal_T(1:3, 1:3) = rotx(orient(1)) * roty(orient(2)) * rotz(orient(3)); goal_T(1:3,4) = pos'; end function r = residual(Ta, Td) na = Ta(1:3,1); oa = Ta(1:3,2); aa = Ta(1:3,3); pa = Ta(1:3,4); nd = Td(1:3,1); od = Td(1:3,2); ad = Td(1:3,3); pd = Td(1:3,4); %Residual position as described by GOLDENBERG et. al (1985) r(1) = na' * (pd - pa); r(2) = oa' * (pd - pa); r(3) = aa' * (pd - pa); % r(5) = atan2(((na' * ad)*cos(r(4)) + (oa' * ad) * sin(r(4))),(aa' * ad)); % r(6) = atan2((-(na' * nd)* sin(r(4)) + (oa' * nd)*cos(r(4))), ... % (-(na' * oa)*sin(r(4)) + (oa' * od) * cos(r(4))) ); %Residual orientation vector (for a set of x,y,z rotation axes) r(4) = 0.5* (aa' * od - ad' * oa); r(5) = 0.5* (na' * ad - nd' * aa); r(6) = 0.5* (oa' * nd - od' * na); end function theta = Correcting_angles(q) % wrap angles for revolute joints theta = zeros(size(q)); for i = 1:size(q,1) q_row = q(i,:); k = q_row > 360; q_row(k) = rem(q_row(k), 360); k = q_row < -360; q_row(k) = -rem(q_row(k), 360); theta(i,:) = q_row; end end function delta = error_f(Tc, Td, s) %extract rotation matrices Rc = Tc(1:3,1:3); Rd = Td(1:3,1:3); %orientation error O_err = [s(1) * ((Rd(:,1)'* Rc(:,1))-1),... s(2) * ((Rd(:,2)'* Rc(:,2))-1),... s(3) * ((Rd(:,3)'* Rc(:,3))-1)]; %s: sigma factor (if the jth direction needs specifying) wo = -1; %orientation weight delta = [ (Td(1:3,4) - Tc(1:3,4))', wo * O_err ]; end %Residual orientation vector (for yaw-pitch-roll angles) % r(4) = atan2((oa' * ad), (aa' * ad)) + 180; %========================================================================== % >>>>>>>>>>>>>> FUNCTION PF-C.1: INTERPOLATED TRAJECTORY <<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. The trajectory will be interpolated by the use % of a 5th order polynom. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = %% LOAD FILES PF_Interpolated_Traj(varargin) S = evalin('base', 'S'); % varargin: {1}time, {2}either 'cart' Settings (from base workspace) or 'joint', {3}either p or q vetors %Load APÊNDICE B. Rotinas em Matlab® do Software de Controle H = evalin('base', 'H'); History (from base workspace) %Load cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second if cn == 1 q0 = S.value{'home_q'}; %get current theta angles else q0 = H(cn - 1).q(end,:); %get current theta angles end ti = varargin{1}; %% Generate the trajectory % Get joint variable target or cartesian target position from arguments in if nargin ~= 3 return else %check the space of the input if strcmp(varargin{2}, 'cart') %cartesian space - so the next variable pt = varargin{3}; %will be pos vector (xyz) elseif strcmp(varargin{2}, 'joint')%joint space qt = varargin{3}; end if exist('pt', 'var') && ~exist('qt', 'var') qt = PF_Inverse_Kinematics(pt, q0); %compute target q if not passed end end % Time vector from 0 to final time, with sp rows tv = linspace(0,ti,sp)'; %Preallocating matrices for the output q = zeros(sp, n); dq = zeros(sp, n); ddq = zeros(sp, n); % Fifth order polynomial function for i=1:n a0 = q0(i); a1 = 0; a2 = 0; a3 = (10*(qt(i) - q0(i))) / ti^3; a4 = -(15*(qt(i) - q0(i))) / ti^4; a5 = (6*(qt(i) - q0(i))) / ti^5; %Assembling joint position coefficients q_cf = [a5 a4 a3 a2 a1 a0]; %Assembling joint velocity coefficients dq_cf = [5*a5, 4*a4, 3*a3, 2*a2, a1]; %Assembling joint acceleration coefficients ddq_cf = [20*a5, 12*a4, 6*a3, 2*a2]; % Computing theta, d_theta, dd_theta (column vector) q(:,i) = polyval(q_cf,tv); dq(:,i) = polyval(dq_cf,tv); ddq(:,i) = polyval(ddq_cf,tv); end end n = size(qt, 2); %number of joints (scalar) % Computing step points (sp) sp = ceil(ti * fps); %========================================================================== % >>>>>>> FUNCTION PF-C.2: INTERPOLATED TRAJECTORY WITH VIA POINTS <<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. The trajectory will be interpolated by the use % of a 5th order polynom, for each via point. If the vecity is not given % for the via points, the function will try to compute by the use of % Jacobian or heuristic method. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = % ot = orientation of each via point PF_Interpolated_wvp_Traj(pt, tvp, % vl = linear velocity via point (endvl,va,qt) effector) [mm/sec] % tvp = time of via points % va = angular velocity via point (end% pt = cartesian via points effector) [deg/sec] APÊNDICE B. Rotinas em Matlab® do Software de Controle %% LOAD FILES S = evalin('base', 'S'); %Load Settings (from base workspace) H = evalin('base', 'H'); %Load History (from base workspace) RP = evalin('base', 'RP'); %Load Robot Parameters a = RP.a; d = RP.d; A = RP.alpha; %DH parameters cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second if cn == 1 q0 = S.value{'home_q'}; %get current theta angles else q0 = H(cn - 1).q(end,:); current theta angles end %get %% Generate the trajectory % Obtain target theta angles from the target position and orientation if exist('pt', 'var') && ~exist('qt', 'var') qt = PF_Inverse_Kinematics(pt, q0); %compute target q if not passed end n = size(q0,2); %number of joints (scalar) tt = sum(tvp); %Total time; sp = ceil(tt * fps);% Computing step points (sp) tv = linspace(0,tt,sp)'; % Time vector from 0 to via point time, with sp rows spvp = []; for i = 1:size(pt,1) if i ~= size(pt,1) spvp = [spvp, round(tvp(i) * fps)]; %step points of each via point tvvp{i} = linspace(0, tvp(i), spvp(i))';%time vector of via point else sp_sum = sum(spvp); tvvp{i} = linspace(0, tvp(i), abs(sp - sp_sum))'; end end dqt = zeros(size(pt,1), n); ddqt = zeros(size(pt,1), n); % Compute the joint velocity for each via points for vp = 1:size(pt,1) if any(vl ~= 0) || any(va ~= 0) [~, T_m] = PF_Forward_Kinematics(qt(vp,:), d, a, A); J = PF_Jacobian(T_m); %Computes the Jacobian vlt = (1/sqrt(3)) * (ones(1,3) * vl(vp));%linear velocity terms vat = (1/sqrt(3)) * (ones(1,3) * va(vp));%angular velocity terms dqt(vp,:) = (pinv(J) * [vlt, vat]')'; else %Apply heuristic in the joint space %get previous theta if vp == 1 q_p = q0; else q_p = qt(vp-1, :); end %get next theta if vp == size(pt,1) dqt(vp, :) = zeros(1,n);%the last via point has angular veloc 0 else q_n = qt(vp+1,:); end mp = qt(vp,:) - q_p; %angular coefficient of the line mn = qt(vp,:) - q_n; %angular coefficient of the line for j = 1:n if sign(mp(j)) ~= sign(mn(j)) dqt(vp, j) = (mp(j) + mn(j)) / 2; else dqt(vp, j) = 0; end end end end % Compute the joint acceleration for each via points for idx = 1:size(pt,1) %Apply heuristic in the joint space %get previous angular velocity if idx == 1 dq_p = zeros(1,n); %angular acceleration starts from 0 else dq_p = dqt(idx-1, :); end %get next angualar velocity APÊNDICE B. Rotinas em Matlab® do Software de Controle if idx == size(pt,1) dq_n = zeros(1,n); %the last via point has angular acceleration 0 else dq_n = dqt(idx+1,:); end mp = dqt(idx,:) - dq_p; %angular coefficient of the line mn = dqt(idx,:) - dq_n; %angular coefficient of the line for j = 1:n if sign(mp(j)) ~= sign(mn(j)) ddqt(idx, j) = (mp(j) + mn(j)) / 2; else ddqt(idx, j) = 0; end end end %Preallocating matrices for the output q = []; dq = []; ddq = []; % Fifth order polynomial function for ii = 1:size(pt,1) qvp = []; dqvp = []; ddqvp = []; for i=1:n if ii == 1 a0 = q0(i); a1 = 0; a2 = 0; a3 = -(20*q0(i) - 20*qt(ii, i) + 8*dqt(ii, i)*tvp(ii) - ... ddqt(ii, i)*tvp(ii)^2)/(2*tvp(ii)^3); a4 = (30*q0(i) - 30*qt(ii, i) + 14*dqt(ii, i)*tvp(ii) - ... 2*ddqt(ii, i)*tvp(ii)^2)/(2*tvp(ii)^4); a5 = -(12*q0(i) - 12*qt(ii, i) + 6*dqt(ii, i)*tvp(ii)- ... ddqt(ii, i)*tvp(ii)^2)/(2*tvp(ii)^5); else a0 = qt(ii-1, i); a1 = dqt(ii-1, i); a2 = ddqt(ii-1, i)/2; a3 = -(20*qt(ii-1, i) 20*qt(ii, i) + 8*dqt(ii,i)*tvp(ii) +... 12 * dqt(ii-1, i)*tvp(ii) - ddqt(ii,i)*tvp(ii)^2 + ... 3*ddqt(ii-1, i)*tvp(ii)^2)/(2*tvp(ii)^3); a4 = (30*qt(ii-1, i) 30*qt(ii, i) + 14*dqt(ii,i)*tvp(ii) +... 16 * dqt(ii-1, i)*tvp(ii) - 2*ddqt(ii,i)*tvp(ii)^2 + ... 3*ddqt(ii-1, i)*tvp(ii)^2)/(2*tvp(ii)^4); a5 = -(12*qt(ii-1, i) 12*qt(ii, i) + 6*dqt(ii,i)*tvp(ii) +... 6 * dqt(ii-1, i)*tvp(ii) - ddqt(ii,i)*tvp(ii)^2 + ... ddqt(ii-1, i)*tvp(ii)^2)/(2*tvp(ii)^5); end %Assembling joint position coefficients q_cf = [a5 a4 a3 a2 a1 a0]; %Assembling joint velocity coefficients dq_cf = [5*a5, 4*a4, 3*a3, 2*a2, a1]; %Assembling joint acceleration coefficients ddq_cf = [20*a5, 12*a4, 6*a3, 2*a2]; % Computing theta, d_theta, dd_theta (column vector) qvp = [qvp, polyval(q_cf,tvvp{ii})]; dqvp = [dqvp, polyval(dq_cf,tvvp{ii})]; ddqvp = [ddqvp, polyval(ddq_cf,tvvp{ii})]; end q = [q; qvp]; dq = [dq; dqvp]; ddq = [ddq; ddqvp]; end end APÊNDICE B. Rotinas em Matlab® do Software de Controle %========================================================================== % >>>>>>>>>>>>>> FUNCTION PF-C.3: UNCOORDINATED TRAJECTORY <<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. The trajectory generated in this function is % uncoordinated and uses a constant joint velocity. The joints end their % movement in different moments. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = ti = abs(qt - q0)/va; %computing time PF_Uncoodinated_Traj(varargin) ncessary for each joint reach its goal % pt: targe position and orientation(for uncoordinated traj: pt % Computing step points (sp) => 1x6) sp = ceil(max(ti) * fps); % va = constant angular velocity; %% Load structures and calculate necessary variables S = evalin('base', 'S'); Settings (from base workspace) H = evalin('base', 'H'); History (from base workspace) %Load %Load cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second if cn == 1 q0 = S.value{'home_q'}; %get theta angles of home position else q0 = H(cn - 1).q(end,:); %get current theta angles end % Time vector from 0 to total time, with sp rows tv = linspace(0,max(ti), sp)'; %% Generate the trajectory q = zeros(sp,n); dq = zeros(sp,n); ddq = zeros(sp,n); for i=1:n %Assembling joint position coefficients if qt(i) > q0(i) a1 = va; elseif qt(i) < q0(i) a1 = -va; elseif qt(i) == q0(i) a1 = 0; end q_cf = [a1, q0(i)]; % Computing theta, d_theta, dd_theta (column vector) va = varargin{1}; % Get joint variable target or cartesian target position from arguments in if nargin ~= 3 return else if strcmp(varargin{2}, 'cart') pt = varargin{3}; elseif strcmp(varargin{2}, 'joint') qt = varargin{3}; end if exist('pt', 'var') && ~exist('qt', 'var') qt = PF_Inverse_Kinematics(pt, q0); %compute target q if not passed end end n = size(qt,2); %number of joints (scalar) spi = round(ti(i) * fps); if spi < 1; spi = 1; end q(1:spi, i) = polyval(q_cf, tv(1:spi)); q(spi, i) = polyval(q_cf, ti(i)); if spi < sp q((spi+1: sp), i) = q(spi, i); end dq(:,i) = linspace(va, va, sp); ddq(:,i) = linspace(0, 0, sp); %for a constant velocity the accel is 0 end end APÊNDICE B. Rotinas em Matlab® do Software de Controle %========================================================================== % >>>>> FUNCTION PF-C.4: SEQUENTIAL WITH CONSTANT ANGULAR VELOCITY <<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. The trajectory formed here is sequential (one % joint move at a time) and has constant joint velocity. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = PF_Sequential_cav_Traj(varargin) % Computing step points (sp) % pt: targe position and sp = sum(ceil(ti * fps)); orientation(for uncoordinated traj: pt => 1x6) % Time vector from 0 to total time, % dqc = constant angular velocity; with sp rows %% Load structures and calculate tv = linspace(0,sum(ti), sp)'; necessary variables S = evalin('base', 'S'); %Load tvj = zeros(sp, n); Settings (from base workspace) for i=1:n H = evalin('base', 'H'); %Load spi = ceil(ti(i) * fps); History (from base workspace) cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second if cn == 1 q0 = S.value{'home_q'}; %get theta angles of home position else q0 = H(cn - 1).q(end,:); %get current theta angles end dqc = varargin{1}; % Get joint variable target or cartesian target position from arguments in if nargin ~= 3 return else if strcmp(varargin{2}, 'cart') pt = varargin{3}; elseif strcmp(varargin{2}, 'joint') qt = varargin{3}; end if exist('pt', 'var') && ~exist('qt', 'var') qt = PF_Inverse_Kinematics(pt, q0); %compute target q if not passed end end n = size(qt,2); %number of joints (scalar) ti = abs(qt - q0)/dqc; %time ncessary for each joint reach its goal if i==1 spp = 0; tvj(1:spi, i) = linspace(0, ti(i), spi)'; tvj(spi:sp, i) = ti(i); else tvj(1:spp, i) = 0; tvj((spp : spp+spi-1), i) = linspace(0, ti(i), spi)'; tvj(spp+spi-1:sp, i) = ti(i); end spp = spp+spi; end %% Generate the trajectory q = zeros(sp,n); dq = zeros(sp,n); ddq = zeros(sp,n); for i=1:n %Assembling joint position coefficients if qt(i) > q0(i) a1 = dqc; elseif qt(i) < q0(i) a1 = -dqc; elseif qt(i) == q0(i) a1 = 0; end q_cf = [a1, q0(i)]; % Computing theta, d_theta, dd_theta (column vector) q(:, i) = polyval(q_cf, tvj(:, i)); dq(:,i) = linspace(dqc, dqc, sp); ddq(:,i) = linspace(0, 0, sp); %for a constant velocity the accel is 0 end APÊNDICE B. Rotinas em Matlab® do Software de Controle end %========================================================================== % >>>>>>>> FUNCTION PF-C.5: SEQUENTIAL TRAJECTORY WITH TIME INPUT <<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. The trajectory generated is sequential (one % joint move at a time) and is interpolated by the use of a 5th order % polynom considering initial and final velocity and acceleration equals to % 0. Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = PF_Sequential_ti_Traj(varargin) % pt: targe position and %% Check inputs orientation(for uncoordinated traj: pt if size(ti, 2) == 1 => 1x6) ti = ones(1, n) * ti; %% Load structures and calculate necessary variables S = evalin('base', 'S'); Settings (from base workspace) H = evalin('base', 'H'); History (from base workspace) end %Load %Load cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second if cn == 1 q0 = S.value{'home_q'}; %get theta angles of home position else q0 = H(cn - 1).q(end,:); %get current theta angles end % Time vector from 0 to final ti, with sp rows tv = linspace(0, sum(ti), sp)'; tvj = zeros(sp, n); for i=1:n spi = ceil(ti(i) * fps); if i==1 spp = 0; tvj(1:spi, i) = linspace(0, ti(i), spi)'; tvj(spi:sp, i) = ti(i); else ti = varargin{1}; % Get joint variable target or cartesian target position from arguments in if nargin ~= 3 return else if strcmp(varargin{2}, 'cart') pt = varargin{3}; elseif strcmp(varargin{2}, 'joint') qt = varargin{3}; end if exist('pt', 'var') && ~exist('qt', 'var') qt = PF_Inverse_Kinematics(pt, q0); %compute target q if not passed end end n = size(qt, 2); joints (scalar) %% % Computing step points (sp) sp = sum(ceil(ti * fps)); %number of tvj(1:spp, i) = 0; tvj((spp : spp+spi-1), i) = linspace(0, ti(i), spi)'; tvj(spp+spi-1:sp, i) = ti(i); end spp = spp+spi; end %% Generate the trajectory q = zeros(sp,n); dq = zeros(sp,n); ddq = zeros(sp,n); % Fifth order polynomial function for i=1:n a0 = q0(i); a1 = 0; a2 = 0; a3 = (10*(qt(i) - q0(i))) / ti(i)^3; APÊNDICE B. Rotinas em Matlab® do Software de Controle a4 = -(15*(qt(i) - q0(i))) / ti(i)^4; a5 = (6*(qt(i) - q0(i))) / ti(i)^5; %Assembling joint position coefficients q_cf = [a5 a4 a3 a2 a1 a0]; %Assembling joint acceleration coefficients ddq_cf = [20*a5, 12*a4, 6*a3, 2*a2]; % Computing theta, d_theta, dd_theta (column vector) q(:,i) = polyval(q_cf,tvj(:, i)); dq(:,i) = polyval(dq_cf,tvj(:, i)); ddq(:,i) = polyval(ddq_cf,tvj(:, i)); end end %Assembling joint velocity coefficients dq_cf = [5*a5, 4*a4, 3*a3, 2*a2, a1]; %========================================================================== % >>>>>>>> FUNCTION PF-C.6: LINEAR TRAJECTORY W/ CST LINEAR VEL <<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. A linear trajectory is generated using the % Settings value linear increment distance. The time is computed based on % the end-effector to goal distance and velocity provided. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = PF_StraightL_clv_Traj(pt, vl) n = size(qt, 2); %number of % pt: targe position and joints (scalar) orientation(for uncoordinated traj: pt => 1x6) [p0, T] = PF_Forward_Kinematics(q0, d, % vl = constant linear velocity; a, A); %% Load structures and calculate necessary variables S = evalin('base', 'S'); Settings (from base workspace) H = evalin('base', 'H'); History (from base workspace) RP = evalin('base', 'RP'); %Load Robot Parameters p0(4:6) = PF_Transform_to_Orient(T{n}); %get the current orientation %Load %Load a = RP.a; d = RP.d; A = RP.alpha; H parameters %D- cn = S.value{'cn'}; %get the command number from Settings structure % fps = S.value{'fps'}; %get the number of frames per second liv = S.value{'lin_increm'}; %get linear increment value (mm) if cn == 1 q0 = S.value{'home_q'}; %get theta angles of home position else q0 = H(cn - 1).q(end,:); %get current theta angles end qt = PF_Inverse_Kinematics(pt, q0); deltap = pt - p0; L = norm(deltap(1:3)); %Linear distance between initial and target points ti = L/vl; %Time for completing the traj. with linear velocity. nbp = ceil(L / liv); %number of break points in the line sp = nbp; % Time vector from 0 to final time, with sp rows tv = linspace(0, ti, nbp)'; %% Generating Trajectory % break point positions (coordinates xyz) pbp = zeros(nbp, 6); %posit. & orient. of each break point(x,y,z,rx,ry,rz) for i=1:6 pbp(:,i) = linspace(p0(i),pt(i), nbp)'; end qbp = PF_Inverse_Kinematics(pbp(2:end, :), q0); APÊNDICE B. Rotinas em Matlab® do Software de Controle q = [q0; qbp]; %joint displacement dq = diff([zeros(1,n); q])./diff(tm); dq(1,:) = 0; dq(end,:) = 0; ddq = diff([zeros(1,n); dq])./diff(tm); ddq(1,:) = 0; %ddq(end,:) = 0; %joint velocity and acceleration are computed by numerical differentiation % dq = diff([zeros(1,n); q]);%/liv; % ddq = diff([zeros(1,n); dq]);%/liv; end tm = [zeros(1,n); tv * ones(1, n)]; %========================================================================== % >>>>>>>>>> FUNCTION PF-C.7: LINEAR TRAJECTORY W/ TIME INPUT <<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. A linear trajectory is generated using the % Settings value linear increment distance. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = PF_StraightL_ti_Traj(pt, ti) [p0, T] = PF_Forward_Kinematics(q0, d, % pt: targe position and a, A); orientation(for uncoordinated traj: pt p0(4:6) = PF_Transform_to_Orient(T{n}); => 1x6) %get the current orientation % dqc = constant angular velocity; %% Load structures and calculate necessary variables S = evalin('base', 'S'); Settings (from base workspace) H = evalin('base', 'H'); History (from base workspace) RP = evalin('base', 'RP'); %Load Robot Parameters %Load %Load a = RP.a; d = RP.d; A = RP.alpha; H parameters %D- cn = S.value{'cn'}; %get the command number from Settings structure % fps = S.value{'fps'}; %get the number of frames per second liv = S.value{'lin_increm'}; %get linear increment value (mm) if cn == 1 q0 = S.value{'home_q'}; %get theta angles of home position else q0 = H(cn - 1).q(end,:); %get current theta angles end qt = PF_Inverse_Kinematics(pt, q0); deltap = pt - p0; nbp = ceil(norm(deltap(1:3)) / liv); %number of break points in the line sp = nbp; % Time vector from 0 to final time, with sp rows tv = linspace(0, ti, nbp)'; %% Generating Trajectory % break point positions (coordinates xyz) pbp = zeros(nbp, 6); %posit. & orient. of each break point(x,y,z,rx,ry,rz) for i=1:6 pbp(:,i) = linspace(p0(i),pt(i), nbp)'; end qbp = PF_Inverse_Kinematics(pbp(2:end, :), q0); q = [q0; qbp]; %joint displacement %joint velocity and acceleration are computed by numerical differentiation % dq = diff([zeros(1,n); q]);%/liv; % ddq = diff([zeros(1,n); dq]);%/liv; tm = [zeros(1,n); tv * ones(1, n)]; dq = diff([zeros(1,n); q])./diff(tm); dq(1,:) = 0; dq(end,:) = 0; ddq = diff([zeros(1,n); dq])./diff(tm); ddq(1,:) = 0; %ddq(end,:) = 0; end n = size(qt, 2); %number of joints (scalar) %========================================================================== % >>>>>>>> FUNCTION PF-C.8: PARAMETRISED TRAJECTORY W/ CST LIN VEL <<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. APÊNDICE B. Rotinas em Matlab® do Software de Controle % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. This function makes use of MATLAB ability to % work with symbolic variables and functions to create a path based on the % expression inputted. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = xv = symvar(pex); PF_Parametrised_clv_Traj(pex, pey, pez, yv = symvar(pey); ... zv = symvar(pez); if ~isempty(xv) vl, fvv, orient) pex = symfun(pex, xv); %convert % pex, pey, pez: parametric equation in symbolic expr. to symb. function x y and z respectively dpex = diff(pex, % lv: linear velocity symvar(pex));%taking the derivative of % fvv: final variable value: Because the param eq. the parametric equation is in function else % of time it is needed a final value to dpex = 0; determine where the parametric end % curve will stop in space. For the C.10 where the user input is time and if ~isempty(yv) % not a constant linear velocity, this pey = symfun(pey, yv); value (fvv) is the final time, here dpey = diff(pey, yv); % is used only to know where the curve else stops, and the time is computed dpey = 0; % using the linear velocity end %% LOAD FILES S = evalin('base', 'S'); Settings (from base workspace) H = evalin('base', 'H'); History (from base workspace) %Load %Load ~isempty(zv) pez = symfun(pez, zv); dpez = diff(pez, symvar(pez)); else dpez = 0; cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second if cn == 1 q0 = S.value{'home_q'}; current theta angles else q0 = H(cn - 1).q(end,:); current theta angles end if %get %get %make a vector of time if passed a scalar if size(fvv, 2) == 1 fvv(2) = fvv; fvv(1) = 0; end n = size(q0, 2); %Computing S pex = sym(pex); %converting the string expression to symbolic pey = sym(pey); pez = sym(pez); end try %Parametric curve length S = int(sqrt(dpex^2 + dpey^2 + dpez^2), fvv(1), fvv(2)); S = double(S); ti = S / vl; %Computing trajectory time catch ti = abs(fvv(2) - fvv(1)); %If not possible to compute S, set fvv as time end sp = ceil(ti * fps); % Computing step points (sp) % Time vector from 0 to final time, with sp rows tv = linspace(0,ti,sp)'; %time vector used for plots %time vector used for solving the parametric equation tvfpe = linspace(0, fvv(2), sp)'; %% Generate the trajectory xp = []; yp = []; zp = []; if ~isempty(xv) APÊNDICE B. Rotinas em Matlab® do Software de Controle xp = [xp; double(pex(tvfpe))]; %solving for tv entries else xp = [xp; pex * ones(sp, 1)]; end if ~isempty(yv) yp = [yp; double(pey(tvfpe))]; else yp = [yp; pey * ones(sp, 1)]; end if ~isempty(zv) zp = [zp; double(pez(tvfpe))]; else zp = [zp; pez * ones(sp, 1)]; end p = double([xp, yp, zp, ones(sp, 1) * orient]); %positions and orientations s = [1 1 1]; if any(orient == 0) s = [0 0 0]; %then the orientation is not specified in IK end q = PF_Inverse_Kinematics(p, q0, s); tm = [zeros(1,n); tv * ones(1, n)]; dq = diff([zeros(1,n); q])./diff(tm); dq(1,:) = 0; dq(end,:) = 0; ddq = diff([zeros(1,n); dq])./diff(tm); ddq(isnan(ddq)) = 0; %ddq(end, :) = 0; end %========================================================================== % >>>>>>>> FUNCTION PF-C.9: PARAMETRISED TRAJECTORY W/ CST ANG VEL <<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. This function makes use of MATLAB ability to % work with symbolic variables and functions to create a path based on the % expression inputted. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = PF_Parametrised_cav_Traj(pej, jn, va, %make a vector of time if passed a fvv) scalar % pej: joint parametric equation if size(fvv, 2) == 1 % jn = joint number to apply the fvv(2) = fvv; parametrisation fvv(1) = 0; % va: angular velocity end % fvv: final variable value: Because the parametric equation is in function % of time it is needed a final value to determine where the parametric % curve will stop in space. %% LOAD FILES S = evalin('base', 'S'); %Load Settings (from base workspace) H = evalin('base', 'H'); %Load History (from base workspace) cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second if cn == 1 q0 = S.value{'home_q'}; %get current theta angles else q0 = H(cn - 1).q(end,:); current theta angles end n = size(q0, 2); %Computing S pej = sym(pej); %converting the string expression to symbolic jv = symvar(pej); if ~isempty(jv) pej = symfun(pej, jv); %convert symbolic expr. to symb. function dpej = diff(pej, jv); %taking the derivative of the parametric eq. else dpej = 0; end try %get %Parametric curve length S = int(sqrt(1+dpej^2), fvv(1), fvv(2)); S = double(S); APÊNDICE B. Rotinas em Matlab® do Software de Controle ti = S / va; %Computing trajectory time catch ti = abs(fvv(2) - fvv(1)); %If not possible to compute S, set fvv as time end sp = ceil(ti * fps); % Computing step points (sp) % Time vector from 0 to final time, with sp rows tv = linspace(0,ti,sp)'; %% Generate the trajectory qp = []; if ~isempty(jv) pej = symfun(pej, jv); %convert symbolic expr. to symb. function qp = [qp; double(pej(tv))]; %solving for tv entries else qp = [qp; pej * ones(sp, 1)]; end % all joints will remain q0 but the joint jn q = (q0' * ones(1, sp))'; %making a matrix with the q0 values q(:, jn) = qp;%Replacing column jn with the results of the parametrisation tm = [zeros(1,n); tv * ones(1, n)]; dq = zeros(sp, n); %preallocate dq dq(q~=0) = va; %substitute va in dq where the position is not 0 % dq(1,:) = 0; dq(end,:) = 0; ddq = diff([zeros(1,n); dq])./diff(tm); ddq(isinf(ddq)) = 0; ddq(isnan(ddq)) = 0; % ddq(1,:) = 0; %ddq(end, :) = 0; end %========================================================================== % >>>>> FUNCTION PF-C.10: PARAMETRISED JOINT TRAJECTORY W/ TIME INPUT <<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for the joint % selected based on the user's input. This function makes use of MATLAB % ability to work with symbolic variables and functions to create a path % based on the expression inputted. The expression generates a joint curve % not a cartesian curve like the others, so the inverse kinematics function % is not used here. Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = PF_Parametrised_J_ti_Traj(pej, jn, deltat) % pej: joint parametric equation % jn = joint number to apply the parametrisation % deltat: delta time: Because the parametric equation is in function of % time it is needed an initial and final value to determine where the % parametric curve starts and stops in space. if cn == 1 q0 = S.value{'home_q'}; %get current theta angles else q0 = H(cn - 1).q(end,:); current theta angles end %PS: The inputs check is made in the Secondary function that calls this one %% LOAD FILES S = evalin('base', 'S'); %Load Settings (from base workspace) H = evalin('base', 'H'); %Load History (from base workspace) if size(deltat, 2) == 1 ti = deltat; else ti = abs(deltat(2) - deltat(1)); end cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second %get n = size(q0, 2); pej = sym(pej); %converting the string expression to symbolic sp = ceil(ti * fps); % Computing step points (sp) % Time vector from 0 to final time, with sp rows tv = linspace(0,ti,sp)'; APÊNDICE B. Rotinas em Matlab® do Software de Controle %% qp jv if Generate the trajectory = []; = symvar(pej); ~isempty(jv) pej = symfun(pej, jv); %convert symbolic expr. to symb. function qp = [qp; double(pej(tv))]; %solving for tv entries else qp = [qp; pej * ones(sp, 1)]; end q = (q0' * ones(1, sp))'; %making a matrix with the q0 values q(:, jn) = qp;%Replacing column jn with the results of the parametrisation tm = [zeros(1,n); tv * ones(1, n)]; dq = diff([zeros(1,n); q])./diff(tm); % dq(1,:) = 0; dq(end,:) = 0; ddq = diff([zeros(1,n); dq])./diff(tm); % ddq(1,:) = 0; %ddq(end, :) = 0; end % all joints will remain q0 but the joint jn %========================================================================== % >>>>>>>> FUNCTION PF-C.11: PARAMETRISED TRAJECTORY W/ TIME INPUT <<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. This function makes use of MATLAB ability to % work with symbolic variables and functions to create a path based on the % expression inputted. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = %converting the string expression to PF_Parametrised_ti_Traj(pex, pey, pez, symbolic deltat, orient) pex = sym(pex); % pex, pey, pez: parametric equation in pey = sym(pey); x y and z respectively pez = sym(pez); % lv: linear velocity % deltat: delta time: Because the if size(deltat, 2) == 1 parametric equation is in function of ti = deltat; % time it is needed an initial and else final value to determine where the ti = abs(deltat(2) - deltat(1)); % parametric curve starts and stops in end space. %% LOAD FILES sp = ceil(ti * fps); % Computing S = evalin('base', 'S'); %Load step points (sp) Settings (from base workspace) % Time vector from 0 to final time, H = evalin('base', 'H'); %Load with sp rows History (from base workspace) tv = linspace(0,ti,sp)'; cn = S.value{'cn'}; %get the command number from Settings structure fps = S.value{'fps'}; %get the number of frames per second if cn == 1 q0 = S.value{'home_q'}; %get current theta angles else q0 = H(cn - 1).q(end,:); current theta angles end n = size(q0, 2); %get %% Generate the trajectory xp = []; yp = []; zp = []; xv yv zv if = symvar(pex); = symvar(pey); = symvar(pez); ~isempty(xv) pex = symfun(pex, xv); %convert symbolic expr. to symb. function xp = [xp; double(pex(tv))]; %solving for tv entries else xp = [xp; pex * ones(sp, 1)]; end if ~isempty(yv) APÊNDICE B. Rotinas em Matlab® do Software de Controle pey = symfun(pey, yv); %convert symbolic expr. to symb. function yp = [yp; double(pey(tv))]; else yp = [yp; pey * ones(sp, 1)]; end if ~isempty(zv) pez = symfun(pez, zv); %convert symbolic expr. to symb. function zp = [zp; double(pez(tv))]; else zp = [zp; pez * ones(sp, 1)]; end end p = double([xp, yp, zp, ones(sp, 1) * orient]); %positions and orientations q = PF_Inverse_Kinematics(p, q0, s); % if norm(deltap(1:3)) % % end tm = [zeros(1,n); tv * ones(1, n)]; dq = diff([zeros(1,n); q])./diff(tm); % dq(1,:) = 0; dq(end,:) = 0; ddq = diff([zeros(1,n); dq])./diff(tm); % ddq(1,:) = 0; %ddq(end, :) = 0; end s = [1 1 1]; if any(orient == 0) s = [0 0 0]; %then the orientation is not specified in IK %========================================================================== % >>>>>>>>> FUNCTION PF-C.12: TRAJECTORY BY TABLE W/ TIME INPUT <<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. This function receives a matrix with cartesian % coordinates and a time (in seconds) for completing the trajectory. The % function converts each coordinate to joint position by inverse kinematics % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = n = size(q0, 2); PF_Table_ti_Traj(p, ti, s) % p: position matrix (n x 6: x, y, z, sp = size(p, 1); % Computing step rx, ry, rz) points (sp) % ti: time for completing the trajectory % Time vector from 0 to final time, % s: orientation specification (vector with sp rows 1x3), if [1 1 1] the inverse tv = linspace(0,ti,sp)'; % kinematics will attempt to satisfy %% Generate the trajectory the orientation presented in the table if ~exist('s', 'var') %PS: The inputs check is made in the if (size(p,2) < 6) || (n < 6) Secondary function that calls this one s = [0 0 0]; % no need to %% LOAD FILES satisfy orientation in IK S = evalin('base', 'S'); %Load p(:, 4:6) = zeros(size(p, 1), Settings (from base workspace) 3); H = evalin('base', 'H'); %Load else History (from base workspace) s = [1 1 1];% satisfy cn = S.value{'cn'}; %get the command number from Settings structure if cn == 1 q0 = S.value{'home_q'}; current theta angles else q0 = H(cn - 1).q(end,:); current theta angles end %get %get orientation in IK end end q = PF_Inverse_Kinematics(p, q0, s); tm = [zeros(1,n); tv * ones(1, n)]; dq = diff([zeros(1,n); q])./diff(tm); dq(1,:) = 0; dq(end,:) = 0; ddq = diff([zeros(1,n); dq])./diff(tm); ddq(1,:) = 0; %ddq(end, :) = 0; end APÊNDICE B. Rotinas em Matlab® do Software de Controle %========================================================================== % >>>>>>>>> FUNCTION PF-C.13: TRAJECTORY BY TABLE W/ CST LIN VEL <<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will generate the trajectory for each joint % based on the user's input. This function receives a matrix with cartesian % coordinates and a constant linear velocity for the end-effector in % cartesian space. The function computs the length of the path and them % computes the time using the velocity. After that it converts each % coordinate to joint position by inverse kinematics. % Refer to section 4 of documentation for details. %========================================================================== function [q, dq, ddq, tv, sp] = S = 0; PF_Table_clv_Traj(p, vl, s) for i = 1:sp-1 % p: position matrix (n x 6: x, y, z, S = S + norm(p(i, 1:3) - p(i+1, rx, ry, rz) 1:3)); % vl: linear velocity (constant) end ti = S/vl; %% LOAD FILES S = evalin('base', 'S'); %Load % Time vector from 0 to final time, Settings (from base workspace) with sp rows H = evalin('base', 'H'); %Load tv = linspace(0,ti,sp)'; History (from base workspace) %% Generate the trajectory if ~exist('s', 'var') if (size(p,2) < 6) || (n < 6) cn = S.value{15}; %get the command p(:, 4:6) = zeros(size(p, 1), number from Settings structure 3); s = [0 0 0]; % no need to if cn == 1 satisfy orientation in IK q0 = S.value{16}; %get else current theta angles s = [1 1 1];% satisfy else orientation in IK q0 = H(cn - 1).q(end,:); %get end current theta angles end end q = PF_Inverse_Kinematics(p, q0, s); n = size(q0, 2); sp = size(p, 1); points (sp) % Computing step %Computing S (approximation by linear distance between points) tm = [zeros(1,n); tv * ones(1, n)]; dq = diff([zeros(1,n); q])./diff(tm); dq(1,:) = 0; dq(end,:) = 0; ddq = diff([zeros(1,n); dq])./diff(tm); ddq(1,:) = 0; %ddq(end, :) = 0; end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION PF-D: FORWARD DYNAMICS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Peter I. Corke, % Modified by Diego Varalda de Almeida with permission under the terms of % the GNU. % Date: June 05th, 2016. % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will compute the compute the forwards dynamics % given the torque inputted by the user in tab 4: Dynamics simulation. The % solution is done by the use of the ode45 function and the inverse % dynamics function using the Recursive Newton Euler method (also % implemented by Peter I. Corke in his Robotics Matlab Toolbox). Refer to % section 4 of documentation for details. %========================================================================== APÊNDICE B. Rotinas em Matlab® do Software de Controle function [t, q, dq] = PF_Forward_Dynamics(t1, torq_matrix, q0, qd0, varargin) S = evalin('base', 'S'); %Load Settings (from base workspace) ctrl_rate = S.value{'ctrl_rate'}; % check the Matlab version, since ode45 syntax has changed if verLessThan('matlab', '7') error('fdyn now requires Matlab version >= 7'); end n = S.value{'dof'}; if nargin == 1 torq_matrix = 0; q0 = zeros(1,n); qd0 = zeros(1,n); elseif nargin == 2 q0 = zeros(1,n); qd0 = zeros(1,n); elseif nargin == 3 qd0 = zeros(1,n); end % concatenate q and qd into the initial state vector q0 = [q0(:); qd0(:)]; [t,y] = ode45(@fdyn2, [0 t1], q0, [], n, torq_matrix, ctrl_rate); q = y(:,1:n); dq = y(:,n+1:2*n); end %FDYN2 private function called by FDYN % % XDD = FDYN2(T, X, FLAG, ROBOT, TORQUEFUN) % % Called by FDYN to evaluate the robot velocity and acceleration for % forward dynamics. T is the current time, X = [Q QD] is the state vector, % ROBOT is the object being integrated, and TORQUEFUN is the string name of % the function to compute joint torques and called as % % TAU = TORQUEFUN(T, X) % % if not given zero joint torques are assumed. % % The result is XDD = [QD QDD]. function [xd,qdd] = fdyn2(t, x, n, torq_matrix, varargin) q = x(1:n)'; qd = x(n+1:2*n)'; % qdd = x(2*n+1:3*n)'; ctrl_rate = varargin{1}; % % evaluate the torque function if one is given % if isa(torqfun, 'function_handle') % tau = torqfun(t, q, qd, varargin{:}); % else % tau = zeros(1,n); % end if t==0 idx = 1; else idx = ceil(t*ctrl_rate); end tq_row = torq_matrix(idx, :); qdd = accel(n, x(1:n,1)', x(n+1:2*n,1)', tq_row); xd = [x(n+1:2*n,1); qdd]; end function qdd = accel(n, Q, qd, torque) if nargin == 2 if length(Q) ~= (3*n) error('RTB:accel:badarg', 'Input vector X is length %d, should be %d (q, qd, tau)', length(Q), 3*robot.n); end % accel(X) Q = Q(:)'; % make it a row vector q = Q(1:n); qd = Q(n+1:2*n); torque = Q(2*n+1:3*n); elseif nargin == 4 % accel(Q, qd, torque) if size(Q, 1) > 1 % handle trajectory by recursion if size(Q,1) ~= size(qd,1) error('for trajectory q and qd must have same number of rows'); end if size(Q,1) ~= size(torque,1) error('for trajectory q and torque must have same number of rows'); end qdd = []; for i=1:size(Q,1) qdd = cat(1, qdd, accel(Q(i,:), qd(i,:), torque(i,:))'); end return APÊNDICE B. Rotinas em Matlab® do Software de Controle else q = Q'; if length(q) == n q = q(:)'; qd = qd(:)'; end if size(Q,2) ~= n error('q must have %d columns', n); end if size(qd,2) ~= n error('qd must have %d columns', n); end if size(torque,2) ~= n error('torque must have %d columns', n); end end else error('RTB:accel:badargs', 'insufficient arguments'); end % compute current manipulator inertia % torques resulting from unit acceleration of each joint with % no gravity. M = PF_Inverse_Dynamics(rad2deg(ones(n,1)*q ), rad2deg(zeros(n,n)), ... rad2deg(eye(n)), [0;0;0]); % compute gravity and coriolis torque % torques resulting from zero acceleration at given velocity & % with gravity acting. tau =PF_Inverse_Dynamics(rad2deg(q), rad2deg(qd), rad2deg(zeros(1,n))); qdd = M \ (torque - tau)'; end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION PF-E: INVERSE DYNAMICS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % Date: March 30th, 2016. % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will compute the torque for each joint based % on the trajectory provided. % Refer to section 4 of documentation for details. %========================================================================== function Torque = %center of mass vectors converted to PF_Inverse_Dynamics_Lagrange(q, dq, metres ddq, g) for i=1:length(r_cm); r_cm(i) = % q: matrix sp x n (step points per {r_cm{1} / 1000}; end dof) with the joints positions % dq: matrix sp x n (step points per if ~exist('g', 'var') dof) with the joints velocities g = [0 0 9.08665]; %Gravity % ddq: matrix sp x n (step points per acceleration in metres/s^2 dof) with the joints accelerations end %% Loading neccessary files RP = evalin('base', 'RP'); %Load % mass = RP.mass; Robot Parameters (from base workspace) % m = cat(1,mass(:)); %% Assembling variables n = size(q,2); alpha = RP.alpha'; a = RP.a' / 1000; %DH parameter converted to metres d = RP.d' / 1000; %DH parameter converted to metres r_cm = cellfun(@transpose,RP.r_cm,'UniformOutp ut',false); m = Ixx Ixy Ixz Iyy Iyz Izz RP.mass; = RP.Ixx'; = RP.Ixy'; = RP.Ixz'; = RP.Iyy'; = RP.Iyz'; = RP.Izz'; % Assembling the Inertia matrix for each link I(1:n) = {zeros(3,3)}; for i=1:n I{i} = [Ixx(i), -Ixy(i), -Ixz(i); APÊNDICE B. Rotinas em Matlab® do Software de Controle -Ixy(i), Iyy(i), -Iyz(i); -Ixz(i), -Iyz(i), Izz(i)]; end end %% Torque algorithm Torque = zeros(size(q)); for row = 1:size(q,1) qr = q(row, :); dqr = dq(row, :); ddqr = ddq(row, :); % Getting the transform matrices [~, T_m] = PF_Forward_Kinematics(qr, d, a, alpha); %Converting position, velocity and acceleration to rad, rad/s, rad/s^2 qr = deg2rad(qr); dqr = deg2rad(dqr); ddqr = deg2rad(ddqr); p_vecs(1:n) = {zeros(3,3)}; for i = 1:n p_vecs{i} = T_m{i}((1:3),4); end z_vec(1:n) = {zeros(3,3)}; for i = 1:n if i == 1 R_m = eye(3); else R_m = T_m{i1}((1:3),(1:3)); end z_vec{i} = R_m(:,3); %R_m * k_hat; - k_hat: unit vector end %Term one a_ij = zeros(n,n); T2 = zeros(n,1); %T3 = zeros(n,1); for i = 1:n for j = 1:n I_ij = Inertia_tensor(I, m, p_vecs, r_cm, i, j); a_ij(i,j) = z_vec{i}' * I_ij * z_vec{j}; for k = 1:n if j>>>>>>>>>>>>>>>>> FUNCTION PF-F: JACOBIAN MATRIX <<<<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will compute the Jacobian matrix numerically % (wihtout taking the partial derivative of the position function). Refer % to Robot Dynamics and Control (SPONG, VIDYASAGAR, 1989; p. 112 - 115) and % section 4 of documentation for details. %========================================================================== function J = PF_Jacobian(T_m) d_im1_n = T_m{n}((1:3),4); % % T_m: Homogeneous transform matrices p_vector (cell array w/ all n matrices from FK) % T: Vector with the theta angles else (array 1xn) R0_i_1 = T_m{i-1}((1:3),(1:3)); % n: number of degrees of freedom % Rotation matrix (number of variables) d_im1_n = T_m{n}((1:3),4) % J: Jacobian matrix (6xn: 3xn for the T_m{i-1}((1:3),4); %position vector linear velocity and 3xn angular vel) end n = length(T_m); k = [0 0 1]'; %unit vector k_hat zi_1 = R0_i_1 * k; term i equation XX % Preallocating the Jacobian matrices Jv = zeros(3,n); J_omega = zeros(3,n); for i=1:n if i == 1 R0_i_1 = eye(3); % Rotation matrix from 0 to 0; Jv(:,i) = cross(zi_1, d_im1_n); % Jacobian (for linear velocity) J_omega(:,i) = zi_1; % Jacobian (for angular velocity) end %Assemblying Jacobian matrix J = [Jv ; J_omega]; % z APÊNDICE B. Rotinas em Matlab® do Software de Controle end %========================================================================== % >>> FUNCTION PF-H: CONFIGURATION, CONSTRAINTS & REACHEABLE COORDINATE <<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will check if the position input (can be a % joint position or a end-effector cartesian position) is reachable, and if % it is permitted in the sense that thhe position will not make the robot % collide with something. For robots that works with 2 different % configuration (arm backwards like a scorpion) this function will tell if % the configuration changes. % Refer to section 4 of documentation for details. %========================================================================== function [is_insidelim, is_allowed, is_reachable] = % Check if position is PF_Conf_Const_Reach(varargin) reachable % p: a vector or a matrix with all the if p_rad > max_reach target cartesian position (xyz) r_prob = [r_prob, i]; % q: a vector or a matrix with all is_reachable = false; target joint variable end % Check if end-effector will %% Loading and defining variables hit table S = evalin('base', 'S'); %Load if p(i, 3) < 0 Settings from base workspace a_prob = [a_prob, i]; RP = evalin('base', 'RP'); %Load is_allowed = false; Robot Parameters end end end d = RP.d'; a = RP.a'; alpha = RP.alpha'; %DH parameters if exist('q', 'var') for i = 1:size(q, 1) max_reach = S.value{'max_reach_p'}; [p_vec, ~] = %get max reach distance from Settings PF_Forward_Kinematics(q(i,:), d, a, alpha); q_lim = RP.j_range; %limit of each p_rad = norm(p_vec); joint % Check if position is reachable %start variables as true if p_rad > max_reach is_reachable = true; is_allowed = true; r_prob = [r_prob, i]; is_insidelim = true; is_reachable = false; end for i=1:nargin if strcmp(varargin{i}, 'p') % Check if end-effector will p = varargin{i+1}; hit table elseif strcmp(varargin{i}, 'p') if p_vec(3) < 0 q = varargin{i+1}; a_prob = [a_prob, i]; end is_allowed = false; end end %% Check if the coordinate is reachable r_prob = []; %rows with reach problem a_prob = []; %rows with allowability problem l_prob = []; %rows with limit problem if exist('p', 'var') for i = 1:size(p, 1) p_rad = norm(p(i, 1:3)); %compute the radius formed by p vector % Check if joint position is inside bounds for j = 1:n if q(j) < q_lim{j}(1) || q(j) > q_lim{j}(2) l_prob = [l_probl, j]; is_insidelim = false; end end end APÊNDICE B. Rotinas em Matlab® do Software de Controle end %% Display messages if ~is_reachable MF_Update_Message(25, 'warnings', num2str(r_prob)); elseif ~is_allowed MF_Update_Message(26, 'warnings', num2str(a_prob)); elseif ~is_insidelim MF_Update_Message(27, 'warnings', num2str(l_prob)); end end APÊNDICE B. Rotinas em Matlab® do Software de Controle 2 FUNÇÕES SECUNDÁRIAS %========================================================================== % >>>>>>>>>> FUNCTION SF-1: MOVE END-EFFECTOR TO HOME POSITION <<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will load the home position from Settings % structure and process this to move the end-effector to home position % orientation in interpolated trajectory. This function works as a % intermediate for the GUI (user inputs) and the primary function % responsible for generating the trajectory. Refer to section 4 of % documentation for details. %========================================================================== function SF_Move_EE_to_Home() % If dynamics is enabled, compute %% Load files and variables torque S = evalin('base', 'S'); %Load enable_dynamics = Settings (from base workspace) S.value{'enable_dynamics'}; H = evalin('base', 'H'); %Load if enable_dynamics History (from base workspace) tq = PF_Inverse_Dynamics(q, dq, RP = evalin('base', 'RP'); %Load ddq); %get the computed torque Robot Parameters (from base workspace) % If control system is enabled, cn = S.value{'cn'}; %get the command simulate the result of the control number from Settings structure enable_control = home_q = S.value{'home_q'}; %get home S.value{'enable_control'}; joint variables if enable_control max_speed = RP.max_speed; tqc = PF_Robot_Control(q, dq, %% ddq, tq); %get the control system %PS: No need to check if home position torque is reached or allowed (already done) [qc, dqc, ddqc] = PF_Forward_Dynamics(tqc); else % Get current joint variables to qc = []; dqc = []; ddqc = []; compute time end if cn == 1 %End-effector already in else home position qc = []; dqc = []; ddqc = []; tq = MF_Update_Message(16, 'warnings'); []; tqc = []; return end else %% Save outputs into the History q0 = H(cn - 1).q(end,:); structure end MF_Save_Structures('H', 'q', q, 'dq', dq, 'ddq', ddq, 'qc', qc, ... if abs(home_q - q0) == 0 %End-effector 'dqc', dqc, 'ddqc', ddqc, 'sp', sp, already in home position 'time',tv, 'tq', tq); MF_Update_Message(16, 'warnings'); %% return % Animate command else % Note: First the user will see the %Compute time using half of the command animation on the screen and maximum joint speed % then the motion is performed in time = max(abs(home_q - q0)) / the robot. For motion and animation (min(max_speed) / 4); % occuring at the same time, amend end this code using Multithreading MF_Animate_Commands(cn); %Call the respective trajectory function [q, dq, ddq, tv, sp] = % Drive Servos (Send command to robot PF_Interpolated_Traj(time, 'joint', it connected) home_q); simulation_only = S.value{'simul_only'}; APÊNDICE B. Rotinas em Matlab® do Software de Controle if ~simulation_only id = S.value{'robotnum'}; % Update Message box. pauset = S.value{'servo_pause'}; MF_Update_Message(4, 'notice'); baudnum = S.value{'baudnum'}; portnum = S.value{'portnum'}; % Update Transformation Matrix, sliders %Drive servos if NOT in simulate and other ui components mode. MF_Update_Cn(); %update command number PF_Driving_Actuators(id, q, dq, MF_Update_UI_Controls(); pauset, portnum, baudnum); end end %========================================================================== % >>>>>>>>> FUNCTION SF-2: MOVE ARM TO THE INPUTTED COORDINATE <<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will receive the user inputs from GUI and % process this to move the end-effector to a cartesian position and % orientation. This function works as a intermediate for the GUI (user % inputs) and the primary function responsible for generating the % trajectory.Refer to section 4 of documentation for details. %========================================================================== function SF_Move_EE_to_Pos(In) [q, dq, ddq, tv, sp] = % In structure should have the fields: PF_Interpolated_Traj(In.time, In.space, % In.trajopt, In.pos, In.time, In.lv, In.pos); In.av. %% Load files and variables case 2 S = evalin('base', 'S'); %Load [q, dq, ddq, tv, sp] = Settings (from base workspace) PF_Interpolated_wvp_Traj(In.pos, cn = S.value{'cn'}; %get the command In.time, In.lv, In.av); number from Settings structure %% % Check if coordinate is reachable and configuration mode used [is_allowed, is_reachable] = PF_Conf_Const_Reach('p', In.pos); if ~is_reachable MF_Update_Message(1, 'warnings'); return elseif ~is_allowed MF_Update_Message(2, 'warnings'); return end % Check if the coordinate is in a collision rote. collision_detected = PF_Collision_Check (In.pos); %Implement Collision_Check in future versions. if collision_detected MF_Update_Message(6, 'warnings'); return end %Call the respective trajectory function switch In.trajopt case 1 case 3 [q, dq, ddq, tv, sp] = PF_Uncoodinated_Traj(In.av, In.space, In.pos); case 4 [q, dq, ddq, tv, sp] = PF_Sequential_cav_Traj(In.av, In.space, In.pos); case 5 [q, dq, ddq, tv, sp] = PF_Sequential_ti_Traj(In.time, In.space, In.pos); case 6 [q, dq, ddq, tv, sp] = PF_StraightL_clv_Traj(In.pos, In.lv); case 7 [q, dq, ddq, tv, sp] = PF_StraightL_ti_Traj(In.pos, In.time); otherwise MF_Update_Message(7, 'warnings'); return end % If dynamics is enabled, compute torque APÊNDICE B. Rotinas em Matlab® do Software de Controle enable_dynamics = S.value{'enable_dynamics'}; if enable_dynamics tq = PF_Inverse_Dynamics(q, dq, ddq); %get the computed torque % then the motion is performed in the robot. For motion and animation % occuring at the same time, amend this code using Multithreading MF_Animate_Commands(cn) % If control system is enabled, simulate the result of the control % Drive Servos (Send command to robot enable_control = it connected) S.value{'enable_control'}; simulation_only = if enable_control S.value{'simul_only'}; tqc = PF_Robot_Control(q, dq, if ~simulation_only ddq, tq); %get the control system id = S.value{'robotnum'}; torque pauset = S.value{'servo_pause'}; [qc, dqc, ddqc] = baudnum = S.value{'baudnum'}; PF_Forward_Dynamics(tqc); portnum = S.value{'portnum'}; else %Drive servos if NOT in simulate qc = []; dqc = []; ddqc = []; mode. end PF_Driving_Actuators(id, q, dq, else pauset, portnum, baudnum); qc = []; dqc = []; ddqc = []; tq = end []; tqc = []; end % Update Message box. %% Save outputs into the History msg_in = num2cell(In.pos(end,1:3)); structure %messages inputs MF_Save_Structures('H', 'q', q, 'dq', MF_Update_Message(1, 'notice', msg_in); dq, 'ddq', ddq, 'qc', qc, ... 'dqc', dqc, 'ddqc', ddqc, 'sp', sp, % Update Transformation Matrix, sliders 'time',tv, 'tq', tq); and other ui components %% MF_Update_Cn(); %update command number % Animate command MF_Update_UI_Controls(); % Note: First the user will see the end command animation on the screen and %========================================================================== % >>>>>>>>>>> FUNCTION SF-3: MOVE END-EFFECTOR BY INCREMENT <<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will receive the user inputs from GUI and % process these to move the end-effector to a cartesian position and % orientation. The user will provide an increment (either from Settings or % the value itself) that will be added to the current position. This % function works as a intermediate for the GUI (user inputs) and the % primary function responsible for generating the trajectory. Refer to % section 4 of documentation for details. %========================================================================== function SF_Move_EE_by_Increment(In) try % In.inc_opt, In.axes, In.trajopt, pos = H(cn-1).p(end,:); %get In.lv, In.av, In.time. current position from history %% Load files and variables catch S = evalin('base', 'S'); %Load %compute position by FK Settings (from base workspace) RP = evalin('base', 'RP'); H = evalin('base', 'H'); %Load %Load Robot Parameters History (from base workspace) a = RP.a'; d = RP.d'; alpha = cn = S.value{'cn'}; %get the command RP.alpha'; number from Settings structure [pos, ~] = PF_Forward_Kinematics(H(cn-1).q(end,:), d, a, alpha); % get current position pos = [pos, if cn == 1 S.value{'home_p'}(4:6)]; pos = S.value{'home_p'}; end else APÊNDICE B. Rotinas em Matlab® do Software de Controle end %% % increm is a 1x3 vector with the increment in each axes X,Y,Z try if strcmp(In.inc_opt, 'coarse'); increm = S.value{'increm_coarse'}; elseif strcmp(In.inc_opt, 'fine'); increm = S.value{'increm_fine'}; end increm = increm * In.axes; %Apply increment in the selected axes (x, y or z) catch increm = In.increm; end % Update target position pos(1:3) = pos(1:3) + increm; % Check if coordinate is reachable and configuration mode used [is_allowed, is_reachable] = PF_Conf_Const_Reach('p', pos); if ~is_reachable MF_Update_Message(1, 'warnings'); return elseif ~is_allowed MF_Update_Message(2, 'warnings'); return end % Check if the coordinate is in a collision rote. collision_detected = PF_Collision_Check (pos); %Implement Collision_Check in future versions. if collision_detected MF_Update_Message(6, 'warnings'); return end %Call the respective trajectory function switch In.trajopt case 1 [q, dq, ddq, tv, sp] = PF_Interpolated_Traj(In.time, In.space, pos); case 3 [q, dq, ddq, tv, sp] = PF_Uncoodinated_Traj(In.av, In.space, pos); case 4 [q, dq, ddq, tv, sp] = PF_Sequential_cav_Traj(In.av, In.space, pos); case 5 [q, dq, ddq, tv, sp] = PF_Sequential_ti_Traj(In.time, In.space, pos); case 6 [q, dq, ddq, tv, sp] = PF_StraightL_clv_Traj(pos, In.lv); case 7 [q, dq, ddq, tv, sp] = PF_StraightL_ti_Traj(pos, In.time); otherwise MF_Update_Message(7, 'warnings'); return end % If dynamics is enabled, compute torque enable_dynamics = S.value{'enable_dynamics'}; if enable_dynamics tq = PF_Inverse_Dynamics(q, dq, ddq); %get the computed torque % If control system is enabled, simulate the result of the control enable_control = S.value{'enable_control'}; if enable_control tqc = PF_Robot_Control(q, dq, ddq, tq); %get the control system torque [qc, dqc, ddqc] = PF_Forward_Dynamics(tqc); else qc = []; dqc = []; ddqc = []; end else qc = []; dqc = []; ddqc = []; tq = []; tqc = []; end %% Save outputs into the History structure MF_Save_Structures('H', 'q', q, 'dq', dq, 'ddq', ddq, 'qc', qc, ... 'dqc', dqc, 'ddqc', ddqc, 'sp', sp, 'time',tv, 'tq', tq); %% % Animate command % Note: First the user will see the command animation on the screen and % then the motion is performed in the robot. For motion and animation % occuring at the same time, amend this code using Multithreading APÊNDICE B. Rotinas em Matlab® do Software de Controle MF_Animate_Commands(cn) end % Update Message box. axis = ['x', 'y', 'z']; % Drive Servos (Send command to robot c = find(increm~=0); it connected) msg_inp = {axis(c), simulation_only = num2str(increm(c))}; %message inputs S.value{'simul_only'}; MF_Update_Message(3, 'notice', if ~simulation_only msg_inp); id = S.value{'robotnum'}; pauset = S.value{'servo_pause'}; baudnum = S.value{'baudnum'}; % Update Transformation Matrix, sliders portnum = S.value{'portnum'}; and other ui components %Drive servos if NOT in simulate MF_Update_Cn(); %update command number mode. MF_Update_UI_Controls(); PF_Driving_Actuators(id, q, dq, end pauset, portnum, baudnum); %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION SF-4: MOVE JOINTS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will receive the user inputs from GUI and % process these to move the joint specified. The function accept values for % moving one single joint or a vector for moving several joints at once. % This function works as a intermediate for the GUI (user inputs) and the % primary function responsible for generating the trajectory. Refer to % section 4 of documentation for details. %========================================================================== function SF_Move_Joints(In) % Compute target cartesian position % In.av, In.time, In.qt, In.joint, (given the target theta) In.trajopt [pos, ~] = PF_Forward_Kinematics(qt, d, % In.qt: target joint variable a, alpha); % In.joint = joint number that the movement will be made (scalar or a %Note: The variables: is_allowed, vector) is_reachable are not used here because %% Load files and variables the S = evalin('base', 'S'); %Load %user is using the joints sliders with Settings (from base workspace) the limits imposed in Settings RP = evalin('base', 'RP'); %Load Robot Parameters % Check if the coordinate is in a H = evalin('base', 'H'); %Load collision rote. History (from base workspace) collision_detected = PF_Collision_Check cn = S.value{'cn'}; %get the command (pos); number from Settings structure %Implement Collision_Check in future a = RP.a; d = RP.d; alpha = RP.alpha; %D-H parameters %% if cn == 1 qc = S.value{'home_q'}; else qc = H(cn-1).q(end,:); end qt = qc; %set target joint variable to current q qt(In.joint) = In.qt;%update target q with the inputs (only in the In.joint col) versions. if collision_detected MF_Update_Message(6, 'warnings'); MF_Update_Message(9, 'warnings'); return end %Call the respective trajectory function switch In.trajopt case 1 [q, dq, ddq, tv, sp] = PF_Interpolated_Traj(In.time, 'joint', qt); case 3 APÊNDICE B. Rotinas em Matlab® do Software de Controle [q, dq, ddq, tv, sp] = PF_Uncoodinated_Traj(In.av, 'joint', qt); case 4 [q, dq, ddq, tv, sp] = PF_Sequential_cav_Traj(In.av, 'joint', qt); case 5 [q, dq, ddq, tv, sp] = PF_Sequential_ti_Traj(In.time, 'joint', qt); otherwise MF_Update_Message(7, 'warnings'); return end % If dynamics is enabled, compute torque enable_dynamics = S.value{'enable_dynamics'}; if enable_dynamics tq = PF_Inverse_Dynamics(q, dq, ddq); %get the computed torque MF_Save_Structures('H', 'q', q, 'dq', dq, 'ddq', ddq, 'qc', qc, ... 'dqc', dqc, 'ddqc', ddqc, 'sp', sp, 'time',tv, 'tq', tq); %% % Animate command % Note: First the user will see the command animation on the screen and % then the motion is performed in the robot. For motion and animation % occuring at the same time, amend this code using Multithreading MF_Animate_Commands(cn) % Drive Servos (Send command to robot it connected) simulation_only = S.value{'simul_only'}; if ~simulation_only id = S.value{'robotnum'}; pauset = S.value{'servo_pause'}; baudnum = S.value{'baudnum'}; portnum = S.value{'portnum'}; %Drive servos if NOT in simulate mode. PF_Driving_Actuators(id, q, dq, pauset, portnum, baudnum); end % If control system is enabled, simulate the result of the control % Update Message box. enable_control = m_inputs = {num2str(In.joint), S.value{'enable_control'}; num2str(In.qt)}; %messages inputs if enable_control MF_Update_Message(5, 'notice', tqc = PF_Robot_Control(q, dq, m_inputs); ddq, tq); %get the control system torque % Update Transformation Matrix, sliders [qc, dqc, ddqc] = and other ui components PF_Forward_Dynamics(tqc); MF_Update_UI_Controls(); else qc = []; dqc = []; ddqc = []; end % Update Transformation Matrix, sliders else and other ui components qc = []; dqc = []; ddqc = []; tq = MF_Update_Cn(); %update command number []; tqc = []; MF_Update_UI_Controls(); end end %% Save outputs into the History structure %========================================================================== % >>>>>>>>>>>>>> FUNCTION SF-6: MOVE END-EFFECTOR BY TABLE <<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % % % % % % % % DESCRIPTION: This function will receive the user inputs from GUI and process these to move manipulator through a series of via points. The table is loaded from a Microsoft Excel spreadsheet. The user have to specify the time for trajectory completion. Each coordinate is converted to joint position using the respective primary function. This function works as a intermediate for the GUI (user inputs) and the primary function responsible for generating the trajectory. Refer to section 6.4.1 for details. APÊNDICE B. Rotinas em Matlab® do Software de Controle %========================================================================== function SF_Move_by_Table(In) % Check if the coordinate is in a % In.time, In.lv collision rote. % In: {1}: 'command' or 'program' - the collision_detected = PF_Collision_Check tab origin that called this (TP); % function, if it was called in command %Implement Collision_Check in future tab, then load the table, otherwise versions. % it is not necessary to load the if collision_detected table. MF_Update_Message(6, 'warnings'); % {2}: either 'time' or 'clv' return => constant linear velocity end % {3}: value: time or clv (scalar) %% Loading variables S = evalin('base', 'S'); %Load Settings (from base workspace) RP = evalin('base', 'RP'); %Load Robot Parameters (from base workspace) H = evalin('base', 'H'); %Load History (from base workspace) cn = S.value{'cn'}; %get the command number from Settings structure try %Attempt to load Table of Points (from base workspace) TP = evalin('base', 'TP'); catch MF_Update_Message(17, 'warnings'); end %% % TO DO: the way it is programmed (loading the TP from base workspace, it % is possible to have only 1 trajectory by table per Program. Change the % way it is importing the TP so then the user can insert as many traj by % table as necessary in Program tab % Make sure all rows are number if ~isnumeric(TP) MF_Update_Message(14, 'warnings'); return end % Check if coordinate is reachable and configuration mode used [is_allowed, is_reachable] = PF_Conf_Const_Reach('p', TP); if ~is_reachable MF_Update_Message(1, 'warnings'); return elseif ~is_allowed MF_Update_Message(2, 'warnings'); return end %To do list: % When the path is loaded, show all points of the path in the graphical % window (independet of trail option). if In.trajopt == 12 && ~isempty(In.time) ti = In.time; [q, dq, ddq, tv, sp] = PF_Table_ti_Traj(TP, ti); elseif In.trajopt == 11 && ~isempty(In.lv) clv = In.lv; [q, dq, ddq, tv, sp] = PF_Table_clv_Traj(TP, clv); else MF_Update_Message(23, 'warnings'); return end %% %PS: The parametric equation given by the user may result in a start %position very far away from the current position, so it is necessary to %first move the end-effector to the first point given by the parametric %equation and then start the trajectory itself, it is done by a coordinated %trajectory using the medium speed of the joints (such as in MOVE_HOME fcn) % Create a smooth trajectory between first point of parametric trajectory % and current position. max_speed = RP.max_speed; % Get current joint variables to compute time if cn == 1 %End-effector already in home position q0 = S.value{'home_q'}; else q0 = H(cn - 1).q(end,:); end APÊNDICE B. Rotinas em Matlab® do Software de Controle if any(abs(q(1,:) - q0) ~= 0) % joints not in starting point %Compute time using half of the maximum joint speed time = max(abs(q(1,:) - q0)) / (min(max_speed) / 4); %Call interpolated trajectory and get pre-trajectory values [qp, dqp, ddqp, tvp, spp] = PF_Interpolated_Traj(time,'joint', q(1,:)); % Update variables (q, dq, ddq, tv, sp): merge both trajectories q = [qp; q]; dq = [dqp; dq]; ddq = [ddqp; ddq]; sp = spp + sp; tv = linspace(0, (tvp(end,1) + tv(end, 1)), sp)'; end %% % If dynamics is enabled, compute torque enable_dynamics = S.value{'enable_dynamics'}; if enable_dynamics tq = PF_Inverse_Dynamics(q, dq, ddq); %get the computed torque qc = []; dqc = []; ddqc = []; tq = []; tqc = []; end %% Save outputs into the History structure MF_Save_Structures('H', 'q', q, 'dq', dq, 'ddq', ddq, 'qc', qc, ... 'dqc', dqc, 'ddqc', ddqc, 'sp', sp, 'time',tv, 'tq', tq); % Animate command % Note: First the user will see the command animation on the screen and % then the motion is performed in the robot. For motion and animation % occuring at the same time, amend this code using Multithreading MF_Animate_Commands(cn) % Drive Servos (Send command to robot it connected) simulation_only = S.value{'simul_only'}; if ~simulation_only id = S.value{'robotnum'}; pauset = S.value{'servo_pause'}; baudnum = S.value{'baudnum'}; portnum = S.value{'portnum'}; %Drive servos if NOT in simulate mode. PF_Driving_Actuators(id, q, dq, pauset, portnum, baudnum); end % If control system is enabled, simulate the result of the control enable_control = S.value{'enable_control'}; if enable_control % Update Message box. tqc = PF_Robot_Control(q, dq, MF_Update_Message(11, 'notice'); ddq, tq); %get the control system torque % Update Transformation Matrix, sliders [qc, dqc, ddqc] = and other ui components PF_Forward_Dynamics(tqc); MF_Update_Cn(); %update command number else MF_Update_UI_Controls(); qc = []; dqc = []; ddqc = []; end end else %========================================================================== % >>>>>>>>> FUNCTION SF-7: MOVE ARM BY PARAMETRIC TRAJECTORY <<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will receive the user inputs from GUI and % process these to move manipulator through a parametric curve in cartesian % space or a curve in joint space. This function works as a intermediate % for the GUI (user inputs) and the primary function responsible for % generating the trajectory. Refer to section 6.4.1 for details. %========================================================================== function SF_Move_by_Parametric_Eq(In) %% Load files and In.avriables % In.trajopt, In.pex, In.pey, In.pez, S = evalin('base', 'S'); %Load In.pej, In.joint, In.av, In.lv, Settings (from base workspace) % In.pos, In.time, In.space APÊNDICE B. Rotinas em Matlab® do Software de Controle RP = evalin('base', 'RP'); %Load Robot Parameters (from base workspace) H = evalin('base', 'H'); %Load History (from base workspace) cn = S.value{'cn'}; %get the command number from Settings structure if any(size(In.time) > 1) delt = abs(In.time(2) In.time(1)); else delt = In.time(1); end %% Call the respective trajectory function if strcmp(In.space, 'cart') %get the input space (cartesian space) orient = In.pos(4:6); if ~ischar(In.pex) || ~ischar(In.pey) || ~ischar(In.pez) MF_Update_Message(11, 'warnings'); return elseif In.trajopt == 10 [q, dq, ddq, tv, sp] = PF_Parametrised_ti_Traj(In.pex, In.pey, ... In.pez, delt, orient); elseif In.trajopt == 8 [q, dq, ddq, tv, sp] = PF_Parametrised_clv_Traj(In.pex, In.pey,... In.pez, In.lv, delt, orient); else MF_Update_Message(12, 'warning'); return end elseif strcmp(In.space, 'joint') %get the input space (joint space) if ~ischar(In.pej) MF_Update_Message(11, 'warnings'); return elseif In.trajopt == 9 [q, dq, ddq, tv, sp] = PF_Parametrised_cav_Traj(In.pej, In.joint, In.av, delt); elseif In.trajopt == 10 [q, dq, ddq, tv, sp] = PF_Parametrised_J_ti_Traj(In.pej, In.joint, delt); else MF_Update_Message(12, 'warning'); return end end %% %PS: The parametric equation given by the user may result in a start %position very far away from the current position, so it is necessary to %first move the end-effector to the first point given by the parametric %equation and then start the trajectory itself, it is done by a coordinated %trajectory using the medium speed of the joints (such as in MOVE_HOME fcn) % Create a smooth trajectory between first point of parametric trajectory % and current position. max_speed = RP.max_speed; % Get current joint variables to compute time if cn == 1 %End-effector already in home position q0 = S.value{'home_q'}; else q0 = H(cn - 1).q(end,:); end if any(abs(q(1,:) - q0) ~= 0) % joints not in starting point %Compute time using half of the maximum joint speed time = max(abs(q(1,:) - q0)) / (min(max_speed) / 4); %Call interpolated trajectory and get pre-trajectory values [qp, dqp, ddqp, tvp, spp] = PF_Interpolated_Traj(time,'joint', q(1,:)); % Update variables (q, dq, ddq, tv, sp): merge both trajectories q = [qp; q]; dq = [dqp; dq]; ddq = [ddqp; ddq]; sp = spp + sp; tv = linspace(0, (tvp(end,1) + tv(end, 1)), sp)'; end %% % Check if coordinate is reachable and configuration mode used [is_allowed, is_reachable] = PF_Conf_Const_Reach('q', q); if ~is_reachable APÊNDICE B. Rotinas em Matlab® do Software de Controle MF_Update_Message(1, 'warnings'); return elseif ~is_allowed MF_Update_Message(2, 'warnings'); return end % Check if the coordinate is in a collision rote. collision_detected = PF_Collision_Check ('q', q); %Implement Collision_Check in future versions. if collision_detected MF_Update_Message(6, 'warnings'); return end % If dynamics is enabled, compute torque enable_dynamics = S.value{'enable_dynamics'}; if enable_dynamics tq = PF_Inverse_Dynamics(q, dq, ddq); %get the computed torque end %% Save outputs into the History structure MF_Save_Structures('H', 'q', q, 'dq', dq, 'ddq', ddq, 'qc', qc, ... 'dqc', dqc, 'ddqc', ddqc, 'sp', sp, 'time',tv, 'tq', tq); %% % Animate command % Note: First the user will see the command animation on the screen and % then the motion is performed in the robot. For motion and animation % occuring at the same time, amend this code using Multithreading MF_Animate_Commands(cn) % Drive Servos (Send command to robot it connected) simulation_only = S.value{'simul_only'}; if ~simulation_only id = S.value{'robotnum'}; pauset = S.value{'servo_pause'}; baudnum = S.value{'baudnum'}; portnum = S.value{'portnum'}; %Drive servos if NOT in simulate mode. PF_Driving_Actuators(id, q, dq, pauset, portnum, baudnum); end % If control system is enabled, simulate the result of the control enable_control = S.value{'enable_control'}; if enable_control % Update Message box. tqc = PF_Robot_Control(q, dq, MF_Update_Message(6, 'notice'); ddq, tq); %get the control system torque % Update Transformation Matrix, sliders [qc, dqc, ddqc] = and other ui components PF_Forward_Dynamics(tqc); MF_Update_Cn(); %update command number else MF_Update_UI_Controls(); qc = []; dqc = []; ddqc = []; end end else qc = []; dqc = []; ddqc = []; tq = []; tqc = []; %========================================================================== % >>>>>>>>>>>>>>>>>>> FUNCTION SF-8: RANDOM POSITION <<<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will generate a set of random cartesian % positions and call the interpolated trajectory function to generate the % trajectory. The user is asked if the command should or not be sent to the % robot (if connected) % . Refer to section 4 of documentation for details. %========================================================================== function SF_Random_Movement() S = evalin('base', 'S'); %Load % In structure should have the fields: Settings (from base workspace) % In.trajopt, In.pos, In.time, In.lv, RP = evalin('base', 'RP'); %Load Robot In.av. Parameters %% Load files and variables APÊNDICE B. Rotinas em Matlab® do Software de Controle cn = S.value{'cn'}; %get the command number from Settings structure %% Assembling random inputs nvp = ceil(rand(1)*5); %number of via points In.pos = ((rand(nvp, 3) - 0.5) * 1.5) * S.value{'max_reach_p'}; In.pos(:,3) = abs(In.pos(:,3)); %making Z values all positive In.pos(:, 4:6) = rand(nvp, 3) * 90; %orientation In.time = rand(1, nvp) * 20; In.lv = rand(1, nvp) * (max(abs(RP.a)) / (max(RP.max_speed)/10)); In.av = rand(1, nvp) * (max(RP.max_speed)/10); %% Check if coordinate is reachable and configuration mode used [is_allowed, is_reachable] = PF_Conf_Const_Reach('p', In.pos); if ~is_reachable MF_Update_Message(1, 'warnings'); return elseif ~is_allowed MF_Update_Message(2, 'warnings'); return end % Check if the coordinate is in a collision rote. collision_detected = PF_Collision_Check (In.pos); %Implement Collision_Check in future versions. if collision_detected MF_Update_Message(6, 'warnings'); return end %Call the respective trajectory function [q, dq, ddq, tv, sp] = PF_Interpolated_wvp_Traj(In.pos, In.time, In.lv, In.av); % If dynamics is enabled, compute torque enable_dynamics = S.value{'enable_dynamics'}; if enable_dynamics tq = PF_Inverse_Dynamics(q, dq, ddq); %get the computed torque enable_control = S.value{'enable_control'}; if enable_control tqc = PF_Robot_Control(q, dq, ddq, tq); %get the control system torque [qc, dqc, ddqc] = PF_Forward_Dynamics(tqc); else qc = []; dqc = []; ddqc = []; end else qc = []; dqc = []; ddqc = []; tq = []; tqc = []; end %% Save outputs into the History structure MF_Save_Structures('H', 'q', q, 'dq', dq, 'ddq', ddq, 'qc', qc, ... 'dqc', dqc, 'ddqc', ddqc, 'sp', sp, 'time',tv, 'tq', tq); %% % Animate command % Note: First the user will see the command animation on the screen and % then the motion is performed in the robot. For motion and animation % occuring at the same time, amend this code using Multithreading MF_Animate_Commands(cn) % Drive Servos (Send command to robot it connected) simulation_only = S.value{'simul_only'}; if ~simulation_only id = S.value{'robotnum'}; pauset = S.value{'servo_pause'}; baudnum = S.value{'baudnum'}; portnum = S.value{'portnum'}; %Drive servos if NOT in simulate mode. PF_Driving_Actuators(id, q, dq, pauset, portnum, baudnum); end % Update Message box. msg_in = num2cell(In.pos(end,1:3)); %messages inputs MF_Update_Message(1, 'notice', msg_in); % Update Transformation Matrix, sliders and other ui components MF_Update_Cn(); %update command number MF_Update_UI_Controls(); end % If control system is enabled, simulate the result of the control %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION SF-10: RUN PROGRAM <<<<<<<<<<<<<<<<<<< %========================================================================== APÊNDICE B. Rotinas em Matlab® do Software de Controle % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will read the programs table, generated in % Programs tab and for each row in this table call the respective command % function (secondary functions). % Refer to section 6.4.1 for details. %========================================================================== function SF_Run_Program() %0: The inst_variables will not get the value of that column from the table %% Loading and defining variables P = evalin('base', 'P'); %Load % 'Command','Position', 'Joint', Program table (P) from base workspace 'Trajectory Option', 'time', 'Lin vel', S = evalin('base', 'S'); %Load 'Ang vel' Settings from base workspace AS = evalin('base', 'AS'); %Load Additional Settings from base workspace %% cmd_list = AS.commands; %Get commands list in Additional Settings structure traj_list = AS.traj_opts;%Get trajectory list % Get the number of columns in P from AS structure nc = size(AS.program_table, 2); %Commands list: % 'Home Position' % 'Move to Coordinate' % 'Move by Increment' % 'Move Joints' % 'Parametric Cart. Traj.' % 'Parametric Joint Traj.' % 'Save actual Joints position' % 'Load Table of Coordinates' % 'Open Gripper' % 'Close Gripper' % 'Add a Pause' % 'Copy commands from History' %pgm_var allow the attribution of value from the program Table %to the cell array that holds all variables values for that specific command %for example: the Move to Coordinate will have the variables: %x, y, z, A, speed and path options enabled (the 1's in the first row below) % the cell array with all variables used for that specific function will be % passed to the Function that run that command, for command Move to % Coordinate, the function is Function 1: MOVE ARM TO THE INPUTTED % COORDINATE. %1: The inst_variables (cell array) will get the value from the table % Check if the program file is not empty; if isempty(P(:,1)) || size(P, 2) ~= nc MF_Update_Message(4, 'warnings'); return end % Check if the command is valid (all commands); cmd_r = []; %Will store the rows with problems. for i = 1:1:length(P(:,1)) %For each row in the program Table. %Get the command number cmd = find(strcmp(char(P(i,1)), cmd_list)); if isempty(cmd) continue end % In = []; %reset the inputs variables % Assembling the Input structure In(i).trajopt = find(strcmp(P{i, 2}{:}, traj_list)); In(i).time = P{i, 5}{:}; In(i).pos = P{i, 3}{:}; In(i).qt = P{i, 3}{:}; In(i).lv = P{i, 6}{:}; In(i).av = P{i, 7}{:}; In(i).increm = P{i, 3}{:}; In(i).pex = P{i, 3}{1}; In(i).pey = P{i, 3}{2}; In(i).pez = P{i, 3}{3}; In(i).pej = P{i, 3}{:}; In(i).space = P{i, 3}{1}; In(i).joint = P{i, 4}{:}; In(i).cn = P{i, 8}{:}; %Call function Check command Inputs to check if values inputted % in that row of the program table are OK. APÊNDICE B. Rotinas em Matlab® do Software de Controle try %Used here for eventual errors not caught in the Function (below) status = MF_Check_Command_Inputs(cmd, In(i)); %If the function that checks the inputs return false (found a problem), %then the handle below will take note of the row to display to the user. if ~status cmd_r = [cmd_r, i]; end catch cmd_r = [cmd_r, i]; end end % If there is a list with the problematic rows then stop this function % (Do not run the program until the user corrects the rows). if not(isempty(cmd_r)) %Display the rows with problems in the message box MF_Updating_Message(5,'warnings', num2str(cmd_r)); return %Exit (Return) function else MF_Updating_Message(2,'notice'); end % Call the respective function for each command for i = 1:1:length(P(:,1)) %For each row in the program Table. stop = S.value{'stop'}; %check if uesr pressed STOP button if stop MF_Update_Message(8, 'warnings'); MF_Update_Stop_status(false); %reseting stop status return end %Continue with function otherwise. case 2 %Move to Coordinate SF_Move_EE_to_Pos(In.(i)); case 3 %Move by Increment SF_Move_EE_by_Increment(In.(i)); case 4 %Move Joints SF_Move_Joints(In.(i)); case 5 %Parametric Cart. Traj. SF_Move_by_Parametric_Eq(In.(i)); case 6 %Parametric Joint Traj. SF_Move_by_Parametric_Eq(In.(i)); case 7 %Save actual Joints position SF_Move_Joints(In.(i)) %Note about this command: %When added to the table, the program will save the joints %angles, but when the program RUN, the command Save Joints %Angles will be transformed to: Move joints to that saved %angle (in this manner, this function will be similar to a %teach pendant). That is why it is calling Function 3 here. case 8 %Load Table of Coordinates SF_Move_by_Table(In.(i)); case 9 %Open Gripper SF_End_Effector_Control(In.(i)); case 10 %Close Gripper SF_End_Effector_Control(In.(i)); %Get the command number cmd = find(not(cellfun('isempty', strfind(cmd_list,char(P(i,1)))))); switch cmd case 1 %Home Position SF_Move_EE_to_Home(); case 11 %Add a Pause SF_Pause_Program(In.(i)); case 12 %Copy commands from History SF_Repeat_Commands(In.(i)); end end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION SF-11: PAUSE PROGRAM <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will pause the program by a determined time APÊNDICE B. Rotinas em Matlab® do Software de Controle % (in seconds). This funciton is used in Program tab, where a pause between % commands can be added by the user. % Refer to section 4 of documentation for details. %========================================================================== function SF_Pause_Program(In) end %Function making reference to Program Command: Add a pause if isnan(time) %Pause the program by the time inputted time = In.time; by the user. end tic if ischar(In.time) add_ti = toc; time = str2double(In.time); pause(time + add_ti); else %>>> END OF FUNCTION time = In.time; %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION SF-14: DYNAMIC SIMULATION <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - June 05th, 2016. % DESCRIPTION: This function will read the expressions in Torque Table and % create a table of torque and time that is passed as input to the primary % function Forward_Dynamics. Refer to section 4 of documentation for % details. %========================================================================== function SF_Dynamic_Simulation() joint = str2num(TT{i, %% Loading variables 1}{:}); try TT = evalin('base', 'TT'); % end Load Torque Table (from base workspace) else catch joint = TT{i, 1}{:}; return end end RP = evalin('base', 'RP'); % if ~isnumeric(TT{i,3}{:}) Load Robot Parameters time = str2num(TT{i,3}{:}); S = evalin('base', 'S'); % else Load Settings (from base workspace) time = TT{i,3}{:}; H = evalin('base', 'H'); % end Load Settings (from base workspace) expression = TT{i,2}{:}; cn = S.value{'cn'}; n = S.value{'dof'}; if cn == 1 q0 = S.value{'home_q'}; dq0 = zeros(1,n); else q0 = H(cn - 1).q(end,:); dq0 = H(cn - 1).dq(end,:); end ctrl_rate = S.value{'ctrl_rate'}; % Get control rate %% torquei{n} = []; %preallocating torque for i=1:size(TT,1) if ~isnumeric(TT{i, 1}{:}) try sp = ceil(time * ctrl_rate); Computing step points (sp) % tv = linspace(0,time,sp)'; %converting the string expression to symbolic expression = sym(expression); torquevar = symvar(expression); if isempty(torquevar) torquei{joint} = double(ones(sp,1).*expression); else %convert symbolic expr. to symb. function expression = symfun(expression, torquevar); %solving for tv entries APÊNDICE B. Rotinas em Matlab® do Software de Controle torquei{joint} = [torquei{joint};double(expression(tv))] ; end end nrows = zeros(1,6); for i=1:n nrows(i) = size(torquei{i},1); end tq = zeros(max(nrows),n); tv = linspace(0, (max(nrows)/ctrl_rate), max(nrows)); for i = 1:n tq(1:size(torquei{i},1), i) = torquei{i}; end % Call the primary function [tv, q, dq] = PF_Forward_Dynamics(max(tv), tq, deg2rad(q0), deg2rad(dq0)); q = rad2deg(q); dq = rad2deg(dq); sp = size(q,1); tm = [zeros(1,n); tv * ones(1, n)]; ddq = diff([zeros(1,n); dq])./diff(tm); ddq(1,:) = 0; %% % Animate command % Note: First the user will see the command animation on the screen and % then the motion is performed in the robot. For motion and animation % occuring at the same time, amend this code using Multithreading MF_Animate_Commands(cn, 'full') % Drive Servos (Send command to robot it connected) simulation_only = S.value{'simul_only'}; if ~simulation_only id = S.value{'robotnum'}; pauset = S.value{'servo_pause'}; baudnum = S.value{'baudnum'}; portnum = S.value{'portnum'}; %Drive servos if NOT in simulate mode. PF_Driving_Actuators(id, q, dq, pauset, portnum, baudnum); end qc = []; dqc = []; ddqc = []; % Update Message box. MF_Update_Message(13, 'notice'); %% Save outputs into the History structure MF_Save_Structures('H', 'q', q, 'dq', dq, 'ddq', ddq, 'qc', qc, ... 'dqc', dqc, 'ddqc', ddqc, 'sp', sp, 'time',tv, 'tq', tq); % Update Transformation Matrix, sliders and other ui components MF_Update_Cn(); %update command number MF_Update_UI_Controls(); end APÊNDICE B. Rotinas em Matlab® do Software de Controle 3 SMART_GUI.M E FUNÇÕES TERCIÁRIAS =========================================================================== %************************************************************************** % >>>>>>>>>>>>>>>>>>>>>>>>>>>>> SMART GUI <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< %************************************************************************** %========================================================================== %SOFTWARE FOR ARTICULATED ROBOTIC ARM SIMULATION %AUTHOR: DIEGO VARALDA DE ALMEIDA. %VERSION 2.0 %DATE: NOVEMBER 14nd OF 2015. %========================================================================== function varargout = gui_State = struct('gui_Name', SMART_GUI(varargin) mfilename, ... % clc; close all; clear variables; 'gui_Singleton', % SMART_GUI MATLAB code for gui_Singleton, ... SMART_GUI.fig 'gui_OpeningFcn', % SMART_GUI, by itself, creates a @SMART_GUI_OpeningFcn, ... new SMART_GUI or raises the existing 'gui_OutputFcn', % singleton*. @SMART_GUI_OutputFcn, ... % 'gui_LayoutFcn', [] % H = SMART_GUI returns the handle , ... to a new SMART_GUI or the handle to 'gui_Callback', % the existing singleton*. []); % if nargin && ischar(varargin{1}) % gui_State.gui_Callback = SMART_GUI('CALLBACK',hObject,eventData, str2func(varargin{1}); handles,...) calls the local end % function named CALLBACK in SMART_GUI.M with the given input if nargout arguments. [varargout{1:nargout}] = % gui_mainfcn(gui_State, varargin{:}); % else SMART_GUI('Property','Value',...) gui_mainfcn(gui_State, creates a new SMART_GUI or raises the varargin{:}); % existing singleton*. Starting end from the left, property value pairs are % End initialization code - DO NOT EDIT % applied to the GUI before SMART_GUI_OpeningFcn gets called. An %% --- Executes just before SMART_GUI % unrecognized property name or is made visible. invalid value makes property function SMART_GUI_OpeningFcn(hObject, application eventdata, handles, varargin) % stop. All inputs are passed to %Add the directory where GUI is and all SMART_GUI_OpeningFcn via varargin. subfolders to the search path. % Dir = pwd; % *See GUI Options on GUIDE's addpath(genpath(Dir),'-frozen'); Tools menu. Choose "GUI allows only %'-frozen' disables Windows change one notification % instance to run (singleton)". % assignin('base', 'HD', handles); % See also: GUIDE, GUIDATA, GUIHANDLES %Save handles as HD in base workspace % Edit the above text to modify the response to help SMART_GUI % Last Modified by GUIDE v2.5 05-Jun2016 18:02:17 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; %Configure Position of GUI form MF_Init_Smart_GUI(hObject, eventdata, handles, varargin); program_cmd_opts_Callback(handles); % %Load Settings table % MF_Load_Settings_table(); APÊNDICE B. Rotinas em Matlab® do Software de Controle % Choose default command line output for SMART_GUI handles.output = hObject; % Update handles structure guidata(hObject, handles); % UIWAIT makes SMART_GUI wait for user response (see UIRESUME) % uiwait(handles.smart_gui_fig); function varargout = SMART_GUI_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; % MF_Update_UI_Controls(); %Update sliders and static texts % % Create Robot Representation GUI (for displaying simulation) % MF_Creating_RR_GUI(); % MF_Update_Message(10, 'notice'); %Show message about software status %% % --- Outputs from this function are returned to the command line. ======================================================================== %************************************************************************** % >>>>>>>>>>>>>>>>>>> TERTIARY FUNCTIONS: CALLBACKS <<<<<<<<<<<<<<<<<<<<<< %************************************************************************** %========================================================================== %THE FUNCTIONS BELOW CONTROL THE ACTION OF EACH UI OBJECT. %% ======================================================================== % >>>>>>>>>>>>>>>>>>>>>>>>>>> TAB 1: SETTINGS <<<<<<<<<<<<<<<<<<<<<<<<<<<< %========================================================================== %% #ok function %% settings_table_CellSelectionCallback(hO function bject, eventdata, handles) load_settings_bt_Callback(hObject, indice = eventdata.Indices(); eventdata, handles) selec_row = double(indice(:,1)'); %Transpose of the indice matrix (Tranform %% %the first function column of the indice matrix into a row) reset_settings_bt_Callback(hObject, MF_Update_Message(selec_row, eventdata, handles) 'settings'); %>>> END OF FUNCTION %% #ok function function edit_robot_param_bt_Callback(hObject, settings_table_CellEditCallback(hObject eventdata, handles) , eventdata, handles) MF_Load_Robot_param_table(); %% %% #ok function function save_settings_bt_Callback(hObject, clear_history_bt_Callback(hObject, eventdata, handles) eventdata, handles) settings_data = MF_Clear_History(); get(handles.settings_table, 'Data'); MF_Save_Settings(settings_data); %>>> END OF FUNCTION %% ======================================================================== % >>>>>>>>>>>>>>>>>>>>>>>>>>> TAB 2: COMMANDS <<<<<<<<<<<<<<<<<<<<<<<<<<<< %========================================================================== x_pos = round(str2num(get(handles.p_x, 'string')),3); function undock_bt_Callback(hObject, y_pos = round(str2num(get(handles.p_y, eventdata, handles) 'string')),3); z_pos = round(str2num(get(handles.p_z, %% #ok 'string')),3); function move_coord_Callback(hObject, orient_angle = eventdata, handles) round(str2num(get(handles.orientation_a % Get the values from the edit box (X, ngle, 'string')),3); Y and Z); APÊNDICE B. Rotinas em Matlab® do Software de Controle time = round(str2num(get(handles.time, 'String')), 3); lv = round(str2num(get(handles.v_linear, 'String')), 3); av = round(str2num(get(handles.v_angular, 'String')), 3); % Check if the inputs are numbers if (isempty(x_pos) || isempty(y_pos)||isempty(z_pos)||isempty (orient_angle)) MF_Update_Message(19, 'warnings'); return end if isempty(time) && isempty(lv) && isempty(av) MF_Update_Message(23, 'warnings'); return end % Check if the number of inputs are the same for x, y and z if any(size(x_pos) ~= size(y_pos)) || any(size(x_pos) ~= size(z_pos)) || ... any(size(y_pos) ~= size(z_pos)) MF_Update_Message(20, 'warnings'); return end % Assembling pos matrix in case of more than 1 position pos = []; for i = 1:length(x_pos) pos = [pos; [x_pos(i), y_pos(i), z_pos(i), orient_angle(i, :)]]; end % Asembling inputs structure (In) In.trajopt = get(handles.traj_opt, 'Value'); In.time = time; In.space = 'cart'; %cartesian space In.pos = pos; In.lv = lv; In.av = av; % Call secondary function SF_Move_EE_to_Pos(In); %>>> END OF FUNCTION function slider_jointn_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor' )) set(hObject,'BackgroundColor',[.9 .9 .9]); end if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor' )) set(hObject,'BackgroundColor',[.9 .9 .9]); end if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor' )) set(hObject,'BackgroundColor',[.9 .9 .9]); end %% #ok function move_ee_neg_x_Callback(hObject, eventdata, handles) % Get the increment option (coarse, fine) coarse = get(handles.coarse_rb,'Value'); fine = get(handles.fine_rb,'Value'); % Assemble the inputs (In) if coarse ==1 && fine == 0 In.inc_opt = 'coarse'; else In.inc_opt = 'fine'; end In.space = 'cart'; %cartesian space In.axes = [-1 0 0]; %increment only in X (negative). In.trajopt = get(handles.traj_opt, 'Value'); In.lv = str2num(get(handles.v_linear, 'String')); In.av = str2num(get(handles.v_angular, 'String')); In.time = str2num(get(handles.time, 'String')); if isempty(In.time) && isempty(In.av) && isempty(In.lv) MF_Update_Message(23, 'warnings'); return end SF_Move_EE_by_Increment(In); %>>> END OF FUNCTION %% #ok function move_ee_pos_x_Callback(hObject, eventdata, handles) % Get the increment option (coarse, fine) APÊNDICE B. Rotinas em Matlab® do Software de Controle coarse = get(handles.coarse_rb,'Value'); fine = get(handles.fine_rb,'Value'); % Assemble the inputs (In) if coarse ==1 && fine == 0 In.inc_opt = 'coarse'; else In.inc_opt = 'fine'; end In.space = 'cart'; %cartesian space In.axes = [1 0 0]; %increment only in X (positive). In.trajopt = get(handles.traj_opt, 'Value'); In.lv = str2num(get(handles.v_linear, 'String')); In.av = str2num(get(handles.v_angular, 'String')); In.time = str2num(get(handles.time, 'String')); if isempty(In.time) && isempty(In.av) && isempty(In.lv) MF_Update_Message(23, 'warnings'); return end SF_Move_EE_by_Increment(In); %>>> END OF FUNCTION %% #ok function move_ee_neg_y_Callback(hObject, eventdata, handles) % Get the increment option (coarse, fine) coarse = get(handles.coarse_rb,'Value'); fine = get(handles.fine_rb,'Value'); % Assemble the inputs (In) if coarse ==1 && fine == 0 In.inc_opt = 'coarse'; else In.inc_opt = 'fine'; end In.space = 'cart'; %cartesian space In.axes = [0 -1 0]; %increment only in Y (negative). In.trajopt = get(handles.traj_opt, 'Value'); In.lv = str2num(get(handles.v_linear, 'String')); In.av = str2num(get(handles.v_angular, 'String')); In.time = str2num(get(handles.time, 'String')); if isempty(In.time) && isempty(In.av) && isempty(In.lv) MF_Update_Message(23, 'warnings'); return end SF_Move_EE_by_Increment(In); %>>> END OF FUNCTION %% #ok function move_ee_pos_y_Callback(hObject, eventdata, handles) % Get the increment option (coarse, fine) coarse = get(handles.coarse_rb,'Value'); fine = get(handles.fine_rb,'Value'); % Assemble the inputs (In) if coarse ==1 && fine == 0 In.inc_opt = 'coarse'; else In.inc_opt = 'fine'; end In.space = 'cart'; %cartesian space In.axes = [0 1 0]; %increment only in Y (positive). In.trajopt = get(handles.traj_opt, 'Value'); In.lv = str2num(get(handles.v_linear, 'String')); In.av = str2num(get(handles.v_angular, 'String')); In.time = str2num(get(handles.time, 'String')); if isempty(In.time) && isempty(In.av) && isempty(In.lv) MF_Update_Message(23, 'warnings'); return end SF_Move_EE_by_Increment(In); %>>> END OF FUNCTION %% #ok function move_ee_neg_z_Callback(hObject, eventdata, handles) % Get the increment option (coarse, fine) coarse = get(handles.coarse_rb,'Value'); fine = get(handles.fine_rb,'Value'); % Assemble the inputs (In) if coarse ==1 && fine == 0 In.inc_opt = 'coarse'; else In.inc_opt = 'fine'; end In.space = 'cart'; %cartesian space In.axes = [0 0 -1]; %increment only in Z (negative). In.trajopt = get(handles.traj_opt, 'Value'); In.lv = str2num(get(handles.v_linear, 'String')); APÊNDICE B. Rotinas em Matlab® do Software de Controle In.av = str2num(get(handles.v_angular, 'String')); In.time = str2num(get(handles.time, 'String')); if isempty(In.time) && isempty(In.av) && isempty(In.lv) MF_Update_Message(23, 'warnings'); return end SF_Move_EE_by_Increment(In); %>>> END OF FUNCTION %% #ok function move_ee_pos_z_Callback(hObject, eventdata, handles) % Get the increment option (coarse, fine) coarse = get(handles.coarse_rb,'Value'); fine = get(handles.fine_rb,'Value'); % Assemble the inputs (In) if coarse ==1 && fine == 0 In.inc_opt = 'coarse'; else In.inc_opt = 'fine'; end In.space = 'cart'; %cartesian space In.axes = [0 0 1]; %increment only in Z (positive). In.trajopt = get(handles.traj_opt, 'Value'); In.lv = str2num(get(handles.v_linear, 'String')); In.av = str2num(get(handles.v_angular, 'String')); In.time = str2num(get(handles.time, 'String')); if isempty(In.time) && isempty(In.av) && isempty(In.lv) MF_Update_Message(23, 'warnings'); return end SF_Move_EE_by_Increment(In); %>>> END OF FUNCTION %% #ok function slider_jointn_Callback(hObject, eventdata, handles) In.trajopt = get(handles.traj_opt, 'Value'); In.joint = get(handles.jointn_menu, 'Value'); In.qt = get(handles.slider_jointn, 'Value'); In.time = str2num(get(handles.time, 'String')); In.av = str2num(get(handles.v_angular, 'String')); In.lv = str2num(get(handles.v_linear, 'String')); SF_Move_Joints(In); %% #ok function move_joints_bt_Callback(hObject, eventdata, handles) % Get the values from the edit box (Joints and A); joints = round(str2num(get(handles.joints_move, 'String')),3); qvalue = round(str2num(get(handles.angle_move_jo ints, 'string')),3); % Check if the inputs are numbers if isempty(joints) || isempty(qvalue) MF_Update_Message(19, 'warnings'); return end % Check if the number of inputs are the same for joints and qvalue if size(joints) ~= size(qvalue) MF_Update_Message(21, 'warnings'); return end % Asembling inputs structure (In) In.trajopt = get(handles.traj_opt, 'Value'); In.time = str2num(get(handles.time, 'String')); In.joint = joints; In.qt = qvalue; In.av = str2num(get(handles.v_angular, 'String')); % Call secondary function SF_Move_Joints(In) %>>> END OF FUNCTION %% #ok function go_param_joint_Callback(hObject, eventdata, handles) % Get the values from the menu and text box (Joint and theta(t)); joint = get(handles.joint_param_eq_menu, 'Value'); pej = get(handles.param_joint_eq, 'string'); APÊNDICE B. Rotinas em Matlab® do Software de Controle % Check if the inputs are numbers if isempty(joint) || isempty(pej) MF_Update_Message(22, 'warnings'); return end % Asembling inputs structure (In) In.trajopt = get(handles.traj_opt, 'Value'); In.time = str2num(get(handles.time, 'String')); In.joint = joint; In.pej = pej; In.space = 'joint'; In.av = str2num(get(handles.v_angular, 'String')); In.lv = str2num(get(handles.v_linear, 'String')); % Call secondary function SF_Move_by_Parametric_Eq(In) %>>> END OF FUNCTION %% #ok function go_param_traj_Callback(hObject, eventdata, handles) % Get the values from the menu and text box (X(t), Y(t), Z(t)) pex = get(handles.x_param, 'string'); pey = get(handles.y_param, 'string'); pez = get(handles.z_param, 'string'); % Check if the fields are not blank if isempty(pex) || isempty(pey) || isempty(pez) MF_Update_Message(22, 'warnings'); return end % Asembling inputs structure (In) In.trajopt = get(handles.traj_opt, 'Value'); In.time = str2num(get(handles.time, 'String')); In.lv = str2num(get(handles.v_linear, 'String')); In.pex = pex; In.pey = pey; In.pez = pez; In.pos = zeros(1,6); %for orientation orient = str2num(get(handles.orientation_angle, 'String')); %get orientation if ~isempty(orient) try In.pos(4:6) = orient; end end In.space = 'cart'; % Call secondary function SF_Move_by_Parametric_Eq(In) %>>> END OF FUNCTION %% #ok function load_path_Callback(hObject, eventdata, handles) MF_Load_Table_Coord(); try %Attempt to load Table of Points (from base workspace) TP = evalin('base', 'TP'); % Create a figure and insert the table inside to display to the user TP_fig = figure('Name','TABLE OF COORDINATES'); RPT = uitable(TP_fig,'Data', TP); catch MF_Update_Message(17, 'warnings'); end %>>> END OF FUNCTION %% #ok function start_path_Callback(hObject, eventdata, handles) In.time = str2num(get(handles.time, 'String')); In.lv = str2num(get(handles.v_linear, 'String')); In.trajopt = get(handles.traj_opt, 'Value'); if isempty(In.time) && isempty(In.lv) MF_Update_Message(23, 'warnings'); return end SF_Move_by_Table(In); %% function open_gripper_bt_Callback(hObject, eventdata, handles) %FUNCTION DESCRIPTION: %1 - Get the default angle to close gripper; %2 - Get the speed for the gripper %2 - Call Function 4: Gripper Control joint = 5; handles.gripper_status = handles.gripper_status + 1; %>>>1: Get the default angle to close gripper (Theta_World) switch handles.gripper_status case 1 %Fully opened; set text for the next option set(hObject, 'String', 'Grab BB'); APÊNDICE B. Rotinas em Matlab® do Software de Controle joint_value = handles.gripper_fully_opened; case 2 %Grab Big Block; set text for the next option set(hObject, 'String', 'Grab SB'); joint_value = handles.gripper_open_bb; case 3 %Grab Small Block; set text for the next option set(hObject, 'String', 'Open'); joint_value = handles.gripper_open_sb; handles.gripper_status = 0; %Reset gripper button status end %% #ok function gripper_slider_Callback(hObject, eventdata, handles) %>>>1: Get the joint angle; gripper_value = round(get(hObject, 'Value'),1); %>>>2: Get the speed_slider of the servo joint_speed = handles.speed; %% function gripper_slider_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor' )) set(hObject,'BackgroundColor',[.9 .9 .9]); end %>>>3: Call Function 4: Gripper Control; function_variables = {joint_value, joint, joint_speed}; Gripper_Control(function_variables, eventdata, handles); % Update handles structure guidata(hObject, handles); %>>> END OF FUNCTION %% function close_gripper_bt_Callback(hObject, eventdata, handles) %FUNCTION DESCRIPTION: %1 - Get the default angle to close gripper; %2 - Get the speed for the gripper %2 - Call Function 4: Gripper Control joint = 5; %>>>1: Get the default angle to close gripper (Theta_World) joint_value = handles.gripper_close; %>>>2: Get the speed_slider of the servo joint_speed = handles.speed; %>>>3: Call Function 4: Gripper Control; function_variables = {joint_value, joint, joint_speed}; Gripper_Control(function_variables, eventdata, handles); %>>> END OF FUNCTION % Update the static text for angle; set(handles.gripper_angle_value, 'String', gripper_value); % Call Function Gripper Control; In.qt = gripper_value; SF_End_Effector_Control(In); %>>> END OF FUNCTION %% #ok function jointn_menu_Callback(hObject, eventdata, handles) MF_Update_UI_Controls(); %update maximum and minimum values of slider %% #ok function transformation_options_Callback(hObject , eventdata, handles) MF_Update_UI_Controls(); %update UI controls %% #ok function home_pos_Callback(hObject, eventdata, handles) SF_Move_EE_to_Home(); %% #ok function stop_robot_Callback(hObject, eventdata, handles) MF_Update_Stop_status(true); %% function update_joints_Callback(hObject, eventdata, handles) Update_Theta(eventdata, handles); APÊNDICE B. Rotinas em Matlab® do Software de Controle %% #ok function cmd_random_bt_Callback(hObject, eventdata, handles) SF_Random_Movement() set(handles.orientation_angle, 'String', ''); set(handles.time, 'String', ''); if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor' )) %% #ok function clear_field_Callback(hObject, set(hObject,'BackgroundColor','white'); eventdata, handles) end set(handles.p_x, 'String', ''); %>>> END OF FUNCTION set(handles.p_y, 'String', ''); set(handles.p_z, 'String', ''); %% ======================================================================== % >>>>>>>>>>>>>>>>>>>>>>>>>>> TAB 3: PROGRAMS <<<<<<<<<<<<<<<<<<<<<<<<<<<< %========================================================================== function 'program_pos_z', 'prg_orient_txt', program_cmd_opts_Callback(varargin) 'prg_orient','inst_time_txt',... if nargin == 3 'inst_time','prg_linvel_txt','prg_linve handles = varargin{3}; l','prg_angvel_txt','prg_angvel'};... elseif nargin == 1 handles = varargin{1}; %'Move Joints' end %Get the selected command from the pop{'inst_path_option_txt','program_traj_o up menu. pts', 'inst_time_txt',... cmd_sel = get(handles.program_cmd_opts, 'inst_time','inst_joints_txt', 'Value'); 'inst_joints', 'inst_A_txt', 'inst_A',... handle_obj = {'inst_path_option_txt', 'prg_angvel_txt','prg_angvel'};... 'program_traj_opts',... 'inst_save_grip_pos','inst_torque_cb',' %'Parametric Cart. Traj.' program_xpos_txt','program_pos_x',... 'program_ypos_txt','program_pos_y','pro {'inst_path_option_txt','program_traj_o gram_zpos_txt','program_pos_z',... pts','program_xpos_txt',... 'prg_orient_txt','prg_orient','inst_tim 'program_pos_x', 'program_ypos_txt', e_txt','inst_time','inst_joints_txt',.. 'program_pos_y','program_zpos_txt',... . 'program_pos_z', 'prg_orient_txt', 'inst_joints','inst_A_txt','inst_A','pr 'prg_orient','inst_time_txt',... g_linvel_txt','prg_linvel',... 'inst_time', 'prg_linvel_txt', 'prg_angvel_txt','prg_angvel','program_ 'prg_linvel'};... cn_txt','program_cn'}; %'Pametric Joint Traj.' cmd_enabled_field = { {};... %'Home Position' %'Move to Coordinate' {'inst_path_option_txt','program_traj_o pts','program_xpos_txt',... 'program_pos_x', 'program_ypos_txt', 'program_pos_y','program_zpos_txt',... 'program_pos_z', 'prg_orient_txt', 'prg_orient','inst_time_txt',... 'inst_time', 'prg_linvel_txt', 'prg_linvel'};... {'inst_path_option_txt','program_traj_o pts', 'inst_time_txt',... 'inst_time','inst_joints_txt', 'inst_joints', 'inst_A_txt', 'inst_A',... 'prg_angvel_txt','prg_angvel'};... %'Save actual Joints position' {'inst_path_option_txt','program_traj_o pts', 'inst_time_txt',... 'inst_time', 'prg_angvel_txt','prg_angvel'};... %'Move by Increment' {'inst_path_option_txt','program_traj_o pts','program_xpos_txt',... 'program_pos_x', 'program_ypos_txt', 'program_pos_y','program_zpos_txt',... %'Load Table of Coordinates' {'inst_path_option_txt','program_traj_o pts', 'inst_time_txt',... APÊNDICE B. Rotinas em Matlab® do Software de Controle 'inst_time', 'prg_linvel_txt', 'prg_linvel'};... %'Open Gripper' {'inst_path_option_txt','program_traj_o pts', 'inst_A_txt', 'inst_A',... 'inst_time_txt', 'inst_time', 'prg_angvel_txt','prg_angvel'};... {'Modelled - Table (CST linear vel.)', 'Modelled - Table (time)'};... %'Open Gripper' {'Interpolated', 'Uncoordinated'};... %'Close Gripper' {'Interpolated', 'Uncoordinated'};... %'Close Gripper' %'Add a Pause' {''};... {'inst_path_option_txt','program_traj_o pts', 'inst_time_txt',... 'inst_time', 'prg_angvel_txt','prg_angvel'};... %'Add a Pause' {'inst_time_txt', 'inst_time'};... %'Repeat commands in History' {'program_cn_txt', 'program_cn'}}; cmd_trajopts = { %'Home Position' {''}; %'Move to Coordinate' {'Interpolated', 'Interpolated w/ via pts', 'Uncoordinated',... 'Sequential (CST angular vel.)', 'Sequential (time)',... 'Straight line (CST linear vel.)', 'Straight line (time)'};... %'Move by Increment' {'Interpolated', 'Uncoordinated', 'Sequential (CST angular vel.)',... 'Sequential (time)','Straight line (CST linear vel.)','Straight line (time)'};... %'Move Joints' {'Interpolated', 'Interpolated w/ via pts', 'Uncoordinated',... 'Sequential (CST angular vel.)', 'Sequential (time)'};... %'Parametric Cart. Traj.' {'Parametrised (CST linear vel.)','Parametrised (time)'};... %'Pametric Joint Traj.' {'Parametrised (CST angular vel.)', 'Parametrised (time)'};... %'Save actual Joints position' {'Interpolated', 'Uncoordinated','Sequential (CST angular vel.)',... 'Sequential (time)'};... %'Load Table of Coordinates' %'Repeat commands in History' {''};... }; %Enable or disable each handle for i = 1:1:length(handle_obj) if any(strcmp(cmd_enabled_field{cmd_sel}, handle_obj{i})) set(handles.(handle_obj{i}), 'Enable', 'on'); else %Enable: off set(handles.(handle_obj{i}), 'Enable', 'off'); end end % Adjusting trajectory options based on the command selected set(handles.program_traj_opts, 'String', cmd_trajopts{cmd_sel}); %>>> END OF FUNCTION %% function add_inst_Callback(hObject, eventdata, handles) HD = handles; %create a copy of handles (just for shortening the name) AS = evalin('base', 'AS'); % Load Additional Settings ptvar = AS.program_table; %Program table variables %Get P table from base workspace try P = evalin('base', 'P'); % Load Program table (P) from base workspace if isempty(P{1,1}{:}) prow = 1; else prow = size(P,1) + 1; % get the first empty rows. end catch P = table(); % create an empty table APÊNDICE B. Rotinas em Matlab® do Software de Controle P.Properties.Description='Valid Smart GUI program table';%for validation for i =1:length(ptvar) P.(ptvar{i}){1,1} = []; end prow = 1; % set number of row to 1; end % Get selected command and trajectory cmd_list = AS.commands; traj_list = get(handles.program_traj_opts, 'String'); cmd_sel = cmd_list{get(handles.program_cmd_opts, 'Value')}; if ~isempty(traj_list) traj_sel = traj_list{get(handles.program_traj_opts , 'Value')}; else traj_sel = ''; end % All editable handle objects edit_handles = {'program_pos_x','program_pos_y','progr am_pos_z',... 'prg_orient','inst_time','inst_joints', 'inst_A','prg_linvel', ... 'prg_angvel','program_cn'}; %Get handles values (stored in hv) for i = 1:length(edit_handles) %try to convert to number value = str2num(get(HD.(edit_handles{i}), 'String')); if ~isempty(value) hv{i} = value; else hv{i} = get(HD.(edit_handles{i}), 'String'); end end P.(ptvar{1}){prow,1} = cmd_sel; %First column is always the P.(ptvar{2}){prow,1} = traj_sel; if strcmp(get(HD.('program_pos_x'), 'Enable'), 'on') && ... strcmp(get(HD.('inst_A'), 'Enable'), 'off') P.(ptvar{3}){prow,1} = hv(1:4); else P.(ptvar{3}){prow,1} = hv{7}; end P.(ptvar{4}){prow,1} = hv{6}; P.(ptvar{5}){prow,1} = hv{5}; P.(ptvar{6}){prow,1} = hv{8}; P.(ptvar{7}){prow,1} = hv{9}; P.(ptvar{8}){prow,1} = hv{10}; % Saving P in base workspace assignin('base', 'P', P); %Displaying P structure in uitable MF_Load_Program_table(P); % Cleaning all UI controls for i=1:length(edit_handles) set(HD.(edit_handles{i}), 'String', ''); end %>>> END OF FUNCTION %% function program_traj_opts_Callback(hObject, eventdata, handles) function program_traj_opts_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor' )) set(hObject,'BackgroundColor','white'); end function prg_clear_Callback(hObject, eventdata, handles) % Create a new empty P table AS = evalin('base', 'AS'); % Load Additional Settings ptvar = AS.program_table; %Program table variables P = evalin('base', 'P'); % Load P from base workspace % Ask user if the table should be saved before cleaned try if ~isempty(P) || size(P,1)>1 answer = questdlg('Do you want to save the program?',... 'Save confirmation'); if strcmp(answer,'Yes') uisave('P', 'Program_'); end end end APÊNDICE B. Rotinas em Matlab® do Software de Controle % Cleaning P P = table(); % create an empty table P.Properties.Description='Valid Smart GUI program table';%for validation for i =1:length(ptvar) P.(ptvar{i}){1,1} = []; end %Saving in base workspace assignin('base', 'P', P); %Updating uiTable MF_Load_Program_table(P); %>>> END OF FUNCTION %% #ok function program_table_CellSelectionCallback(hOb ject, eventdata, handles) pgr_ind = eventdata.Indices(); setappdata(0,'program_table_indice', pgr_ind); %% #ok function inst_moveup_Callback(hObject, eventdata, handles) %Get the table content from base workspace try P = evalin('base', 'P'); catch MF_Update_Message(29, 'warnings'); return end try pgr_ind = getappdata(0, 'program_table_indice'); pgr_row = pgr_ind(:, 1)'; catch return end if isempty(pgr_row) MF_Update_Message(30, 'warnings'); return end if pgr_row(1) == 1 return elseif pgr_row(end) > size(P,1) + 2 return elseif pgr_row(1) > 2 row_order = [1:pgr_row(1)-2, pgr_row, pgr_row(1)-1, ... pgr_row(end)+1:size(P,1)]; elseif pgr_row(1) == 2 row_order = [pgr_row, pgr_row(1)-1, pgr_row(end)+1:size(P,1)]; end %Reorganizing P P = P(row_order,:); %Saving in base workspace assignin('base', 'P', P); %Updating uiTable MF_Load_Program_table(P); %>>> END OF FUNCTION %% #ok function inst_movedown_Callback(hObject, eventdata, handles) %Get the table content from base workspace try P = evalin('base', 'P'); catch MF_Update_Message(29, 'warnings'); return end try pgr_ind = getappdata(0, 'program_table_indice'); pgr_row = pgr_ind(:, 1)'; catch return end if isempty(pgr_row) MF_Update_Message(30, 'warnings'); return end if pgr_row(end) == size(P,1) return elseif pgr_row(end) > size(P,1) + 2 return elseif pgr_row(1) ~= 1 row_order = [1:pgr_row(1)-1, pgr_row(end)+1, ... pgr_row, pgr_row(end)+2:size(P,1)]; elseif pgr_row(1) == 1 row_order = [pgr_row(end)+1, pgr_row, pgr_row(end)+2:size(P,1)]; end %Reorganizing P P = P(row_order,:); %Saving in base workspace assignin('base', 'P', P); APÊNDICE B. Rotinas em Matlab® do Software de Controle %Updating uiTable MF_Load_Program_table(P); %>>> END OF FUNCTION %% #ok function save_inst_Callback(hObject, eventdata, handles) %>>>1: Open window to select folder for saving file P = evalin('base', 'P'); %get Program table from base workspace uisave('P', 'Program_'); %>>> END OF FUNCTION %% #ok function load_inst_Callback(hObject, eventdata, handles) %Open dialog box for selecting file to load into workspace %'load' will display only .mat files uiopen('load') %Validate the table loaded (using P.Properties.Description) try valid_msg = P.Properties.Description; %compare both strings if strcmp(valid_msg, 'Valid Smart GUI program table'); isvalidfile = true; else isvalidfile = false; end catch isvalidfile = false; end % Save program table in workspace for future use if isvalidfile assignin('base', 'P', P); %save program table (P) in base workspace else MF_Update_Message(3, 'warnings'); return end % Display the loaded file in the program table handle MF_Load_Program_table(P); %>>> END OF FUNCTION %% #ok function run_inst_Callback(hObject, eventdata, handles) SF_Run_Program(); %>>> END OF FUNCTION %% #ok function delete_inst_Callback(hObject, eventdata, handles) %Get the table content from base workspace try P = evalin('base', 'P'); catch MF_Update_Message(29, 'warnings'); return end try pgr_ind = getappdata(0, 'program_table_indice'); pgr_row = pgr_ind(:, 1)'; catch return end P(pgr_row, :) = []; % Delete selected rows; %Saving in base workspace assignin('base', 'P', P); %Updating uiTable MF_Load_Program_table(P); %>>> END OF FUNCTION function inst_torque_cb_Callback(hObject, eventdata, handles) status = get(hObject, 'Value'); Torque_On_Off(handles.id, status, handles); %>>> END OF FUNCTION function stop_inst_Callback(hObject, eventdata, handles) global stop_state %stop_state needs to be a global variable because Function 10: Run %Instructions will update this value, and it cannot update handles %0: Let the instructions Run; 1:Prevent from running. stop_state = 1; %>>> END OF FUNCTION %% #ok function traj_opt_Callback(hObject, eventdata, handles) MF_Enable_Commands(hObject, handles); if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor' )) set(hObject,'BackgroundColor','white'); APÊNDICE B. Rotinas em Matlab® do Software de Controle end ======================================================================== % >>>>>>>>>>>>>>>>>>> TAB 4: DYNAMIC SIMULATION <<<<<<<<<<<<<<<<<<<<<<<<< %========================================================================== assignin('base', 'TT', TT); %% #ok function sim_add_bt_Callback(hObject, %Displaying TT structure in uitable eventdata, handles) set(HD.sim_functions_table,'Data',table HD = handles; %create a copy of 2cell(TT)); handles (just for shortening the name) % All editable handle objects edit_handles = {'sim_joint', 'sim_function', 'sim_time'}; expression = get(HD.(edit_handles{2}), 'String'); time = get(HD.(edit_handles{3}), 'String'); if isempty(expression) || isempty(time) return end ptvar = {'joint', 'expression', 'time'}; %Program table variables %Get TT table from base workspace try TT = evalin('base', 'TT'); % Load Torque table (TT) from base workspace if isempty(TT{1,1}{:}) prow = 1; else prow = size(TT,1) + 1; % get the first empty rows. end catch TT = table(); % create an empty table TT.Properties.Description='Valid Smart GUI Torque table';%for validation for i =1:size(ptvar,2) TT.(ptvar{i}){1,1} = []; end prow = 1; % set number of row to 1; end TT.(ptvar{1}){prow,1} = get(HD.(edit_handles{1}), 'Value'); TT.(ptvar{2}){prow,1} = expression; TT.(ptvar{3}){prow,1} = time; % Cleaning all UI controls set(HD.(edit_handles{2}), 'String', ''); set(HD.(edit_handles{3}), 'String', ''); %% function sim_clear_table_bt_Callback(hObject, eventdata, handles) TT = table(); assignin('base', 'TT', TT); set(handles.sim_functions_table,'Data', []); %% #ok function sim_move_to_bt_Callback(hObject, eventdata, handles) cart_pos = get(handles.sim_cart_pos, 'String'); joint_pos= get(handles.sim_joint_pos, 'String'); if ~isempty(joint_pos) In.space = 'joint'; %joint space In.pos = str2num(joint_pos); elseif ~isempty(cart_pos) && isempty(joint_pos) In.space = 'cart'; %cartesian space In.pos = str2num(cart_pos); else return end In.trajopt = 1; %coordinated trajectory In.time = 10; SF_Move_EE_to_Pos(In); %% #ok function sim_begin_bt_Callback(hObject, eventdata, handles) SF_Dynamic_Simulation(); % Saving P in base workspace ======================================================================== % >>>>>>>>>>>>>>>>>>>>>>>>>>> CONTEXT MENU <<<<<<<<<<<<<<<<<<<<<<<<<<<<<< %========================================================================== function S = evalin('base', 'S'); %Load repeat_last_cmd_Callback(hObject, Settings (from base workspace) eventdata, handles) cn = S.value{'cn'}; APÊNDICE B. Rotinas em Matlab® do Software de Controle if cn ~= 1 MF_Animate_Commands(cn - 1); end function clear_trail_Callback(hObject, eventdata, handles) %Clear trail setappdata(0,'xtrail',[]); setappdata(0,'ytrail', []); setappdata(0,'ztrail',[]); Tr = plot3(0,0,0,'r', 'LineWidth',2); set(Tr,'xdata',[],'ydata',[],'zdata',[] ); % ------------------------------------function display_cg_Callback(hObject, eventdata, handles) ischecked = get(hObject, 'Checked'); if strcmp(ischecked, 'on') set(hObject, 'Checked', 'off'); MF_Plot_CG_frames(0, false); % delete axes else set(hObject, 'Checked', 'on'); MF_Plot_CG_frames(0, true); % display axes end % ------------------------------------function disp_ref_frames_Callback(hObject, eventdata, handles) ischecked = get(hObject, 'Checked'); if strcmp(ischecked, 'on') set(hObject, 'Checked', 'off'); MF_Plot_Base_frame(false); % delete axes else set(hObject, 'Checked', 'on'); MF_Plot_Base_frame(true); % display axes end % ------------------------------------function disp_link_frame_Callback(hObject, eventdata, handles) ischecked = get(hObject, 'Checked'); if strcmp(ischecked, 'on') set(hObject, 'Checked', 'off'); MF_Plot_Links_frame(0, false); delete axes else set(hObject, 'Checked', 'on'); MF_Plot_Links_frame(0, true); display axes end % % % ------------------------------------------------------------------function plot_graphs_Callback(hObject, eventdata, handles) S = evalin('base', 'S'); %Load Settings (from base workspace) cn = S.value{'cn'} - 1; %get the last command number MF_Plot_Graphs(cn); APÊNDICE B. Rotinas em Matlab® do Software de Controle 4 FUNÇÕES MISCELÂNEAS %========================================================================== % >>>>>>>>>>>>>>>>> FUNCTION MF-1: INITIALISE SMART GUI <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will load the joints trajectory from History % and animate it in the Animation window by rotating the LD matrices that % contain the vertices and patches got from SF_Importing_CAD. The rotation % is made using the Transform matrix; the animation will have fps frames % per second (From Settings). Refer to section 6.4.1 for details. %========================================================================== function MF_Init_Smart_GUI(hObject, % >>>> Tab 4: Dynamics Simulation eventdata, handles, varargin) set(handles.sim_joint, 'String', joints_list); clc %Clear Command Window % >>>> Context Menu: Tools % close all; %Close all set(handles.display_cg, 'Checked', opened figures 'off'); set(handles.disp_ref_frames, 'Checked', 'off'); %% Loading and defining variables set(handles.disp_link_frame, 'Checked', MF_Load_Structures(); %Load 'off'); structures to base workspace S = evalin('base', 'S'); %Load Settings from base workspace AS = evalin('base', 'AS'); %Load Additional Settings from base workspace %% %% %Atributing values to pop-up menus % >>>> Tab 2: Commands joints_list = {}; T_list = {}; n = S.value{'dof'}; for i = 1:n joints_list = [joints_list, num2str(i)]; T_list = [T_list, ['T_0_', num2str(i)]]; end set(handles.jointn_menu, 'String', joints_list); set(handles.joint_param_eq_menu, 'String', joints_list); set(handles.transformation_options, 'String', T_list); set(handles.transformation_options, 'Value', n); % >>>> Tab 3: Programs cmd_list = AS.commands; traj_opts = AS.traj_opts; set(handles.program_cmd_opts, 'String', cmd_list); set(handles.program_traj_opts, 'String', traj_opts); screen_size = get( groot, 'Screensize' ); FIG = handles.smart_gui_fig; FIG_SIZE = screen_size(4)/864.*[0 0 155 58]; % FIG_SIZE = screen_size(4)/864.*[0 0 330 55]; set(FIG, 'Units', 'characters', 'Position', FIG_SIZE); movegui('northwest'); %Move GUI to northwest of the screen on opening. %TAB FUNCTION %Create tab group painel_dim = get(handles.P1,'position'); fig_pos = get(FIG, 'position'); menu_comp = 1; %Height compensation for menus painel_pos = ceil([5,fig_pos(4)painel_dim(4)-menu_comp, painel_dim(3), painel_dim(4)+menu_comp]); painel_unit = get(handles.P1, 'Units'); handles.tgroup = uitabgroup('Parent', handles.smart_gui_fig, 'Units', painel_unit, ... 'TabLocation', 'top','Position',painel_pos); handles.tab1 = uitab('Parent', handles.tgroup, 'Title', 'SETTINGS'); APÊNDICE B. Rotinas em Matlab® do Software de Controle handles.tab2 = uitab('Parent', handles.tgroup, 'Title', 'COMMANDS'); handles.tab3 = uitab('Parent', handles.tgroup, 'Title', 'PROGRAMS'); handles.tab4 = uitab('Parent', handles.tgroup, 'Title', 'DYNAMIC SIMULATION'); %Place panels into each tab set(handles.P1,'Parent',handles.tab1); set(handles.P2,'Parent',handles.tab2); set(handles.P3,'Parent',handles.tab3); set(handles.P4,'Parent',handles.tab4); %Reposition each panel to same location of panel 1 P_pos = [0, 0, painel_dim(3), painel_dim(4)]; %Placing Logo logo_pos = get(handles.logo, 'position'); % logo_pos(1:2) = [fig_pos(3)logo_pos(3)-5, 0]; set(handles.logo, 'Units', painel_unit, 'position', logo_pos); %Insert the figure UoD_Crest as the logo axes(handles.logo); imshow('Logo.png'); % Enable handles items in Commands tab depending on the Trajectory Option MF_Enable_Commands(hObject, handles); %Load Settings table MF_Load_Settings_table(); PF_pos = get(handles.PF, 'position'); PF_pos(1:3) = [painel_pos(1), % Create Robot Representation GUI (for fig_pos(4)-painel_pos(4)-PF_pos(4)-1, displaying simulation) painel_dim(3)]; MF_Creating_RR_GUI(); set(handles.P1, 'Units', painel_unit, MF_Update_UI_Controls(); 'position', P_pos); %Update sliders and static texts set(handles.P2, 'Units', painel_unit, MF_Update_Message(10, 'notice'); 'position', P_pos); %Show message about software status set(handles.P3, 'Units', painel_unit, 'position', P_pos); end set(handles.P4, 'Units', painel_unit, 'position', P_pos); set(handles.PF, 'Units', painel_unit, 'position', PF_pos); %========================================================================== % >>>>>>>>> FUNCTION MF-2: INITIALISE GRAPHICAL REPRESENTATION <<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will .... The robot must be saved in STL. The % position and orientation saved is the position that will be displayed as % initial in the Simulation Window. Refer to section 6.4.1 for details. %========================================================================== function MF_Init_Graph_Rep() cn = S.value{'cn'}; if cn > 1 q0 = H(cn - 1).q(end,:);%get home %% Loading structures and collecting joint position from History(last cmd) variables else S = evalin('base', 'S'); %Load q0 = S.value{'home_q'}; %get home Settings (from base workspace) joint position from SETTINGS H = evalin('base', 'H'); %Load end History (from base workspace) RP = evalin('base', 'RP'); %Load Settings (from base workspace) nl = S.value{'dof'}-1; %get the LD = evalin('base', 'LD'); %LD contain number of links (dof-1) all the face and vertices points d = RP.d'; a = RP.a'; alpha = AF = evalin('base', 'AF'); % Load RP.alpha'; %get D-H parameters figure %% Setting the axes area % Making the Robot Representation the current figure HD = evalin('base', 'HD'); % This figure was created in: %Load Settings (from base workspace) MF_Creating_RR_GUI() APÊNDICE B. Rotinas em Matlab® do Software de Controle % figure(RR_fig); %light('Position',[-1 0 0]); try figure(AF); clf('reset'); catch return; end hold on; grid('on'); light %Add a default light % Clearing the trail setappdata(0,'xtrail',[]); % used for trail tracking. setappdata(0,'ytrail', []); % used for trail tracking. setappdata(0,'ztrail',[]); % used for trail tracking. Tr = plot3(0,0,0,'r', 'LineWidth',2); %'Color', [0.5 0 0.5]); % holder for trail paths set(Tr,'xdata',[],'ydata',[],'zdata',[] ); %Get maximum reach distance max_r = S.value{'max_reach_p'}; ax_size = max_r+200; %set the axes dimensions % axes('XLim',[-ax_size ax_size],'YLim',[-ax_size ax_size],'ZLim',[-0 ax_size]); daspect([1 1 1]) %Setting the aspect ratio view(135,25) %Adjust the view orientation. xlabel('X'),ylabel('Y'),zlabel('Z'); %set the axes dimensions axis([-ax_size ax_size -ax_size ax_size -0 (ax_size + RP.d(1))]); %Boundary lines % line1 = plot3([ax_size,ax_size],[-ax_size,-ax_size],[0,-0],'k'); % line2 = plot3([-ax_size,ax_size],[-ax_size,ax_size],[-0,0],'k'); % line3 = plot3([-ax_size,ax_size],[-ax_size,-ax_size],[0,ax_size],'k'); % line4 = plot3([-ax_size, ax_size],[ax_size,ax_size],[0,ax_size],'k'); % line5 = plot3([-ax_size, ax_size],[-ax_size,ax_size],[ax_size,ax_size],'k'); % line6 = plot3([-ax_size,ax_size],[ax_size,ax_size],[ax_size,ax_size],'k') ; % GR.boundary = [line1, line2, line3, line4, line5, line6]; Base_vert = LD(nl+1).Ve; %get the base vertices for patch %the base is the immediate item after the joints % Setting and drawing base Base_patch = patch('faces', LD(nl+1).Fa, 'vertices', Base_vert(:,1:3)); set(Base_patch, 'facec', [.8,.8,.8]);% set base color and draw set(Base_patch, 'EdgeColor','none');% set edge color to none % *************************************** ************ [~, T_m] = PF_Forward_Kinematics(q0, d, a, alpha); for j = 1:nl %number of links (dof1) L_vert = (T_m{j} * LD(j).Ve')'; L_patch = patch('faces', LD(j).Fa, 'vertices', L_vert(:,1:3)); set(L_patch, 'facec', RP.color{j},'EdgeColor','none');% set link color and draw % set(L_patch, );% set edge color to none end % Setting and drawing endeffector if it exists if S.value{'enable_ee'} %if the robot has end-effector ee_n = nl + 1; End_effector = LD(nl+2).Ve; %get the end-effector vertices for % patch the end-effector is the immediate item after the base ee_vert = (T_m{ee_n} * End_effector')'; ee_patch = patch('faces', LD(nl+2).Fa, 'vertices', ee_vert(:,1:3)); APÊNDICE B. Rotinas em Matlab® do Software de Controle set(ee_patch, 'facec', end RP.color{nl+2});% set base color and end draw set(ee_patch, 'EdgeColor','none');% set edge color to none %========================================================================== % >>>>>>>> FUNCTION MF-3: Creating GUI for Robot Representation <<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will create a new figure to host the axes of % the graphical animation % Refer to section 4 of documentation for details. %========================================================================== function MF_Creating_RR_GUI() %% Configuring size and position 'Callback',@Show_ref_frame_mi_Callback) screen_size = get( groot, 'Screensize' ; ); fig_pos = screen_size(3:4)/4; Tm5 = uimenu(Tools_menu,'Label', Position = [fig_pos, 'Display Links frames',... screen_size(3:4)/2]; 'Callback',@Show_links_frame_mi_Callbac % Create and then hide the UI as it is k); being constructed. % (AF: animation figure) Tm6 = uimenu(Tools_menu,'Label', 'Plot % AF = Graphs', 'Callback',... figure('Visible','off','Position',Posit ion, 'Name','ANIMATION WINDOW'); @Plot_graphs_mi_Callback); AF = Tm7 = uimenu(Tools_menu,'Label', figure('Visible','off','Name','ANIMATIO 'Exit', 'Callback',@Exit_mi_Callback); N WINDOW'); assignin('base', 'AF', AF); %save % >>> Views menu AF in base workspace View_menu = uimenu(AF,'Label', 'Change % AF = see hoe to change name to View'); Simulation Window vm1 = uimenu(View_menu,'Label', 'View %% Configuring the menu bar % RR_fig.MenuBar = 'none'; % Hide standard menu bar menus. % >>> Tool menu Tools_menu = uimenu(AF,'Label', 'Tools'); Tm1 = uimenu(Tools_menu,'Label','Repeat Simulation', 'Callback',... @RS_mi_Callback ); Tm2 = uimenu(Tools_menu,'Label', 'Clear trail', 'Callback',... @Clear_trail_mi_Callback); Tm3 = uimenu(Tools_menu,'Label', 'Display CG frames', 'Callback',... @Show_cg_mi_Callback); Tm4 = uimenu(Tools_menu,'Label', 'Display Reference frame',... 1', vm2 2', vm3 3', vm4 4', vm5 5', vm6 6', 'Callback', @view_Callback); = uimenu(View_menu,'Label', 'View 'Callback', @view_Callback); = uimenu(View_menu,'Label', 'View 'Callback', @view_Callback); = uimenu(View_menu,'Label', 'View 'Callback', @view_Callback); = uimenu(View_menu,'Label', 'View 'Callback', @view_Callback); = uimenu(View_menu,'Label', 'View 'Callback', @view_Callback); %% Configuring Toolbar % RR_fig.Toolbar = 'figure'; % Display the standard toolbar Toolbar = findall(AF,'Type','uitoolbar'); %% Creating axes % R_GR = axes('Units','pixels','Position',[0,0, Position(3:4)]); R_GR = axes(); AF.Visible = 'on'; APÊNDICE B. Rotinas em Matlab® do Software de Controle MF_Init_Graph_Rep(); end %% Callbacks function RS_mi_Callback(hObject,eventdata) function Exit_mi_Callback(hObject,eventdata) end function Clear_trail_mi_Callback(hObject,eventda ta) end function Show_cg_mi_Callback(hObject,eventdata) end function view_Callback(source,eventdata, callbackdata) switch source.Label case 'View 1' end function Show_ref_frame_mi_Callback(hObject,even tdata) case 'View 2' case case case case end function Show_links_frame_mi_Callback(hObject,ev entdata) end 'View 'View 'View 'View 3' 4' 5' 6' end end end function Plot_graphs_mi_Callback(hObject,eventda ta) %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-4: ANIMATE COMMANDS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will load the joints trajectory from History % and animate it in the Animation window by transforming (rotaion and % translation) the LD matrices that contain the vertices and patches got % from SF_Importing_CAD. The transformation is made by using the Transform % matrix (primary function Forward_Kinematics); the animation will have fps % frames per second (From Settings). Refer to section 4 of documentation % for details. %========================================================================== function MF_Animate_Commands(cn, LD = evalin('base', 'LD'); % LD axessize) contain all the face and vertices % cn: command number (a scalar or a points vector with all commands the user wants AF = evalin('base', 'AF'); % Load % to animate). figure (Animation Figure) %% Loading structures and collecting variables HD = evalin('base', 'HD'); % Load S = evalin('base', 'S'); % Load Handles (from base workspace) Settings (from base workspace) RP = evalin('base', 'RP'); % Load sp = H(cn).sp; %get the Robot parameters (from base workspace) number of steps H = evalin('base', 'H'); % Load History (from base workspace) APÊNDICE B. Rotinas em Matlab® do Software de Controle save_video = S.value{'save_video'};%get the save video option (save as avi) % Trail and axes (display options) trail = S.value{'trail'}; clear_trail_after_command = S.value{'ctrail_ac'}; if strcmp(get(HD.display_cg, 'Checked'), 'on') show_cg_frame = true; else show_cg_frame = false; end if strcmp(get(HD.disp_link_frame, 'Checked'), 'on') show_link_frame = true; else show_link_frame = false; end show_axes = S.value{'axes'}; % Robot parameters fps = S.value{'fps'}; tpf = 1/fps; %time per frame nl = S.value{'dof'}-1; %get the number of links (dof-1) d = RP.d'; a = RP.a'; alpha = RP.alpha'; %get D-H parameters if S.value{'enable_control'} q = H(cn).qc; %get theta angles from history else q = H(cn).q; end % Creating a new q list if the number of rows in H is larger than fps*time if size(q,1) > ceil(fps * H(cn).time(end)) idxlist = round(linspace(1, size(q,1), 2.5*(fps * H(cn).time(end)))); q = q(idxlist, :); sp = size(q,1); end %% % Making the Robot Representation the current figure % This figure was created in: MF_Creating_RR_GUI() try figure(AF); catch MF_Creating_RR_GUI(); end set(gcf,'Visible', 'on'); try children = get(gca, 'children'); delete(children); end hold on; grid('on'); light %Add a default light max_r = S.value{'max_reach_p'}; %Get maximum reach distance ax_size = max_r+200; %set the axes dimensions % daspect([1 1 1]) %Setting the aspect ratio % view(135,25) %Adjust the view orientation. % xlabel('X'),ylabel('Y'),zlabel('Z'); if exist('axessize', 'var') if strcmp(axessize, 'full') %set the axes dimensions axis([-ax_size ax_size -ax_size ax_size -ax_size ax_size]); end else %set the axes dimensions axis([-ax_size ax_size -ax_size ax_size -0 (ax_size + RP.d(1))]); end %% Setting and drawing base Base_vert = LD(nl+1).Ve; %get the base vertices for patch %The base is the immediate item after the joints Base_patch = patch('faces', LD(nl+1).Fa, 'vertices', Base_vert(:,1:3)); set(Base_patch, 'facec', [.8,.8,.8]);% set base color and draw set(Base_patch, 'EdgeColor','none');% set edge color to none if clear_trail_after_command %Clear trail setappdata(0,'xtrail',[]); setappdata(0,'ytrail', []); setappdata(0,'ztrail',[]); Tr = plot3(0,0,0,'r', 'LineWidth',2); set(Tr,'xdata',[],'ydata',[],'zdata',[] ); else Tr = plot3(0,0,0,'r', 'LineWidth',2); end APÊNDICE B. Rotinas em Matlab® do Software de Controle %% Drawing the links and end-effector for all points in the trajectory for i = 1:sp %1 to setp points tic; %Start computing time stop = S.value{'stop'}; if stop set(EE_patch, 'EdgeColor','none');% set edge color to none end % % store trail in appdata if trail == 1 %1: Show trail MF_Update_Message(8,'warnings'); MF_Update_Stop_status(false); %reseting stop status break end %Delete previous patches try for idx = 1:nl delete(L_patch{idx}); end delete(EE_patch); end q_row = q(i,:); [p_xyz, T_m] = PF_Forward_Kinematics(q_row, d, a, alpha); % Setting and drawing links for j = 1:nl %number of links (dof1) L_vert = (T_m{j} * LD(j).Ve')'; % get the vertices transformed L_patch{j} = patch('faces', LD(j).Fa, 'vertices', L_vert(:,1:3)); % set link color and draw set(L_patch{j}, 'facec', RP.color{j},'EdgeColor','none'); end % Setting and drawing end-effector if it exists ee_enabled = S.value{'enable_ee'}; if ee_enabled %if the robot has end-effector ee_n = nl + 1; End_effector = LD(nl+2).Ve; %get the end-effector vertices for % patch the end-effector is the immediate item after the base EE_vert = (T_m{ee_n} * End_effector')'; EE_patch = patch('faces', LD(nl+2).Fa, 'vertices', EE_vert(:,1:3)); set(EE_patch, 'facec', RP.color{nl+2});% set base color and draw x_trail = getappdata(0,'xtrail'); y_trail = getappdata(0,'ytrail'); z_trail = getappdata(0,'ztrail'); % xdata = [x_trail p_xyz(1)]; ydata = [y_trail p_xyz(2)]; zdata = [z_trail p_xyz(3)]; % setappdata(0,'xtrail',xdata); % used for trail tracking. setappdata(0,'ytrail',ydata); % used for trail tracking. setappdata(0,'ztrail',zdata); % used for trail tracking. % set(Tr,'xdata',xdata,'ydata',ydata,'zda ta',zdata); end if show_link_frame MF_Plot_Links_frame(T_m); elseif show_cg_frame MF_Plot_CG_frames(T_m, true, q_row); end drawnow if save_video F(i) = getframe; %get frame if save video is set to true % %save images each 5 frames (in jpeg) % if i==1 || rem(i,17) == 0 % Image = frame2im(F(i)); % imagename = strcat('image', num2str(cn), '-', num2str(i),'.jpg'); % imwrite(Image, imagename); % end end APÊNDICE B. Rotinas em Matlab® do Software de Controle %Pause loop if step was quicker than time per frame (make animation %more accurate with the time imposed by the user) st = toc; %step time if st < tpf pause(tpf - st); end end if save_video %convert F frames to avi video videoname = strcat('command', num2str(cn),'.avi'); movie2avi(F, videoname, 'quality', 75, 'fps', S.value{'fps'}); end end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-5: UPDATE MESSAGES <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will update the messages displayed in the % message box. % Refer to section 6.4.1 for details. %========================================================================== function MF_Update_Message(mn, fname, inputs = {}; inputs) %create an empty cell array % mn: messge number (M(mn).fname) end integer number end % fname: field name in M structure (eg: else robotP, or settings) - string inputs = {}; % inputs: message inputs (cell array), end applicable for some messages only % in the message use (%0.2f) for msg_txt = getfield(M(mn), fname); input numbers and (%s) for input text %get message from M strucute % M structure: previous_msg = get(HD.messages_txt, % M(mn).settings 'String'); % M(mn).commands divider = ' '; % M(mn).programs if ~isempty(msg_txt)%if message exists % M(mn).notice in M structure % M(mn).warnings % msg_ass = % M(mn).animW strcat(previous_msg,'\n', divider, % M(mn).robotP '\n\n', msg_txt); % M(mn).simulation msg_ass = strcat('\n\n', msg_txt); % M(mn).controlS %check message number of inputs ni = strfind(msg_txt,'%'); HD = evalin('base', 'HD'); if size(inputs, 2) > size(ni, 2) %Load message structure from base inputs = inputs(1:ni); workspace end M = evalin('base', 'M'); msg = sprintf(msg_ass, inputs{:}); %assembled message (with inputs) %Check if the inputs variables was passed to the function set(HD.messages_txt, 'String', if exist('inputs', 'var') msg); %set message in handle if ~iscell(inputs) % set(handles.messages_txt, try 'String', msg); %set message in handle inputs = num2cell(inputs); end %convert to cell array if not %>>>END OF FUNCTION catch %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-6: UPDATE UI CONTROLS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. APÊNDICE B. Rotinas em Matlab® do Software de Controle % DESCRIPTION: This function will update the ui controls (handles in the % commands tab (transformation matrix and other menus) % Refer to section 4 of documentation for details. %========================================================================== function MF_Update_UI_Controls() %------%% Load files and variables HD = evalin('base', 'HD'); %Get %Update sliders and static text on UI. handles as HD from base workspace jn = get(HD.jointn_menu, 'Value'); RP = evalin('base', 'RP'); %Load qmin = RP.j_range{jn}(1); Robot Parameters qmax = RP.j_range{jn}(2); S = evalin('base', 'S'); %Load %Joint n slider Settings (from base workspace) set(HD.slider_jointn, 'Value', qc(jn)); H = evalin('base', 'H'); %Load set(HD.slider_jointn, 'Min', qmin); History (from base workspace) set(HD.slider_jointn, 'Max', qmax); set(HD.slider_jointn, cn = S.value{'cn'}; %get the command 'SliderStep',[1/qmax, 10/qmax]); number from Settings structure set(HD.jointn_value, 'String', a = RP.a'; num2str(round(qc(jn)), 2)); d = RP.d'; %------alpha = RP.alpha'; %Enabling Gripper ui if S.value{'enable_ee'} g_status = 'on'; %gripper will % get current position be enabled in Commands tab if cn == 1 gn = S.value{'dof'} + 1; qc = S.value{'home_q'}; qming = RP.j_range{gn}(1); else qmaxg = RP.j_range{gn}(2); try set(HD.gripper_slider, 'Min', qc = H(cn-1).q(end,:); qming); catch set(HD.gripper_slider, 'Max', qc = S.value{'home_q'}; qmaxg); end set(HD.gripper_slider, end 'SliderStep',[1/qmaxg, 10/qmaxg]); else %% g_status = 'off'; %Get the number of the matrix chosen end (Eg.: T_03 will return Tn = 3) set(HD.open_gripper_bt, 'Enable', Tn = g_status); get(HD.transformation_options,'Value'); set(HD.close_gripper_bt, 'Enable', g_status); % Call Forward Kinematics and get the T set(HD.opening_txt, 'Enable', matrix. g_status); [pos, Tm] = PF_Forward_Kinematics(qc, set(HD.gripper_angle_value, 'Enable', d, a, alpha); g_status); Tm = Tm{Tn}; %T_Matrix is the set(HD.gripper_slider, 'Enable', transformation matrix chosen g_status); set(HD.transformation_table, 'Data', round(Tm, 3)); %------ %------% Update frames MF_Plot_Links_frame(0); MF_Plot_CG_frames(0); %Update current end-effector and joint position (text box below messages) end set(HD.current_ee_pos, 'String', num2str(round(pos, 4))); %>>> END OF FUNCTION set(HD.current_joint_pos, 'String', num2str(round(qc, 2))); %========================================================================== % >>>>>>>>>>>>>>>>> FUNCTION MF-7: RESET HISTORY STRUCTURE <<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - May 14th, 2016. APÊNDICE B. Rotinas em Matlab® do Software de Controle % DESCRIPTION: This function will clear history structure and reset command % number. Refer to section 4 of documentation for details. %========================================================================== function MF_Clear_History() end %% Loading variables M = evalin('base', 'M'); H = evalin('base', 'H'); S = evalin('base', 'S'); %% answer = questdlg(M(28).warnings,'Confirmation') ; if strcmp(answer,'No') return elseif strcmp(answer, 'Yes') History_path = which('HISTORY.mat'); %get file path settings_path = which('SETTINGS.mat'); H_fields = fieldnames(H); H_size = size(H); if H_size(2) > 1 H(2:H_size(2)) = []; %deleting 'layers' (indices nonscalar) of H end % Reset command number S.value{'cn'} = 1; %Save the empty H in base workspace and .mat file assignin('base', 'H', H); %Save H in base workspace save(History_path, 'H', '-append'); %Save the mat file. save(settings_path, 'S', 'append'); assignin('base', 'S', S); %Save S in base workspace MF_Update_Message(12, 'notice'); %Update message MF_Init_Graph_Rep(); %Update animation window MF_Load_Settings_table(); %Update table in Settings tab MF_Update_UI_Controls(); else return end %Clear GR_Data % Deleting fields of layer 1; for i = 1:length(H_fields) end H(1).(H_fields{i}) = []; %========================================================================== % >>>>>>>>>>>>>>>> FUNCTION MF-8: LOADING SETTINGS TABLE <<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will load the settings table to the GUI in settings tab. Refer to section 4 of documentation for details. %========================================================================== function MF_Load_Settings_table() Settings_table{i,1} = HD = evalin('base', 'HD'); num2str(S.param{i}); S = evalin('base', 'S'); else Settings_table{i,2} = S.value{i}; Settings_table = {}; Settings_table{i,1} = S.param{i}; for i = 1:1:height(S) %Each row end of Settings catch try end %Use str2num to convert string end to a list of numbers set(HD.settings_table,'Data', %It is necessary because Settings_table); Instructions section send angles as a end % string not as numbers. if isnumeric(S.value{i}) Settings_table{i,2} = num2str(S.value{i}); %========================================================================== APÊNDICE B. Rotinas em Matlab® do Software de Controle % >>>>>>>>>>>> FUNCTION MF-9: LOADING ROBOT PARAMETERS TABLE <<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will create a new figure displaying a table % for the user to edit the parameters of the robot, such as: D-H % parameters, mass of each link, joints range, Inertia, links colors, etc. % Refer to section 4 of documentation for details. %========================================================================== function MF_Load_Robot_param_table() nc = n; %number of columns in the table %% Loading variables if S.value{'enable_ee'} S = evalin('base', 'S'); %Load col_names = {col_names, 'EndSettings from base workspace Effector'}; n = S.value{'dof'}; nc = nc + 1; end %% Configuring size and position screen_size = get( groot, 'Screensize' RP.color{nc} = [0.8, 0.8, 0.8]; ); %Setting a color to end-effector fig_pos = screen_size(3:4)/4; Position = [fig_pos, % (PS: The user can change) screen_size(3:4)/1.75]; % Create and then hide the UI as it is being constructed. RPT_fig = figure('Visible','off','Position',Posit ion, 'Name', ... %% Converting table to string so it can be loaded in handle table tf = RP; tf_cell = table2cell(RP); %table converted to cell 'ROBOT PARAMETERS'); %Converting to string (cts: converted table to string) for row = 1:size(tf,1) for col=1:size(tf,2) cts(row, col) = {num2str(tf_cell{row, col})}; end end edit_col = boolean(ones(1,nc)); %set the editable columns RPT_fig.MenuBar = 'none'; standard menu bar menus. % Hide % Construct the buttons. bt_pos = [Position(3)-120,Position(4)50, 100,50]; Save_bt = uicontrol('Style','pushbutton','String' ,'SAVE','Position',... bt_pos, 'Callback', @save_bt_Callback); %% Setting table %get the folder where ROBOT_DATA is located if exist('ROBOT_DATA.mat') ~= 0 Robot_data_path = which('ROBOT_DATA.mat'); Parameters = matfile(Robot_data_path,'Writable',true ); %load as matfile RP = Parameters.RP; else return end row_names = RP.Properties.VariableNames; %getting fields names in RP table col_names = linspace(1,n,n); % Creating UITable and attributing properties RPT = uitable(RPT_fig,'Data',cts','ColumnName ',col_names,... 'ColumnEditable', edit_col, 'RowName', row_names, 'ColumnWidth',{80},... 'CellEditCallback', @table_edit, 'CellSelectionCallback',@cell_selection ); %Extend the table columns RPT.Position(3) = RPT.Position(4) = RPT_fig.Visible = to on. to the size of its RPT.Extent(3); RPT.Extent(4); 'on'; %set visibility %% Callback Functions %% APÊNDICE B. Rotinas em Matlab® do Software de Controle function table_edit(hObject,callbackdata) %*** INSERT CODE TO CHECK TO CHECK THE USER INPUTS**** % numval = eval(callbackdata.EditData); % r = callbackdata.Indices(1) % c = callbackdata.Indices(2) % hObject.Data{r,c} = numval; end %% function cell_selection(hObject,callbackdata) r = callbackdata.Indices(1); %get selection row MF_Update_Message(r, 'robotP'); end %% function save_bt_Callback(hObject,callbackdata) %Show confirmation of save if isnan(str2double(table{r_idx, c_idx})) if isempty(str2num(table{r_idx, c_idx})) RP{r_idx, c_idx}{:} = table{r_idx, c_idx}; else RP{r_idx, c_idx}{:} = str2num(table{r_idx, c_idx}); end else RP(r_idx, c_idx) = num2cell(str2double(table{r_idx, c_idx})); end end end save('ROBOT_DATA.mat', 'RP', 'append'); % Save file MF_Update_Message(8,'notice'); % Display message MF_Clear_History(); % Clear history % Clear GR_data % Run SF_Import_CAD table = get(RPT, 'Data')'; %get the table from the handle %Load structures files to workspace again MF_Load_Structures() end end % Reconverting to UItable to table array for r_idx = 1 : size(table, 1) for c_idx = 1 : size(table, 2) %========================================================================== % >>>>>>>>>>>> FUNCTION MF-10: LOADING PROGRAM TABLE <<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will load the parameters table when the button % edit robot parameter in settings tab is pressed. % Refer to section 4 of documentation for details. %========================================================================== function MF_Load_Program_table(P) Ptable(row, col) = % Loading variables {''}; HD = evalin('base', 'HD'); %Load length(Pcell{row,col}) handles from base workspace for i = Pcell = table2cell(P); %table 1:length(Pcell{row,col}) converted to cell if %% Converting to string (cts: converted isnumeric(Pcell{row,col}{i}) table to string) Ptable{row, for row = 1:size(P,1) col} = [Ptable{row, col},' ', ... for col=1:size(P,2) if isnumeric(Pcell{row,col}) num2str(Pcell{row, col}{i})]; Ptable(row, col) = else {num2str(Pcell{row, col})}; Ptable(row, else col) = [Ptable{row, col},' ', ... if iscell(Pcell{row, col}) APÊNDICE B. Rotinas em Matlab® do Software de Controle end Pcell{row, col}{i}]; end end end end else set(HD.program_table,'Data',Ptable); Ptable(row, col) = %Display in the table. {num2str(Pcell{row, col})}; end end %========================================================================== % >>>>>>>>>>>>>> FUNCTION MF-11: LOAD TABLE OF COORDINATES <<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will open a window for the user to choose the % excel spreadsheet with the coordinates, then the function will check the % coordinates in the table and if valid, it will be saved in base workspace % under the variable name TB. Refer to section 4 of documentation for % details. %========================================================================== function MF_Load_Table_Coord() % Make sure all rows are number (it %open an window to the user choose the does not check here if the points are table (excel) % reacheable or not) [filename, folder] = uigetfile('*'); if ~isnumeric(TP) ffname = fullfile(folder, filename); MF_Update_Message(14, 'warnings'); %full filename return end % Attempt to open the file try if size(TP,2) < 6 TP = xlsread(ffname); MF_Update_Message(24, 'warnings'); catch end try TP = open(ffname); % saving in base workspace catch assignin('base', 'TP', TP); MF_Update_Message(13, 'warnings'); % update message about load status return MF_Update_Message(7, 'notice'); end end end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-12: LOAD STRUCTURES FILES <<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will .... % Refer to section 6.4.1 for details. %========================================================================== function MF_Load_Structures() History = Settings_path = which('SETTINGS.mat'); matfile(History_path,'Writable',true); Rdata_path = which('ROBOT_DATA.mat'); %load as matfile GR_data_path = which('GR_DATA.mat'); Settings = History_path = which('HISTORY.mat'); matfile(Settings_path,'Writable',true); Messages_path = which('MESSAGES.mat'); %load as matfile Messages = matfile(Messages_path,'Writable',true); Robot_data = %load as matfile matfile(Rdata_path,'Writable',true); %load as matfile RP = Robot_data.RP; %Read the variable topo from the MAT-file. APÊNDICE B. Rotinas em Matlab® do Software de Controle LD = Robot_data.LD; %Read the variable topo from the MAT-file. H = History.H; %Load History S = Settings.S; %Load Settings AS = Settings.AS; %Load Additional Settings M = Messages.M; %Load History assignin('base', assignin('base', assignin('base', assignin('base', assignin('base', assignin('base', 'RP', RP); 'H', H); 'S', S); 'AS', AS); 'LD', LD); 'M', M); end %========================================================================== % >>>>>>>>>>>>>> FUNCTION MF-13: PLOT BASE REFERENCE FRAME <<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will plot base frame on the robot % representation in animation window. The function checks if % Tools>Reference Frames is checked and then create the axes frames in % robot 0 coordinate. The color for the axes follow a standar for % coordinates frames: X: red; Y: green; Z: blue. Refer to section 4 of % documentation for details. %========================================================================== function MF_Plot_Base_frame(show) Vbx = [[0; axis_length], [0; %Load and assemble variables 0], [0; 0]]; RP = evalin('base', 'RP'); % Load Vby = [[0; 0], [0; Robot parameters (from base workspace) axis_length], [0; 0]]; AF = evalin('base', 'AF'); % Load Vbz = [[0; 0], [0; 0], [0; figure (Animation Figure) axis_length]]; % show = true: display the axes on the screen; false: delete the axes if ~exist('show', 'var') HD = evalin('base', 'HD'); % Load Handles (from base workspace) if strcmp(get(HD.disp_ref_frames, 'Checked'), 'on') show = true; else show = false; end end x_axis = plot3(Vbx(:, 1), Vbx(:, 2), Vbx(:, 3),'r'); y_axis = plot3(Vby(:, 1), Vby(:, 2), Vby(:, 3),'g'); z_axis = plot3(Vbz(:, 1), Vbz(:, 2), Vbz(:, 3),'b'); plotted_axes = [x_axis, y_axis, z_axis]; setappdata(0, 'base_frame', plotted_axes); end try figure(AF); catch return end %% Display axes if show show_axes(RP); else %If the axes lines exist, delete it. delete_axes() end %% Functions function show_axes(RP) axis_length = ceil(norm(RP.d + RP.a)/length(RP.d))*2.5; % Creating base frame vertices function delete_axes() try axes_lines = getappdata(0, 'base_frame'); for j = 1:size(axes_lines,1) %for each joint for c = 1 : size(axes_lines,2) %for each coordinate line = axes_lines(j,c); delete(line); end end end end end APÊNDICE B. Rotinas em Matlab® do Software de Controle %========================================================================== % >>>>>>>>>>> FUNCTION MF-14: PLOT CENTER OF GRAVITY FRAMES <<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will plot the center of gravity reference % frames on the robot representation in animation window. The function % checks if the user selected the option Display CG Frames and transform a % reference frame from the base to the center of mass of the selected link % (in Transformation Matrices menu - Commands tab). If the Settings tab % option: Show all axes are set to true, then the center of mass reference % frame for all links will be plotted. Refer to section 4 of documentation % for details. %========================================================================== function MF_Plot_CG_frames(T, show, q) %Load and assemble variables try S = evalin('base', 'S'); % Load figure(AF); Settings (from base workspace) catch H = evalin('base', 'H'); % Load return History (from base workspace) end HD = evalin('base', 'HD'); % Load %% Display axes Handles (from base workspace) if show RP = evalin('base', 'RP'); % Load delete_axes(); Robot parameters (from base workspace) show_axes(T, RP, HD, S); AF = evalin('base', 'AF'); % Load else %If the axes lines exist, figure (Animation Figure) delete it. cn = S.value{'cn'}; delete_axes() end % show = true: display the axes on the screen; false: delete the axes %% Functions if ~exist('show', 'var') function show_axes(T, RP, HD, S) if strcmp(get(HD.display_cg, if all_links 'Checked'), 'on') link = 1:1:S.value{'dof'}; show = true; else else link = show = false; get(HD.transformation_options, end 'Value'); end end for i= 1:length(link) all_links = S.value{'axes'}; % Tm = T{link(i)}; display the axes for all joints %Transformation matrix from base to link chosen if exist('T', 'var') if ~exist('q', 'var') axis_length = if cn == 1 ceil(norm(RP.d + RP.a)/length(RP.d)); q = S.value{'home_q'}; else % Creating base frame q = H(cn - 1).q(end,:); vertices end Vbx = [[0; axis_length], end [0; 0], [0; 0], [0; 0]]; Vby = [[0; 0], [0; if ~iscell(T) && (T == 0) axis_length], [0; 0], [0; 0]]; d = RP.d; a = RP.a; alpha = Vbz = [[0; 0], [0; 0], [0; RP.alpha; axis_length], [0; 0]]; [~, T] = PF_Forward_Kinematics(q, d, a, alpha); %Transforming the base end frame vertices to chosen link else % Rm = Tm(1:3, 1:3); return if link(i) == 1 end Tpj = eye(4); APÊNDICE B. Rotinas em Matlab® do Software de Controle else Tpj = T{link(i)-1}; %tranform matrix of the previous joint end r_cm = [RP.r_cm{link(i)}'; 1]; %4x1 vector %Transform from CG of link i to axis frame of link i [~, Tli] = PF_Forward_Kinematics(q(i), 0, 0, 0); %Transform of CG to base. Tcmi = Tpj * Tli{1}; x_axis = plot3(Vtx(:, 1), Vtx(:, 2), Vtx(:, 3),'r'); y_axis = plot3(Vty(:, 1), Vty(:, 2), Vty(:, 3),'g'); z_axis = plot3(Vtz(:, 1), Vtz(:, 2), Vtz(:, 3),'b'); plotted_axes(i,:) = [x_axis, y_axis, z_axis]; setappdata(0, 'cg_frames', plotted_axes); end end %Position of CG of link i to function delete_axes() try p_cm = Tcmi * r_cm; axes_lines = getappdata(0, 'cg_frames'); for j = Vtx = (Tcmi * Vbx')'; 1:size(axes_lines,1) %for each Vty = (Tcmi * Vby')'; joint Vtz = (Tcmi * Vbz')'; for c = 1 : size(axes_lines,2) %for each coordinate p_cm = p_cm(1:3)'; line = axes_lines(j,c); Vtx = Vtx(:, 1:3) + [p_cm; delete(line); p_cm]; end Vty = Vty(:, 1:3) + [p_cm; end p_cm]; end Vtz = Vtz(:, 1:3) + [p_cm; end p_cm]; end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-15: PLOT LINKS FRAME <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. base. % DESCRIPTION: This function will plot the links reference frames on the % robot representation in animation window. The function checks if the user % selected the option Display Links Frames and transform a reference frame % from the base to the selected link (in Transformation Matrices menu % Commands tab). If the Settings tab option: Show all axes are set to true, % then all links(joints) reference frames are plotted at once. Refer to % section 4 of documentation for details. %========================================================================== function MF_Plot_Links_frame(T, show) %Load and assemble variables % show = true: display the axes on the S = evalin('base', 'S'); % Load screen; false: delete the axes Settings (from base workspace) if ~exist('show', 'var') H = evalin('base', 'H'); % Load if strcmp(get(HD.disp_link_frame, History (from base workspace) 'Checked'), 'on') HD = evalin('base', 'HD'); % Load show = true; Handles (from base workspace) else RP = evalin('base', 'RP'); % Load show = false; Robot parameters (from base workspace) end AF = evalin('base', 'AF'); % Load end figure (Animation Figure) cn = S.value{'cn'}; APÊNDICE B. Rotinas em Matlab® do Software de Controle all_links = S.value{'axes'}; % display the axes for all joints if exist('T', 'var') if ~iscell(T) && (T == 0) if cn == 1 q = S.value{'home_q'}; else q = H(cn - 1).q(end,:); end d = RP.d; a = RP.a; alpha = RP.alpha; [~, T] = PF_Forward_Kinematics(q, d, a, alpha); end else return end try figure(AF); catch return end %% Display axes if show delete_axes(); show_axes(T, RP, HD, S); else %If the axes lines exist, delete it. delete_axes() end %% Functions function show_axes(T, RP, HD, S) if all_links link = 1:1:S.value{'dof'}; else link = get(HD.transformation_options, 'Value'); end for i= 1:length(link) Tm = T{link(i)}; %Transformation matrix from base to link chosen axis_length = ceil(norm(RP.d + RP.a)/length(RP.d)); % Creating base frame vertices Vbx = [[0; axis_length], [0; 0], [0; 0], [1; 1]]; Vby = [[0; 0], [0; axis_length], [0; 0], [1; 1]]; Vbz = [[0; 0], [0; 0], [0; axis_length], [1; 1]]; %Transforming the base frame vertices to chosen link Vtx = (Tm * Vbx')'; Vty = (Tm * Vby')'; Vtz = (Tm * Vbz')'; x_axis = plot3(Vtx(:, 1), Vtx(:, 2), Vtx(:, 3),'r'); y_axis = plot3(Vty(:, 1), Vty(:, 2), Vty(:, 3),'g'); z_axis = plot3(Vtz(:, 1), Vtz(:, 2), Vtz(:, 3),'b'); plotted_axes(i,:) = [x_axis, y_axis, z_axis]; setappdata(0, 'links_frames', plotted_axes); end end function delete_axes() try axes_lines = getappdata(0, 'links_frames'); for j = 1:size(axes_lines,1) %for each joint for c = 1 : size(axes_lines,2) %for each coordinate line = axes_lines(j,c); delete(line); end end end end end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-16: PLOT GRAPHS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will plot the 4 graphs: position, velocity, % acceleration and torque (each graph will have the n joints) % Refer to section 6.4.1 for details. %========================================================================== function MF_Plot_Graphs(cn) APÊNDICE B. Rotinas em Matlab® do Software de Controle %cn: command number (cn can be an array w/ all command numbers in history) %mode: layout of the plots, mode = 1: each plot in a new figure; 2: %% Loading structures; H = evalin('base', 'H'); %Load History (from base workspace) %% Assembling data to plot time = zeros(length(0)); theta = []; theta_dot = []; theta_ddot = []; torque = []; for idx = 1:length(cn) time(idx) = H(cn(idx)).time(end); theta = [theta; H(cn(idx)).q]; theta_dot = [theta_dot; H(cn(idx)).dq]; theta_ddot = [theta_ddot; H(cn(idx)).ddq]; torque = [torque; H(cn(idx)).tq]; end n = size(theta,2); time = linspace(0,sum(time),length(theta)); %If the user didn't enable torque computation in Settings (make a 0 array) if size(torque, 1) ~= size(time, 2) plot_torque = false; else plot_torque = true; end %% Plotting data figure set(gcf,'color','white'); %Set figure background to white set(0,'defaultTextInterpreter','latex') ; %Use Latex interpreter for symbols lw = 0.5; %Plot Line width plot_r = 3; %Define plot layout in the figure (4 rows) plot_c = 1; %Define plot layout in the figure (1 column) %1 - Plotting Theta over time for each joint subplot(plot_r,plot_c,1); hold on grid on for i = 1:n try plot(time, theta(:,i),'LineWidth',lw); lenged_txt{i} = strcat('$\theta' , num2str(i),'$'); end end title('Joint displacement'); xlabel('$t [s]$','interpreter','latex'); ylabel('$\theta(t) [^ \circ]$','interpreter','latex'); legend(lenged_txt,'interpreter','latex' ); %2- Plotting Angular Velocity (dtheta/dt) over time for each joint subplot(plot_r,plot_c,2); hold on grid on for i = 1:n try plot(time, theta_dot(:,i),'LineWidth',lw); lenged_txt{i} = strcat('$\dot\theta' , num2str(i),'$'); end end title('Joint velocity'); xlabel('$t [s]$','interpreter','latex'); ylabel('$\dot\theta(t) [^ \circ /s]$','interpreter','latex'); legend(lenged_txt,'interpreter','latex' ); %3 - Plotting Angular Acceleraion (ddtheta/dt^2) over time for each joint subplot(plot_r,plot_c,3); hold on grid on for i = 1:n try plot(time, theta_ddot(:,i),'LineWidth',lw); lenged_txt{i} = strcat('$\ddot\theta' , num2str(i),'$'); end end title('Joint acceleration'); xlabel('$t [s]$','interpreter','latex'); ylabel('$\ddot\theta(t) [^ \circ /s^{2}]$','interpreter','latex'); legend(lenged_txt,'interpreter','latex' ); %4 - Plotting Torque over time for each joint % subplot(plot_r,plot_c,4); if plot_torque figure set(gcf,'color','white'); %Set figure background to white hold on grid on for i = 1:n try APÊNDICE B. Rotinas em Matlab® do Software de Controle plot(time, ylabel('$\tau(t) torque(:,i),'LineWidth',lw); [Nm]$','interpreter','latex'); lenged_txt{i} = strcat('$\tau' , num2str(i),'$'); legend(lenged_txt,'interpreter','latex' end ); end end title('Joint Torque'); end xlabel('$t [s]$','interpreter','latex'); %========================================================================== % >>>>>>>>>>>>>> FUNCTION MF-17: PLOT COMPARISON GRAPHS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will plot the 4 graphs: position, velocity, % acceleration and torque (each graph will have the n joints) % Refer to section 6.4.1 for details. %========================================================================== function MF_Plot_Comparison_Graphs(cn) set(gcf,'color','white'); %cn: command number (cn can be an array %Set figure background to white w/ all command numbers in history) set(0,'defaultTextInterpreter','latex') %mode: layout of the plots, mode = 1: ; %Use Latex interpreter for symbols each plot in a new figure; 2: lw = 0.5; %Plot Line width %% Loading structures; plot_c = 2; %Define plot layout in H = evalin('base', 'H'); %Load the figure (2 columns) History (from base workspace) plot_r = n/plot_c; %Define plot layout in the figure (4 rows) %% Assembling data to plot time = zeros(length(0)); q_ideal = []; %1 - Plotting Theta over time for each q_cs = []; joint for idx = 1:n subplot(plot_r, plot_c, idx); hold on for ii = 1:cn grid on time(ii) = H(cn(ii)).time(end); try q_ideal = [q_ideal; H(cn(ii)).q]; plot(time, q_ideal(:,idx), 'k', %ideal joint variable values ... q_cs = [q_cs; H(cn(ii)).qc]; 'LineWidth',lw); %real joint variable (control result) end n = size(q_ideal,2); plot(time, q_cs(:,idx), 'ktime = .',... linspace(0,sum(time),length(q_ideal)); 'LineWidth',lw,... 'MarkerSize', 5); % plot(time, % cn = 1; q_cs(:,idx),'LineWidth',lw,'Colour', % for testing the function 'k'); % clc lenged_txt = {strcat('$\theta' % close all , num2str(idx),'$ ideal'), ... % strcat('$\theta' % time = linspace(0,300,500); , num2str(idx),'$ real')}; % q_ideal = sin(linspace(pi,pi,500)'*linspace(1,6,6)); legend(lenged_txt,'interpreter','latex' % q_cs = sin(linspace(); pi+0.25,pi+0.25,500)'*linspace(1,6,6)); end % title('Joint displacement'); % n = size(q_ideal,2); xlabel('$t [s]$','interpreter','latex'); %% Plotting data ylabel('$\theta(t) [^ figure \circ]$','interpreter','latex'); APÊNDICE B. Rotinas em Matlab® do Software de Controle end end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-18: UPDATE COMMAND NUMBER <<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will update the command number that is used in % the history structure and primary functions. % When the GUI is started this number is set to 1, and each time the user % gives the GUI a command (eg.: move arm by coordinate), this number is % incremented by 1, this is used as a index for saving the position, % velocity, acceleration and torque in the history structure. Refer to % section 4 of documentation for details. %========================================================================== function MF_Update_Cn() S = evalin('base', 'S'); Settings_path = which('SETTINGS.mat'); % Load S from base workspace % Get Settings path S.value{'cn'} = S.value{'cn'} + 1; save(Settings_path, 'S', '-append'); % Update cn % Save the mat file. assignin('base', 'S', S); end % Save in base workspace %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-19: UPDATE STOP STATUS <<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - May 16th, 2016. % DESCRIPTION: This function will update the stop status in Settings % structure. The other functions check this status whenever they start a % new command, if this is status is set to true then the robot interrupts % its movement. Refer to section 4 of documentation for details. %========================================================================== function MF_Update_Stop_status(status) S.value{'stop'} = status; % status: either true or false (boolean type) % saving in base workspace S = evalin('base', 'S'); %Load assignin('base', 'S', S); Settings from base workspace end %========================================================================== % >>>>>>>>>>>>>>>> FUNCTION MF-20: LOAD STRUCTURES FILES <<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will load structures to base workspace on % software opening % Refer to section 4 of documentation for details. %========================================================================== function MF_Save_Structures(sname, % function will know in which varargin) field of the structure the variable % sname: structure name the function % should be updated needs saving %% % varargin: variables the function %get command number to save the needs update before saving in mat file variables in the correct 'layer' in % for H structure, varargin must S = evalin('base', 'S'); have the field names followed by the %structure % variable values. cn = S.value{'cn'}; % Eg.: varargin = {'q', q, 'dq', dq, 'ddq', ddq} etc. so then the Settings_path = which('SETTINGS.mat'); APÊNDICE B. Rotinas em Matlab® do Software de Controle Rdata_path = which('ROBOT_DATA.mat'); GR_data_path = which('GR_DATA.mat'); History_path = which('HISTORY.mat'); Messages_path = which('MESSAGES.mat'); % S = evalin('base', 'S'); %Get Settings structure (S) from base workspace % RP = evalin('base', 'RP'); %Get Settings structure (S) from base workspace for i = 1:size(varargin, 2) if isfield(H, varargin{i}) %update field with input given H(cn).(varargin{i}) = varargin{i+1}; end end end save(History_path, 'H', 'append'); %Save the mat file. assignin('base', 'H', H); %Save in base workspace case 'S' switch sname case 'H' %History' end H = evalin('base', 'H'); %Get H end structure from base workspace if nargin > 1 %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-21: SAVE SETTINGS <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will save settings to S structure % Refer to section 4 of documentation for details. %========================================================================== function elseif ~(isnumeric(Settings{5, 2})) MF_Save_Settings(settings_data) %ROW 5: ROBOT NUMBER settings_path = which('SETTINGS.mat'); msgbox('the number you entered is load (settings_path); not numeric'); for i=1:length(settings_data) try S.value = str2num(S.value{i}); end end %% Check entries for inconsistencies Settings((2:length(settings_data)+1),2) = settings_data(:,2); %>>Check each row for inconsistent numbers; if ~(isboolean(Settings{2, 2})) %ROW 2: ENABLE GRIPPER msgbox('the number you entered is not boolean'); elseif ~(isnumeric(Settings{3, 2})) %ROW 3: RANGE OF ROTATION OF GRIPPER msgbox('the number you entered is not numeric'); elseif ~(isnumeric(Settings{4, 2})) %ROW 4: PORT NUMBER (COM) msgbox('the number you entered is not numeric'); elseif ~(isboolean(Settings{6, 2})) %ROW 6: SIMULATION MODE msgbox('the number you entered is not boolean'); elseif ~(isnumeric(Settings{7, 2})) %ROW 7: TRAJECTORY OPTION**** msgbox('the number you entered is not boolean'); elseif ~(isboolean(Settings{8, 2})) %ROW 8: SHOW TRAIL msgbox('the number you entered is not boolean'); elseif ~(isboolean(Settings{9, 2})) %ROW 9: SHOW AXES msgbox('the number you entered is not boolean'); elseif ~(isboolean(Settings{10, 2})) %ROW 10: ENABLE DYNAMIC SIMULATION msgbox('the number you entered is not boolean'); elseif ~(isboolean(Settings{11, 2})) %ROW 11: LINEAR SPEED APÊNDICE B. Rotinas em Matlab® do Software de Controle msgbox('the number you entered is not boolean'); elseif ~(isboolean(Settings{12, 2})) %ROW 12: MOVE BY INCREMENT: COARSE msgbox('the number you entered is not boolean'); d = RP.d'; a = RP.a'; alpha = RP.alpha'; %DH parameters n = S.value{'dof'}; max_reach = S.value{'max_reach_p'}; %get max reach distance from Settings elseif ~(isboolean(Settings{13, 2})) %ROW 13: MOVE BY INCREMENT: FINE msgbox('the number you entered is not boolean'); if isempty(max_reach) || max_reach == 0 || isnan(max_reach) a_sum = sum(a); d_sum = sum(d); elseif ~(isboolean(Settings{14, 2})) %ROW 14: ANGLES SET msgbox('the number you entered is not boolean'); DH_reach = norm([a_sum, d_sum]); %max reach by DH parameters elseif ~(isboolean(Settings{15, 2})) %ROW 15: NUMBER OF JOINTS msgbox('the number you entered is not boolean'); elseif ~(isboolean(Settings{16, 2})) %ROW 16: HOME POSITION msgbox('the number you entered is not boolean'); end % Compute forward kinematics settings joint position to 0 (q1...qn = 0) [~, Tm] = PF_Forward_Kinematics(zeros(1,n), d, a, alpha); pos_q0 = Tm{n}(1:3, 4)'; % get end-effector position from FK when q=0 FK_reach = norm(pos_q0); max_reach = max(DH_reach, FK_reach); %get the highest value end %% %% Compute either home_q or home_p given one of them %Update the SETTINGS.MAT file save(settings_path, 'S', '-append'); %Load structures files to workspace again MF_Load_Structures() S_F.SETTINGS = Settings; end %% Compute max_reach if not given %========================================================================== % >>>>>>>>>>>>>> FUNCTION MF-22: ENABLE COMMANDS HANDLES <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will enable or disable the handles in commands % table depending on the user selection of trajectory % Refer to section of documentation for details. %========================================================================== H_Tools_Objects = {handles.v_linear, handles.v_angular, handles.time, ... function MF_Enable_Commands(hObject, handles.x_param, handles.y_param, handles) handles.z_param, handles.go_param_traj,... traj_opt = get(handles.traj_opt, handles.load_path, handles.start_path, 'Value'); handles.p_x, handles.p_y, ... handles.p_z, handles.orientation_angle, %Get all handles objects related to the handles.clear_field,... Instructions Panel. APÊNDICE B. Rotinas em Matlab® do Software de Controle handles.move_coord, handles.move_ee_neg_x, handles.move_ee_pos_x,... handles.move_ee_neg_y, handles.move_ee_pos_y, handles.move_ee_neg_z,... handles.move_ee_pos_z, handles.coarse_rb, handles.fine_rb, ... handles.jointn_menu, handles.slider_jointn, handles.joints_move, ... handles.angle_move_joints, handles.move_joints_bt, ... handles.joint_param_eq_menu, handles.param_joint_eq, ... handles.go_param_joint}; [0 0 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0],... [1 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0],... [0 0 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0],... [1 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0],... [0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1],... [0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1],... [1 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0],... [0 0 1 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]}; 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 %Set Boolean values for each handle, 1: enable, 0: disable. % in the same order as appears in the variable H_Tools_Objects (above) Enable_Tool_Handle = {... [0 0 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0],... [1 1 1 0 0 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0],... [0 1 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0],... [0 1 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0],... %Enable or disable each handle for i = 1:1:length(H_Tools_Objects) if Enable_Tool_Handle{traj_opt}(i) == 1 %Enable: on set(H_Tools_Objects{i}, 'Enable', 'on'); else %Enable: off set(H_Tools_Objects{i}, 'Enable', 'off'); end end end %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION MF-23: CLEAR TRAIL <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will clear the trail in the graphical % representation in each command or when the user click on the button % Refer to section 4 of documentation for details. %========================================================================== %% Clear trail path function MF_Clear_trail() %disp('pushed clear trail bottom'); Patch_data = getappdata(0,'patch_h'); %delete line Tr = Patch_data(7); %delete line % setappdata(0,'xtrail',0); % used for trail tracking. setappdata(0,'ytrail',0); % used for trail tracking. setappdata(0,'ztrail',0); % used for trail tracking. % set(Tr,'xdata',0,'ydata',0,'zdata',0); end APÊNDICE B. Rotinas em Matlab® do Software de Controle 5 FUNÇÕES DE SUPORTE %========================================================================== % >>>>>>>>>>>>>>>>>> FUNCTION SPF-1: IMPORT CAD TO MAT <<<<<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - March 30th, 2016. % DESCRIPTION: This function will convert the cad robot file (saved in STL) % in a MAT file called ROBOT_DATA that will be used to generate the % graphical representation of the robotic arm and perform the simulations. % Each link should be saved in a different stl file. For a 6dof robot with % end effector, there should be 7 files: base.stl, link1.stl, % link2.stl,..., link5.stl, end_effector.stl (user must enable end-effector % in Settings. The files should be inserted in the directory OTHER_FILES). % Refer to section 4 of documentation for details. %========================================================================== function SF_Importing_CAD() filenames(n_links+2) = % n: robot degrees of freedom (number strcat(file(3), extension); %adding the of joints) end-effector name % end_effector: 1 for robots with end end_effector, 0 without end_effector clc % %prompt the user to select the stl Settings_path = which('SETTINGS.mat'); files Robots_data_path = % [filename, folder] = uigetfile('*', which('ROBOT_DATA.mat'); 'MultiSelect','on'); % fname = fullfile(folder, filename); load(Settings_path); % %#release in future version: open R_data = window to select the stl files and matfile(Robots_data_path,'Writable',tru % then create a figure with a table e); %load as matfile so then the user can inform the R_info = R_data.RP; %load the table % program which of the file is that contains the D-H parameters base, link and end-effector n_links = S.value{'dof'} - 1; %number of links = number of dof - 1 end_effector = true;%S.value{2}; %true for robot w/ end-effector, false otherwise d = R_info.d'; %get parameter d from L structure (ROBOT_DATA) a = R_info.a'; %get parameter a Alpha = R_info.alpha'; %get parameter alpha %Generating the names for accessing the files extension = '.stl'; file = {'base', 'link', 'end_effector'}; % Assembling the components names for i=1:n_links filenames(i) = strcat(file(2), num2str(i), extension); end filenames(n_links+1) = strcat(file(1), extension); %adding the base name if end_effector==1 % Field names in L structure, Ve: Vertice, Fa: Faces, Co: Colors % (Ve, Fa, Co) are used in patch command for drawing the robot structure_names = {'Ve', 'Fa', 'Co'}; for x = 1:1:length(filenames) file_name = char(filenames(x)); LD(x) = rndread(file_name, structure_names); end % In order to simulate the movement each link must have its frame placed in % the reference frame (global 0), so then the Forward Kinematics will % transform to the position it needs to go. But for facilitate the % conversion the user will load the robot as it is assembled, so an inverse % transform must be applyed to each link (make each link frame go to % global 0) APÊNDICE B. Rotinas em Matlab® do Software de Controle theta = zeros(1, n_links+2); robot is considered in zero configuration %The %Only the vertices need to be transformed (the faces remain the same) for j = 1:n_links %number of links (dof-1) [~, T_m] = PF_Forward_Kinematics(theta(1:j), d(1:j), a(1:j),... Alpha(1:j)); LD(j).Ve = (inv(T_m{j}) * LD(j).Ve')'; end if end_effector==1 % In LD the end-effector is the immediate file after the base (last % row in LD structure) ee_n = size(filenames,2); [~, T_m] = PF_Forward_Kinematics(theta(1:ee_n), d(1:ee_n),... a(1:ee_n), Alpha(1:ee_n)); LD(ee_n).Ve = (inv(T_m{ee_n}) * LD(ee_n).Ve')'; end % figure % ax_size = 1350; % axis([-ax_size/5 ax_size -ax_size/2 ax_size/2 -0 ax_size/3]); % view(135,25) %Adjust the view orientation. % hold on; % grid('on'); % light % % for i=1:length(filenames) % AAA{i} = patch('faces', LD(i).Fa, 'vertices', LD(i).Ve(:,1:3)); % set(AAA{i}, 'facec', [.8,.8,.8]);% set base color and draw % set(AAA{i}, 'EdgeColor','none');% set edge color to none % input('Press a key to continue'); % try % children = get(gca, 'children'); % delete(children); % end % % end save('ROBOT_DATA', 'LD', '-append'); disp('CAD files converted successfully'); end function [STRUCTURE] = rndread(filename, struc_name) % Reads CAD STL ASCII files, which most CAD programs can export. % Used to create Matlab patches of CAD 3D data. % Returns a vertex list and face list, for Matlab patch command. fid=fopen(filename, 'r'); %Open the file, assumes STL ASCII format. if fid == -1 filename error('File could not be opened, check name or path.') end % The first line is object name, then comes multiple facet and vertex lines. % A color specifier is next, followed by those faces of that color, until % next color line. % CAD_object_name = sscanf(fgetl(fid), '%*s %s'); %CAD object name, if needed. % %Some STLs have it, some don't. vnum=0; %Vertex number counter. report_num=0; %Report the status as we go. VColor = 1; % while feof(fid) == 0 % test for end of file, if not then do stuff tline = fgetl(fid); % reads a line of data from file. fword = sscanf(tline, '%s '); % make the line a character string % Check for color if strncmpi(fword, 'c',1) == 1; % Checking if a "C"olor line, as "C" is 1st char. VColor = sscanf(tline, '%*s %f %f %f'); % & if a C, get the RGB color data of the face. end % Keep this color, until the next color is used. APÊNDICE B. Rotinas em Matlab® do Software de Controle if strncmpi(fword, 'v',1) == 1; % % Checking if a "V"ertex line, as "V" fnum = vnum/3; %Number of faces, is 1st char. vnum is number of vertices. STL is vnum = vnum + 1; triangles. % If a V we count the # of V's flist = 1:vnum; %Face list of report_num = report_num + 1; vertices, all in order. % Report a counter, so long files show F = reshape(flist, 3,fnum); %Make a "3 status by fnum" matrix of face list data. if report_num > 249; disp(sprintf('Reading vertix F = F'; num: %d.',vnum)); V = v'; report_num = 0; V(:,4) = 1; %let the same end number of columns for future v(:,vnum) = sscanf(tline, '%*s C = c'; %f %f %f'); % & if a V, get the XYZ %multiplication with the transform data of it. matrix c(:,vnum) = VColor; % A color for each vertex, which will s = {V, F, C}; color the faces. STRUCTURE = cell2struct(s, struc_name, end 2); % we "*s" skip the name "color" and get the data. fclose(fid); end end % Build face list; The vertices are in order, so just number them. %========================================================================== % ========= DYNAMICS FORMULATION USING EULER-LAGRANGE METHOD ============== %========================================================================== % Created by Diego Varalda de Almeida % Date: March 27th, 2016. % This program will generate the formulation for dynamics equations of % motion using the Euler-Lagrange method %========================================================================== function SF_Dynamics_Formulation() %tHE Symbolic Position for each joint %% (q) is the angle T (Theta), used in the clc D-H clear all %parameters below close all dq = sym('dq%d',[1, n],'real')'; %Symbolic Position for each joint ddq = sym('ddq%d',[1, n],'real')'; Robot_data_path = %Symbolic Position for each joint which('ROBOT_DATA.mat'); % m = sym('m%d',[1, n],'real')'; R_data = %Symbolic masses matfile(Robot_data_path,'Writable',true % syms gx gy gz ); %load as matfile % g = [gx gy gz]'; %simbolic gravity LP = R_data.LP; vector. %% CREATE SYMBOLIC VARIABLES tic %starts measurement of time n=3; %NUMBER OF DEGREES OF FREEDOM. %Create structure for saving the matrices S = struct('Tm_j', {zeros(4)}, 'Tm_cm', {zeros(4)}, 'J_L', {zeros(3,n)}, ... 'J_A', {zeros(3,n)},'R', {zeros(3)}); %Symbolic inputs for computation of Torque % %Symbolic Variables for D-H Parameters for each link (for the frame) T = sym('T%d',[1,n],'real'); %Symbolic Theta angles (joint variable) % A = sym('A%d',[1,n],'real'); %Symbolic Alpha angles (constant) % a = sym('a%d',[1,n],'real'); %Symbolic a distance (constant) % d = sym('d%d',[1,n],'real'); %Symbolic d distance (constant) % % %Symbolic Variables for center of mass vector from link axis APÊNDICE B. Rotinas em Matlab® do Software de Controle % rx = sym('rx%d',[1,n],'real'); %distance in x direction (constant) % ry = sym('ry%d',[1,n],'real'); %distance in y direction (constant) % rz = sym('rz%d',[1,n],'real'); %distance in z direction (constant) % % %Inertia Matrix terms % %I = sym('I%d',[1, n],'real'); % Ixx = sym('Ixx%d',[1,n],'real'); % Ixy = sym('Ixy%d',[1,n],'real'); % Ixz = sym('Ixz%d',[1,n],'real'); % Iyy = sym('Iyy%d',[1,n],'real'); % Iyz = sym('Iyz%d',[1,n],'real'); % Izz = sym('Izz%d',[1,n],'real'); % A = sym(deg2rad(LP.alpha)'); Ixx = sym(LP.Ixx'); Ixy = sym(LP.Ixy'); Ixz = sym(LP.Ixz'); Iyy = sym(LP.Iyy'); Iyz = sym(LP.Iyz'); Izz = sym(LP.Izz'); a = sym(LP.a'); d = sym(LP.d'); r_cm = LP.r_cm; r_cm = cat(1, r_cm{:}); rx = sym(r_cm(:,1)'); ry = sym(r_cm(:,2)'); rz = sym(r_cm(:,3)'); g = sym([0 0 9.81]'); mass = LP.mass; m = sym(cat(1,mass{:})); for i=1:n I_matrix = [Ixx(i), -Ixy(i), -Ixz(i); -Ixy(i), Iyy(i), -Iyz(i); -Ixz(i), -Iyz(i), Izz(i)]; S(i).I = I_matrix; end %% COMPUTING JACOBIAN and HOMOGENEOUS TRANSFORMATION(Tm) FOR EACH JOINT Tm_j = eye(4); %T_m: homogeneous transformation matrix (D-H parameters) for i=1:n %-----------------------------------------------% T MATRIX FOR FOR EACH LILNK - D-H PARAMETERS Tm_i = ... [cos(T(i)), -cos(A(i))*sin(T(i)), sin(A(i))*sin(T(i)), a(i)*cos(T(i)); sin(T(i)), cos(A(i))*cos(T(i)),sin(A(i))*cos(T(i)), a(i)*sin(T(i)); 0, sin(A(i)), cos(A(i)), d(i); 0, 0, 0, 1]; Tm_j = Tm_j * Tm_i; %Tm_j = simplify(Tm_j); %Attempt to simplify the matrix. S(i).Tm_j = Tm_j; %Write matrix in the structure. %----------------------------------------------% POSITION VECTOR OF THE CENTER OF MASS OF LINK i TO REFERENCE FRAME % See equation (XX): r0,i = T0,i * ri r_cm = [rx(i), ry(i), rz(i)]'; Pcm = S(i).Tm_j((1:3),(1:3)) * r_cm; if (i > 1) Pcm = Pcm + S(i1).Tm_j((1:3),4); S(i-1).Tm_j((1:3),4) end % Pcm = collect(Pcm); Pcm = combine(Pcm); Pcm = simplify(Pcm); S(i).P = Pcm (1:3); %p vector (last column and 3 first rows of T_cm matrix) % ROTATION MATRIX FOR JOINT i S(i).R = S(i).Tm_j((1:3),(1:3)); %Take the 3x3 rotation matrix from Tm_j. % LINEAR JACOBIAN J_L = zeros(3,n); J_L = jacobian(S(i).P, T); %compute jacobian of the % J_L = simplify(J_L); S(i).J_L = J_L; % ANGULAR JACOBIAN J_A = zeros(3,n); J_A(3,(1:i)) = 1; S(i).J_A = J_A; disp('MATRICES FOR JOINT '); i disp('COMPUTED'); end disp('CALCULATION OF NECESSARY MATRICES COMPLETED'); disp('INITIATING COMPUTATION EQUATIONS OF MOTION'); %% MATRIX M APÊNDICE B. Rotinas em Matlab® do Software de Controle M = zeros(n:n); for i=1:n term_1 = collect(m(i) * S(i).J_L' * S(i).J_L); % term_1 = combine(term_1); % term_1 = simplify(term_1); term_2 = S(i).J_A' * S(i).R * S(i).I * S(i).R' * S(i).J_A; % term_2 = collect(term_2); % term_2 = combine(term_2); % term_2 = simplify(term_2); M = M + (term_1 + term_2); %M = simplify(M); end disp('matriz M computed'); % simplify(M) %The results will be stored in a structure called EM. EM(1).M = M; %EM: Equations of Motion Structure %% TOTAL KINETIC ENERGY K = (1/2) * dq' * EM(1).M * dq; %% FIRST TERM OF THE EQUATION OF MOTION: MATRIX M . ddq %Vector nx1 for i=1:n M_ij = zeros(1:1); for j=1:n M_ij = M_ij + (EM(1).M(i,j) * ddq(j)); end EM(1).Mq(i,1) = M_ij; %EM: Equations of Motion Structure end disp('First term: (MATRIX M . ddq) computed'); %% SECOND TERM OF THE EQUATION OF MOTION: MATRIX C . dq %Vector nx1 for i=1:n C_row = 0; for j=1:n C_terms = 0; for k=1:n M_ij = EM(1).M(i,j); partial_M_ij = diff(M_ij, T(k)); M_jk = EM(1).M(j,k); partial_M_jk = diff(M_jk, T(i)); c_ijk = partial_M_ij (1/2) * partial_M_jk; % c_ijk = collect(c_ijk); % c_ijk = combine(c_ijk); % c_ijk = simplify(c_ijk); C_terms = C_terms + c_ijk * dq(j) * dq(k); end C_row = C_row + C_terms; end EM(1).C(i,1) = C_row; %EM: Equations of Motion Structure end disp('Second term: (MATRIX C . dq) computed'); %% THIRD TERM OF THE EQUATION OF MOTION: MATRIX G %Vector nx1 for i=1:n G_cell = zeros(1,1); for j=1:n G_cell = G_cell + (m(j) * g' * S(j).J_L(:,i)); end EM(1).G(i,1) = G_cell; %EM: Equations of Motion Structure end disp('Third term: matrix G computed'); %% FINAL RESULT: EQUATIONS OF MOTION TORQUE = EM(1).Mq + EM(1).C + EM(1).G; %TORQUE = simplify(TORQUE); TORQUE = collect(TORQUE); disp('terms collected'); TORQUE = combine(TORQUE); disp('terms combined'); % TORQUE = simplify(TORQUE); disp('terms simplifyed'); EM(1).TORQUE = TORQUE; D = R_data.D; EM = TORQUE; save(Robot_data_path, 'EM', '-append'); toc % save('EQUATIONS_MOTION.mat', 'EM'); % disp('EQUATIONS OF MOTION FOR THE ROBOT GENERATED SUCCESSFULLY'); % % % ======================================= =================================== % % ================================ RESULTS =============================== % % ======================================= =================================== % for i=1:n % disp('================================= ============================='); APÊNDICE B. Rotinas em Matlab® do Software de Controle % % disp('================================= % toc ============================='); % end % disp('TORQUE FOR JOINT:'); % i % EM(1).TORQUE(i) % end %========================================================================== % >>>>>>>>>>>> FUNCTION SF-3: GENERATE JACOBIAN EQUATION <<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 2.0 - March 30th, 2016. % DESCRIPTION: This function will generate the jacobian linear matrix to be % used in the Inverse Kinematics function. % Refer to section 6.4.1 for details. %========================================================================== function SF_Jacobian() %% LOAD THE NECESSARY FILES %% Compute the transformation matrix tic Tm_j = eye(4); %T_m: homogeneous transformation matrix (D-H parameters) for i=1:n %---------------------------------%get the folder where the structures --------------are located % T MATRIX FOR FOR EACH LILNK - D-H Rdata_path = which('ROBOT_DATA.mat'); PARAMETERS Tm_i = ... [cos(T(i)), -cos(A(i))*sin(T(i)), Robot_data = sin(A(i))*sin(T(i)), a(i)*cos(T(i)); matfile(Rdata_path,'Writable',true); sin(T(i)), cos(A(i))*cos(T(i)),%load as matfile sin(A(i))*cos(T(i)), a(i)*sin(T(i)); n=6; 0, sin(A(i)), cos(A(i)), d(i); %Symbolic Variables for D-H Parameters 0, 0, for each link (for the frame) 0, 1]; LP = Robot_data.LP; %Read the variable topo from the MAT-file. d = (LP.d(1:n))'; %Get D-H parameter d from the ROBOT_DATA a = (LP.a(1:n))'; %Get D-H parameter a from the ROBOT_DATA Alpha = (LP.alpha(1:n))';%Get D-H parameter Alpha from the ROBOT_DATA % Converting Alpha angles to rad (symbolic variables can only compute in rad) A = deg2rad(Alpha); % d = sym('d%d',[1,n],'real'); %Symbolic Theta angles (joint variable) % a = sym('a%d',[1,n],'real'); %Symbolic Theta angles (joint variable) % A = sym('A%d',[1,n],'real'); %Symbolic Theta angles (joint variable) T = sym('T%d',[1,n],'real'); %Symbolic Theta angles (joint variable) Tm_j = Tm_j * Tm_i; %Tm_j = simplify(Tm_j); to simplify the matrix. %Attempt end %% Get the end-effector position vector P_vector = Tm_j((1:3),4); %try to simplify the equation P_vector = collect(P_vector); P_vector = combine(P_vector); P_vector = simplify(P_vector); %% COMPUTE LINEAR JACOBIAN J_L = zeros(3,n); J_L = jacobian(P_vector, T); %compute jacobian of the p vector J_L = simplify(J_L); %attempt to simplify the vector APÊNDICE B. Rotinas em Matlab® do Software de Controle %% Save the Jacobian in the Equations of motion structure toc D = Robot_data.D; end D(1).Jacobian = J_L; save('ROBOT_DATA.mat','D', '-append'); %========================================================================== % >>>>>>>>>>> FUNCTION SPF-4: ORDERING TABLE OF COORDINATES <<<<<<<<<<<<<<<< %========================================================================== % Created by Diego Varalda de Almeida % version 1.0 - October 27th, 2016. % DESCRIPTION: This function will reorder the table of points from CAD, % that is used in commands tab (Trajectory: modelled by table) % Refer to section 4 of documentation for details. %========================================================================== %DESCRIÇÃO: % UM PERCURSO PODE SER DESENHADO EM UM SOFTWARE DE DESENHO COMO %O AUTOCAD, QUE PODE SER GERADO UMA TABELA DE PONTOS(VER PROCEDIMENTO %ABAIXO). A ORDEM DOS PONTOS NESSA TABELA NÃO SAEM CORRETAS, E COMO A ORDEM %DOS PONTOS É IMPORTANTE PARA UMA TRAJETÓRIA, É NECESSÁRIO REORGANIZAR OS %PONTOS UTILIZANDO O ALGORÍTIMO ABAIXO. % O ALGORÍTMO SUBTRAI O PONTO INICIAL (ESCOLHIDO PELO USUÁRIO) DA TABELA DE % PONTOS DEPOIS É ENCONTRADO O MÓDULO DESSA SUBTRAÇÃO DE VETORES, O MENOR % MÓDULO É O PONTO POSTERIOR AO PONTO INICIAL, ESTE PONTO É ADICIONADO À % UMA NOVA TABELA E APAGADO DA TABELA ANTIGA, O PROCESSO É REPETIDO ATÉ QUE % TODOS OS PONTOS ESTEJAM REORGANIZADOS. %************************************************************************** % PROCEDIMENTO PARA CONVERTER UM TEXTO EM UMA TABELA DE PONTOS (NO AUTOCAD): % OBS.: SERVE PARA TRAJETÓRIAS DESENHADAS TAMBÉM. % % % % % % % % % % % % - ABRA UM ARQUIVO NOVO DO AUTOCAD; ESCREVA UM TEXTO (COM A FONTE DESEJADA); DEFINA O TAMANHO DO TEXTO USANDO O COMANDO SCALE; USE O COMANDO TXTEXP PARA EXPLODIR O TEXTO EM LINHAS; USE O COMANDO EXPLODE PARA EXPLODIR AINDA MAIS AS LINHAS QUE PERMANECEREM JUNTAS; - APAGUE AS LINHAS DESNECESSÁRIAS (DEIXE APENAS O CONTORNO DE CADA LETRA) - USE O COMANDO JOIN PARA JUNTAR TODAS AS LINHAS DE UMA LETRA (CADA LETRA DEVE SER UM OBJETO SÓ). AS VEZES É NECESSÁRIO REPETIR O COMANDO VÁRIAS VEZES PARA TORNAR A LETRA EM UM OBJETO SÓ; - USE O COMANDO DIVIDE PARA DIVIDIR CADA LETRA EM PONTOS (DIGITE A QUANTIDADE DESEJADA DE PONTOS PARA CADA LETRA); % - POSICIONE AS LETRAS PRÓXIMO DA ORIGEM (COORDENADA 0,0) DO ARQUIVO; % - CLIQUE NO BOTÃO TABLE; % - SELECIONE A OPCAO: From object data in the drawing (Data Extraction); % - CLIQUE EM OK E DEPOIS EM NEXT; % % % % % % % % - DIGITE UM NOME PARA SALVAR O DESENHO ATUAL EM .DXE, CLIQUE EM SALVAR; CLIQUE EM NEXT; SELECIONE A OPÇÃO POINT E DESMARQUE AS DEMAIS, CLIQUE EM NEXT; NA CAIXA DE OPÇÕES DO LADO DIREITO MARQUE APENAS GEOMETRY E DESMARQUE AS DEMAIS; - A LISTA DO LADO ESQUERDO VAI SER FILTRADA, SELECIONE Position X e Position Y, CLIQUE EM NEXT - CLIQUE EM NEXT NOVAMENTE; APÊNDICE B. Rotinas em Matlab® do Software de Controle % % % % % - MARQUE AS DUAS OPÇÕES: Insert data extraction into drawing e Output data to external file; - CLIQUE EM NEXT DUAS VEZES E DEPOIS EM FINISH; - CLIQUE EM NA TELA DE DESENHO PARA FIXAR A TABELA COM OS PONTOS. - COPIE A TABELA DE PONTOS GERADOS EM EXCEL EM UMA VARIÁVEL DO MATLAB. %========================================================================== % C: table with points from CAD file (excel) p = C; %points p = p{:,:}; pnew = zeros(size(p)); %preallocating new organized matrix start = [594, 765, 0]; pc = start; %set current position equal to start position for i = 1:size(p,1) dist = []; %cleaning dist matrix for j = 1:size(p,1) dist(j) = norm(pc - p(j,:)); end min_idx = find(dist == min(dist)); %find which point is the closest pnew(i,:) = p(min_idx, :); %copying that row to the new matrix pc = p(min_idx, :); %set the closest point as the current point p(min_idx,:) = []; %clear that row in the table of points; end %converting to table T = table(pnew); % saving to excel file writetable(T,'ORGANIZED_TABLE.xlsx','Wr iteVariableNames',false) 223 APÊNDICE C Projeto do Manipulador