Projeto, Controle e Análise de Um Manipulador Robótico Modular

UNIVERSIDADE DE MOGI DAS CRUZES
Diego Varalda de Almeida
PROJETO, CONTROLE E ANLISE DE UM

MANIPULADOR ROBTICO MODULAR
Mogi das Cruzes

Junho de 2016
UNIVERSIDADE DE MOGI DAS CRUZES
Diego Varalda de Almeida 11121502987

MANIPULADOR ROBTICO MODULAR
Projeto de Graduao apresentado ao curso

de Engenharia Mecnica da Universidade de
Mogi das Cruzes, como parte dos requisitos
necessrios obteno do ttulo de Engenheiro.
Orientador: Prof. Ms. Gilberto Tomz Junior
Universidade de Mogi das Cruzes UMC

Engenharia Mecnica
Programa de Graduao
Mogi das Cruzes

Junho de 2016
Almeida, Diego Varalda de

MANIPULADOR ROBTICO MODULAR/ Diego Varalda de Almeida. Mogi das
Cruzes, Junho de 2016236 p. : il. (algumas color.) ; 30 cm.
Orientador: Prof. Ms. Gilberto Tomz Junior
Monografia Universidade de Mogi das Cruzes UMC
Engenharia Mecnica
Programa de Graduao, Junho de 2016.
1. Manipulador Robtico. 2. Controle. 2. Anlise Dinmica. I. Gilberto Tomaz
Junior. II. Universidade de Mogi das Cruzes. III. Curso de Engenharia Mecnica IV.
Projeto, Controle e Anlise de um Manipulador Robtico Modular
Dedico este trabalho aos meus pais,

Suleine e Arlindo e todos os
moradores do flat 33
Agradecimentos
No decorrer de minha formao algumas pessoas e instituies foram de vital importncia
e merecem os meus agradecimentos. Primeiramente, gostaria de expressar minha gratido
ao meu orientador, prof. Ms. Gilberto Tomz Junior, pelas orientaes proporcionadas
que definitivamente contriburam para a realizao dessa monografia.
Alm do meu orientador, meus sincero agradecimento vai tambm aos professores
ph.D. Jan Vorstius e ph.D. William Lewinger, da Universidade de Dundee na Esccia,
pelo auxlio e orientaes durante o desenvolvimento do software no estgio de vero no
Laboratrio de Robtica. Seus conselhos e ensinamentos me proporcionaram uma maior
compreenso sobre este fascinante campo que a robtica.
Agradeo tambm ao Governo Brasileiro, em especial, ao Ministrio da Educao e
CNPq que criaram programas de incentivo educao tais como o PROUNI e o Cincias
sem Fronteiras, o que me possibilitou concluir o curso de engenharia e participar de um
intercmbio.
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 diverso que compartilhamos durante estes anos de graduao,
tanto em sala de aula quanto fora dela.
Jamais considere seus estudos como uma obrigao,

mas como uma oportunidade invejvel (. . . ) para aprender
a conhecer a influncia libertadora da beleza do reino do
esprito, para seu prprio prazer pessoal e para proveito
da comunidade qual seu futuro trabalho pertencer.
(Albert Einstein)
Resumo
Manipuladores robticos so amplamente utilizados na indstria por serem mquinas de
grande versatilidade. Este tipo de rob pode realizar tarefas como: pintura, soldagem,
identificao e movimentao de objetos alm de montagem de componentes em uma linha
de montagem entre outras. Contudo, para se realizar tais tarefas o rob necessita de uma
srie de instrues fornecidas pelo usurio. Estas instrues so geralmente linhas de cdigo
em uma linguagem prpria da mquina, que so armazenadas na memria do controlador
e posteriormente lidas pelo rob. Com o intuito de agilizar a fase de programao de
uma tarefa, um software de controle baseado em interface grfica se faz necessrio. Esta
interface grfica possibilita o usurio a passar instrues para o manipulador robtico
atravs de simples opes pr-programadas podendo, inclusive, combinar estas opes para
criar tarefas mais complexas. Neste trabalho, ser proposto um projeto de manipulador
robtico modular com seis graus de liberdade e apresentado um programa baseado em
interface grfica desenvolvido em MATLAB para controle e simulao do manipulador.
Palavras-chave: manipulador. rob. projeto. controle. software. anlise. interface
grfica.
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 ilustraes
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 trs ngulos de rotao de um punho esfrico . . . . . . . . . . . . .
Figura 4 Configuraes bsicas de um manipulador robtico: (a: cartesiano; b:
cilndrico; c: polar; d: articulado). . . . . . . . . . . . . . . . . . . . . .
Figura 5 Tipos de juntas empregadas em robs . . . . . . . . . . . . . . . . . .
Figura 6 Representao esquemtica das juntas . . . . . . . . . . . . . . . . . .
Figura 7 Manipulador planar de 2 GDL com suporte em O . . . . . . . . . . . .
Figura 8 Tpico volume de trabalho para configuraes comuns de robs . . . . .
Figura 9 Cinemtica: a)direta; b)inversa . . . . . . . . . . . . . . . . . . . . . .
Figura 10 Movimento descoordenado (esquerda: deslocamento de cada junta; direita: trajetria do manipulador no plano) . . . . . . . . . . . . . . . .
Figura 11 Movimento sequencial (esquerda: deslocamento de cada junta; direita:
trajetria do manipulador no plano) . . . . . . . . . . . . . . . . . . .
Figura 12 Movimento coordenado (esquerda: deslocamento de cada junta; direita:
trajetria do manipulador no plano) . . . . . . . . . . . . . . . . . . .
Figura 13 Aproximao numrica de derivada . . . . . . . . . . . . . . . . . . . .
Figura 14 Exemplo de trajetria desenhada em CAD . . . . . . . . . . . . . . . .
Figura 15 Grfcos da trajetria linear: a) trajetria em linha reta; b) aproximao
da linha reta por pontos de passagem; c) curvas de coordenadas das
juntas em funo da distncia linear; d) velocidade angular; e) acelerao
angular . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Figura 16 Problemas geomtricos com trajetria cartesiana: a)tipo 1; b)tipo 2;
c)tipo 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Figura 17 Parmetros de Denavit-Hartenberg . . . . . . . . . . . . . . . . . . . .
Figura 18 Manipulador do tipo articulado com os parmetros D-H . . . . . . . .
Figura 19 Diviso da matriz de transformao homognea . . . . . . . . . . . . .
Figura 20 Representao geomtrica do Jacobiano . . . . . . . . . . . . . . . . .
Figura 21 Fluxograma de soluo da Cinemtica Reversa por Jacobiano . . . . .
Figura 22 Trajetria com restrio de orientao . . . . . . . . . . . . . . . . . .
Figura 23 Diagrama de blocos para o problema de controle do rob. . . . . . . .
Figura 24 Sistema de simulao do rob e controle . . . . . . . . . . . . . . . . .
Figura 25 Esquema do controle antecipatrio . . . . . . . . . . . . . . . . . . . .
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
Compensao de distrbio por torque computado . . . . . . . . . . . .

Diagrama de blocos do mtodo de torque computado . . . . . . . . . .
Manipulador proposto em uma configurao arbitrria . . . . . . . . .
Junta do cotovelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Frameless brushless DC motor . . . . . . . . . . . . . . . . . . . . . . .
Harmonic drive escolhido para a junta do cotovelo . . . . . . . . . . . .
Freio permanente eletromagntico, modelo Combiperm . . . . . . . . .
Trava mecnica proposta . . . . . . . . . . . . . . . . . . . . . . . . . .
Sensor de torque . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Eixo principal da junta do cotovelo . . . . . . . . . . . . . . . . . . . .
Sensor de posio angular (encoder) . . . . . . . . . . . . . . . . . . .
Conjunto do elo 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Vista em corte do punho . . . . . . . . . . . . . . . . . . . . . . . . . .
Resultado da anlise feita no elo 2 . . . . . . . . . . . . . . . . . . . .
Resultado da anlise feita no elo 3 . . . . . . . . . . . . . . . . . . . .
Resultado da anlise feita no eixo principal da junta da base . . . . . .
Resultado da anlise feita no eixo principal da junta do cotovelo . . . .
Resultado da anlise feita no eixo principal da junta do punho . . . . .
Manipulador proposto com os eixos de referncia de cada elo . . . . . .
Diagrama de blocos do controle torque computado . . . . . . . . . . .
Diagrama de blocos do controle antecipatrio . . . . . . . . . . . . . .
Pontos de passagem com as velocidades desejadas nos pontos indicados
pelas tangentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Exemplo 1 - coordenado; esquerda: percurso no espao cartesiano; direita:
curvas de posio, velocidade e acelerao das juntas . . . . . . . . . .
Exemplo 2 - descoordenado; esquerda: percurso no espao cartesiano;
direita: curvas de posio, velocidade e acelerao das juntas . . . . . .
Exemplo 3 - sequencial; esquerda: percurso no espao cartesiano; direita:
Exemplo 4 - linear; esquerda: percurso no espao cartesiano; direita:
Exemplo 5 - circulo; esquerda: percurso no espao cartesiano; direita:
Exemplo 6 - elipse; esquerda: percurso no espao cartesiano; direita:
Exemplo 7 - curva de Lissajous; esquerda: percurso no espao cartesiano;
Exemplo 8 - hipotrocoide; esquerda: percurso no espao cartesiano;
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 - hlice cnica; esquerda: percurso no espao cartesiano;

direita: curvas de posio, velocidade e acelerao das juntas . . . . . . 99
Figura 57 Vetores do centro de gravidade de cada elo . . . . . . . . . . . . . . . . 100
Figura 58 Curvas de torque da trajetria 1 - coordenado . . . . . . . . . . . . . . 102
Figura 59 Curvas de torque da trajetria 2 - descoordenado . . . . . . . . . . . . 102
Figura 60 Curvas de torque da trajetria 3 - sequencial . . . . . . . . . . . . . . . 103
Figura 61 Curvas de torque da trajetria 4 - linear . . . . . . . . . . . . . . . . . 103
Figura 62 Curvas de torque da trajetria 5 - crculo . . . . . . . . . . . . . . . . . 104
Figura 63 Curvas de torque da trajetria 6 - elipse . . . . . . . . . . . . . . . . . 104
Figura 64 Curvas de torque da trajetria 7 - curva de Lissajous . . . . . . . . . . 105
Figura 65 Curvas de torque da trajetria 8 - hipotrocoide . . . . . . . . . . . . . 105
Figura 66 Curvas de torque da trajetria 9 - hlice . . . . . . . . . . . . . . . . . 106
Figura 67 Posio do manipulador em diferentes momentos da simulao . . . . . 107
Figura 68 Resultados da simulao dinmica . . . . . . . . . . . . . . . . . . . . 108
Figura 69 Estrutura e princpio de funcionamento do SJM III . . . . . . . . . . . 111
Figura 70 Mensagem exibida ao iniciar o programa . . . . . . . . . . . . . . . . . 118
Figura 71 Aba Configuraes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
Figura 72 Conjuntos de ngulos - (a)conjunto da junta, (b)conjunto global, (c)conjunto
do motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
Figura 73 Tabela de parmetros 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 atravs de funes paramtricas . . . . . . . . . . . . . . . . . 130
Figura 79 Aba Programas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
Figura 80 Aba Simulao Dinmica . . . . . . . . . . . . . . . . . . . . . . . . . . 133
Figura 81 Janela de Animao . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134
Lista de tabelas
Tabela 1 Comparao entre diferentes mtodos de cinemtica inversa; modelo
com um efetuador, 18 GDL, e metas dentro do volume de trabalho .
Tabela 2 Comparao entre diferentes mtodos de cinemtica inversa; modelo
com um efetuador, 18 GDL, e metas fora do volume de trabalho . . .
Tabela 3 Capacidade de carga do manipulador . . . . . . . . . . . . . . . . . .
Tabela 4 Especificaes tcnicas estimadas para o manipulador . . . . . . . . .
Tabela 5 Especificaes tcnicas das juntas . . . . . . . . . . . . . . . . . . . .
Tabela 6 Parmetros de DenavitHartenberg do manipulador proposto . . . .
Tabela 7 Tipos e caractersticas de trajetrias . . . . . . . . . . . . . . . . . .
Tabela 8 Propriedades dos elos . . . . . . . . . . . . . . . . . . . . . . . . . .
Tabela 9 Variveis da estrutura Histrico (H) . . . . . . . . . . . . . . . . . .
Tabela 10 Variveis da estrutura Mensagens (M) . . . . . . . . . . . . . . . . .
Tabela 11 Variveis da estrutura Configuraes 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 Numrico Computadorizado
DC
Corrente contnua.
FEA
Anlise de Elementos Finitos
GDL
Graus de liberdade
GUI
Interface Grfica do Usurio
GUIDE
Graphical User Interface Developer Environment
NASA
Administrao Nacional da Aeronutica e Espao
PCI
Placa de circuito impresso
PID
Sistema de controle do tipo Proporcional, Integral e Derivado.
PWM
Modulao por Largura de Pulso
RIA
Robotics Institute of America
RPM
Rotaes por minuto
SI
Sistema Internacional de Unidades
SISO
nica Entrada - nica Sada
Lista de smbolos
J
Matrix Jacobiana
Tempo [s]
Coordenada de uma junta de revoluo, [rad]
Velocidade da varivel de junta, [rad/s]
Acelerao da varivel da junta, [rad/s2 ]
Constante de amortecimento
ai
Coeficiente i da funo polinomial
Lagrangiano
qi
Coordenada generalizada
Matriz de transformao homognea
Qi
Foras externas generalizadas
Energia cintica, [kg.m2 s2 ]
Energia potencial, [kg.m2 s2 ]
Tensor de inrcia, [kg.m2 ]
Vetor distncia
Densidade do corpo, [kg.m3 ]
Transformada de Laplace
Sumrio
1
1.1
1.1.1
1.2
1.3
1.4
1.5
INTRODUO . . . . . . . . . . . . . . . . . . . . .
Histrico dos manipuladores robticos industriais
Manipuladores modulares . . . . . . . . . . . . . . . . . .
Motivao . . . . . . . . . . . . . . . . . . . . . . . . .
Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . .
Metodologia . . . . . . . . . . . . . . . . . . . . . . . .
Organizao 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
REVISO BIBLIOGRFICA . . . . . . . . . .
Manipuladores robticos . . . . . . . . . . . . . .
Estrutura e componentes de um rob . . . . . . . . .
Sistemas de acionamento e conguraes do rob . . .
Esquema de notao de juntas . . . . . . . . . . . . .
Movimentos do rob e graus de liberdade . . . . . . .
Volume de trabalho . . . . . . . . . . . . . . . . . . .
Ciclo de funcionamento de um manipulador . . . . .
Cinemtica e gerao de trajetria . . . . . . . .
Dinmica e controle . . . . . . . . . . . . . . . . .
Modelagem dinmica . . . . . . . . . . . . . . . . . .
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 . . . . . . . . . . . . . . . . . .
Gerao de trajetrias . . . . . . . . . . . . . . .
Trajetria ponto-a-ponto . . . . . . . . . . . . . . . .
Trajetrias parametrizadas . . . . . . . . . . . . . . .
Trajetria modelada em AutoCad . . . . . . . . . . .
Trajetria linear . . . . . . . . . . . . . . . . . . . .
Algoritmo anticoliso . . . . . . . . . . . . . . . . . .
Cinemtica do manipulador . . . . . . . . . . . .
Cinemtica direta . . . . . . . . . . . . . . . . . . . .
Cinemtica inversa: mtodo pseudo-inverso Jacobiano
Mnimos 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
Redundncia e Singularidades . . . . . . . .
Velocidade e acelerao . . . . . . . . . . . .
Restries na congurao do manipulador .
Dinmica do Manipulador . . . . . . . .
Conceitos preliminares . . . . . . . . . . . .
Equaes de movimento do manipulador . .
Extendendo para um manipulador de n GDL
Sistemas de controle . . . . . . . . . . . .
Rastreio de um ponto de referncia . . . . .
Controle antecipatrio 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 ROBTICO

Requisitos iniciais . . . . . . . . . . . . . . . . . . . . . . . .
Capacidade de carga . . . . . . . . . . . . . . . . . . . . . . . .
Nmero de graus de liberdade . . . . . . . . . . . . . . . . . .
Velocidade linear . . . . . . . . . . . . . . . . . . . . . . . . .
Detalhamento do mecanismo . . . . . . . . . . . . . . . . .
Mdulo da junta . . . . . . . . . . . . . . . . . . . . . . . . . .
Elos do manipulador . . . . . . . . . . . . . . . . . . . . . . . .
Punho . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Anlise 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
ANLISE E RESULTADOS
Cinemtica . . . . . . . . . . .
Jacobiano . . . . . . . . . . . . .
Controle . . . . . . . . . . . . .
Trajetria . . . . . . . . . . . .
Exemplo de trajetrias . . . . .
Dinmica . . . . . . . . . . . .
Curvas de torque dos exemplos .
Exemplo de simulao dinmica
6
6.1
6.1.1
6.1.2
6.1.3
CONSIDERAES FINAIS . . . . . . . . . . . . . . . . .
Sugestes para trabalhos futuros . . . . . . . . . . . . . . .
Implementar um sistema de controle de fora no software . . . .
Reprogramao do software em outra linguagem de programao
Implementao de mecanismo de segurana . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
. .
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
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
REFERNCIAS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
APNDICES
116
APNDICE A

SOFTWARE DE CONTROLE .
A.1
Leiaute e funcionalidades do programa . . . . .
A.1.1
Caixa de Mensagens . . . . . . . . . . . . . . . . . . .
A.1.2
Aba Conguraes . . . . . . . . . . . . . . . . . . . .
A.1.2.1
Obtendo a matriz de inrcia 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 Simulao Dinmica
Janela de Animao . . .
Arquivos do programa
SMART-GUI.m . . . . .
SMART-GUI.g . . . . .
Funes . . . . . . . . . .
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
APNDICE B
R DO SOFTWARE
ROTINAS EM MATLAB
DE CONTROLE . . . . . . . . . . . . . . . . . . . 139
APNDICE C
PROJETO DO MANIPULADOR . . . . . . . . 223
17
Captulo 1
Introduo
1.1
Histrico dos manipuladores robticos 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 props as quatro leis da robtica em sua curta histria de fico
cientfica Runaround, com o intuito de proteger a humanidade de geraes inteligentes de
robs. As quatro leis so:
Lei zero: um rob no pode causar mal humanidade ou, por omisso,
permitir que a humanidade sofra algum mal.
Lei um: Um rob no pode ferir um ser humano ou, por inao, 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 trs: Um rob deve proteger sua prpria existncia desde que tal
proteo no entre em conflito com a Primeira ou Segunda Leis (JAZAR,
2007).
Posteriormente, o rob industrial foi definido pelo Instituto de Robtica da Amrica (RIA), como um manipulador multifuncional reprogramvel, projetado para mover
materiais, peas, ferramentas, ou outros dispositivos especializados atravs de movimentos
programveis e realizar uma srie de outras atividades (DUYSINX; GERADIN, 2004).
O primeiro rob moderno foi o Unimates, produzido por J. Engelberger no incio
dos anos 60. A empresa Unimation foi a primeira a comercializar robs. Por este motivo
Engelberger conhecido como o pai da robtica. Nos anos 80 a indstria de robs cresceu
muito rpido principalmente devido ao grande investimento pela indstria automotiva.
Robs apareceram como um resultado da combinao de duas tecnologias: teleoperadores e controle numrico computadorizado (CNC) de mquinas de usinagem.
Teleoperadores foram desenvolvidos durante a Guerra Mundia II para manipulagem de
materiais radioativos, e CNC foi desenvolvido para aumentar a preciso que era requisitada
para a manufatura de peas com novas tecnologias. Portanto, os primeiros robs no
passavam de estruturas mecnicas com comando numrico que basicamente transferiam
um material de um ponto A para B (JAZAR, 2007).
Os robs tradicionais possuem manipuladores de preciso controlados por PID com
altos ganhos que corrigem distrbios rapidamente. O manipulador so movidos geralmente
Captulo 1. Introduo
18
por motores de passo ou DC conectados a uma reduo enquanto que encoders rastreiam as
juntas permitindo o controle da posio. Atuadores dinmicos (incluindo massas, refletores
inerciais e rigidez) influenciam drasticamente no sistema de controle e performance geral.
Eles transmitem alta (idealmente infinita) impedncia mecnica, o que significa que o rob
resiste ao movimento quando sujeito uma fora. Os robs tambm possuem alta largura
de banda e movem para a posio comandada no importa a fora externa agindo em suas
juntas. Esta caracterstica perfeita para automao industrial por que permite que os
robs rastreiam trajetrias em ambientes estticos ou mapeados, como em selecionar e
levantar, por exemplo.
1.1.1 Manipuladores modulares

Manipuladores robticos so cinematicamente compostos de elos conectados por juntas
para formar uma cadeia cinemtica. Contudo, o rob como um sistema, consiste de um
manipulador, um punho, um efetuador, atuadores, sensores, controladores, processadores,
e software (JAZAR, 2007).
Manipuladores modulares so geralmente, formados por juntas rotativas e ligados
por elos. Possuem duas juntas na regio do ombro, uma na parte do cotovelo e trs juntas
compondo o punho. A maioria dos manipuladores robticos moudulares so colaborativos,
isto , permitem que o operador trabalhe no mesmo ambiente em que o rob est sem a
necessidade de grades de proteo, caracterstica esta vantajosa para tarefas complexas
que o rob no 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)
Captulo 1. Introduo
1.2
19
Motivao
Uma das motivaes para a realizao desse trabalho se baseia na escassez de robs
modulares na indstria. A maioria dos modelos presentes no mercado atualmente so de
grande porte e possuem configurao de elos e juntas fixa, o que dificulta sua aplicao
em determinados ambientes.
Outro motivo o fato de que os software controladores de robs e os prprios robs
que possuem certa preciso terem preos muito elevados, dificultando a aquisio dessas
mquinas em pequenas e mdias empresas. Alm disso, os softwares de controle presentes
atualmente so de pouco ou quase nenhuma versatilidade, isto , so produzidos para
um nico rob. Por este motivo, o software de controle foi desenvolvido de maneira que
possa ser empregado em vrios manipuladores com configurao de juntas de revoluo,
precisando apenas de fazer uma modificao dos parmetros passados ao software tais
como: quantidade de juntas, massa, comprimento e centro de massa de cada elo.
Por ltimo, a interface grfica foi projetada de maneira a apresentar ferramentas
didticas para o aprendizado da disciplina de robtica e automao nas salas de aula. O
modo de simulao permite que o software simule movimentos do manipulador sem que
um manipulador fsico esteja conectado ao programa.
1.3
Objetivos
Este trabalho tem como objetivo principal desenvolver o projeto de um prottipo de

manipulador robtico modular na qual seja possvel acoplar um determinado nmero de
juntas. Tambm ser desenvolvido neste trabalho um software de controle com interface
grfica utilizando o software Matlab capaz de controlar o manipulador robtico em questo.
1.4
Metodologia
O trabalho comea com o projeto de um manipulador robtico modular utilizando o

software Solidworks. Durante o desenvolvimento do manipulador, o software ANSYS
ser empregado para anlise de elementos finitos (FEA) de suas partes. Em seguida, as
equaes e mtodos para a anlise comportamental do manipulador so estudados. Como o
manipulador modular e pode possuir diversas juntas, mtodos algbricos para se calcular
a cinemtica e dinmica do rob no so apropriados, devendo ento fazer uso de mtodos
numricos para obteno dos resultados.
Em seguida, um sistema de controle usando as equaes da dinmica criado para
o manipulador e simulado no software MATLAB. Este sistema de controle ser utilizado
no software desenvolvido permitindo um controle preciso do manipulador.
Captulo 1. Introduo
20
Por fim a interface grfica desenvolvida, utilizando a toolbox GUIDE do MATLAB.

A interface grfica apresentar uma opo de trabalhar apenas em modo de simulao,
onde simular os movimentos do rob quando este no estiver conectado ao computador,
ou em modo real, que efetivamente manda o sinal para o manipulador, movimentando-o.
1.5
Organizao e escopo do trabalho
Este trabalho est dividido em 6 captulos. O primeiro consistiu de uma breve introduo
ao tema abordado e histrico dos manipuladores robticos.
No Captulo 2 apresentado uma reviso bibliogrfica dos principais temas relacionados a este trabalho. Nesta reviso mostrado os mtodos existentes para modelagem
cinemtica e dinmica para manipuladores robticos, alm dos diferentes tipos de sistemas de controle. mostrado nesse captulo quais os mtodos que sero empregados e a
justificativa de se usar tais mtodos.
O Captulo 3 apresenta toda a metodologia para o trabalho. Neste captulo a
definio e modelagem matemtica dos mtodos escolhidos para a cinemtica, dinmica e
sistema de controle utilizados em manipuladores robticos so apresentados e discutidos.
No Captulo 4, a modelagem do manipulador modular proposto apresentada.
Ser mostrado neste captulo os elementos de mquinas utilizado, tais como: redutor de
velocidade, transmissor de potncia, sensores eltricos e outros componentes que compem
as juntas. Ao final realizada uma anlise de elementos finitos dos componentes estruturais
do rob.
O Captulo 5 apresenta a anlise e resultados para o manipulador de seis graus-deliberdade apresentado no Captulo 4. aplicado neste captulo as equaes e conceitos
visto no Captulo 3. feita tambm uma discusso sobre o comportamento cinemtico e
dinmico para diferentes tipos de trajetrias.
No Apndice A, o software de controle baseado em interface grfica apresentado.
mostrado como o programa foi desenvolvido, alm do papel de cada funo dentro do
programa. mostrado ao final deste apndice como a interface grfica utilizada para o
controle do manipulador robtico.
Por fim, no Captulo 6, so apresentadas as concluses do trabalho e as propostas
de trabalhos futuros.
21
Captulo 2
Reviso Bibliogrfica
Spong e Vidyasagar (1989) sugerem que existem seis problemas que devem ser estudados
para se controlar efetivamente um manipulador robtico, so eles: cinemtica direta,
cinemtica inversa, velocidade cinemtica, dinmica, controle da posio e controle da
fora. Existem inmeros mtodos para se tratar cada problema citado, o modelo dinmico
do manipulador, por exemplo, pode ser desenvolvido usando-se a segunda lei de Newton
ou o mtodo de energia de Euler-Lagrange. Neste captulo feita uma reviso bibliogrfica
em que procura-se apresentar os mtodos existentes na literatura e justificar a escolha dos
mtodos escolhidos que sero empregados no software de controle do manipulador.
2.1
Manipuladores robticos
Na indstria os robs do tipo manipulador so empregados em uma variedade de aplicaes,

sendo a maioria delas para a movimentao de materiais, peas e ferramentas de diversos
tipos (GROOVER, 1988). As aplicaes industriais podem ser dividas nas reas: aplicaes
de manuseio de materiais e carregamento e descarregamento de mquinas; aplicaes
de processamento (soldagem a ponto, soldagem a arco; pintura a pistola, entre outras);
montagem e inspeo (GROOVER, 1988).
2.1.1 Estrutura e componentes de um rob

Um manipulador robtico tem uma estrutura composta de cinco componentes que interagem juntos para garantir que o manipulador consiga completar a funo passada a ele
(DUYSINX; GERADIN, 2004).
A estrutura mecnica, ou mecanismo articulado, feito idealmente de membros
rgidos ou elos que articula junto com juntas mecnicas. Esta estrutura carrega o efetuador
que pode ser uma ferramenta, garra, ou outro dispositivo especfico.
Os atuadores proporcionam a potncia mecnica de forma a mover a estrutura
mecnica contra a ao de foras externas, gravidade, inrcia para modificar a configurao
e, portanto, a posio geomtrica da ferramenta. Estes podem ser eltricos, hidrulicos, ou
pneumticos e devem ser controlados apropriadamente.
A transmisso mecnica, conecta e adapta os atuadores estrutura mecnica. Esta
transmisso adapta os atuadores para carga aplicada e ao mesmo tempo transmitem os
Captulo 2. Reviso Bibliogrfica
22
esforos do atuador para as juntas mecnicas.

Os sensores proporcionam os sentidos para o rob. Eles podem, por exemplo,
providenciar informaes de tato, viso, e temperatura. Eles so divididos em dois grupos:
sensores que fornece informao sobre o prprio rob ou sobre o ambiente externo, na qual
o manipulador se encontra.
E por fim, tem-se a unidade de controle, que assume algumas funes, como: coletar
e processar as informaes dadas pelos sensores; tomar decises sobre o plano de movimento
do manipulador; e organizar o fluxo de informaes 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 mecnica detalhada a seguir.
Elos
Os corpos rgidos individuais que compem o rob so chamados elos. Em robtica,
geralmente quando dizemos brao, nos referimos aos elos. Uma brao robtico um
membro rgido que pode ter movimento relativo com respeito de todos os outros elos. Do
ponto de vista cinemtico, dois ou mais membros conectados juntos de forma que no
possvel ter movimento relativo entre eles so considerados um nico elo (JAZAR, 2007).
23
Juntas
Dois elos so conectados por contato na junta onde seus movimentos relativos podem ser
expressos por uma nica coordenada. Juntas so tipicamente de revoluo ou prismtica.
A junta de revoluo permite rotao relativa entre os elos, j a junta prismtica permite
movimento de translao relativo entre dois elos.
Rotao relativa de elos conectados por uma junta de revoluo ocorre sobre um
linha chamada de eixo da junta, esse tambm o caso para movimento linear que ocorre
em elos conectados por uma junta prismtica. O valor da coordenada que descreve a
posio relativa de dois elos conectados a uma junta chamado de coordenada da junta
ou varivel da junta (JAZAR, 2007).
Punhos
As juntas na cadeia cinemtica entre o antebrao e o efetuador so referidos como punho.
Enquanto que a estrutura do manipulador responsvel por posicionar o efetuador em
uma posio do espao, o punho responsvel em orientar o efetuador nos trs ngulos
desejados. A maioria dos punhos so projetados como esfricos, o que significa que trs
eixos de juntas rotativas se intersectam em um ponto chamado de ponto do punho. Este
tipo de configurao, simplifica a anlise cinemtica, permitindo a separao da posio e
orientao do efetuador (DUYSINX; GERADIN, 2004; JAZAR, 2007).
Figura 3 Os trs ngulos de rotao de um punho esfrico
Fonte: PLW (2013)
24
Efetuadores
O efetuador a parte montada no ltimo elo para fazer o trabalho necessrio do rob.
O tipo mais simples do efetuador a garra, que geralmente capaz de executar duas
aes: abrir e fechar. A montagem do punho e brao do rob so usados essencialmente
para posicionar e orientar o efetuador e a possvel ferramenta que ele pode carregar. o
efetuador ou a ferramenta que ir realmente executar a tarefa (JAZAR, 2007). Por ser um
item muito especfico de cada tarefa, o projeto do efetuador no faz parte do escopo desse
trabalho, mas apontado nas sugestes de trabalhos futuros.
2.1.2 Sistemas de acionamento e conguraes do rob

Robs industriais so acionados por um dos trs tipos de sistema de acionamento (GROOVER, 1988):
1 Acionamento hidrulico;
2 Acionamento eltrico;
3 Acionamento pneumtico.
Robs industriais esto disponveis em uma variedade de tamanhos, formas e
configuraes fsicas. As quatro configuraes bsicas so (GROOVER, 1988):
1 Configurao polar;
2 Configurao cilndrica;
3 Configurao de coordenadas cartesianas;
4 Configurao articulada.
A configurao adotada para o projeto do manipulador apresentado neste trabalho,
foi a configurao articulada. Sua configurao semelhante de um brao humano.
composta por juntas rotacionais e elos em sequncia alternada. Um punho pode ser unido
extremidade do antebrao, proporcionando, assim, um maior nmero de graus-de-liberdade.
Manipulador articulado
De acordo com Craig (1989), um manipulador antropormfico, ou articulado, geralmente
consiste de duas juntas no ombro, sendo uma para elevao para fora do plano horizontal
e outro para rotao em torno de um eixo vertical (geralmente o eixo z), uma junta no
cotovelo, que muitas vezes paralelo junta de elevao do ombro e duas ou trs juntas
no punho.
25
Figura 4 Configuraes bsicas de um manipulador robtico: (a: cartesiano; b: cilndrico;

c: polar; d: articulado).
Fonte: Duysinx e Geradin (2004)
Os robs articulados tm a vantagem de serem capazes de alcanar espao confinados, como a soldagem dentro de um automvel, por exemplo, o que minimiza a intruso da
estrutura do rob no espao de trabalho. Outra vantagem que eles geralmente requerem
uma estrutura muito menor do que a dos robs cartesianos, o que os tornam mais baratos
e mais comuns em aplicaes industriais (CRAIG, 1989).
2.1.3 Esquema de notao de juntas

A configurao de um manipulador robtico pode ser descrita em termos de tipos de
juntas. Comea-se pela junta mais prxima a base e prosseguindo para as outras 2 juntas
seguintes. Para um manipulador do tipo configurao articulada, a denominao se torna:
TRR ou VVR. Onde T uma junta de toro, R junta de revoluo e V uma junta
revolvente, como mostrado na figura abaixo.
A notao do rob pode incluir os movimentos do punho, mediante designao
de dois ou trs tipos de juntas de punho. A importncia de se designar um manipulador
em relao ao tipo de suas juntas se deve ao fato que a configurao 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 realizao de alguma
tarefa especfica, como por exemplo, o efetuador pode ser uma ferramenta ou uma garra,
de maneira a movimentar ou usinar uma pea. Os movimentos de juntas individuais e do
26
Figura 5 Tipos de juntas empregadas em robs
Fonte: Duysinx e Geradin (2004)

Figura 6 Representao esquemtica das juntas
Fonte: Groover (1988)
punho so chamados de graus de liberdade, sendo um rob industrial tpico equipado com
4 a 6 graus de liberdade (GROOVER, 1988).
Em um corpo rgido, grau de liberdade definido como o nmero de movimentos
independentes que ele possui. Pode-se calcular o nmero de graus de liberdade de um
sistema utilizando a equao Gruebler (YI FINGER SUSAN, 2010).
GDL = 3 (e 1) 2n h
Onde:
GDL o total de graus de liberdade de um mecanismo
e o nmero de elos (incluindo suporte)
(2.1)
27
n o nmero de pares inferiores (um grau de liberdade)

h equivale ao nmero de pares superiores (dois graus de liberdade)
Os pares inferiores so considerados as juntas (tanto de rotao como de translao), j os
pares superiores so considerados as engrenagens ou cames. Para um manipulador com
duas juntas e dois elos como mostrado na Figura 7, o nmero 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 no
possui engrenagens e/ou cames. Se fizermos e = 4 e n = 3, ento GDL = 3 e para e = 5 e
n = 4, GDL = 4. possvel perceber que, para um manipulador com juntas de revoluo,
a cada par de junta e elo inserido, o nmero 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 espao 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 brao esfrico (TRL), por exemplo,
seu volume de trabalho seria, teoricamente, uma esfera cujo raio corresponde a extenso
mxima do brao esticado. A figura abaixo mostra o volume de trabalho para cada
configurao do rob.
28
Figura 8 Tpico volume de trabalho para configuraes comuns de robs
Fonte: Groover (1988)
2.1.6 Ciclo de funcionamento de um manipulador

Fazendo uso de um software de controle baseado em interface grfica, o usurio especifica
uma posio para o efetuador, esta posio pode ser uma coordenada final apenas ou
uma trajetria no espao ; o usurio ento define a velocidade do efetuador (esta pode ser
constante ou uma funo do tempo) ou o tempo desejado para finalizar o trajeto, o computador ento precisa converter as coordenadas definidas pelo usurio (estas coordenadas
so cartesianas: xyz) em coordenadas das juntas (ngulo ), esta converso feita usando
as equaes da cinemtica e chamada de cinemtica inversa. Quando o usurio especifica
apenas o ponto final (meta) para o efetuador, o computador calcula os ngulos das juntas
apenas para este ponto e faz uma interpolao polinomial utilizando o tempo do trajeto
para fazer um movimento suave, evitando picos de velocidade e acelerao. Para o caso de
ser definido uma trajetria, ento o computador deve calcular os ngulos das juntas para
cada ponto da trajetria, neste caso a velocidade e acelerao das juntas podem apresentar
descontinuidades (DUYSINX; GERADIN, 2004).
Uma vez obtidos a posio, velocidade e acelerao de cada junta em qualquer
instante t, o computador calcula o torque em cada junta necessrio para efetuar este
movimento. Este torque calculado usando as equaes de movimento do manipulador.
As equaes da dinmica podem apresentar imprecises no valor do torque devido
a foras que so difceis de quantificar, como atritos presentes na transmisso da junta
entre outros fatores. Por este motivo utiliza-se sensores que medem a posio (ngulo) da
junta a cada perodo de tempo, esta informao utilizada em um sistema de controle, que
29
vai calcular a quantidade de torque que deve ser adicionado ao valor inicial para garantir
que o manipulador chegue sua meta nas condies (velocidade, ou tempo) impostas pelo
usurio.
Este ciclo conhecido como inteligncia comportamental do rob, onde cada etapa
fundamental para a realizao do movimento do manipulador. Este ciclo repetido cada
vez que o usurio define uma nova meta para o efetuador.
2.2
Cinemtica e gerao de tra jetria
Em robtica, o movimento do efetuador de um manipulador deve ser o mais suave possvel,

evitando assim variaes abruptas de posio, velocidade e acelerao. Movimentos abruptos
necessitam de uma quantidade ilimitada de potncia para ser implementado (CRAIG,
1989). Por este motivo, implementar um algoritmo eficaz para gerao de trajetria
essencial para um bom funcionamento do manipulador.
Como visto na subseo 2.1.1, um manipulador composto por elo e juntas. Estas
juntas podem ser de quatro formas: junta de revoluo, junta prismtica, de rosca e
esfrica, cada uma possuindo um nmero diferente de graus de liberdade. O problema da
cinemtica direta faz uso das equaes da cinemtica e dos parmetros de cada junta para
determinar a posio e orientao do efetuador.
O ngulo entre os elos a varivel da equao para uma junta de revoluo, assim
como a extenso do elo a varivel para uma junta prismtica. Na cinemtica direta, as
variveis de cada junta so conhecidas e deseja-se saber a posio e orientao de cada
junta.
Cinemtica inversa se refere ao uso das equaes da cinemtica para a determinao
dos parmetros das juntas que providenciam uma posio desejada do efetuador (CRAIG,
1989). Na maioria das aplicaes de um manipulador, deseja-se que o efetuador v para
uma certa posio no espao sem se ter conhecimento das variveis das juntas (ngulo
ou deslocamento). O usurio ento programa a posio final ou a trajetria do efetuador
e as equaes da cinemtica inversa transformam essas coordenadas em ngulos ou
deslocamentos para os atuadores. O problema da cinemtica inversa considerado mais
complexo que a cinemtica direta (BUSS, 2009), por se ter vrias possveis solues (ou
em alguns casos nenhuma soluo encontrada) de varivel da junta para um determinado
conjunto de coordenada (x, y e z).
Os mtodos para se resolver cinemtica inversa so divididos em dois grandes
grupos: solues analtica e numrica, sendo o primeiro utilizado para manipuladores com
seis juntas ou menos em que o punho esfrico. Neste, obtm-se uma equao fechada
para cada junta.
30
Figura 9 Cinemtica: a)direta; b)inversa
Fonte: Buss (2009)
O segundo grupo de solues, os mtodos numricos so dividos em pela forma

como so abordados, sendo os principais: uso da matriz Jacobiana, uso de um sistema de
controle, e uso de mtodos de otimizao que minimizam uma funo erro (objetiva).
Ao longo dos anos, diversos modelos numricos para resolver o problema de cinemtica inversa foram implementados (ARISTIDOU; LASENBY, 2009). De acordo com
Fdor (2003), a maioria dos sistemas usam o mtodo da peseudoinversa Jacobiana ou sua
transposta para solucionar o problema contudo este mtodo sofre de singularidades. Zhao
e Badler (1994) resolve a cinemtica inversa como um problema de otimizao, em que
se minimiza um conjunto de equaes no-lineares, definidas no espao Cartesiano com
restries.
A soluo por mtodo Jacobiano lineariza o movimento do efetuador relativo
mudanas instantneas de posio das variveis das juntas aristidou. Muitos mtodos tem
side desenvolvidos para calcular or aproximar a inversa do Jacobiano, tais como: Transposta
Jacobiano, Mnimos quadrados amortecido (DLS), mnimos quadrados amortecido com
decomposio de valor nico (SVD-DLS) e mnimos quadrados amortecido seletivo (SDLS)
e outras extenses (BUSS, 2009; NAKAMURA; HANAFUSA, 1986; W.WAMPLER, 1986;
BAILLIEUL, 1985; WOLOVICH; ELLIOTT, 1984; BALESTRINO; SCIAVICCO, 1984).
Outros tipo de soluo baseado no mtodo de Newton. Estes algoritmos procuram
uma posio e orientao do efetuador que ir minimizar a funo objetiva, retornando
portanto, um movimento suave sem descontinuidades (ARISTIDOU; LASENBY, 2009).
Os mais conhecidos so o metdo de Powell, o mtodo de Broyden e o mtodo de Broyden,
Fletcher, Goldfarb e Shanno (BFGS)(FLETCHER, 1987), utilizados extensivamente em
otimizao.
O mtodo cclico de coordenadas descendente(CCD), apresentado por Wang e Chen
(1991), um mtodo iterativo heurstico bastante popular por ser considerado um dos mais
31
rpidos a convergir para a proximidade da resposta, contudo relativamente lento para

alcanar alta preciso e frequentemente produz movimentos errticos e descontnuos. Por
este motivo, CCD geralmente empregado em conjunto com outros mtodos de otimizao,
como o BFGS, que assume quando o manipulador j est na proximidade da meta desejada,
como apresentado por Wang e Chen (1991).
Estes mtodos so extensivamente empregados em animaes grficas e em manipuladores robticos com elevado nmero de graus de liberdade. Podem ser usados como
solues em tempo real onde a posio da junta do manipulador atualizada a cada
iterao do algoritmo ou pode-se computar todas as iteraes previamente e obter os
valores de ngulos de cada junta para s ento depois atualizar a posio do manipulador.
Tabela 1 Comparao entre diferentes mtodos de cinemtica inversa;
modelo com um efetuador, 18 GDL, e metas dentro do volume
de trabalho
Mtodo
Nm. de iteraes
Tempo de cada iterao (s)
Pseudoinversa
32
96,4
Pseudoinversa
46
104,1
Mnimos quadrados amortecido
35
89,2
Transposta
59
83,3
Transposta
163
84,8
Fonte (NILSSON, 2009)
A Tabela 1 mostra alguns dos mtods que utiliza a matriz Jacobiana. Nesta tabela
precebe-se que o mtodo de pseudoinversa tem o menor nmero de iteraes, contudo
o tempo para cada iterao um pouco maior que o mtodo dos mnimos quadrados
amortecido (NILSSON, 2009)
Tabela 2 Comparao entre diferentes mtodos de cinemtica inversa;
modelo com um efetuador, 18 GDL, e metas fora do volume de
trabalho
Mtodo
Nm. de iteraes
Tempo de cada iterao (s)
Pseudoinversa
Pseudoinversa
Mnimos quadrados amortecido
74
81,5
Transposta
117
73,8
Transposta
98
77,8
Fonte (NILSSON, 2009)
A Tabela 2 mostra que ambos os mtodos da pseudoinversa no puderam produzir

nenhum resultado quando as metas esto fora de alcance (NILSSON, 2009). O metdo da
32
pseudoinversa puro fica instvel prximos a singularidades (ASADA, 2008), j os outros

mtodos necessitam de um maior nmero de iteraes para alcanar o ponto mais prximo
possvel da meta.
Conclui-se que o mtodo dos mnimos quadrados amortecido que utiliza a matriz
Jacobiana do sistema o mtodo ideal para ser empregado em manipuladores robticos,
uma vez que se mostra estvel e consegue alcanar a meta em menos iteraes. Este
mtodo ser empregado no software de controle apresentado no Apndice A.
2.3
Dinmica e controle
A dinmica e sistemas de controle so partes fundamentais para o controle do manipulador.

A simulao do movimento s pode ser feita se as equaes de movimento do manipulador
forem obtidas, assim como o movimento real no rob fsico, s obter preciso se for
implementado um sistema de controle adequado para rastreio de trajetria.
2.3.1 Modelagem dinmica

A descrio do movimento do rob quando h consideraes das foras agindo nele
dada pelas equaes de movimento, que so obtidas atravs da modelagem dinmica do
manipulador (CRAIG, 1989).
Assim como na cinemtica, em dinmica de manipuladores, existem dois problemas
a resolver: no primeiro, tem-se a trajetria desejada e deve-se encontrar o torque necessrio
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 dinmico do rob descrito em termos de taxa de variao de
tempo da configurao do rob em relao aos torques aplicados por cada atuador. Esta
relao pode ser expressa por um conjunto de equaes diferenciais, chamadas equaes
de movimento.
Existem vrias tcnicas para se determinar tais equaes (FU et al., 1987; SHAHINPOOR, 1987; SPONG; VIDYASAGAR, 1989; MURRAY et al., 1994; CRAIG, 1989).
A formulao por Newton-Euler, por exemplo, implica em determinar as equaes de
movimento baseando-se no balanceamento de foras agindo em cada elo. Este mtodo
resulta em equaes para cada corpo, ou elo do rob, o que faz o sistema volumoso e
difcil de se trabalhar para robs com grande nmero de elos. Fang A. Basu e Wang
(1994), apresenta um mtodo recursivo baseado no princpio de DAlambert e trabalho
virtual para se determinar o modelo dinmico de um manipulador genrico. O mtodo
de Lagrange-Euler usa a conservao de energia no sistema (princpio da ao mnima de
Hamilton) com um Lagrangiano, para determinar as equaes de movimento (ASADA,
33
2008).
O problema ser abordado neste texto fazendo uso do mtodo de Euler-Lagrange
para equaes de movimento. Este mtodo foi preferido por apresentar algumas vantagens
sobre o mtodo de Newton-Euler, algumas delas so: o mtodo de Lagrange fornece
automaticamente tantas equaes quanto h graus de liberdade; as equaes de Lagrange
utilizam naturalmente as coordenadas generalizadas do sistema1 sem a necessidade de
converter tudo para Cartesiano como o caso do mtodo de Newton-Euler; o mtodo de
Lagrange tambm elimina as foras de suportes, o que torna o mtodo de certa forma
mais direto e menos laborioso (VANDIVER, 2011). A seo 3.3 mostra o equacionamento
para obter-se as equaes de movimento de um manipulador com n graus de liberdade.
2.3.2 Controle do manipulador

O problema de controle para manipuladores robticos consiste em determinar as entradas
das juntas em funo do tempo que faa o efetuador executar o movimento comandado
(sequncia de posies e orientaes ou um percurso contnuo). Estas entradas podem
ser foras e torques nas juntas, ou podem ser entradas do atuadores, como por exemplo,
voltagem (SPONG; VIDYASAGAR, 1989) Existem diferentes tcnicas e metodologias para
controle de manipuladores, o mtodo escolhido tem impacto significante na performance do
manipulador. Rastreio de percursos contnuos requer uma arquitetura de controle diferente
que requer o controle para ponto-a-ponto (SPONG; VIDYASAGAR, 1989).
Um dos mais simples tipos de estratgia de controle o controle independente de
junta. Neste tipo de controle cada eixo do manipulador controlado como um sistema de
uma nica-entrada/nica-sada. Qualquer efeito de acoplamento devido ao movimento dos
outros elos tratado como uma perturbao (SPONG; VIDYASAGAR, 1989).
O usurio ou o algortmo do software determina a trajetria desejada para o movimento do manipulador. O sistema de controle deve ser projetado para que o manipulador
consiga se aproximar o melhor possvel da trajetria desejada (KOIVO, 1989).
Os valores da trajetria real so retroalimentados atravs de retroao para determinar possveis erros de trajetria, o sistema de controle vai ento trabalhar para compensar
os erros observados (KOIVO, 1989).
Na realidade, as equaes dinmicas para um manipulador robtico formam um
sistema de multi-variveis complexo e no-linear (SPONG; VIDYASAGAR, 1989). A
anlise de controle no contexto no-linear permite uma anlise rigorosa da performance do
sistema de controle, e tambm permite o projeto de leis de controle robustas e adaptivas
que garantem a estabilidade e rastreio de trajetrias arbitrrias (SPONG; VIDYASAGAR,
1989).
1
para manipuladores articulados, a coordenada generalizada o ngulo da junta .
34
Captulo 3
Metodologia
Neste captulo so apresentados os conceitos tericos que constituem as bases para a anlise
e controle de um manipulador robtico. So tratados neste captulo os seis problemas
apresentados no captulo anterior que, de acordo com Spong e Vidyasagar (1989), so
fundamentais para o desenvolvimento do software de controle do manipulador e para a
anlise apresentados no Captulo 5
3.1
Gerao de trajetrias
A trajetria consiste de um histrico de posio, velocidade e acelerao da coordenada da

junta em funo do tempo, para cada grau de liberdade, e descreve o movimento desejado
de um manipulador no espao multidimensional.
A inteno de se criar uma interface para gerao de trajetria para um sistema
robtico, que o usurio no deve ter a necessidade de escrever funes complicadas de
tempo e espao para especificar a tarefa, ao invs disso, o usurio especificaria apenas
uma posio meta desejada e a orientao do efetuador, e o sistema decide a forma exata
do percurso para chegar l, a durao, o perfil de velocidade e outros detalhes (CRAIG,
1989).
3.1.1 Trajetria 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 posio e orientao e ordena-se que
ele v para uma posio e orientao final sem se importar com o caminho intermedirio
entre esses dois pontos. O computador converte essa meta descrita no espao cartesiano
em espao das juntas usando a cinemtica inversa e define como ser a curva de posio
da junta com base na velocidade ou tempo estipulado pelo usurio.
Este tipo de movimento adequado para tarefas de transferncia quando a rea
de trabalho est livre de obstculos (SPONG; VIDYASAGAR, 1989). Com relao ao
movimento das juntas neste tipo de movimento, existem algumas possveis maneiras de
acionamento, mostrados a seguir.
Captulo 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 distncia diferente a percorrer,
uma vez que todas esto a uma velocidade constante igual, elas terminaro seu movimento
em momentos diferentes. O caminho criado por este tipo de movimento circular com
descontinuidades, como mostrado na Figura 10. Este tipo de trajetria foi implementado na
interface grfica, 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:
trajetria do manipulador no plano)
Fonte: Marion e Thornton (1981)
Movimento sequencial
O movimento sequencial consiste em mover uma junta por vez at o efetuar alcanar seu
destino. A junta mais prxima da base geralmente movida primeiro, e apenas quando
seu movimento for concludo que o movimento da prxima junta se inicia, a velocidade
para cada junta pode ser diferente e no 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
mtodo apresenta vrias descontinuidades na trajetria do efetuador e em muitos casos, um
controle adicional necessrio para evitar que o efetuador colida com objetos ou superfcies.
A figura a seguir mostra o movimento de um manipulador com 2 graus-de-liberdade com
movimento sequencial.
36
Figura 11 Movimento sequencial (esquerda: deslocamento de cada junta; direita: trajetria do manipulador no plano)
Junta interpolada
O mtodo de junta interpolada um dos mais usados em sistemas robticos 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, no h grandes variaes em
velocidade e acelerao. O caminho descrito pelo efetuador, por ser curvo, no apresenta
descontinuidades e pode ser descrito por uma interpolao polinomial.
Movimento suave considerado como uma funo contnua que possui, no mnimo,
duas derivadas contnuas. Isto evita movimentos bruscos, com solavancos diminuindo,
assim, o desgaste do mecanismo e possveis vibraes que podem provocar ressonncias no
manipulador (CRAIG, 1989).
Figura 12 Movimento coordenado (esquerda: deslocamento de cada junta; direita: trajetria do manipulador no plano)
Para um percurso suave, a velocidade inicial e e velocidade final so iguais zero,
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 restries podem ser satisfeitas por um polinmio de terceiro grau. Este polinmio
tem a forma
(t) = a0 + a1 t + a2 t2 + a3 t3
(3.2)
tomando-se a derivada desse polinmio para calcular a velocidade e acelerao, obtm-se
(t) = a1 + 2a2 t + 3a3 t2
(t) = 2a2 + 6a3 t
(3.3)
Combinando as equaes (3.2) e (3.3) com as restries desejadas (3.1), obtm-se quatro
equaes com quatro incgnitas:
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 equaes, obtm-se:
a0 = 0
a1 = 0
3
a2 = 2 (f 0 )
tf
2
a3 = 3 (f 0 )
tf
(3.5)
Com esses coeficientes possvel calcular o polinmio cbico que conecta qualquer posio
inicial de ngulo de junta com qualquer posio final, de forma que as velocidades inicial e
final sejam zero, formando portanto, um movimento suave. As funes (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)
38
possvel tambm descrever alguns pontos de passagem para o efetuador1 , de

forma que o efetuador passar por esses pontos sem parar. Cada um desses pontos so
convertidos para o espao das juntas pelo uso da cinemtica inversa.
As restries 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 equaes que descrevem o polinmio cbico so:

0 = a0
f = a0 + a1 tf + a2 t2f + a3 t3f
0 = a1
f = a1 + 2a2 tf + 3a3 t2
(3.10)
Obtm-se, portanto, os coeficientes da funo:

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 equaes (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

polinmio para cada junta. Essa velocidade pode ser especificada pelas seguintes formas:
pelo usurio, como uma velocidade cartesiana linear e angular para o efetuador naquele
instante; o sistema escolhe automaticamente as velocidades aplicando uma heusterstica
adequada no espao das juntas ou no espao cartesiano; ou o sistema escolhe as velocidades
de forma que a acelerao no ponto de passagem seja contnua (CRAIG, 1989).
1
A expresso ponto se refere sistemas de referncia que fornecem tanto a posio quanto a orientao
do efetuador.
39
3.1.2 Trajetrias parametrizadas

Em certas ocasies o usurio necessita que o efetuador percorra um caminho especfico, ao
invs de apenas especificar um ponto inicial e uma meta. Esta trajetria pode ser descrita
no espao cartesiano de forma paramtrica, onde as coordenadas x, y, z so funes de
tempo. Para este caso, o usurio especifica um tempo desejado para se completar o trajeto,
ou uma velocidade linear constante.
Para cada ponto da trajetria, necessrio calcular o valor da varivel de junta
utilizando o mtodo de cinemtica inversa apresentado na subseo 3.2.3. Para manipuladores com at trs juntas, pode-se utilizar o mtodo geomtrico, e substituindo as
variveis cartesianas pela funo paramtrica se tem a funo fechada de varivel de junta
em funo do tempo. Nos casos em que o manipulador possui mais de trs juntas, ou graus
de liberdade, um mtodo numrico usado para calcular a varivel de junta para certos
ponto da trajetria, com espaamento s entre cada ponto, uma tabela ento formada
para cada junta.
Para a implementao da interface grfica, o mtodo numrico de cinemtica inversa
escolhido foi o mtodo dos mnimos quadrados amortecido , por se mostrar ser o mais
rpido a convergir os valores. Este mtodo reduz o nmero total de iteraes para se obter
a tabela de variveis de junta para cada ponto, tornando o processo mais rpido e estvel.
Em trajetrias parametrizadas, os pontos esto muito prximos um do outro, o
que torna invivel computar um polinmio cbico para cada ponto. Por este motivo no
possvel determinar a derivada de forma algbrica, uma derivada numrica ento
empregada de forma a obter a velocidade e acelerao do espao de junta.
Uma derivada numrica obtida utilizando a definio de derivada:
f (x + h) f (x)
h0
h
f 0 = lim
(3.15)
Onde a aproximao numrica se torna:

f0
f (x0 + h) f (x0 )
h
(3.16)
A acelerao 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 aproximao, mas no
to pequeno que se carregue muito o processamento do algoritmo.
40
Figura 13 Aproximao numrica de derivada
Fonte: Produzido pelo autor
3.1.3 Trajetria modelada em AutoCad

Quando a obteno das equaes 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 usurio desenha a trajetria desejada para o efetuador a
converte em uma tabela de pontos relativamente prximos e uniformemente espaados.
Com o AutoCAD possvel exportar uma tabela de pontos presentes em um arquivo de
desenho para o Microsoft Excel, que posteriormente pode ser incorporado na interface
grfica do usurio. Contudo, a tabela de pontos nem sempre est em ordem, e necessrio
utilizar um algoritmo para reorganizao dos pontos (a ordem dos pontos de uma trajetria
fundamental). Este algoritmo no ser explicado aqui, mas descrito brevemente na
descrio do programa feito em Matlab para este fim. Neste programa, contido nos anexos,
tambm descreve o mtodo 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 trajetria desenhada em AutoCAD correspondente s
trs letras que compem o logo da universidade, isto , UMC. A Figura 14 mostra os
pontos distribudos no espao de trabalho do manipulador.
3.1.4 Trajetria linear

Em um manipulador de juntas de revoluo, o movimento do efetuador nunca linear
(BUSS, 2009; CRAIG, 1989). Porm, possvel aproximar o trajeto em um percurso linear,
41
Figura 14 Exemplo de trajetria desenhada em CAD
informando pontos de passagens relativamente prximos uns dos outros.

A trajetria retilnea pode ser entendida como um caso especfico da trajetria
parametrizada. Para este caso, tambm necessrio calcular os ngulos das juntas para
pontos igualmente espaados do percurso da linha. O efetuador ter sua velocidade linear
constante durante o percurso, fazendo com que a curva de varivel de junta se torne
complexa. Em percursos de linha reta, as derivadas primeira e segunda das coordenadas
de juntas, so quase sempre curvas com descontinuidades, por isso aconselhvel evitar os
percursos de linha reta em tarefas com elevao de cargas.
Um dos mtodos mais simples para se obter uma trajetria em linha reta o
algoritmo de Taylor, que utiliza pontos de passagem igualmente espaados 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 mdio, no espao das juntas e Cartesiano;
3o Se o erro no trajeto planejado for maior que o permitido, ento 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 nmero de pontos de passagem, mais prximo ser a aproximao
da trajetria necessria.
42
Figura 15 Grfcos da trajetria linear: a) trajetria em linha reta; b) aproximao da

linha reta por pontos de passagem; c) curvas de coordenadas das juntas em
funo da distncia linear; d) velocidade angular; e) acelerao angular
Alguns problemas podem aparecer quando utilizado trajetrias lineares. O primeiro

tipo o problema da trajetria possuir pontos intermedirios inalcanveis. Neste caso,
embora a localizao inicial do manipulador e a meta estejam dentro do volume de trabalho,
possvel que alguns pontos da reta sejam inalcancveis, um exemplo disso seria uma linha
reta em que seu meio passe muito prximo base do manipulador, no sendo possvel o
efetuador alcanar esses pontos sem que haja uma coliso do efetuador com algum elo.
O segundo problema ocorre quando os pontos da trajetria so prximos singularidades. Se um manipulador est seguindo uma trajetria cartesiana em linha reta e se
aproxima de uma configurao singular do mecanismo, a velocidade de uma ou mais juntas
pode aumentar ao infinito. Sendo as velocidades de mecanismos limitadas, o que ocorre na
prtica que existir um desvio do manipulador do curso desejado (CRAIG, 1989).
O problema do tipo trs surge quando um manipulador tem os pontos inicial e alvo
atingveis por diferentes solues. Neste caso o manipulador consegue alcanar todos os
pontos da trajetria com algumas solues ao invs 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 Apndice A, existe uma funo especfica que ir checar a
trajetria gerada antes de iniciar a simulao ou enviar o movimento ao rob. A Figura 16
ilustra os trs tipos de problemas.
43
Figura 16 Problemas geomtricos com trajetria cartesiana: a)tipo 1; b)tipo 2; c)tipo 3
Fonte: Craig (1989)
3.1.5 Algoritmo anticoliso

Em uma ocasio ideal, o usurio iria entrar com um ponto meta desejado e o sistema
determinaria quantos pontos de passagem seria necessrio para que a trajetria gerada
fosse capaz de se livrar de qualquer obstculo. Contudo, para se alcanar tal estado, o
sistema precisaria ter modelos do manipulador, da rea de trabalho e de todos os possveis
obstculos que nela se encontram(atravs de sistema de viso) (CRAIG, 1989).
Atualmente, no existem sistemas que conseguem planejar trajetrias que sejam
totalmente livres de coliso (CRAIG, 1989). As tcnicas existentes conseguem evitar
algumas das possveis colises buscando em uma representao grfica do ambiente
uma trajetria livre de coliso. Contudo esses mtodos so exponencialmente complexos
se considerar um obstculo mvel (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 coliso consiste em descrever o
objeto presente na rea de trabalho do manipulador como uma aproximao do formato
usando uma funo parametrizada, como a da esfera por exemplo. O sistema iria ento
verificar se algum ponto da trajetria estaria dentro do volume da esfera e caso estivesse,
este iria procurar atravs de uma anlise vetorial, a menor distncia fora da esfera em que
no haveria trajetria. Este mtodo laborioso, pois o usurio teria que descrever o objeto
toda vez que esse mudasse de lugar.
Como o sistema de viso no faz parte do escopo deste trabalho, no ser implementado no software de controle o algoritmo anticoliso. apresentado no Captulo 6
como sugesto para trabalho futuro, a implementao de um sistema de viso e algoritmo
anticoliso no software de controle. Tambm sugerido uma atualizao para a junta
44
do manipulador para que esta consiga detectar quando o manipulador colide com algum
objeto acionando, assim, um sistema de segurana que desliga o torque dos atuadores das
juntas.
3.2
Cinemtica do manipulador
Cinemtica o campo da Mecnica clssica que descreve os movimentos de pontos,

corpos de sistemas de corpos sem considerao s causas do movimento (foras e torques).
Cinemtica de grande importncia no estudo de manipuladores robticos pois permite a
determinao da posio de cada junta e do efetuador para qualquer instante.
3.2.1 Cinemtica direta

Em um manipulador, juntas e elos so 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 varivel: ngulo para juntas de revoluo e deslocamento para juntas prismticas.
As equaes da cinemtica direta determinam o efeito acumulativo do conjunto inteiro de
variveis.
Para um manipulador de n juntas, ele apresentar n + 1 elo, uma vez que cada
junta se conecta com dois elos. Por conveno, as juntas so 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 varivel qi associada a esta, sendo que para uma junta de revoluo, a varivel qi o
ngulo entre as juntas, portanto: qi = i .
Permitindo que Ai seja a matriz homognea de transformao que descreve a
posio e orientao do link i com respeito as coordenadas x, y, z a partir da origem. Para
um manipulador com uma matriz nica Ai , esta matriz funo de uma nica varivel,
isto , Ai (qi ). Para um manipulador com duas ou mais juntas, a matriz de transformao
das juntas j at a junta i obtida a partir da multiplicao de todas as matrizes A de i
at j:
Tij = Ai+1 Ai+2 ...Aj1 Aj
(3.18)
Para resolver o problema da cinemtica direta, necessria determinar a matriz de
transformao Ai , uma vez determinada, a posio e orientao pode ser obtida diretamente
da matriz final Tij . A conveno e mtodo de obteno de Ai apresentado a seguir.
Critrio de notao de Denavit-Hartenberg

Jacques Denavit e Richard Hartenberg introduziram uma conveno em 1995 para a
definio de matrizes de juntas e matrizes de elo com o intuito de padronizar a estrutura
45
de coordenadas de uma cadeia cinemtica. Na conveno de D-H, cada matriz homognea

de transformao representada com um produto de quatro outras transformaes bsicas:
Ai = Rz,i Transx,di Transz,ai Rx,i
Onde:
Rz,i
c si
i
si
ci
=
0
0
0
0
Transx,di
0
=
Transz,ai
Rx,i
0
=
0
0
1
0
0
1
0
0
0
0
1
0
di
0
1
0
0
0
0
1
0
ai
1
0
0
0 ci
si
=
0 si
c i
0
0
0
(3.19)
(3.20)
(3.21)
(3.22)
(3.23)
As matrizes de rotao geralmente so apresentadas com dimenso 3 3. A quarta

linha e coluna da matriz so adicionados para permitir a descrio no apenas da rotao
em torno de um eixo, mas tambm de uma translao d. Portanto, a matriz Ai que
produto de duas matrizes de rotao e duas de translao, possui dimenso 4 4.
Efetuando a multiplicao das quatro matrizes, a matriz Ai se torna:
c si ci si ci ai ci
i
si
ci ci ci si ai si
Ai =
0
si
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 parmetros ai , i , di e i so: comprimento do elo, giro do elo, deslocamento do elo e
ngulo da junta, respectivamente.
Com estes parmetros estabelecidos, a posio e orientao do efetuador com
respeito a origem global do rob O pode ser encontrada, multiplicando-se n matrizes de
46
Figura 17 Parmetros de Denavit-Hartenberg
Fonte: Craig (1989)
transformao homogneas A, obtendo-se a matriz de transformao Tn0 , ou seja:

n
Y
n o a p
T0n =
Aii1 (qi ) =
0 0 0 1
i=1
(3.25)
A matriz Tij uma matriz quadrada 4 4 onde as primeiras trs linhas e colunas
representam a orientao 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 parmetros D-H
para um manipulador do tipo articulado e a Figura 19 mostra como divida a matriz de
transformao homognea.
3.2.2 Cinemtica inversa: mtodo pseudo-inverso Jacobiano

A cinemtica direta pode ser interpretada com uma funo da varivel da junta qi . A
posio do efetuador pode, ento, ser escrita como:
e = f ()
(3.26)
Onde e a posio do efetuador, isto : e = (e1 , e2 , ..., em )T , (em espao tridimensional: m = 3), e um vetor-coluna composto por todos as n variveis de juntas, isto :
= (1 , 2 , ..., n )T .
Para a cinemtica inversa, deseja-se encontrar uma funo que retorna um vetor de
variveis de juntas (ngulos ou deslocamentos) que vai fazer com que o efetuador alcance
47
Figura 18 Manipulador do tipo articulado com os parmetros D-H

Figura 19 Diviso da matriz de transformao homognea
alguma posio desejada. Neste caso e conhecido e queremos encontrar o valor de ,

ento computa-se a funo inversa de (3.26).
= f 1 (e)
(3.27)
Esta funo inversa nem sempre tem soluo nica, e muito mais complexa
quando o manipulador possui mltiplos graus-de-liberdade ou mltiplos efetuadores, o que
faz o processo de encontrar as solues mais exigente computacionalmente.
Para manipuladores com mltiplos efetuadores (k), a posio no espao denotada
por p1 , ..., pk , onde a posio de cada efetuador uma funo da varivel da junta. Pode-se
tambm 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 posio desejada para cada efetuador, isto pode ser
representado como um vetor coluna com k entradas: t = (t1 , ..., tk )T . A mudana em
posio de cada efetuador pode ser calculada usando o vetor de posio atual s e a posio
de destino t, considere e como o vetor de diferena entre os dois vetores, pode-se escrever
para cada efetuador: e = t p. Uma vez que a posio atual do efetuador uma funo
48
de , a equao (3.26) para mltiplos efetuadores, se torna:
ti = pi () para todos k efetuador
(3.28)
Os valores para que satisfazem as equaes (3.28) pode, em alguns casos, ter
mltiplas solues ou nenhuma soluo. Contudo, possvel aproximar a funo usando
uma matriz Jacobiana, que definida por:
J () =
pi
j
(3.29)
i,j
Onde J a matriz k n cujos elementos so vetores do R3 . A justificativa de se

usar uma matriz Jacobiana que a funo de posio de cada efetuador pi () sempre
diferencivel, portanto, a matriz Jacobiana apresentar a melhor aproximao linear
da funo ao redor do ponto escolhido, como mostrado na figura abaixo. As equaes
de velocidade e acelerao do efetuador tambm podem ser obtidas a partir da matriz
Jacobiana. O algoritmo para se obter as solues da cinemtica inversa, pode ser descrito
Figura 20 Representao geomtrica do Jacobiano
como segue:
49
Suponha que se conhea o valor da varivel da junta e posio atual da junta

(vetores p e t) em um dado instante. A partir desses valores, calcula-se a matriz Jacobiana
usando a equao (3.29). Procuramos um valor de atualizao para que ir incrementar
o ngulo da junta:
:= +
(3.30)
Esta mudana da varivel vai causar uma mudana da posio atual do efetuador
p, e pode ser estimado:
p J
(3.31)
O valor escolhido de forma que s aproximadamente igual a diferena entre
a posio final e a posio inicial (vetor e). Podemos encontra um valor de que v
satisfazer a equao:
e = J
(3.32)
O valor de encontrado calculando-se a inversa de matriz Jacobiana:
= J 1 e
(3.33)
Contudo, dependendo do nmero de juntas e do nmero de efetuadores do manipulador, a

matriz Jacobiana pode no ser quadrada, da qual no se pode calcular a inversa. Por isso,
empregado alguns mtodos para se contornar este problema, sendo um deles o uso do
mtodo da Transposta Jacobiana, que consiste de usar a transposta da matriz Jacobiana
ao invs da inversa de J para encontrar . Outro mtodo utilizado conhecido como
mtodo pseudoinverso, que usado quando a matriz Jacobiana for retangular, a equao
(3.33) se torna:
= J e
(3.34)
Onde a matriz J que possui dimenso n m o pseudo-inverso de J, tambm conhecida
como Moore-Penrose inversa de J e fornece a melhor soluo possvel para a equao
(3.32), uma vez que este mtodo calcula esta equao usando o mtodo dos quadrados
mnimos. O pseudo-inverso de J pode ser calculada usando a equao:

J = JT J
1
JT
(3.35)
Onde J T a transposta da Jacobiana.
3.2.3 Mnimos quadrados amortecido.

O mtodo pseudo-inverso Jacobiano apresenta alguns problemas quando a meta est
fora de alcance do manipulador ou quando est muito prxima da distncia mxima de
alcance. Estes problemas fazem o mtodo numericamente instvel quando o programa
50
Figura 21 Fluxograma de soluo da Cinemtica Reversa por Jacobiano
Fonte: Joubert (2008)
estiver tentando calcular o valor de . Para evitar estes problemas com singularidades, o
mtodo de mnimos quadrados amortecido utilizado.
Este mtodo se difere do pseudo-inverso Jacobiano no sentido que ao invs de
encontrar um mnimo vetor que fornece a melhor soluo para a equao (3.32), o
algoritmo vai procurar o valor de que ir minimizar a quantidade
kJ ek2 + 2 kk2
(3.36)
Onde R uma constante de amortecimento no nulo. Pode-se reescrever a expresso

acima como:

J T J + 2 I = J T e
(3.37)
O valor de J T J + 2 I no-linear (BUSS, 2009), e portanto, a soluo dos mnimos
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 nmero de variveis

de juntas (graus-de-liberdade), considerando que

J T J + 2 I
1
J T = J T JJ T + 2 I
1
(3.39)
Ento:

= J T JJ T + 2 I
1
(3.40)

1
A vantagem de se usar (3.40) ao invs de (3.38) que JJ T + 2 I

mais fcil
T
de se calcular sua inversa, uma vez que JJ uma matriz m m , em que m o espao
da meta (para uma espao 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 equao (3.40) numericamente
estvel. A constante deve ser grande o suficiente de maneira que as solues para
tenha bom comportamento prximo de singularidades, mas no to grandes que a taxa de
convergncia se torne muito lenta (BUSS, 2009).
3.2.4 Redundncia e Singularidades

Um manipulador deve possuir pelo menos seis graus de liberdade de forma a poder alocar
o efetuador em uma ponto e orientao arbitrria no espao. Os manipuladores com uma
nmero menor que 6 graus de liberdade no so capazes de executar o posicionamento de
tais pontos arbitrrios. Contudo, se o manipulador tiver mias que 6 graus de liberdade,
ir existir um infinito nmero de solues para as equaes da cinemtica. So, 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 transformao linear (SPONG; VIDYASAGAR, 1989)
dX = J (q) dq
(3.42)
entre os diferenciais dq e dX. Uma vez que a jacobiana uma funo da configurao q,
aquelas configuraes em que o ranque de J diminui so de significncia especial. Tais configuraes so chamadas singularidades ou configuraes singular (SPONG; VIDYASAGAR,
1989).
52
3.2.5 Velocidade e acelerao

Como mostrado acima, as velocidades da junta e as velocidades do efetuador so relacionadas pela matriz jacobiana:
= J (q) q
X
(3.43)
Portanto, a velocidade inversa se torna um problema de resolver um sistema de
equaes lineares. Para encontrar a acelerao, pode-se diferenciar a equao acima. Usando
a regra do produto:
!
d
= J (q) q
X
+
J (q) q
dt
(3.44)
de aceleraes do efetuador, o vetor de acelerao

Portanto, dado um vetor X
instantnea da junta q
dado como a soluo 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 equaes da velocidade e acelerao
inversa podem ser escritas como (SPONG; VIDYASAGAR, 1989):
q = J(q)1 X
(3.47)
q
= J(q)1 b
(3.48)
Considerando que det J (q) 6= 0. Para manipuladores redundantes ou manipuladores com

menos que 6 elos a matriz jacobiana no pode ser invertida, por no ser quadrada. Nestes
casos, apenas existir uma soluo para a equao se o vetor do lado esquerdo da equao
possuir o mesmo intervalo de espao que a matriz jacobiana (SPONG; VIDYASAGAR,
1989)
determinada pelo usurio no software de
A velocidade linear do efetuado X,
controle. Uma vez que a inversa da jacobina varia dependendo da configurao do brao,
a matriz de velocidade das juntas tambm iro variar, mesmo que a velocidade linear do
efetuador seja constante (ASADA, 2008).
3.2.6 Restries na congurao do manipulador

Em alguns casos necessrio que o efetuador se movimente com uma orientao 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
53
qualquer de forma que o plano do fundo da lata fique, durante todo o trajeto, paralelo
ao cho evitando que a bebida derrame. Isso alcanado descrevendo o ngulo do punho
em funo dos outros ngulos das juntas do cotovelo e ombro. Berenson e Kuffner (2012)
e Ratliff Matt Zucker e Srinivasa (), apresentam tcnicas baseada em otimizao para
restringir o movimento do punho durante a gerao de trajetria.
Figura 22 Trajetria com restrio de orientao
Fonte: Berenson e Kuffner (2012)
Uma das formas de se restringir a orientao do punho especificar trs ngulos

fixos com relao aos planos da base do rob e utilizar-se de um algoritmo de otimizao
como o mtodo de Newton, que um dos mais simples, para minimizar a funo custo.
Como o mtodo de Newton tende a no convergir quando o manipulador est longe da
meta, utilizado o mtodo da cinemtica inversa por mnimos quadrados amortecido,
apresentado na 3.2.3 para posicionar o efetuador na posio da meta, e a partir deste
ponto, o mtodo de Newton assume para satisfazer a orientao desejada.
Usando o mtodo de Newton-Raphson , a soluo para o sistema de equaes
determinado usando, iterativamente, a seguinte aproximao (GOLDENBERG; FENTON,
54
1985)
q(k+1) = q(k) + (k)
(3.49)
onde (k) a soluo 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 orientao e posio atuais
do efetuador e meta. Estes valores so obtidos das matrizes de transformao homognea,
Equao 3.25, usando a seguinte relao (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 (, , ) so os ngulos desejados ao redor dos eixos xyz, respectivamente.

Este mtodo, funciona razoavelmente bem, at mesmo prximos de singularidades,
porm deixa a resoluo da cinemtica inversa um pouco lenta e deve ser aplicado apenas
onde h realmente a necessidade de se restringir o movimento do rob, como em operaes
de soldagem e pintura. importante observar que para manipuladores com nmero de
juntas menor que seis, no possvel satisfazer a meta e orientao desejada nos trs eixos,
para estes casos determina-se um ou mais valores do vetor r para 0.
3.3
Dinmica do Manipulador
3.3.1 Conceitos preliminares

Tensor de Inrcia
Um momento de inrcia definido como a integral do segundo momento em relao a um
eixo de todos os elementos de massa dm que compe o corpo. Para um eixo qualquer, o
momento de inrcia (HIBBELER, 1999):
I=
Z
m
r2 dm
(3.53)
55
Para um corpo com densidade constante, podemos reescrever a equao acima, de forma
que seus termos fiquem puramente geomtricos, como segue:
I=
r2 dV
(3.54)
) Onde a densidade do corpo e r o vetor distncia perpendicular do eixo at o elemento

arbitrrio dm. Para um elemento diferencial descrito em coordenadas cartesianas o vetor
r com relao ao eixo de rotao x pode ser descrito como: r = y 2 + z 2 , o momento de

inrcia ento 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
Izz =
Z
x2 + y 2 dV
O produto de inrcia um conjunto de dois planos ortogonais definido como o

produto da massa do elemento e as distncias perpendiculares dos planos at o elemento.
Para o plano y z essa distncia x e para o plano x z, a distncia y. Os produtos de
inrcia do corpo em relao a cada combinao de planos ficam: (HIBBELER, 1999)
Ixy = Iyx =
(xy) dV
Iyz = Izy =
(yz) dV
(3.58)
Ixz = Izx =
(xz) dV
O tensor de inrcia um conjunto nico de valores para um corpo quando determinado

para cada localizao da origem O e orientao 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 inrcia uma matriz simtrica, visto que Ixy = Iyx , Iyz = Izy e Ixz = Izx .
Existem trs eixos de inrcia, 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 inrcia, no existe foras resultantes de
desbalanceamento do corpo pois a massa est igualmente distribuda.
56
Energia Potencial
Energia potencial (simbolizado por U ou Ep) a forma de energia que est associada a
um sistema onde ocorre interao entre diferentes corpos e est relacionada com a posio
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 foras conservativas, ou seja, a trajetria do corpo no
interfere no trabalho realizado pela fora, o que importa so a posio final e a inicial,
ento o percurso no interfere no valor final da variao da energia potencial.
U = mi gr0,ci
(3.60)
Energia Cintica
A energia cintica 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 mdulo da
velocidade do corpo em questo. Quanto maior o mdulo da velocidade do corpo, maior
a energia cintica. Quando o corpo est em repouso, ou seja, o mdulo da velocidade
nulo, a energia cintica nula.
Para um dado sistema que se move a uma velocidade vci (velocidade do centro de
massa) e velocidade angular i sua energia cintica dada por (ASADA, 2008):
K=
n
X
1
i=1
1
mi |vci | + Ii i2
2
2
2
(3.61)
3.3.2 Equaes de movimento do manipulador

As equaes de Lagrange so derivadas do princpio da ao mnima. Este teorema denota
que a ao - uma grandeza fsica com dimenso equivalente de energia multiplicada pela
de tempo (joule-segundo no S.I.) - possui um valor estacionrio - mximo, mnimo, ou
um ponto de sela - para a trajetria que ser efetivamente percorrida pelo sistema em seu
espao de configurao (MARION; THORNTON, 1995).
Para se utilizar das equaes de Lagrange o sistema deve atender alguns requisitos,
como: ser holonmico, possuir coordenadas generalizadas independentes, e ter o tantos
graus-de-liberdade quanto coordenadas generalizadas.
Entende-se como coordenadas generalizadas de um sistema, o grupo mnimo de
parmetros para descrever completamente todas as configuraes do sistema a qualquer
instante t (VANDIVER, 2011).
57
Trabalho virtual
Uma fora aplica trabalho W , quando existe um deslocamento da partcula ou corpo na
direo de aplicao da fora (HIBBELER, 1999). Um trabalho virtual consiste do produto
de uma fora 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 equilbrio esttico, a variao
do conjunto de energia cintica e energia potencial para uma fora no conservativa seria
zero. Considerado um dado instante em tempo, h uma variao infinitesimal na posio
do sistema partindo de sua posio natural de equilbrio dinmico, a equao de variao
de energia cintica, potencial e trabalho seria (VANDIVER, 2011):
K + U W nc = 0
(3.63)
Onde o ndice nc significa que o trabalho feito por uma fora no conservativa.2
A tcnica usada para encontrar as foras generalizadas do sistema assumir que o
sistema experimenta uma pequena deflexo virtual, da qual computa-se o incremento de
variao de trabalho resultante.
Equaes de Lagrange
Para um sistema com n graus de liberdade e coordenadas generalizadas qj , possvel
calcular a funo Lagrangiana L = K U , onde K a energia cintica e U a energia
potencial do sistema. A Lagrangiana uma funo de coordenadas generalizadas qj e
velocidades generalizadas qj (VANDIVER, 2011):
L = L (q1 , ...qi ...qn , q1 , ...qi ....qn )
(3.64)
Onde n o nmero de graus-de-liberdade do sistema. Uma vez que a energia potencial

U , funo da coordenada generalizada qi , e a energia cintica funo da coordenada
generalizada e da velocidade qi , pode-se reescrever a equao (3.64), como segue:
L (qi , qi ) = K (qi , qi ) U (qi )
(3.65)
Utilizando as equaes de Lagrange, as equaes de movimento so, portanto:

d
dt
2
L
qi
L
= Qi
qi
i = 1, 2..., n
(3.66)
Uma fora considerada conservativa quando causa trabalho em um sistema que independente da
trajetria do corpo ou partcula. Exemplos de foras conservativas so o peso de uma partcula e
o trabalho realizado por uma fora de mola (HIBBELER, 1999). Todas as outras foras externas e
momentos que aplicam ou retiram energia do sistema so considerados como foras no conservativas.
58
Onde Qi so as foras externas generalizadas. Uma vez que i vai de 1 n, as equaes de

Lagrange fornecem n equaes de movimento, que o mesmo nmero de graus-de-liberdade
que o sistema apresenta (VANDIVER, 2011). Este conjunto de equaes diferenciais para
um dado sistema, fornecem uma relao entre acelerao, velocidade e coordenadas. Elas
constituem um conjunto de n equaes diferenciais de segunda ordem. A soluo geral
contm n constantes, das quais podem ser determinadas pelas condies de contorno do
sistema, isto , valores das velocidades e coordenadas (GANZ, 2008).
Fazendo uso da equao (3.65), a equao (3.66) pode ser reescrita como:
d
dt
(K U )
(K U )
d
=
qi
qi
dt
(K)
d
qi
dt
(U )
(K) (U )
+
= Qi (3.67)
qi
qi
qi
Em sistemas mecnicos, a energia potencial nunca uma funo da velocidade (VANDIVER,

)
2011), logo o termo dtd (U
= 0, portanto:
qi
d
dt
(K)
(K) (U )
+
= Qi
qi
qi
qi
(3.68)
Multiplicando ambos os lados por qi , obtm-se:

"
d
dt
(K)
(U )
(K)
qi +
qi = Qi qi
qi
qi
qi
(3.69)
A equao acima fica, portanto, da forma da equao (3.63) (VANDIVER, 2011).
3.3.3 Extendendo para um manipulador de n GDL

Matriz de inrcia multi-corpo
Como mostra a equao (3.61), a energia cintica armazenada em um elo composta de
dois termos, energia cintica de translao e o outro devido rotao sobre o centride,
1
1
T
Ki = mi vci
vci + iT Ii i ,
2
2
i = 1, 2, ..., n
(3.70)
onde i e Ii so, respectivamente, o vetor 3 1 de velocidade angular e a matriz 3 3

de inrcia do elo i considerado pelos eixos de coordenada da base (referncia inercial). A
energia cintica total , portanto:
K=
n
X
Ki
(3.71)
i=1
A velocidade linear e angular no so variveis independentes, no atendendo ao

requisito para utilizar as equaes de Lagrange como discutido no comeo da subseo 3.3.2.
Para isto, necessrio reescrever a equao de energia cintica em funo das coordenadas
generalizadas apenas
vci = JLi q
(3.72)
i = JA
q
59
onde JLi e JA
i , so respectivamente, as matriz 3 n relacionando a velocidade linear
no centride, 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 equao (3.72) nas equaes (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
JLi
JA
i
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 inrcia multi-corpos (ASADA, 2008). Como essa matriz
envolve matrizes jacobianas, e estas variam com a configurao do manipulador, portanto,
a matriz de inrcia multi-corpos dependente da configurao e representa as propriedades
de massa instantaneamente para o manipulador naquela configurao. Escrevemos a matriz
de inrcia como M(q), uma funo da varivel da junta q.
Fazendo uso dos componentes da matriz de inrcia multi-corpos M = Mij , escrevemos a energia cintica total na forma quadrtica escalar:
K=
n X
n
1X
Mij qi qj
2 i=1 j=1
(3.76)
Diferenciando, para obter o primeiro termo da equao (3.69),
n
n
n
X
X
d 1 X
dMij
d K
=
Mij qj =
Mij qj +
qj
dt qi
dt 2 j=1
j=1
j=1 dt
(3.77)
Por ser uma matriz simtrica, M, os pares de juntas i e j, possuem os mesmos

coeficientes de iterao dinmica, portanto, Mij = Mji
O segundo termo da equao (3.77), geralmente no nulo (SPONG; VIDYASAGAR, 1989), uma vez que a matriz M dependente da configurao, aplicando a regra da
cadeia neste termo,
n
n
X
X
dMij
Mij dqk
Mij
=
=
qk
dt
k=1 qk dt
k=1 qk
(3.78)
60
O segundo termo da equao de movimento (3.69), tambm requer a derivada

parcial de Mij da equao (3.76),
n X
n
X
n X
n
1
T
1X
Mjk
=
qj qk
Mjk qj qk =
qi
qi 2 j=1 k=1
2 j=1 k=1 qi
(3.79)
Substituindo equao (3.78)no segundo termo da equao (3.77) e combinando os

termos resultantes com a equao (3.79), escrevemos os termos no lineares como (ASADA,
2008)
hi =
n X
n
X
Cijk qj qk
(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 smbolo de Christoffel (de primeira ordem) (SPONG;

VIDYASAGAR, 1989). A equao (3.81) no linear, uma vez que possui produtos de
velocidades das juntas. Equao (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 acelerao centrfuga e o segundo, os torque por acelerao
de Coriolis (ASADA, 2008).
hi =
n
X
Cijj qj2
j=1
n
X
Cijk qj qk
(3.82)
k6=j
Foras generalizadas
Foras que agem no sistema de corpos rgidos, podem ser representados por foras conservativas e no conservativas (ASADA, 2008). Como visto em seo 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 posio do centride Ci que dependente da coordenada da junta.

Substituindo esta equao, na equao de movimento de Lagrange (3.69), obtm-se a
equao 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)
61
onde JLj,i um vetor coluna 3 1 que relaciona a velocidade linear do centride do

elo j com a velocidade da junta.
Para um manipulador articulado, a equao 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 qj
(3.86)
j=1
C descreve os efeitos da acelerao de Coriolis e centrpeta, torques relacionados com a primeira acelerao so proporcioanis qi qj , enquanto que torques
relacionados ao segundo termo so proporcionais qi2 ,
C (q, q)
q =
=
n
X
j=1
n
X
j=1
n
X
Cijj qj2 +
Cijk qj qk
j6=k
1 Mjj
Mij
qj
2 qi
qj2
n
X
j6=k
Mij
1 Mjk
qj qk
qk
2 qi
(3.87)
F descreve os efeitos por atrito viscoso e de Coulomb, e geralmente no

considerado parte da dinmica de corpos rgidos (CORKE, 2011),
G o termo de carga pela gravidade, onde G = [G1 , G2 , ..., Gn ]T .
3.4
Sistemas de controle
Quando considerado em um nvel mais alto, o objetivo do sistema de controle do rob

permitir que este conclua uma tarefa especificada (DUYSINX; GERADIN, 2004). Existem
vrios tipos de controle para robs (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 sada-nica (do ings, SISO:
Single-input / single-output). Qualquer efeito de acoplamento devido ao movimento de
outras juntas tratado como distrbio (SPONG; VIDYASAGAR, 1989).
O objetivo deste tipo de sistema de controle escolher um compensador de tal forma
que a sada da (plant) consiga rastrear ou seguir uma sada desejada (meta, velocidade,
acelerao ou torque), dado um sinal de referncia. O sinal de controle, contudo, no
a nica entrada agindo no sistema (SPONG; VIDYASAGAR, 1989)). O controlador
projetado, portanto, de forma que os efeitos dos distrbios so reduzidos na sada.
62
Figura 23 Diagrama de blocos para o problema de controle do rob.
Fonte: Spong e Vidyasagar (1989)
A equao (3.85) mostrada na subseo 3.3.3, representa a dinmica de uma cadeia

de elos e juntas de um corpo rgido ideal, supondo que exista uma fora agindo nas juntas.
Embora esta equao seja complexa, ela uma idealizao e possui um nmero de
efeitos dinmicos que no so incluidos, ou so difceis de se estimar com preciso como o
atrito. Tambm, nenhum corpo completamente rgido. Uma anlise mais completa iria
incluir vrias fontes de flexibilidade, como deformaes elsticas, engrenagens e rolamentos,
deflexes dos elos sob carga e vibraes (SPONG; VIDYASAGAR, 1989).
3.4.1 Rastreio de um ponto de referncia

Um dos mtodos de se rastrear uma meta utilizando um compensador PID (do ingls:
Proportional, Integral, Derivative). Este tipo de controle adequado para as aplicaes
que no envolvam movimentos muito rpidos, especialmente em robs com altas redues
de engrenagem entre o atuador e o elo. para rastreio de uma meta apenas (no variante
no tempo), uma vez que limitado quando efeitos adicionais como entrada de saturao,
distrbios e dinmica no-modelada devem ser consideradas (SPONG; VIDYASAGAR,
1989). Por este motivo, mtodos de controle mais robustos so empregados.
3.4.2 Controle antecipatrio e torque computado

Controle antecipatrio (Feedforward) um mtodo para rastrear uma trajetria variante no
tempo e ao mesmo tempo rejeitar distrbios variantes no tempo. Um esquema de controle
feedforward consiste em adicionar um bloco antecipatrio com a funo de transferncia
como mostrado (SPONG; VIDYASAGAR, 1989). Permita que cada uma das trs funes
de transferncia seja representada por razes polinomiais:
G (s) =
q (s)
c (s)
a (s)
H (s) =
F (s) =
p (s)
d (s)
b (s)
(3.88)
63
Figura 24 Sistema de simulao do rob e controle
Fonte: Lilly (2012)
Uma simples multiplicao entre os blocos do diagrama mostra que a funo de transferncia
(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 alcanar estabilidade no sistema de malha fechada, necessrio que o compensador H (s) e a funo de transferncia feedfoward F (s) possa ser escolhida de forma que
o polinmio p (s) d (s) + q (s) c (s) e b (s) sejam estveis pelo critrio de RouthHurwitz,
isso significa que alm da funo de transferncia da malha fechada ser estvel a funo
64
Figura 25 Esquema do controle antecipatrio
de transferncia feedfoward F (s), precisa tambm ser estvel (SPONG; VIDYASAGAR,

1989).
Se existe um distrbio entrando no sistema como mostrado abaixo, ento possvel
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 no haver distrbios o sistema fechado ir rastrear qualquer trajetria

desde que o sistema seja estvel. O erro , portanto, devido apenas aos distrbios (SPONG;
VIDYASAGAR, 1989).
Cancelamento de distrbios por Torque computado

O sinal feedforward resulta em um rastreio assinttico de qulquer trajetria com ausncia de
distrbios mas no melhora as propriedades de rejeio de distrbios do sistema. Portanto,
adicionamos para o sinal feedfoward, um termo que antecipa o efeito de distrbio.
Para uma trajetria desejada, sobreimposto o termo:
dd :=
djk q d qjd +
cijk q d qid qjd + gk q d
(3.91)
Uma vez que dd tem unidade de torque, a equao (3.91) chamada de mtodo do
torque computado. A equao acima compensa de uma maneira feedforward o termo no
linear do acoplamento inercial, acelerao de coriolis, centrpeta, e foras gravitacionais que
aparecem devido ao movimento do manipulador. Esta equao de extrema complexidade,
se tornando uma preocupao no tempo de processamento. Uma vez que a trajetria
desejada precisa ser conhecida, muitos dos termos podem ser pr-calculados e armazenados
off-line (SPONG; VIDYASAGAR, 1989).
65
Figura 26 Compensao de distrbio por torque computado
Figura 27 Diagrama de blocos do mtodo de torque computado
66
Captulo 4
Modelamento do Manipulador
Robtico
4.1
Requisitos iniciais
Para que o projeto do manipulador pudesse ser iniciado, teve-se que estabelecer alguns
requisitos iniciais. Estes valores so descritos a seguir.
4.1.1 Capacidade de carga

O manipulador robtico projetado neste trabalho no possui uma misso especfica, ele
pode ser empregado em uma srie de tarefas que vo desde soldagem separao e
montagem de componentes em uma linha de montagem, cada tarefa ir requerer um
efetuador especfico, por este motivo o projeto do efetuador no faz parte do escopo deste
trabalho.
Para fins de se limitar a dimenso do manipulador, determina-se que o manipulador
tenha uma carga til (do ingls: payload) de no mximo 15kg quando estiver em uma
configurao composta de seis juntas (6 GDL).
4.1.2 Nmero de graus de liberdade

O manipulador proposto composto de 6 juntas e 5 elos, contudo, por ser um manipulador
modular, pode-se alterar o nmero de juntas e elos do rob com certa facilidade. O rob
foi projeto de maneira a ser possvel trabalhar com o mnimo de 4 juntas, e no mximo 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 nmero de juntas montados no manipulador.
4.1.3 Velocidade linear

No se faz aqui um requisito mnimo ou mximo de velocidade linear, pois este ir depender
de uma srie de fatores, como: nmero de juntas, comprimento dos elos, tipo de atuador
utilizado, sistema de controle e tipo de trajetria adotada (KOIVO, 1989).
Captulo 4. Modelamento do Manipulador Robtico
67
Tabela 3 Capacidade de carga do manipulador

Nmero de juntas
Carga til mxima (kg)
17
16
15
12
Fonte Produzido pelo autor

Nota Os valores apresentados na tabela foram obtidos aplicando-se um fator de segurana 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 especificaes 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 Apndice C.
Figura 28 Manipulador proposto em uma configurao arbitrria
68
Tabela 4 Especificaes tcnicas estimadas para o

manipulador
Parmetro
Valor
Unidade
Graus de liberdade
Faixa de trabalho (alcance mx.)
1,2
Fora mxima (esttico)
350
Fora mxima (em movimento)
200
Fora contnua em movimento
150
Preciso unidirecional
+/-1,0
mm
Repetibilidade unidirecional
+/-0,4
mm
Velocidade linear mxima do efetuador
1,65
m/s
Frequncia do sistema de controle
50
Hz
Massa total
26,8
kg
4.2.1 Mdulo da junta

A parte mecnica mais complexa do manipulador a junta. Ela deve conter inmeros
componentes para alta capacidade de carga ocupando o mnimo de espao possvel. Alm
disso, ela deve possuir funes adicionais como: freio de emergncia, cabeamento para
conduzir sinal e potncia de uma junta para outra, e em alguns casos possuir atuao
manual. Com o intuito de minimizar a complexidade da unidade central de controle,
controle local do motor e sensores so inevitveis.
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, produo e montagem, portanto, a parte interna contm os mesmos
componentes para todas as juntas, porm com escalas diferentes. Cada junta integrada
dentro de uma carcaa prpria, o que permite que ela possa ser montada e testada fora do
rob. As juntas que compem 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 carcaa, um freio eletromagntico, um harmonic drive para reduo da velocidade, um encoder eltrico na sada do eixo,
um sensor de torque e um sistema de trava contra rotao excessiva. A Figura 29 mostra
a junta do cotovelo e a Tabela 5, mostra as especificaes tcnicas para cada junta.
69
Figura 29 Junta do cotovelo
Atuador: motor DC sem escova e sem carcaa

O conjunto da junta deve ser extremamente compacto, por este motivo, elas foram
projetadas usando-se um motor especial que no possui carcaa, nem eixo prprio. O
motor, conhecido como Direct Drive, foi projetado para ser incorporado diretamente na
mquina, usando os prprios rolamentos da mquina para suportar o rotor. Esse motor
montado dentro da carcaa da junta, e possui sensores de efeito Hall 1 para monitorar a
posio do rotor. A Figura 30 mostra o motor da empresa Kollmorgen, escolhido a junta
do cotovelo.
Harmonic drive
Em manipuladores robticos comum o uso de harmonic drives para a reduo 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 aplicao de um campo magntico,
responde com uma variao em sua tenso de sada.
70
Tabela 5 Especificaes tcnicas das juntas

Junta
Dim. externo
(mm)
Comprimento
(mm)
Torque mx.
(N m)
Variao angular
( )
Veloc. mx.
(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

Nota Os valores apresentados na tabela foram obtidos com base nos catlogos dos fabricantes de cada
componente.
Figura 30 Frameless brushless DC motor
Fonte: Kollmorgen
redues 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 reduo de 160:1.
Os harmonic drives utilizados no projeto so produzidos pela empresa Harmonic Drive
AG, a Figura 31 mostra o redutor aplicado na junta 3 (cotovelo).
Freio eletromagntico
Um freio permanente para alta rotao usado na junta como sistema de segurana e
para parada rpida do movimento. No caso de haver corte da tenso, o freio ativado
automaticamente, uma vez que este utiliza a tenso para desmagnetizar o disco. Como
possvel perceber na Figura 29, o freio montado antes do motor (lado oposto do harmonic
drive), isto acontece pois nessa posio a reduo de velocidade ainda no ocorreu, o que
significa que o freio s precisa ter capacidade de parar o torque proveniente do motor (aprox.
71
Figura 31 Harmonic drive escolhido para a junta do cotovelo
Fonte: Harmonic Drive UG.
4Nm), e no o torque ampliado 160 vezes presente no eixo de sada. O freio escolhido,
mostrado na Figura 32 foi projetado para ambientes secos, sem lubrificao, e produzido
pela empresa KEB.
Figura 32 Freio permanente eletromagntico, modelo Combiperm
Fonte: KEB.
Trava mecnica
A trava limita mecanicamente a variao angular de operao da junta. Foi projetado de
maneira a permitir uma variao de rotao de aproximadamente 400 . Uma rotao de
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 mecnica proposta
Sensor de torque
A junta contm um sensor de torque integrado baseado em extensmetros eltricos (strain
gauges). Uma configurao usando uma ponte Wheatston (ponte completa), compensa
variaes na temperatura tanto quanto variaes no eixo do sensor, como deflexo, cargas
axiais e radiais. O volume e massa requeridos so baixos, de acordo com Schuler et al.
(2006), este tipo de sensor preciso e linear para variaes de torque esttico de at 400Nm.
O sensor feito de um eixo vazado de parede fina assim como o eixo de sada do harmonic
drive, a deformao devido a toro causada em ambos so compensadas pela unidade de
controle com base na leitura dos sensores. A Figura 34 mostra o sensor de torque proposto.
Para alcanar alta qualidade na medio do torque, os sinais do strain gauges devem ser
ampliados antes de serem enviados para o controlador da junta. Para isto necessrio um
circuito eletrnico especfico para este fim, que acoplado na junta junto com as placas de
circuito impresso de controle do atuador e sensor de rotao. Os fios que saem do strain
gauges passam pelo eixo vazado at a PCI.
73
Figura 34 Sensor de torque
Eixo vazado
A junta possui um eixo vazado por onde o cabeamento de alimentao dos sensores e
atuadores podem passar pela junta sem necessidade de ficarem exposto externamente.
possvel tambm passar neste eixo, mangueiras de ar comprimido, caso o rob esteja
usando um efetuador pneumtico (como garras com fechamento pneumtico), 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
74
Encoder
O encoder utilizado produzido pela empresa Netzerprecision. Ele funciona com 5V de
tenso, tem baixo consumo de energia, baixo valor de inrcia e quase nenhum atrito
interno. Este modelo ideal para a junta, pois alm de ser vazado, requisito para permitir a
passagem dos cabos, ele tambm permite rotao contnua, o que necessrio pois a trava
mecnica s age na junta aps 400 . O sinal de sada do encoder uma curva senoidal
ou coseinodal que representa o ngulo do eixo. O sinal digital deve ser obtido atravs
de processamento adicional usando a PCI de controle. A Figura 36 mostra o encoder
utilizado.
Figura 36 Sensor de posio angular (encoder)
Fonte: Netzerprecision
Controlador do atuador
Um controlador de motor um dispositivo eletrnico que atua como um dispositivo
intermedirio entre a unidade central de controle, uma fonte de alimentao e os motores.
Embora a unidade central (computador) decide a velocidade e direo dos motores, no
pode mov-los diretamente por causa de sua potncia limitada (corrente e tenso). O
controlador do motor, por outro lado, pode fornecer a corrente na tenso necessria, mas
no pode decidir o quo rpido 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 atravs de um
mtodo de comunicao padro e simples, como UART (conhecido como serial) ou PWM.
Alm disso, alguns controladores de motor pode ser controlado manualmente por uma
tenso analgica (geralmente criados com um potencimetro) (SCLATER; CHIRONIS,
75
1991).
Na Figura 29 percebe-se que existe trs discos paralelos montados externamente ao
freio. Estas placas representam as PCIs de controle do motor. No faz parte do escopo
deste trabalho o desenvolvimento eletrnico do controlador dos atuadores, mas fica como
sugesto para trabalho futuro.
4.2.2 Elos do manipulador

O mdulo do elo tem a funo 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 eltricos 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
4.2.3 Punho
A Figura 38 mostra o conjunto do punho desenvolvido para o rob. Percebe-se que ele no
esfrico, uma vez que os trs eixos no se cruzam em um ponto comum. Decidiu-se por
usar esta configurao de punho por ser possvel o uso do mesmo conjunto de junta nos
trs eixos que o compe. Ao final do punho existe uma flange com furos roscados em que
possvel afixar uma garra, ferramenta, arco de solda, etc.
76
Figura 38 Vista em corte do punho
4.3
Anlise estrutural por Elementos Finitos
Para verificao e validao do modelo projetado foi feita uma anlise de elementos finitos
(FEA) utilizando o software ANSYS, em que se buscou averiguar os pontos nas peas
que concentram as maiores tenses, qual o fator de segurana para cada modelo e se
vo ou no falhar devido as cargas s quais so submetidos. A anlise foi feita nas peas
estruturais principais do manipulador: elos 2 e 3, e o eixo principal de cada junta. O
material empregado no rob uma liga de alumnio que possui uma tenso de escoamento
de 280MPa e tenso ltima de trao de 310MPa.
A anlise foi feita fixando-se uma extremidade do elo e aplicando uma carga na
outra. Para o elo 2 foi aplicado uma fora de 300N, j para o elo 3, a fora 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.
77
Analisando os resultados percebe-se que a tenso equivalente de von-Mises menor

que a tenso de escoamento do material para todos os casos. Pode-se concluir, portanto,
que os componentes analisados no iro falhar.
As deformaes resultantes nos elos so consideradas altas para um manipulador
robtico, sendo este o principal motivo pela baixa preciso unidirecional do manipulador,
+/ 1, 0mm como mostrado na Tabela 4. As figuras abaixo mostram os resultados das
simulaes: deformao mxima, tenso de von-Mises e fator de segurana distribudo de
cada componente.
Figura 39 Resultado da anlise feita no elo 2
Figura 40 Resultado da anlise feita no elo 3
78
Figura 41 Resultado da anlise feita no eixo principal da junta da base
79
Figura 42 Resultado da anlise feita no eixo principal da junta do cotovelo
80
Figura 43 Resultado da anlise feita no eixo principal da junta do punho
81
82
Captulo 5
Anlise e Resultados
5.1
Cinemtica
A figura Figura 44 mostra a representao cinemtica do manipulador proposto, inserido

nessa figura o alinhamento dos eixos de referncia de cada junta. possvel perceber que
os eixos da junta 1 esto sobrepostos com os eixos de referncia inercial do rob (referncia
global), isto feito com o intuito de simplificar as matrizes de cinemtica direta, nota-se
tambm que os eixos z1 , ..., zn so alinhados com o eixo de rotao das juntas. Usando o
procedimento mostrado na subseo 3.2.1 obtm-se os parmetro D-H que so mostrados
na Tabela 6.
Figura 44 Manipulador proposto com os eixos de referncia de cada elo
Captulo 5. Anlise e Resultados
83
Tabela 6 Parmetros de DenavitHartenberg do manipulador proposto

Parmetro
Elo 1
Elo 2
Elo 3
Elo 4
Elo 5
Elo 6
ai [mm]
620
580
i [ ]
90
90
-90
di [mm]
154
-225
-150
-113,5
i [ ]
Fonte Produzido pelo autor.
Procura-se alinhar os eixos de forma a obter o mximo de zeros nos parmetros

di e i , por isso os eixos de coordenada x das juntas 2 e 3 so colineares. Com base nos
parmetros acima, pode-se estabelecer as matrizes de transformao homognea para
cada junta. Para auxiliar na formatao do texto, ser utilizado a letra C com subndice
para denotar a funo 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 =
c1 s1 c1 s1 s1 a1 c1
s1 c1 c1 c1 s1 a1 s1
0
s1
c 1
d1
0
0
0
1
c2 s2 c2 s2 s2 a2 c2
s2 c2 c2 c2 s2 a2 s2
0
s2
c 2
d2
0
0
0
1
c3 s3 c3 s3 s3 a3 c3
s3 c3 c3 c3 s3 a3 s3
0
s3
c 3
d3
0
0
0
1
c4 s4 c4 s4 s4 a4 c4
s4 c4 c4 c4 s4 a4 s4
0
s4
c 4
d4
0
0
0
1
c1
s1
0
0
0 s1
0
0 c1 0
1
0
154
0
0
1
c2 s2
s2 c2
0
0
0
0
0 620c2
0 620s2
1
0
0
1
c3 s3
s3 c3
0
0
0
0
0 580c3
0 580s3
1
0
0
1
c4
s4
0
0
0 s4
0
0 c4
0
1
0
225
0
0
1
(5.1)
T45 = A5 =
T56 = A6 =
84
c5 s5 c5 s5 s5 a5 c5
s5 c5 c5 c5 s5 a5 s5
0
s5
c 5
d5
0
0
0
1
c6 s6 c6 s6 s6 a6 c6
s6 c6 c6 c6 s6 a6 s6
0
s6
c 6
d6
0
0
0
1
c 5
s5
0
0
0 s5
0
0 c5
0
1
0
150
0
0
1
c6 s6
s6 c6
0
0
0
0
0
0
0
0
1 113, 5
0
1
A transformao da base para o efetuador dada pela multiplicao 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 posio da base do rob ao efetuador, n o vetor de orientao

do ngulo de guinada, s a orientao do ngulo de inclinao e a o vetor de orientao
do ngulo de rolagem.
5.1.1 Jacobiano
Para que seja possvel implementar a cinemtica inversa no software a matriz Jacobiana
deve ser calculada de forma numrica eliminando, assim, a necessidade de se trabalhar
com as derivadas parciais quem compem a matriz. Isto necessrio porque os clculos
com variveis simblicas no MATLAB so consideravelmente mais lentos em comparao
com manipulao de matrizes de nmeros apenas. De acordo com Spong e Vidyasagar
(1989), a matriz Jacobiana pode ser calculada recursivamente da seguinte forma:
zi1 (on oi1 )

J =
zi1
(5.3)
em que zi1 = R0i1 k, e on oi1 = R0i1 dni1 das quais k o vetor unitrio na
direo do eixo z, portanto, k = (0, 0, 1)T , R0i1 a matriz de rotao da junta 0 a i 1,
e d o vetor formado pela junta i 1 at o efetuador: dni1 = pn pi1 . O vetor p e a
matriz R0i1 so obtidos atravs das respectivas matrizes de transformao homognea
T . Por haver exatamente seis GDL, a matriz jacobiana quadrada, isso significa que o
manipulador no redundante.
5.2
85
Controle
Os sistemas de controle foram implementados utilizando o MATLAB Control System

Toolbox (Simulink), e so baseados nos sistemas apresentado por Corke (2011) com algumas
adaptaes para a integrao 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
86
Figura 46 Diagrama de blocos do controle antecipatrio
5.3
Tra jetria
Existem sete possveis tipos de trajetrias que o usurio pode escolher para descrever
um movimento, sendo a mais utilizada a trajetria ponto-a-ponto coordenada, que faz a
interpolao polinomial dos pontos inicial e final. Para se obter um movimento ainda mais
87
suave, optou-se por usar um polinmio de quinto grau, apresentado abaixo, com isto,
possvel se determinar tambm uma velocidade e acelerao 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

20f 200 8f + 120 tf 30 f t2f
2t3f

a5 =
(5.7)
300 30f 14f + 160 tf 30 2f t2f

2t4f

12f 120 6f + 60 tf 0 f t2f

a6 =
2t5f
A velocidade nos pontos de passagem devem ser informados pelo usurio, quando
no informados, usado uma heurstica para se determinar. Para cada junta um possvel
grfico de posio angular por tempo mostrado na Figura 47.
Para trajetria em linha reta, o comprimento L da trajetria dado por
L = kpm p0 k =
(xm x0 )2 + (ym y0 )2 + (zm z0 )2
(5.8)
Para trajetria parametrizada, o comprimento da curva no espao dado pela

integral das funes que parametrizam a curva:
S=
v
Ztf u
u
t
0
dx
dt
!2
dy
+
dt
!2
dz
+
dt
!2
dt
(5.9)
Para trajetria modelada em CAD, o comprimento da curva aproximado por:

S
=
np 1
X
i=1
kpi+1 pi k =
np 1 q
(xi+1 xi )2 + (yi+1 yi )2 + (zi+1 zi )2
i=1
onde np o nmero de pontos da tabela (gerados pelo programa CAD)
(5.10)
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 no for

quadrada, e o vetor v obtido partir da velocidade linear escalar v.
v = kvk =
vx2 + vy2 + vz2
vx = vy = vz
v=
(5.12)
T
1 [v, v, v]
3
A Tabela 7 mostra os sete tipos de trajetrias que foram implementadas no software e o

mtodo de resoluo de cada uma delas.
89
Tabela 7 Tipos e caractersticas de trajetrias

Nm
(1)
Trajetria
(Tipo)
Descoordenado
(ponto-a-ponto)
Entradas do
usurio
meta cartesiana;
Mtodo de resoluo
0 e f
calculados por
cinemtica inversa;
Durao do trajeto
ti =
(0 f )
ti =
(0 f )
funo: (t) = 0 + t
(2.1)
Sequencial
(ponto-a-ponto)
meta cartesiana;
0 e f
calculados por
cinemtica inversa;
funo: (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
cinemtica inversa;
(t): Equao 5.4
(t):
Equao 5.5
(t):
Equao 5.6
0 e f
calculados por
cinemtica inversa;
(t): Equao 5.4
(t):
Equao 5.5
(t):
Equao 5.6
0 e f
calculados por
cinemtica inversa;
(t): Equao 5.4
(t):
Equao 5.5
(t): Equao 5.6

0 e f
calculados por
cinemtica inversa;
(t): algoritmo de Taylor
(t):
derivada numrica (3.16)
(t): derivada numrica (3.17)
Especificado pelo
usurio
Especificado pelo
usurio
Especificado pelo
usurio para cada
ponto de passagem
Especificado pelo
usurio
0 e f
calculados por
cinemtica inversa;
(t): algoritmo de Taylor
(t):
Equao 5.11
(t):
derivada numrica
(3.16)
de ,
t=
L
v
90
Cont. Tipos e caractersticas de trajetrias

Nm
(6.1)
(6.2)
(7.1)
(7.2)
Trajetria
(Tipo)
Parametrizada
(contnua)
Parametrizada
(contnua)
Modelada em CAD
(tabelada)
Modelada em CAD
(tabelada)
Entradas do
usurio
funes paramtricas
no espao cartesiano;
tempo de trajeto
Mtodo de resoluo
(t) : calculado pela
cinemtica inversa
em vrios pontos;
Especificado pelo
usurio
(t):
derivada numrica3.16
(t):
cinemtica inversa
em vrios pontos;
funes paramtricas
no espao cartesiano;
velocidade linear
constante, (v)
tabela de pontos no
espao cartesianos;
tempo de trajeto
Durao do trajeto
t=
(t):
Equao 5.11
(t): derivada numrica

(3.16)
de ,
cinemtica inversa
em vrios pontos;
S
v
Especificado pelo
usurio
(t):
(t): derivada numrica3.17
tabela de pontos no
espao cartesianos;
velocidade linear
constante, (v)

cinemtica inversa
em vrios pontos;
t=
(t):
Equao 5.11
(t): derivada numrica

(3.16)
de ,
S
v
5.3.1 Exemplo de trajetrias

A seguir so apresentados alguns exemplos de trajetrias com o intuito de analisar o
comportamento cinemtico do manipulador.
Ponto-a-ponto: junta interpolada (coordenado)

O primeiro exemplo consiste de um movimento coordenado atravs de interpolao de
quinto grau das juntas (Equao 5.4). O manipulador estava posicionado na origem:
x = 1025, y = 215, z = 841 e foi movido para a posio: x = 100, y = 300, z = 670
com orientao arbitrria e tempo t = 10. A figura Figura 48 mostra o movimento e
as curvas de posio, velocidade e acelerao. A trajetria foi determinada usando as
definies do item (3) da Tabela 7.
91
Figura 48 Exemplo 1 - coordenado; esquerda: percurso no espao cartesiano; direita:

curvas de posio, velocidade e acelerao das juntas
Percebe-se que as curvas so suaves e sem descontinuidades, por este motivo este
o mtodo prefervel para movimentaes 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 trajetria descoordenada, a orientao arbitrria e
velocidade angular das juntas constante = 20[o /s].
92
Figura 49 Exemplo 2 - descoordenado; esquerda: percurso no espao cartesiano; direita:

Ponto-a-ponto: sequencial
Neste exemplo o manipulador tambm parte da posio de origem x = 1025, y = 215, z =
841 para a posio: x = 100, y = 300, z = 670 com orientao arbitrria e tempo t = 10,
mas as juntas so acionadas de forma sequencial, comeando a partir da junta da base.
93
Figura 50 Exemplo 3 - sequencial; esquerda: percurso no espao cartesiano; direita:

Ponto-a-ponto: linear
Nete exemplo, partiu-se da posio de origem x = 1025, y = 215, z = 841 para a posio:
x = 400, y = 550, z = 950 com orientao arbitrria e tempo t = 10 e trajetria linear,
obtida atravs do algoritmo de Taylor.
94
Figura 51 Exemplo 4 - linear; esquerda: percurso no espao cartesiano; direita: curvas

de posio, velocidade e acelerao das juntas
A seguir so apresentados os grficos de algumas trajetrias parametrizadas em

um manipulador tridimensional com 6 graus-de-liberdade:
Parametrizada: crculo
Um crculo de raio r e centro em (h, k), pode ser parametrizado pelas seguintes equaes:
=a
= 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)
95
Figura 52 Exemplo 5 - circulo; esquerda: percurso no espao cartesiano; direita: curvas

Parametrizada: elipse
=c
= 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
96
Figura 53 Exemplo 6 - elipse; esquerda: percurso no espao cartesiano; direita: curvas

Parametrizada: curva de Lissajous
=c
= b sin (kz t) + e
y = a cos (ky t) + d
t [0, 2]
(5.15)
Onde ky e kz so constantes descrevendo o nmero de lbulos 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], obtm-se a curva mostrada na Figura 54.
97
Figura 54 Exemplo 7 - curva de Lissajous; esquerda: percurso no espao cartesiano;

direita: curvas de posio, velocidade e acelerao das juntas
Parametrizada: hipotrocoide
x ()
=a
Rr
y () = (R r) cos + d cos
+b
r
Rr
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]
98
Figura 55 Exemplo 8 - hipotrocoide; esquerda: percurso no espao cartesiano; direita:

Parametrizada: hlice cnica

Um hlice pode ser parametrizada pelas seguintes equaes:
= et + f
= 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.
99
Figura 56 Exemplo 9 - hlice cnica; esquerda: percurso no espao cartesiano; direita:

5.4
Dinmica
Revisitando a Equao 3.85:

= M (q) q
+ C (q, q)
q + F (q, q)
+ G (q)
Assim como no clculo do Jacobiano, o segundo termo da equao de movimento (matriz
C) necessita ser calculada recursivamente de forma a evitar o uso de derivadas parciais.
Segundo Fang A. Basu e Wang (1994), possvel calcular este valor recursivamente da
seguinte maneira:
C (q, q)
q = Cmkj qj qk
Cmkj
Hmkj + Dmkj , j < k

=
Emkj + Dmkj , j k
(5.18)
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 equao so calculados como mostrado na subseo 3.3.3. A Tabela 8,
mostra o tensor de inrcia para cada elo do manipulador. Os valores foram extrados do
software Solidworks e obtidos a partir de eixos paralelos aos eixos de referncia do elo,
porm posicionados no centro de gravidade do elo i(SPONG; VIDYASAGAR, 1989) 3 . O
vetor rcg,i o vetor formado partindo do sistema de referncia 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

3
consulte a subseo A.1.2.1 para mais detalhes
101
Tabela 8 Propriedades dos elos

Elo
Elo 1
Elo 2
Elo 3
Elo 4
Elo 5
Efetuador
Tensor de inrcia[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
5.4.1 Curvas de torque dos exemplos

As figuras a seguir mostram as curvas de torque dos exemplos mostrados na subseo 5.3.1.
Percebe-se que, para trajetrias com velocidade moderada e sem a aplicao de foras
externas no manipulador, o torque da junta 1 e 2 no ultrapassa o valor de 180N m,
valor este menor que o torque mximo da junta de 627, 2N m (Tabela 5), o mesmo
ocorre para todas as outras juntas. Percebe-se tambm que quando a trajetria apresenta
descontinuidades, a curva de torque tambm vai apresentar descontinuidades, por esta
razo desejvel sempre o uso de trajetrias suaves por interpoladas polinomial.
102
Figura 58 Curvas de torque da trajetria 1 - coordenado
Figura 59 Curvas de torque da trajetria 2 - descoordenado
103
Figura 60 Curvas de torque da trajetria 3 - sequencial
Figura 61 Curvas de torque da trajetria 4 - linear
104
Figura 62 Curvas de torque da trajetria 5 - crculo
Figura 63 Curvas de torque da trajetria 6 - elipse
105
Figura 64 Curvas de torque da trajetria 7 - curva de Lissajous
Figura 65 Curvas de torque da trajetria 8 - hipotrocoide
106
Figura 66 Curvas de torque da trajetria 9 - hlice
5.4.2 Exemplo de simulao dinmica

A seguir apresentado um exemplo de dinmica direta na qual o manipulador foi posicionado na posio horizontal (1 , ..., 6 = 0[ ]) e em seguida foi desligado o torque
dos motores(1 , ..., 6 = 0[N m]). Esta simulao feita a partir das equaes de movimento(Equao 3.85) contudo, ao invs de se calcular o torque, calculado as aceleraes
das juntas atravs de mtodos numricos de resoluo de equaes diferenciais como o
Rungee-Kutta e posteriormente aplicado integrais numricas para se calcular a velocidade
e posio das juntas. A simulao foi adaptada do cdigo apresentado por Corke (2011) e
usa a funo ode45 do MATLAB para resoluo das equaes diferenciais. A Figura 67
mostra a posio do manipulador em diferentes instantes da simulao, as curvas de
posio, velocidade e acelerao das juntas so mostradas na Figura 68.
107
Figura 67 Posio do manipulador em diferentes momentos da simulao
108
Figura 68 Resultados da simulao dinmica
possvel perceber claramente que o rob est entrando em colapso devido

gravidade, mas tambm nota-se que a velocidade rotacional dos elos est exercendo um
torque centrpeto e de Coriolis na junta da base o que faz esta rotacionar, o movimento se
assemelha ao de um pndulo duplo.
109
Captulo 6
Consideraes Finais
Neste trabalho foi proposto um projeto de manipulador modular do tipo articulado com
seis graus de liberdade. Alm disso, foi desenvolvido um software em MATLAB para
controle do manipulador onde possvel simular o comportamento cinemtico e dinmico
no apenas do manipulador proposto mas tambm de qualquer manipulador articulado,
no importando o nmero 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 parmetros pre-estabelecidos para seu desenvolvimento, como carga mxima
til, por exemplo. O projeto foi validade atravs de uma anlise de elementos finitos feita
no ANSYS.
A segunda etapa consistiu de uma anlise cinemtica e dinmica do manipulador
proposto. Para desenvolvimento desta anlise procurou-se estabelecer mtodos para a
soluo de cinco dos seis problemas encontrados em controle de manipuladores: cinemtica
inversa, cinemtica direta, velocidade e acelerao cinemtica, equaes de movimento
e controle de posio do rob. O sexto problema, controle da fora, no foi considerado
neste trabalho.
Para resoluo da cinemtica direta, foi utilizado as mastrizes de transformao
homogneas, empregadas em conjunto com os parmetros de Denavit-Hartenberg, onde
foi possvel determinar a posio do efetuador conhecendo-se os valores das variveis das
juntas (ngulos).
Para resoluo da cinemtica inversa foi utilizado dois mtodos principais: o
mtodo dos mnimos quadrados amortecido que se utiliza da matriz Jacobiana para
aproximar o efetuador da meta atravs de iteraes das variveis das juntas. Este mtodo
se mostrou eficaz para posicionar o manipulador em uma posio desejada, mesmo quando
o manipulador est prximo de uma posio singular ou quando a coordenada est prxima
do limite mximo alcanvel. Contudo este mtodo no permite atribuir uma orientao
para o manipulador, para isto foi utilizado o mtodo de Newton-Raphson para restringir a
orientao do punho atravs de trs ngulos ao redor dos eixos de referncia da base. Este
mtodo tambm se mostrou razoavelmente estvel e preciso nas aplicaes realizadas.
As equaes de movimento do manipulador foram obtidas por meio do mtodo
Captulo 6. Consideraes Finais
110
de Euler-Lagrange na qual possvel determinar as curvas de torque necessrias para

que o manipulador efetue uma determinada trajetria no espao. Tambm foi possvel
determinar a dinmica direta do manipulador na qual o usurio determina uma curva
de torque, e atravs de integrais numricas, obtm-se as curvas de posio, velocidade e
acelerao das juntas para cada instante t.
Por fim, foi implementado o sistema de controle de posio do rob, na qual usou-se
de dois mtodos clssicos: torque computado e controle antecipatrio, na qual consistem
em antecipar os valores de torque calculados e trat-los como distrbios para aumentar
a eficincia e velocidade de convergncia para os valores desejados, possibilitando um
rastreio mais fiel da trajetria.
Todos os mtodos acima foram implementados em um software de controle baseado
em interface grfica desenvolvido em MATLAB, que possibilita ao usurio definir e analisar
de maneira gil diferentes trajetrias para o rob. Neste programa possvel simular escolher
entre sete diferentes tipos de trajetrias, alm de possibilitar a criao 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
Sugestes para trabalhos futuros
Alguns tpicos que esto intimamente relacionados ao ttulo do trabalho mas so de

complexidade muito elevadas foram omitidos do escopo deste trabalho, alguns deles so:
sistemas de controle mais avanados que utilizam viso para determinao de trajetria;
sistemas de controle do manipulador que usa sistemas de equaes no lineares multivarivel;
controle de torque; e desenvolvimento eletrnico do controlador. A seguir proposto outras
sugestes para trabalhos futuros.
6.1.1 Implementar um sistema de controle de fora no software

Foi implementado apenas o sistema de controle de posio e velocidade, para atividades
em que o rob no interage com o ambiente (como soldagem) para atividades em que
o rob carrega carga ou monta uma pea em um conjunto necessrio um sistema de
controle de fora.
6.1.2 Reprogramao do software em outra linguagem de programao

O Matlab um software que proporciona uma linguagem de programao de alto nvel. Por
este motivo relativamente simples e rpido implementar cdigo nele, sendo este o motivo
principal de se ter escolhido esta linguagem de programao para a execuo do software
de controle. Contudo, seria ideal no ter um software intermedirio para um controle de
Captulo 6. Consideraes Finais
111
robs, sendo ento indicado reprogramar o software de controle com a interface grfica
em uma outra linguagem de programao de forma que ele possa ser utilizado em outras
plataformas e computadores que no possuem o Matlab instalado. JAVA, PYTHON, C++
e C# so linguagens de cdigo aberto indicados para este fim.
6.1.3 Implementao de mecanismo de segurana

A maioria dos robs industriais atualmente no so colaborativos, ou seja, deve-se instalar
uma cabine de proteo ao redor do rob caso o usurio for trabalhar prximo dele.
Para transformar o manipulador em colaborativo, deve-se adicionar em cada junta um
mecanismo de proteo que consiga detectar quando um certo torque alcanado e parar
o movimento do manipulador, protegendo, assim, o operador caso o manipulador colida
com ele.
Figura 69 Estrutura e princpio de funcionamento do SJM III
Fonte: Laboratory (2010)
112
Referncias
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.
722728, 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. 24352440, 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. 14351460, 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. Disponvel em:
<http://www.kuka-robotics.com/en/products/industrial_robots/sensitiv/>. 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 Lige, 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. 8996, 1994.
FDOR, M. Application of inverse kinematics for skeleton manipulation in real-time.
Proceedings of the 19th spring conference on Computer graphics, ACM Press, n. 03, p.
203212, 2003.
Referncias
113
FLETCHER, R. Practical methods of optimization. 2nd. ed. New York, NY, USA:
Wiley-Interscience, 1987.
FU, K.; GONZLEZ, 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.
1420, 1985.
GROOVER, e. a. M. P. Robtica, Tecnologia e Programao. So Paulo, SP: McGraw-Hill,
1988.
HEATH, M. Scientific computing: an introductory survey. [S.l.]: McGraw-Hill, 2002.
(McGraw-Hill Higher Education).
HIBBELER, R. Mecnica: dinmica. [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. Clculo avanado. [S.l.]: Edgard Bcher 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. Disponvel
em: <https://sites.google.com/a/webmail.korea.ac.kr/intelligent-robot-laboratory/
manipulation/safety-mechanism>. 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.
Referncias
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. 163171, 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. Disponvel em:
<http://www.robotnik.eu>. Acesso em: 18 mar 2016.
ROBOTS, U. UR10 robot, a collaborative industrial robot. 2016. UR10. Disponvel em:
<http://www.universal-robots.com/>. 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. 18,
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.
Referncias
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. 489499, 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. 13591363, 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. 93101, 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. 313336, 1994.
Apndices
117
APNDICE A
Software de Controle
Uma GUI (do ingls: Graphical User Interface), um tipo de interface que permite o
usurio interagir com um aparelho eletrnico atravs de cones grficos e indicaes visuais.
GUI foram introduzidas em reao 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 usurio de aprender uma linguagem de programao ou de digitar comandos
para executar uma aplicao.
Este captulo tem como objetivo apresentar o software desenvolvido. A interface
grfica foi desenvolvida utilizando o software Matlab que possui uma caixa e ferramentas
chamada GUIDE (Graphical User Interface Development Envirolment) que proporciona
ferramentas de criao de interface grfica para aplicaes customizadas.
O programa foi primeiramente desenvolvido na Universidade de Dundee, para
controle dos robs recm adquiridos AX-12A Smart Robotic Arm produzidos pela empresa
CrustCrawler Robotics, por este motivo, foi dado o nome de SMART-GUI ao programa.
Vrias modificaes foram feitas no programa com o intuito de ser possvel controlar e
simular movimentos no s do manipulador proposto, mas tambm, qualquer manipulador
do tipo articulado, no importando a quantidade de graus de liberdade do mesmo 1 .
O programa foi baseado nos conceitos e equaes apresentados nos captulos
anteriores. mostrado, neste captulo, os algoritmos implementados no programa, podendo
ser utilizado como referncia para trabalhos futuros ou para aperfeioamento do programa.
No captulo seguinte ser mostrado uma breve demonstrao de como utilizar o programa
para o controle do manipulador modular.
A.1
Leiaute e funcionalidades do programa
A interface grfica foi projetada de forma a ser possvel atribuir tarefas ao rob (quando
este estiver conectado ao computador), e ao mesmo tempo, simular seus movimentos na
rea grfica do programa.
Para uma melhor disposio do layout, dividiu-se o programa em duas janelas,
1
Para um nmero de GDL maior que 10, o rob se torna super redundante e as equaes da dinmica
ficam extremamente complexas, podendo tornar a simulao bastante lenta
APNDICE A. Software de Controle
118
SMART-GUI e Simulation Windows, sendo a primeira dividida em trs 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 ao especfica para o rob, como no exemplo mostrado na figura abaixo, o comando
1 tem a funo de mover o efetuador para uma coordenada especfica. Um objeto handle
um componente individual do comando e armazenado na estrutura de handles do Matlab
onde cada callback pode acessar suas informaes. Cada comando demarcado por um
painel (contorno cinza), onde todos os objetos handles so inseridos. Abaixo apresentada
cada aba do software.
A.1.1 Caixa de Mensagens

Este componente reservado para mostrar todas as mensagens relacionadas s aes e/ou
configuraes feitas no software. As mensagens tem o propsito de alertar o usurio sobre
alguma mudana ou requisitar alguma ao dele. A figura abaixo mostra a mensagem
exibida quando o usurio pressiona o boto ORIGEM.
Figura 70 Mensagem exibida ao iniciar o programa
A.1.2 Aba Conguraes

Na aba Configuraes todos os parmetros do programa so mostrados em uma tabela.
Ao total so 27 parmetros diferentes que o usurio pode configurar no programa, abaixo
dado uma breve explicao sobre cada um deles. Os botes, do lado direito, permitem
ao usurio salvar as configuraes e abrir a tabela de parmetros especficos do rob.
Graus de liberdade do rob

A primeira ao que o usurio deve tomar quando estiver utilizando o software de
configurar a quantidade de graus de liberdade que o rob ter. Esta ao necessria
tanto para quando o manipulador estiver conectado ao computador quanto para quando o
119
Figura 71 Aba Configuraes
usurio quiser apenas simular o movimento do manipulador no programa. Este campo

aceita apenas nmeros inteiros, um erro ser exibido caso o usurio entre outro tipo de
dado
Posio inicial
O boto ORIGEM vai mandar o rob para uma posio de segurana. Esta posio
estabelecida de forma que o rob possa repousar sem forar os atuadores ou uma posio
arbitrria escolhida pelo usurio, preferencialmente distante o suficiente da superfcie onde
a base est fixada, para no haver chances de coliso. O usurio, seu critrio, pode
120
definir a coordenada Origem para a ponta do efetuador na linha Home Position, para
isso preciso entrar com o dimenso em mm para x, y, z dando um espao simples para
separar os nmeros. A orientao do punho tambm 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 ].
Posio da junta na posio inicial

Caso o usurio no queira especificar uma posio de origem cartesiana para o efetuador,
possvel inserir os valores das juntas, o programa vai ento calcular a posio e orientao
atravs de cinemtica direta.
Distncia mxima alcanvel

Neste campo o usurio insere a distncia mxima que o manipulador pode alcanar quando
este estiver estendido. Caso o usurio deixe este campo em branco o programa vai tentar
calcular este valor usando os parmetros de Denavit-Hartenberg e cinemtica direta, porm
estes valores podem no ser precisos em alguns casos.
Modo de simulao apenas

Modo de Simulao, que vai informar a GUI se o movimento apenas para ser simulado
na janela do programa ou, caso esta opo esteja desmarcada, o programa ir enviar a
informao de posio para cada atuador, movimentando o rob.
Habilitar simulao dinmica

Este um valor booleano, quando ativado (verdadeiro) o software efetua os clculos de
dinmica inversa para cada comando que o usurio der. Dependendo do nmero de graus
de liberdade do manipulador, esta opo pode deixar a simulao do comando um pouco
lenta, quando ativado. Caso desativado (falso) o usurio no poder plotar as curvas de
torque do manipulador e tambm no ser possvel utilizar dos comandos da quarta aba:
Simulao Dinmica e nem ser possvel ativar a opo Sistema de controle, uma vez que
esta necessita dos valores de torque calculados.
Habilitar sistema de controle

Esta opo habilita o sistema de controle nos comandos. Quando habilitado, o software
recalcula a trajetria baseado nas curvas de torque, a trajetria final similar ao que
um rob conectado no programa ir apresentar. Caso desativada esta opo, a trajetria
mostrada na janela de animao ser a ideal com base nos parmetros passados pelo
usurio.
121
Taxa de atualizao do sistema de controle

Uma vez que o sistema de controle trabalha com funes discretas, a taxa de atualizao do
sistema de controle corresponde a diferena de tempo entre uma coleta de dados do sensor
a outra no sistema de controle. Caso o rob no esteja conectado (modo de simulao
apenas) esta taxa usada para calcular a quantidade de pontos na trajetria, portanto
para uma trajetria qualquer que leve t segundos para se concluir, os valores das juntas
do manipulador sero calculados a cada T milisegundos, em que T o valor da taxa de
atualizao determinado pelo usurio, resultando portanto, em T t passos.
Habilitar efetuador
O usurio deve marcar esta opo caso o manipulador a ser simulado ou controlado pelo
programa possuir um efetuador. No caso do efetuador ser uma garra, o usurio poder
controlar o ngulo de abertura da garra na aba comandos e programas. Caso habilitado, o
usurio dever inserir os ngulos limites de trabalho na tabela de parmetros do rob.
Nmero do rob
Para alguns tipos de controladores de robs, exigido um nmero de identificao nico
para o rob, este nmero pode ser inserido no software pelo campo Nmero do rob. Este
campo aceita tanto nmero quanto caracteres de texto.
Nmero da porta de comunicao

Caso o rob esteja conectado, ser necessrio selecionar o nmero de porta para estabelecer
a conexo. Este nmero pode ser obtido no Device Manager em computadores que o
sistema operacional o Microsoft Windows.
Nmero Baud padro

Dependendo do controlador eletrnico utilizado pelo rob, a comunicao entre o software
e rob feita pela porta USB, e portanto, necessita do baud number, que consiste de
uma taxa de modulao por segundo. Se o controlador for um Arduino, o baud padro
geralmente 9600. Caso o usurio utilize o software apenas em modo de simulao, este
parmetro irrelevante e pode ser deixado em branco.
Mostrar rastro
A opo mostrar rastro, quando habilitada exibe um rastro vermelho contnuo na ponta
do efetuador quando este se movimenta. Esta opo no influencia na operao e resultado
dos comandos, usado apenas para melhor compreenso e visualizao do movimento.
122
No possvel 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 opo quando ativada, limpa o rastro a cada movimento que o manipulador fizer. Isto
aconselhvel quando o usurio estiver utilizando a aba programas, em que o manipulador
vai executar uma srie de comandos o que torna o rea grfica bastante poluda caso
estiver desativado.
Quadros por segundo

Esta opo equivalente ao nmero de quadros de um vdeo. Para um determinado
movimento do manipulador, o nmero de pontos ir atualizar a representao grfica
n vezes. Este nmero depende da complexidade do manipulador carregado no software,
se o manipulador tiver muitos detalhes, o tempo para atualizar cada quadro na janela
de animao longo, portanto o nmero de quadros por segundo deve ser reduzido. O
usurio deve primeiro testar uma trajetria na aba comandos inserindo um tempo para a
trajetria, se ao executar o comando a animao levar mais tempo que o tempo imposto,
o nmero de quadros por segundo deve ser reduzido utilizando-se de uma regra de trs
simples. Este nmero no est associado taxa de atualizao do sistema de controle e
nem opo de mostrar rastro, ele usado apenas para a animao do movimento.
Salvar vdeo do comando

Quando habilitado o software ir salvar um vdeo em formato .avi do comando que estiver
sendo animado. A orientao da cmera e luz ser igual ao mostrado durante a animao.
O arquivo salvo na pasta ativa (pasta que contm o software) no momento da animao.
Mostrar eixos
Mostra pequenos segmentos dos eixos x, y, z na ponta do efetuador. Este segmentos so
exibidos tanto quando o manipulador est em movimento quanto em repouso. Esta opo
funciona em associao com a ferramenta matriz de transformao (aba comandos). Os
eixos sero exibidos na junta denotada pelo nmero escolhido na matriz, por exemplo, se a
matriz de transformao escolhida for a T_01, os eixos iro ser mostrados na junta 1. Esta
opo tambm pode ser ativada no menu de contexto: Tools>Display reference frames
Erro permitido para a cinemtica inversa

Uma vez que o software utiliza um mtodo iterativo para calcular a cinemtica inversa,
necessrio inserir um desvio mximo permitido para os valores calculados. O erro
considerado a diferena cartesiana entre a posio efetuador e a meta desejada.
123
aconselhvel deixar este valor prximo de 0,01mm, para precises maiores o software pode
ficar mais lento para calcular a cinemtica inversa e a taxa de atualizao do sistema de
controle pode no ser atendida.
Nmero mximo de iteraes na cinemtica inversa

Este se refere ao nmero mximo de iteraes que o algoritmo de cinemtica inversa vai ter
para fazer com que o efetuador alcance a meta desejada. Caso este nmero seja alcanado,
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 rpido sem
alcanar uma boa aproximao da meta, enquanto que valores muito elevados far com
que se leve muito tempo para calcular os valores das juntas. Este nmero varia de acordo
com o algoritmo utilizado e as dimenses do rob. Para o algoritmo de cinemtica inversa
implementado no software aconselhvel deixar este nmero prximo de 100.
Vetor de gravidade
Nesta opo o usurio pode escolher em que eixo a gravidade ir agir. Esta opo interfere
no resultado da dinmica inversa, uma vez que este valor utilizado para calcular a parcela
de torque relacionado gravidade. Ao utilizar os parmetros de Denavit-Hartenberg no
manipulador, o eixo z da base geralmente fica na vertical, por isso aconselhvel deixar
este vetor da seguinte forma: [0, 0, 9.08665], o valor deve estar em m.s2 .
Pausa entre atualizaes no servo motor

No caso de robs que utilizam servo motores, os controladores necessitam de uma pausa
entre o envio de informao do software para o controlador. Esta pause tem a finalidade
de garantir que todos os pacotes de informao foram entregues antes de comear a enviar
novos pacotes. Geralmente este valor pequeno, por volta de 0.001s. Este parmetro no
interfere na simulao e pode ser deixada em branco caso o rob no esteja conectado.
Passo de incremento cartesiano: grosseiro

Na aba comandos, o comando Mover Efetuador utiliza do valor deste parmetro para
incrementar a posio do efetuador na direo do eixo clicado. O valor de incremento
grosseiro usado para aproximar o efetuador de uma posio desejada, quando este estiver
longe da meta.
Passo de incremento cartesiano: no

Tambm utilizado no mesmo comando: Mover Efetuador por incremento. O incremento
fino geralmente para posicionar o efetuador com maior preciso na meta desejada.
124
Incremento linear para trajetria em linha reta

O software utiliza o algoritmo de Taylor para criar trajetrias cartesiana lineares. O
algoritmo consiste em inserir pontos de quebra na linha formada pela posio atual do
efetuador e a meta desejada. Este parmetro consiste na distncia linear entre um ponto
de quebra e outro (em mm).
Conjunto dos ngulos

O software utiliza os ngulos relativos das juntas para realizar os clculos de cinemtica
direta, inversa e dinmica. Porm, possvel modificar a forma como o programa mostra
esses ngulos na aba comandos. Esta opo tem propsito didtico apenas e serve para
que o usurio possa rapidamente descobrir a posio das juntas em diferentes conjuntos,
como global, relativo e motor. No conjunto global, os ngulos das juntas so medidos a
partir dos planos de referncia da base, j no conjunto relativo, os ngulos so medidos
partindo da posio atual do elo que antecede a junta; e por fim no conjunto dos ngulos
do motor, o valor do ngulo medido a partir da posio zero do motor (para alguns tipos
de robs 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
Nmero do comando
O nmero de comando consiste em uma identificao (valor inteiro) na estrutura do
histrico na qual os resultados dos clculos (posio das juntas, velocidade, acelerao e
torque) so armazenados. aconselhvel deixar este parmetro como 1, e a cada comando
dado ao software este valor automaticamente incrementado. Ao clicar no comando de
125
plotar os grficos, o programa checa o nmero de comando atual e acessa os valores

do histrico salvos nesse endereo. Desta forma o usurio pode plotar os resultados de
qualquer comando dado. Ao limpar o histrico o nmero do comando automaticamente
resetado para 1.
Estado da interrupo de comando

Este parmetro modificado automaticamente pelo software. Se um comando estiver em
execuo e o usurio clicar no boto Parar na aba comandos, este valor modificado para
verdadeiro, e as funes que estiverem em execuo so interrompidas.
Estado do comando
Este comando tambm modificado automaticamente pelo programa. Ao executar um
comando este valor mudado para falso, e ao terminar a execuo do comando o parmetro
mudado para verdadeiro. Este parmetro informa algumas funes se o comando est
em execuo ou se j foi completado.
Boto: Salvar conguraes

Salva as configuraes atuais exibidas na tabela. A tabela salva em um arquivo com
extenso .mat (SETTINGS.mat). aconselhvel fazer uma cpia de segurana deste arquivo.
Sempre que o software aberto, a ltima verso da tabela configuraes carregado.
Boto: Carregar conguraes

O usurio pode salvar uma cpia das configuraes do programa para um determinado
rob, e sempre que for ser utilizar esse rob carregar as configuraes especficas para ele.
Isso impede que o usurio tenha que modificar cada parmetro das configuraes toda vez
que trocar de rob.
Boto: Resetar conguraes

O usurio pode resetar a tabela de configuraes para a verso de fbrica. Nem todos
os parmtros padres (de fbrica) funciona com todos os tipos de robs. O usurio deve
se ater a parmetros mais fundamentais como nmero de graus de liberdade do rob e
posio origem do manipulador, para no haver movimentos inesperados, como colises. O
usurio no estar livre de modificar a tabela de parmetros do rob, pois no existem
parmetros de fbrica para este.
126
Boto: Parmetros do Rob

Abre um formulrio com a tabela de parmetros do rob. Ao terminar de editar os
parmetros o usurio deve clicar no boto SALVAR. Toda vez que o usurio modificar os
parmetros do rob, o software vai atualizar os arquivos de desenho .STL (para a rea
grfica - animao). O usurio deve se certificar que os desenhos esto salvos na pasta
OTHER_FILES e que esto de acordo com os parmetros 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. (nmero de
arquivos de elos devem corresponder ao nmero de graus de liberdade - 1) e caso a opo
Habilitar Efetuador tenha sido marcada (verdadeiro) na tabela de configuraes, deve
haver tambm um arquivo chamado end_effector.STL.
Figura 73 Tabela de parmetros do rob
A.1.2.1 Obtendo a matriz de inrcia do elo

Os valores Ixx , Ixy , Ixz , Iyy , Iyz , Izz podem ser obtidos atravs de clculos ou de um software
CAD. No Solidworks, apresentado 3 opes de momento de Inrcia, como mostrado na
Figura 74.
127
Figura 74 Propriedades de massa do desenho
O primeiro consiste nos momentos de inrcia principais, ao redor dos eixos principais
de inrcia. O segundo o momento de inrcia obtido no centro de massa (CG) e alinhado
com os eixos de coordenada do sistema, e por fim tem os momentos de inrcia obtidos
usando os eixos de coordenada do sistema. Os momentos de inrcia utilizados no software
o segundo mostrado na figura. De acordo com Spong e Vidyasagar (1989), o equacionamento
dinmico feito considerando a matriz de inrcia ao redor de eixos paralelos aos eixos
de referncia 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 referncia do elo,
caso ainda no esteja, como mostrado na Figura 75. Na esquerda mostrado a pea no
Solidworks e os eixos do software, na direita mostrado os eixos de referncia de cada elo.
Figura 75 Alinhamento da modelo no software
128
Os valores inseridos na tabela devem estar em kg.m2 . Os sinais dos produtos

de inrcia Ixy , Ixz , Iyz devem ser iguais aos apresentados pelo software CAD. Os sinais
negativos so inseridos automaticamente que a matriz fique igual ao apresentado na
Equao 3.59.
Boto: Limpar histrico

Este boto limpa o arquivo HISTORY.mat. Quando clicado, abre-se uma janela alertando
que a opo no pode ser desfeita. Ao limpar o histrico o rob volta para sua posio de
origem na janela de Animao. A partir desse ponto no possvel plotar os grficos de
posio, velocidade, acelerao 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 usurio passar aes
ao manipulador. Nesta aba possvel fazer apenas um nico comando por vez.
Mover para a coordenada

Este comando move a ponta do efetuador (ou ltima junta, quando o efetuador no estiver
presente) para uma posio arbitrria determinada pelo usurio. A forma com que as
juntas se movimentaro e o caminho que o efetuador far no espao vai depender do tipo de
trajetria escolhido, no menu Opes de trajetrias. Na maioria das aplicaes industriais
escolhe-se a trajetria interpolada que cria uma curva suave entre os posies iniciais e
finais das juntas utilizando polinmios de quinto grau. As entradas nos campos X, Y, Z
devem ser escalares se a trajetria for interpolada e vetores se as trajetria escolhida for
Interpolada com pontos de passagem. O campo tempo, deve possuir o mesmo nmero
de valores que os campos de coordenadas. Se o usurio desejar, por exemplo, mover o
efetuador para a posio 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 devero
ser preenchidos da seguinte maneira:
O campo O (orientao) deve ter um vetor linha (1x3) para cada ponto de passagem
e final. Se as velocidade dos pontos de passagem no for fornecido, o software vai tentar
preencher esses valores aplicando uma heurstica. O boto C tem a funo apenas de
limpar os campos.
Mover efetuador
Este comando move o efetuador na direo do eixo escolhido pelo usurio. A distncia que
o efetuador ir se mover depende do incremento escolhido, e dos valores destes passados
129
Figura 76 Aba Ferramentas
na aba Configuraes. necessrio informar o tempo de durao do movimento ou a

velocidade linear do efetuador no espao cartesiano. O caminho traado pelo efetuador
tambm depende do tipo de trajetria escolhida.
Trajetria Parametrizada
Neste comando o usurio descreve o trajeto do efetuador no espao cartesiano atravs de
funes paramtricas do tempo. O software permite que o usurio escreva expresses para
designar uma funo, esta expresso ento convertida para expresso simblica utilizando
os comandos do MATLAB, que vai criar um caminho determinado pelas funes. Este
caminho ento convertidos em posio das juntas utilizando a cinemtica inversa. Para
uma trajetria em forma de crculo no plano Y Z, os campos de trajetria paramtrica
devem ser preenchidos da seguinte forma:
130
Figura 77 Mover efetuador com pontos de passagem

Figura 78 Percurso atravs de funes paramtricas
O que resultaria em um crculo de raio 500. O tempo do trajeto tambm deve ser
passado ao programa.
Trajetria por tabela

Neste comando o usurio 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 usurio no campo Tempo (em segundos). As coordenadas podem ser geradas
por um programa CAD, e devem estar prximas uma das outras, uma vez que no feito
interpolao entre os pontos.
131
Mover Juntas
Este comando similar ao comando Mover para coordenada porm, ao invs de se inserir
a meta cartesiana para o efetuador, o usurio vai passar ao programa uma posio meta
para a junta. Neste caso tambm haver interpolao da curva, se a trajetria escolhida
for Interpolada. Como nos outros comandos, o usurio requisitado a passar um tempo
total de trajeto. O software considera que a acelerao e velocidade inicial e final da junta
so ambas iguais a zero.
Garra
Este comando habilitado apenas se a opo Habilitar efetuador estiver selecionada na
aba Configuraes. Esta ferramenta somente til se o efetuador for uma garra. Com isto
possvel mandar comandos de abrir e fechar completamente a garra ou abrir para uma
posio especfica. Quando a garra aberta a posio da ponta da garra em relao
base muda, em comparao de quando a garra est fechada. O usurio deve ficar ciente de
que o software sempre considera a distncia da garra quando esta est fechada para seus
clculos de cinemtica direta e inversa.
Matrizes de transformao
Esta opo exibe ao usurio a posio e orientao de uma junta especfica dada pela matriz
de transformao. Esta opo no influencia no movimento do rob, ela tem propsito
didtico apenas. O usurio pode escolher uma das n juntas do manipulador e a matriz de
transformao homognea daquela junta com relao base se atualiza automaticamente.
Boto Origem
Este comando faz o efetuador se movimentar para a posio de Origem previamente
estabelecido no aba Configuraes. No necessrio inserir um tempo de trajeto para este
comando, pois o software vai utilizar a velocidade mdia das juntas inseridas na tabela de
parmetros do rob para determinar o tempo do percurso. A trajetria para este comando
sempre Interpolada, mesmo que outra opo esteja selecionada no menu.
Movimento aleatrio
Este comando cria um conjunto de metas cartesianas aleatrias e posteriormente chama a
funo de movimentar o efetuador para coordenadas com pontos de passagem. O tempo e
velocidade para cada ponto de passagem determinado pelo programa.
132
Boto Stop
O comando Parar serve como um boto de emergncia, quando pressionado ele para o
rob, caso este esteja conectado no computador, e interrompe a animao na tela, caso
esteja sendo executada. O tempo de parada de no mnimo 1 segundo aps o boto foi
pressionado, isso se d porque o programa no consegue interromper um loop que pode
estar ocorrendo (dependendo da ao do rob) nas funes primrias.
A.1.4 Aba Programas

Na aba Programas possvel criar uma lista de comandos e fazer o manipulador execut-las
de maneira automtica.
Figura 79 Aba Programas
133
Torque
Umas das formas de se gerar uma trajetria para o manipulador conhecida como teach
pendent. Nesta forma, o usurio desliga o torque do manipulador de forma que este fica
livre para ser movimentado manualmente pelo usurio, este ento entra com um comando
no computador para salvar a posio das juntas, que feito a partir da leitura dos sensores.
Para que isso seja possvel, a junta tem que exercer no elo, apenas a fora para sustentar
o peso do manipulador, para que o usurio consiga moviment-lo sem grande esforo. Este
valor de torque conhecido como compensao da gravidade e seu calculo baseado nas
foras nas juntas para manter equilbrio esttico do manipulador.
A.1.5 Aba Simulao Dinmica

Figura 80 Aba Simulao Dinmica
134
Nesta seo possvel inserir curvas paramtricas ou valores constantes de torque para
cada junta e simular o comportamento dinmico. Na parte: posio de incio, o usurio
pode posicionar o rob em uma configurao desejada para iniciar a simulao. Deve-se
inserir um tempo para cada equao paramtrica, a simulao assumir o maior valor de
tempo inserido.
A.1.6 Janela de Animao

Nesta janela exibida a animao de cada comando dado ao rob. A janela conta com
uma barra de ferramentas nativa do MATLAB em que possvel alterar a orientao
do rob na tela, dar zoom ou ainda colher valores dados como coordenadas das juntas,
atrav[es do boto data cursor.
Figura 81 Janela de Animao
Atualizar juntas
O boto 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
posio qualquer. Quando pressionado, este boto ir executar uma funo que ir ler a
posio de cada atuador e atualizar os controles do comando 2 e o texto.
135
Limpar trilha
A opo Limpar Rastro deleta todos os pontos azuis deixados no grfico que traam o
caminho percorrido pelo efetuador do manipulador. Caso este boto seja pressionado
enquanto o grfico est animando algum movimento, os pontos deletados sero os pontos
gerados at o momento que o boto foi pressionado, os pontos gerados depois disso sero
exibidos na tela.
Plotar grcos
O boto plotar grficos tem o propsito de gerar as curvas de velocidade e acelerao para
um determinado movimento do manipulador.
Mudar vista
A qualquer momento durante ou depois do movimento do rob, o boto Mudar Vista pode
ser pressionado, o que far com que a vista de exibio do rob seja alterada para uma das
vistas pr-definidas. Duas das vistas so isomtricas e as outas so projees ortogonais.
A.2
Arquivos do programa
A interface grfica, desenvolvida em Matlab verso R2015a, composta por diversos

arquivos que trabalham interconectados, os principais deles so descritos a seguir.
A.2.1 SMART-GUI.m
O arquivo SMART_GUI.m rene todas as aes para cada componente da interface grfica.
Este o arquivo principal de todo o programa e contm a maior parte das funes do
software. Cada vez que o usurio clica em algum boto da interface grfica, o Matlab vai
procurar a respectiva funo daquele componente e executar as linhas de cdigo dentro da
funo.
A.2.2 SMART-GUI.g
O arquivo SMART_GUI.fig um arquivo gerado pelo Matlab que contm todas as informaes dos componentes da GUI, tais como posio 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.
136
A.2.3 Funes
As funes que compem o programa so programadas em arquivos individuais com
extenso .m
A.3
Estruturas de dados
Todas as configuraes, parmetros e valores resultados de clculos feitos pelas funes

primrias e secundrias so armazenados em estruturas de dados, para posteriormente
serem acessados e processados por outras funes. Abaixo dado uma breve apresentao
de cada estrutura e como as funes acessam cada uma delas. Ao iniciar o software, as
estruturas so carregadas pela funo miscelnea MF_Load_Structures, que por sua vez,
cria uma cpia das estruturas na rea de trabalho base do MATLAB(base workspace). As
funes quando necessitarem acessar valores de alguma estrutura, devem copiar a estrutura
para a rea de trabalho da funo (usando a funo evalin). Escolheu-se este mtodo por
se mostrar ser mais rpido do que carregar o arquivo .mat a cada chamada de funo.
A.3.1 HISTORY.mat
O arquivo HISTORY.mat possui uma estrutura de dados chamada H, na qual todos os valores
calculados da cinemtica direta, inversa, dinmica inversa, direta, e sistema de controle.
Os arquivos so acessados atravs do nome da estrutura seguido pelo nome da varivel
(conhecido como field no MATLAB), usando notao de ponto. Cada comando dado pelo
usurio cria uma nova camada na estrutura, sendo assim, para obter os resultados de
um comando especfico do histrico o usurio precisa do nmero de comando. As funes
que que necessitarem acessar os valores das juntas de um determinado comando, utiliza a
seguinte sintaxe: H(cn).q. A Tabela 9 apresenta as variveis da estrutura H.
Ao clicar no boto Limpar histrico na aba Configuraes, todas as variveis de
todos os comandos dados (camadas da estrutura) so resetadas (as variveis permanecem,
porm vazias).
A.3.2 MESSAGES.mat
O arquivo MESSAGES.mat, possui uma estrutura chamada M. Esta estrutura responsvel
por armazenar todas as frases de avisos, instrues e mensagens que so 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 usurio sobre a utilidade especfica
de algum boto ou opo. A funo que exibe as mensagens a MF_Update_Message que
carrega a estrutura M para sua rea de trabalho e escolhe o texto da mensagem necessria
137
Tabela 9 Variveis da estrutura Histrico (H)

Varivel
Descrio
posio da varivel da junta (matriz sp x n)
dq
velocidade da varivel da junta
ddq
acelerao da varivel da junta
vetor posio do efetuador
lv
velocidade linear do efetuador
la
velocidade angular do efetuador
tq
torque de cada junta
time
durao total do comando
sp
nmero de passos (step points)
matrizes de transformao
homognea de cada junta
qc
posio da varivel da junta,

resultado do sistema de controle
dqc
velocidade da varivel da junta,

ddqc
acelerao da varivel da junta,

tqc
torque de cada junta,

usando o nome do campo e o nmero da mensagem. A Tabela 10 descreve cada varivel

da estrutura.
A.3.3 ROBOT-DATA.mat
O arquivo ROBOT_DATA.mat, possui as estruturas: D,LD,RP. A estrutura D, possui as equaes
da dinmica inversa (torque) e Jacobiano de forma simblica. Estas equaes no so
utilizadas nos clculos, uma vez que o programa funes numricas para calcular o torque
e o Jacobiano (funes primrias PF_Inverse_Dynamics e PF_Jacobian), porm ter as
funes de forma simblica pode ser til para o usurio para outras aplicaes. O usurio
pode copiar essas funes sempre que precisar.
A estrutura LD, contm as informaes sobre o desenho de cada elo do rob.
Quando o usurio edita a tabela de parmetros do rob, o programa converte os arquivos
de extenso .STL em coordenadas de vrtices, faces, e cores, possibilitando a criao de
patches pelo MATLAB que por sua vez so impressos na janela de animao. A estrutura
RP armazena os parmetros do rob, preenchidos pelo usurio na aba configuraes. Os
campos dessa estrutura so idnticos aos da tabela mostrada na Figura 73.
138
Tabela 10 Variveis da estrutura Mensagens (M)

Varivel
Descrio
settings
mensagens da aba configuraes
commands
mensagens da aba comandos
programs
mensagens da aba programas
notice
textos de anncios sobre alguma ao.

Ex: o rob foi movido para a posio de origem
warnings
textos de avisos (erros)

Ex: um erro foi encontrado na linha x do programa
animW
mensagens sobre a janela de animao
robotP
mensagens sobre a tabela de parmetros do rob
simulation
mensagens sobre a aba simulao dinmica
controlS
textos das mensagens sobre o sistema de controle
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 subseo A.1.2. J a estrutura
AS, armazena valores adicionais (additional settings) que no so editveis pelo usurio, a
Tabela 11 descreve cada varivel contida em AS.
Tabela 11 Variveis da estrutura Configuraes Adicionais (AS)
Varivel
Descrio
traj_opts
lista com os textos de todas as trajetrias disponveis
commands
lista com os textos de todos os comandos disponveis
program_table
nomes das variveis usadas para formar

uma tabela de comandos (na aba programas)
robot_param
lista com os textos dos parmetros rob. Usado quando o

programa carrega a tabela de parmetros do rob na tela
validation
texto de validao para quando o usurio

for carregar um arquivo de configuraes externo.
stop
varivel de controle para parar o rob
cmd_state
varivel de controle sobre o comando
co
espeficao da orientao. Usado para informar

funo da cinemtia inversa se deve tentar satisfazes
a orientao imposta pelo usurio.
cn
varivel de controle do nmero de comando
139
APNDICE B
R do Software
Rotinas em Matlab
de Controle
APNDICE B. Rotinas em Matlab do Software de Controle
1 FUNES PRIMRIAS
%==========================================================================
% >>>>>>>>>>>>>>>>>> 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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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)

%
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));
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)))
[~, T_m] =
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)

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 <<<<<<<<<<<<<<<<<
%==========================================================================
% 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)
% varargin: {1}time, {2}either 'cart'
Settings (from base workspace)
or 'joint', {3}either p or q vetors
%Load

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,:);
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 <<<<<<<
%==========================================================================
% 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.
%==========================================================================
% 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]

%% LOAD FILES
%Load
%Load
a = RP.a; d = RP.d; A = RP.alpha;
%DH parameters
if cn == 1
else
q0 = H(cn - 1).q(end,:);
current theta angles
end
%get

% 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);
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
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

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
mn = dqt(idx,:) - dq_n; %angular
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 = [];
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

coefficients
q_cf = [a5 a4 a3 a2 a1 a0];
coefficients
dq_cf = [5*a5, 4*a4, 3*a3,
2*a2, a1];
coefficients
ddq_cf = [20*a5, 12*a4, 6*a3,
2*a2];
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

%==========================================================================
% >>>>>>>>>>>>>> FUNCTION PF-C.3: UNCOORDINATED TRAJECTORY <<<<<<<<<<<<<<<<
%==========================================================================
% 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.
%==========================================================================
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
=> 1x6)
sp = ceil(max(ti) * fps);
% va = constant angular velocity;
%% Load structures and calculate
necessary variables
%Load
%Load

if cn == 1
%get theta angles of home position
else
q0 = H(cn - 1).q(end,:);
%get
end
% Time vector from 0 to total time,

with sp rows
tv = linspace(0,max(ti), sp)';
q = zeros(sp,n);
dq = zeros(sp,n);
ddq = zeros(sp,n);
for i=1:n
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)];
va = varargin{1};
arguments in
if nargin ~= 3
return
else
pt = varargin{3};
elseif strcmp(varargin{2}, 'joint')
qt = varargin{3};
end
~exist('qt', 'var')
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

%==========================================================================
% >>>>> FUNCTION PF-C.4: SEQUENTIAL WITH CONSTANT ANGULAR VELOCITY <<<<<<<
%==========================================================================
% based on the user's input. The trajectory formed here is sequential (one
% joint move at a time) and has constant joint velocity.
%==========================================================================
PF_Sequential_cav_Traj(varargin)
sp = sum(ceil(ti * fps));
=> 1x6)
% Time vector from 0 to total time,
% dqc = constant angular velocity;
with sp rows
tv = linspace(0,sum(ti), sp)';
necessary variables
%Load
tvj = zeros(sp, n);
for i=1:n
%Load
spi = ceil(ti(i) * fps);
if cn == 1
else
q0 = H(cn - 1).q(end,:);
%get
end
dqc = varargin{1};
arguments in
if nargin ~= 3
return
else
pt = varargin{3};
qt = varargin{3};
end
~exist('qt', 'var')
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
q = zeros(sp,n);
dq = zeros(sp,n);
ddq = zeros(sp,n);
for i=1:n
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)];
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

end
%==========================================================================
% >>>>>>>> FUNCTION PF-C.5: SEQUENTIAL TRAJECTORY WITH TIME INPUT <<<<<<<<
%==========================================================================
% 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.
%==========================================================================
PF_Sequential_ti_Traj(varargin)
%% Check inputs
if size(ti, 2) == 1
=> 1x6)
ti = ones(1, n) * ti;
necessary variables
end
%Load
%Load

if cn == 1
else
q0 = H(cn - 1).q(end,:);
%get
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};
arguments in
if nargin ~= 3
return
else
pt = varargin{3};
qt = varargin{3};
end
~exist('qt', 'var')
%compute target q if not passed
end
end
n = size(qt, 2);
joints (scalar)
%%
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
q = zeros(sp,n);
dq = zeros(sp,n);
ddq = zeros(sp,n);
for i=1:n
a0 = q0(i);
a1 = 0;
a2 = 0;
a3 = (10*(qt(i) - q0(i))) /
ti(i)^3;

a4 = -(15*(qt(i) - q0(i))) /
ti(i)^4;
a5 = (6*(qt(i) - q0(i))) / ti(i)^5;
coefficients
q_cf = [a5 a4 a3 a2 a1 a0];

coefficients
ddq_cf = [20*a5, 12*a4, 6*a3,
2*a2];
q(:,i) = polyval(q_cf,tvj(:, i));
dq(:,i) = polyval(dq_cf,tvj(:, i));
ddq(:,i) = polyval(ddq_cf,tvj(:,
i));
end
end

coefficients
dq_cf = [5*a5, 4*a4, 3*a3, 2*a2,
a1];
%==========================================================================
% >>>>>>>> FUNCTION PF-C.6: LINEAR TRAJECTORY W/ CST LINEAR VEL <<<<<<<<<<<
%==========================================================================
% 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.
%==========================================================================
PF_StraightL_clv_Traj(pt, vl)
n = size(qt, 2);
%number of
joints (scalar)
=> 1x6)
[p0, T] = PF_Forward_Kinematics(q0, d,
% vl = constant linear velocity;
a, A);
necessary variables
p0(4:6) = PF_Transform_to_Orient(T{n});
%get the current orientation
%Load
%Load

H parameters
%D-

% fps = S.value{'fps'}; %get the
number of frames per second
liv = S.value{'lin_increm'}; %get
linear increment value (mm)
if cn == 1
else
q0 = H(cn - 1).q(end,:);
%get
end
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;
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
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 <<<<<<<<<<<<
%==========================================================================
% based on the user's input. A linear trajectory is generated using the
% Settings value linear increment distance.
%==========================================================================
PF_StraightL_ti_Traj(pt, ti)
[p0, T] = PF_Forward_Kinematics(q0, d,
a, A);
p0(4:6) = PF_Transform_to_Orient(T{n});
=> 1x6)
%get the current orientation
% dqc = constant angular velocity;
necessary variables
%Load
%Load

H parameters
%D-

% fps = S.value{'fps'}; %get the
number of frames per second
liv = S.value{'lin_increm'}; %get
linear increment value (mm)
if cn == 1
else
q0 = H(cn - 1).q(end,:);
%get
end
deltap = pt - p0;
nbp = ceil(norm(deltap(1:3)) / liv);
%number of break points in the line
sp = nbp;
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;
dq(1,:) = 0; dq(end,:) = 0;
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 <<<<<<<<
%==========================================================================
% 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.
%==========================================================================
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
%Load
%Load
~isempty(zv)
pez = symfun(pez, zv);
dpez = diff(pez, symvar(pez));
else
dpez = 0;

if cn == 1
else
q0 = H(cn - 1).q(end,:);
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
% Computing
step points (sp)
with sp rows
%time
vector used for plots
%time vector used for solving the
parametric equation
tvfpe = linspace(0, fvv(2), sp)';
xp = []; yp = []; zp = [];
if ~isempty(xv)

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);
dq(1,:) = 0; dq(end,:) = 0;
ddq(isnan(ddq)) = 0; %ddq(end, :) = 0;
end
%==========================================================================
% >>>>>>>> FUNCTION PF-C.9: PARAMETRISED TRAJECTORY W/ CST ANG VEL <<<<<<<<
%==========================================================================
%==========================================================================
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
%Load
%Load
if cn == 1
else
q0 = H(cn - 1).q(end,:);
end
n = size(q0, 2);
%Computing S
pej = sym(pej); %converting the string
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);

ti = S / va; %Computing trajectory
time
catch
ti = abs(fvv(2) - fvv(1));
%If
not possible to compute S, set fvv as
time
end
% Computing
step points (sp)
with sp rows
qp = [];
if ~isempty(jv)
function
qp = [qp; double(pej(tv))];
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

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(isinf(ddq)) = 0;
ddq(isnan(ddq)) = 0;
% ddq(1,:) = 0; %ddq(end, :) = 0;
end
%==========================================================================
% >>>>> FUNCTION PF-C.10: PARAMETRISED JOINT TRAJECTORY W/ TIME INPUT <<<<<
%==========================================================================
% 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.
%==========================================================================
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
else
q0 = H(cn - 1).q(end,:);
end
%PS: The inputs check is made in the

Secondary function that calls this one
%% LOAD FILES
%Load
%Load
if size(deltat, 2) == 1
ti = deltat;
else
ti = abs(deltat(2) - deltat(1));
end

%get
n = size(q0, 2);
pej = sym(pej); %converting the string
% Computing
step points (sp)
with sp rows

%%
qp
jv
if
Generate the trajectory

= [];
= symvar(pej);
~isempty(jv)
function
qp = [qp; double(pej(tv))];
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

% dq(1,:) = 0; dq(end,:) = 0;
% 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 <<<<<<<<
%==========================================================================
%==========================================================================
%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
% Computing
%Load
step points (sp)
%Load
with sp rows
if cn == 1
else
q0 = H(cn - 1).q(end,:);
end
n = size(q0, 2);
%get

xp = []; yp = []; zp = [];
xv
yv
zv
if
= symvar(pex);
= symvar(pey);
= symvar(pez);
~isempty(xv)
pex = symfun(pex, xv);
function
xp = [xp; double(pex(tv))];
else
xp = [xp; pex * ones(sp, 1)];
end
if ~isempty(yv)

pey = symfun(pey, yv);
%convert
yp = [yp; double(pey(tv))];
else
yp = [yp; pey * ones(sp, 1)];
end
if ~isempty(zv)
pez = symfun(pez, zv);
%convert
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
% if norm(deltap(1:3))
%
% end
% dq(1,:) = 0; dq(end,:) = 0;
% 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 <<<<<<<<<<
%==========================================================================
% 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
%==========================================================================
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
% s: orientation specification (vector
with sp rows
1x3), if [1 1 1] the inverse
% kinematics will attempt to satisfy
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
%Load
p(:, 4:6) = zeros(size(p, 1),
3);
%Load
else
s = [1 1 1];% satisfy
if cn == 1
else
q0 = H(cn - 1).q(end,:);
end
%get
%get
orientation in IK
end
end
dq(1,:) = 0; dq(end,:) = 0;
ddq(1,:) = 0; %ddq(end, :) = 0;
end

%==========================================================================
% >>>>>>>>> FUNCTION PF-C.13: TRAJECTORY BY TABLE W/ CST LIN VEL <<<<<<<<<<
%==========================================================================
% 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.
%==========================================================================
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
%Load
with sp rows
%Load
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),
3);
s = [0 0 0]; % no need to
if cn == 1
satisfy
orientation in IK
q0 = S.value{16};
%get
else
s = [1 1 1];% satisfy
else
orientation
in IK
q0 = H(cn - 1).q(end,:);
%get
end
end
end
n = size(q0, 2);
sp = size(p, 1);
points (sp)
% Computing step
%Computing S (approximation by linear

distance between points)

dq(1,:) = 0; dq(end,:) = 0;
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.
% 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.
%==========================================================================

function [t, q, dq] =
PF_Forward_Dynamics(t1, torq_matrix,
q0, qd0, varargin)
%Load
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

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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Date: March 30th, 2016.
% DESCRIPTION: This function will compute the torque for each joint based
% on the trajectory provided.
%==========================================================================
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
%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);

-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<k
H_ijk = z_vec{i}' *
I_ij * cross_p(z_vec{k}, z_vec{j});
D_ijk = D_term (i,
j, k, z_vec, I);
b = H_ijk + D_ijk;
else
C_ijk = C_term (i,
j, k, z_vec, r_cm, p_vecs, m);
D_ijk = D_term (i,
j, k, z_vec, I);
b = C_ijk + D_ijk;
end
T2(i) = T2(i) + (b *
dqr(k) * dqr(j));
end
%Term 3 - Gravity term

%
U_i = U_term (i, r_cm,
p_vecs, m);
%
T3(i) = (g(3) * z_vec{1})' *
cross_p(z_vec{i}, U_i);
end
for i=1:n
G_cell = zeros(1,1);
for j=1:n
J = PF_Jacobian(T_m(1:i));
J_L = J(1:3, :);
G_cell = G_cell + (m(j) * g *
J_L(:,i));
end
T3(1,i) = G_cell;
end
T1 = a_ij * ddqr';
Torque(row,:) = (T1 + T2 + T3')';
end
%% Functions
% Make a skew simmetric tensor of a
vector
function sst =
skew_symmetric_tensor(vec)
sst = [0 -vec(3) vec(2) ;
vec(3) 0 -vec(1) ; -vec(2) vec(1) 0 ];
end
function cross_vec = cross_p(v_a,
v_b)
cross_vec = [v_a(2).*v_b(3)v_a(3).*v_b(2);
v_a(3).*v_b(1)v_a(1).*v_b(3);
v_a(1).*v_b(2)v_a(2).*v_b(1)];
end
%Compute the U term that appears in
equation 14 - (equation 17)
function U_i = U_term (i, r_cm,
p_vecs, m)
U_i = 0;
for idx = i:n
rv_i_idx = (p_vecs{idx} p_vecs{i}) + r_cm{idx}; %#verificar se
indices estao certos
U_i = U_i + (m(idx) *
rv_i_idx);
end
end
%Compute the C term that appears in

function C_ijk = C_term (i, j, k,
z_vec, r_cm, p_vecs, m)
C_ijk = 0;
for idx = max([i,j,k]):n
rv_k_idx = (p_vecs{idx} p_vecs{k}) + r_cm{idx}; %#verificar se
c_k_idx = cross_p(z_vec{k},
rv_k_idx);
c_i_idx = cross_p(z_vec{i},
rv_i_idx);
C_ijk = C_ijk +
cross_p(z_vec{k}, c_k_idx)' * (c_i_idx*
m(idx));
end
end
end
end
%Compute the inertia tensor from
link i to j. Equation (15)
function I_ij = Inertia_tensor(I,
m, p_vecs, r_cm, i, j)
I_ij = zeros(3,3);
for idx = max([i,j]):n
rv_j_idx = (p_vecs{idx} p_vecs{j}) + r_cm{idx}; %#verificar se
r_i_idx =
skew_symmetric_tensor(rv_i_idx);%tensor
from vector
r_j_idx =
skew_symmetric_tensor(rv_j_idx);%tensor
from vector
I_ij = I_ij + (I{idx} m(idx) * (r_i_idx * r_j_idx));
end
end
end
%Compute the D term that appears in

function D_ijk = D_term (i, j, k,
z_vec, I)
D_ijk = 0;
for idx = max([i,j,k]):n
D_ijk = D_ijk + z_vec{i}' *
cross_p(z_vec{j}, (I{idx}*z_vec{k}));
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION PF-F: JACOBIAN MATRIX <<<<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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
%==========================================================================
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

end
%==========================================================================
% >>> FUNCTION PF-H: CONFIGURATION, CONSTRAINTS & REACHEABLE COORDINATE <<<
%==========================================================================
% 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.
%==========================================================================
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
%Load
if p(i, 3) < 0
Settings from base workspace
a_prob = [a_prob, i];
%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

end
%% Display messages
if ~is_reachable
MF_Update_Message(25, 'warnings',
num2str(r_prob));
elseif ~is_allowed
num2str(a_prob));
elseif ~is_insidelim
num2str(l_prob));
end
end
2 FUNES SECUNDRIAS
%==========================================================================
% >>>>>>>>>> FUNCTION SF-1: MOVE END-EFFECTOR TO HOME POSITION <<<<<<<<<<<<
%==========================================================================
% 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
%Load
enable_dynamics =
S.value{'enable_dynamics'};
%Load
if enable_dynamics
tq = PF_Inverse_Dynamics(q, dq,
%Load
ddq);
%get the computed torque
% If control system is enabled,
simulate the result of the control
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);
%%
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'};

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 <<<<<<<<<<<
%==========================================================================
% 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)
% 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.
case 2
%Load
PF_Interpolated_wvp_Traj(In.pos,
In.time, In.lv, In.av);
%%
% Check if coordinate is reachable and
configuration mode used
[is_allowed, is_reachable] =
PF_Conf_Const_Reach('p', In.pos);
if ~is_reachable
return
elseif ~is_allowed
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
return
end
function
switch In.trajopt
case 1
case 3
PF_Uncoodinated_Traj(In.av, In.space,
In.pos);
case 4
PF_Sequential_cav_Traj(In.av, In.space,
In.pos);
case 5
PF_Sequential_ti_Traj(In.time,
In.space, In.pos);
case 6
PF_StraightL_clv_Traj(In.pos, In.lv);
case 7
PF_StraightL_ti_Traj(In.pos, In.time);
otherwise
MF_Update_Message(7,
'warnings');
return
end
torque

enable_dynamics =
if enable_dynamics
ddq);
%
then the motion is performed in
the robot. For motion and animation
%
occuring at the same time, amend
MF_Animate_Commands(cn)

enable_control =
it connected)
simulation_only =
if enable_control
if ~simulation_only
ddq, tq);
torque
[qc, dqc, ddqc] =
else
qc = []; dqc = []; ddqc = [];
mode.
end
else
qc = []; dqc = []; ddqc = []; tq =
end
[]; tqc = [];
end
msg_in = num2cell(In.pos(end,1:3));
structure
%messages inputs
MF_Update_Message(1, 'notice', msg_in);
'time',tv, 'tq', tq);
%%
% Animate command
%
end
command animation on the screen and
%==========================================================================
% >>>>>>>>>>> FUNCTION SF-3: MOVE END-EFFECTOR BY INCREMENT <<<<<<<<<<<<<<
%==========================================================================
% 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
%==========================================================================
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
catch
%Load
%compute position by FK
%Load
a = RP.a'; d = RP.d'; alpha =
RP.alpha';
[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

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;
PF_Conf_Const_Reach('p', pos);
if ~is_reachable
return
elseif ~is_allowed
return
end
collision rote.
(pos);
versions.
return
end
function
switch In.trajopt
case 1
PF_Interpolated_Traj(In.time, In.space,
pos);
case 3
PF_Uncoodinated_Traj(In.av, In.space,
pos);
case 4

PF_Sequential_cav_Traj(In.av, In.space,
pos);
case 5
PF_Sequential_ti_Traj(In.time,
In.space, pos);
case 6
PF_StraightL_clv_Traj(pos, In.lv);
case 7
PF_StraightL_ti_Traj(pos, In.time);
otherwise
'warnings');
return
end
torque
enable_dynamics =
if enable_dynamics
ddq);
enable_control =
if enable_control
ddq, tq);
torque
[qc, dqc, ddqc] =
else
qc = []; dqc = []; ddqc = [];
end
else
qc = []; dqc = []; ddqc = []; tq =
[]; tqc = [];
end
structure
%%
% Animate command
%
%
%

end

axis = ['x', 'y', 'z'];
c = find(increm~=0);
it connected)
msg_inp = {axis(c),
simulation_only =
num2str(increm(c))};
%message inputs
'notice',
if ~simulation_only
msg_inp);
mode.
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION SF-4: MOVE JOINTS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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
%==========================================================================
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
the
%Load
%user is using the joints sliders with
the limits imposed in Settings
%Load
Robot Parameters
%Load
collision rote.
(pos);
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.
return
end
function
switch In.trajopt
case 1
PF_Interpolated_Traj(In.time, 'joint',
qt);
case 3

PF_Uncoodinated_Traj(In.av, 'joint',
qt);
case 4
PF_Sequential_cav_Traj(In.av, 'joint',
qt);
case 5
PF_Sequential_ti_Traj(In.time, 'joint',
qt);
otherwise
'warnings');
return
end
torque
enable_dynamics =
if enable_dynamics
ddq);

%%
% Animate command
%
%
%

it connected)
simulation_only =
if ~simulation_only
mode.
end

enable_control =
m_inputs = {num2str(In.joint),
num2str(In.qt)}; %messages inputs
if enable_control
MF_Update_Message(5, 'notice',
m_inputs);
ddq, tq);
torque
[qc, dqc, ddqc] =
else
qc = []; dqc = []; ddqc = [];
end
else
qc = []; dqc = []; ddqc = []; tq =
[]; tqc = [];
end
end
structure
%==========================================================================
% >>>>>>>>>>>>>> FUNCTION SF-6: MOVE END-EFFECTOR BY TABLE <<<<<<<<<<<<<<<<
%==========================================================================
%
%
%
%
%
%
%
%
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.

%==========================================================================
function SF_Move_by_Table(In)
% In.time, In.lv
collision rote.
% In: {1}: 'command' or 'program' - the
tab origin that called this
(TP);
% function, if it was called in command
tab, then load the table, otherwise
versions.
% it is not necessary to load the
table.
%
{2}: either 'time' or 'clv'
return
=> constant linear velocity
end
%
{3}: value: time or clv
(scalar)
%% Loading variables
%Load
%Load
%Load
try
%Attempt to load Table of Points
(from base workspace)
TP = evalin('base', 'TP');
catch
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)
return
end
PF_Conf_Const_Reach('p', TP);
if ~is_reachable
return
elseif ~is_allowed
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;
PF_Table_ti_Traj(TP, ti);
elseif In.trajopt == 11 &&
~isempty(In.lv)
clv = In.lv;
PF_Table_clv_Traj(TP, clv);
else
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.
compute time
home position
else
q0 = H(cn - 1).q(end,:);
end

if any(abs(q(1,:) - q0) ~= 0) % joints
not in starting point
maximum joint speed
time = max(abs(q(1,:) - q0)) /
%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
%%
torque
enable_dynamics =
if enable_dynamics
ddq);
qc = []; dqc = []; ddqc = []; tq =

[]; tqc = [];
end
structure
% Animate command
%
%
%

it connected)
simulation_only =
if ~simulation_only
mode.
end

enable_control =
if enable_control
ddq, tq);
torque
[qc, dqc, ddqc] =
else
qc = []; dqc = []; ddqc = [];
end
end
else
%==========================================================================
% >>>>>>>>> FUNCTION SF-7: MOVE ARM BY PARAMETRIC TRAJECTORY <<<<<<<<<<<<<
%==========================================================================
% 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,
%Load
In.pej, In.joint, In.av, In.lv,
% In.pos, In.time, In.space

%Load
%Load
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)
'warnings');
return
elseif In.trajopt == 10
PF_Parametrised_ti_Traj(In.pex, In.pey,
...
In.pez, delt, orient);
PF_Parametrised_clv_Traj(In.pex,
In.pey,...
In.pez, In.lv, delt, orient);
else
'warning');
return
end
elseif strcmp(In.space, 'joint')
%get the input space (joint space)
if ~ischar(In.pej)
'warnings');
return
PF_Parametrised_cav_Traj(In.pej,
In.joint, In.av, delt);
PF_Parametrised_J_ti_Traj(In.pej,
In.joint, delt);
else
'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.
compute time
home position
else
q0 = H(cn - 1).q(end,:);
end
if any(abs(q(1,:) - q0) ~= 0) % joints
not in starting point
maximum joint speed
time = max(abs(q(1,:) - q0)) /
%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
%%
PF_Conf_Const_Reach('q', q);
if ~is_reachable

return
elseif ~is_allowed
return
end
collision rote.
('q', q);
versions.
return
end

torque
enable_dynamics =
if enable_dynamics
ddq);
end
structure
%%
% Animate command
%
%
%

it connected)
simulation_only =
if ~simulation_only
mode.
end

enable_control =
if enable_control
ddq, tq);
torque
[qc, dqc, ddqc] =
else
qc = []; dqc = []; ddqc = [];
end
end
else
qc = []; dqc = []; ddqc = []; tq =
[]; tqc = [];
%==========================================================================
% >>>>>>>>>>>>>>>>>>> FUNCTION SF-8: RANDOM POSITION <<<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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()
%Load
% In structure should have the fields:
% In.trajopt, In.pos, In.time, In.lv,
RP = evalin('base', 'RP'); %Load Robot
In.av.
Parameters

%% 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
PF_Conf_Const_Reach('p', In.pos);
if ~is_reachable
return
elseif ~is_allowed
return
end
collision rote.
(In.pos);
versions.
return
end
function
PF_Interpolated_wvp_Traj(In.pos,
In.time, In.lv, In.av);
torque
enable_dynamics =
if enable_dynamics
ddq);
enable_control =
if enable_control
ddq, tq);
torque
[qc, dqc, ddqc] =
else
qc = []; dqc = []; ddqc = [];
end
else
qc = []; dqc = []; ddqc = []; tq =
[]; tqc = [];
end
structure
%%
% Animate command
%
%
%

it connected)
simulation_only =
if ~simulation_only
mode.
end
msg_in = num2cell(In.pos(end,1:3));
%messages inputs
MF_Update_Message(1, 'notice', msg_in);
end

%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION SF-10: RUN PROGRAM <<<<<<<<<<<<<<<<<<<
%==========================================================================

% 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
P = evalin('base', 'P');
%Load
% 'Command','Position', 'Joint',
Program table (P) from base workspace
'Trajectory Option', 'time', 'Lin vel',
%Load
'Ang vel'
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
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.

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
'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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will pause the program by a determined time

% (in seconds). This funciton is used in Program tab, where a pause between
% commands can be added by the user.
%==========================================================================
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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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,
1}{:});
try
TT = evalin('base', 'TT');
%
end
Load Torque Table (from base workspace)
else
catch
joint = TT{i, 1}{:};
return
end
end
%
if ~isnumeric(TT{i,3}{:})
Load Robot Parameters
time = str2num(TT{i,3}{:});
%
else
time = TT{i,3}{:};
%
end
expression = TT{i,2}{:};
cn = S.value{'cn'};
n = S.value{'dof'};
if cn == 1
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);

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);
ddq(1,:) = 0;
%%
% Animate command
%
%
%
MF_Animate_Commands(cn, 'full')

it connected)
simulation_only =
if ~simulation_only
mode.
end
qc = []; dqc = []; ddqc = [];


structure

end
3 SMART_GUI.M E FUNES TERCIRIAS

===========================================================================
%**************************************************************************
% >>>>>>>>>>>>>>>>>>>>>>>>>>>>> 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();

% 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');
%% #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);
%% ========================================================================
% >>>>>>>>>>>>>>>>>>>>>>>>>>> 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);

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))
return
end
if isempty(time) && isempty(lv) &&
isempty(av)
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))
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);
function
slider_jointn_CreateFcn(hObject,
eventdata, handles)
if
isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'
))
set(hObject,'BackgroundColor',[.9
.9 .9]);
end
if
))
.9 .9]);
end
if
))
.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.axes = [-1 0 0]; %increment only in
X (negative).
'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)
return
end
SF_Move_EE_by_Increment(In);
%% #ok
function
move_ee_pos_x_Callback(hObject,
eventdata, handles)
fine)

coarse =
else
end
In.axes = [1 0 0]; %increment only in
X (positive).
'Value');
'String'));
'String'));
'String'));
&& isempty(In.lv)
return
end
%% #ok
function
move_ee_neg_y_Callback(hObject,
eventdata, handles)
fine)
coarse =
else
end
In.axes = [0 -1 0]; %increment only in
Y (negative).
'Value');
'String'));
'String'));
'String'));
&& isempty(In.lv)
return
end
%% #ok
function
move_ee_pos_y_Callback(hObject,
eventdata, handles)
fine)
coarse =
else
end
Y (positive).
'Value');
'String'));
'String'));
'String'));
&& isempty(In.lv)
return
end
%% #ok
function
move_ee_neg_z_Callback(hObject,
eventdata, handles)
fine)
coarse =
else
end
In.axes = [0 0 -1]; %increment only in
Z (negative).
'Value');
'String'));

'String'));
'String'));
&& isempty(In.lv)
return
end
%% #ok
function
move_ee_pos_z_Callback(hObject,
eventdata, handles)
fine)
coarse =
else
end
Z (positive).
'Value');
'String'));
'String'));
'String'));
&& isempty(In.lv)
return
end
%% #ok
function
slider_jointn_Callback(hObject,
eventdata, handles)
'Value');
In.joint = get(handles.jointn_menu,
'Value');
In.qt = get(handles.slider_jointn,
'Value');
'String'));
'String'));
'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);

if isempty(joints) || isempty(qvalue)
return
end
% Check if the number of inputs are the
same for joints and qvalue
if size(joints) ~= size(qvalue)
return
end

'Value');
'String'));
In.joint = joints;
In.qt = qvalue;
'String'));
SF_Move_Joints(In)
%% #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');

if isempty(joint) || isempty(pej)
return
end
'Value');
'String'));
In.joint = joint;
In.pej = pej;
In.space = 'joint';
'String'));
'String'));
SF_Move_by_Parametric_Eq(In)
%% #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)
return
end
'Value');
'String'));
'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';

SF_Move_by_Parametric_Eq(In)
%% #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
end
%% #ok
function start_path_Callback(hObject,
eventdata, handles)
'String'));
'String'));
'Value');
if isempty(In.time) && isempty(In.lv)
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');

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
))
.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);
%%
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);
% 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);
%% #ok
function jointn_menu_Callback(hObject,
eventdata, handles)
%update
maximum and minimum values of slider
%% #ok
function
transformation_options_Callback(hObject
, eventdata, handles)
%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);

%% #ok
function
cmd_random_bt_Callback(hObject,
eventdata, handles)
SF_Random_Movement()
set(handles.orientation_angle,
'String', '');
set(handles.time, 'String', '');
if ispc &&
))
%% #ok
function clear_field_Callback(hObject,
set(hObject,'BackgroundColor','white');
eventdata, handles)
end
set(handles.p_x, 'String', '');
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',...
.
'inst_joints','inst_A_txt','inst_A','pr
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'
'prg_linvel'};...
'inst_time','inst_joints_txt',
'inst_joints', 'inst_A_txt',
'inst_A',...
%'Save actual Joints position'
'inst_time',
%'Move by Increment'
%'Load Table of Coordinates'


'prg_linvel'};...
%'Open Gripper'
pts', 'inst_A_txt', 'inst_A',...
'inst_time_txt', 'inst_time',
{'Modelled - Table (CST linear

vel.)', 'Modelled - Table (time)'};...
%'Open Gripper'
{'Interpolated',
'Uncoordinated'};...
%'Close Gripper'
{'Interpolated',
'Uncoordinated'};...
%'Close Gripper'
%'Add a Pause'
{''};...
'inst_time',
%'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});
%%
function add_inst_Callback(hObject,
eventdata, handles)
HD = handles;
%create a copy of
handles (just for shortening the name)
% Load
Additional Settings
ptvar = AS.program_table;
%Program
table variables
%Get P table from base workspace
try
% 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

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
% 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
%%
function
program_traj_opts_Callback(hObject,
eventdata, handles)
function
program_traj_opts_CreateFcn(hObject,
eventdata, handles)
if ispc &&
))
end
function prg_clear_Callback(hObject,
eventdata, handles)
% Create a new empty P table
% Load
Additional Settings
ptvar = AS.program_table;
%Program
table variables
% 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

% 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
%Updating uiTable
%% #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
catch
return
end
try
pgr_ind = getappdata(0,
'program_table_indice');
pgr_row = pgr_ind(:, 1)';
catch
return
end
if isempty(pgr_row)
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,:);
%Updating uiTable
%% #ok
function
inst_movedown_Callback(hObject,
eventdata, handles)
workspace
try
catch
return
end
try
catch
return
end
if isempty(pgr_row)
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,:);
%Updating uiTable
%% #ok
function save_inst_Callback(hObject,
eventdata, handles)
%>>>1: Open window to select folder for
saving file
%get
Program table from base workspace
uisave('P', 'Program_');
%% #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
%save
program table (P) in base workspace
else
return
end
% Display the loaded file in the
program table handle
%% #ok
function run_inst_Callback(hObject,
eventdata, handles)
SF_Run_Program();
%% #ok
function delete_inst_Callback(hObject,
eventdata, handles)
workspace
try
catch
return
end
try
catch
return
end
P(pgr_row, :) = []; % Delete selected
rows;
%Updating uiTable
function
inst_torque_cb_Callback(hObject,
eventdata, handles)
status = get(hObject, 'Value');
Torque_On_Off(handles.id, status,
handles);
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;
%% #ok
function traj_opt_Callback(hObject,
eventdata, handles)
MF_Enable_Commands(hObject, handles);
if ispc &&
))

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.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
%Load
repeat_last_cmd_Callback(hObject,
eventdata, handles)
cn = S.value{'cn'};

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)
MF_Plot_Base_frame(false);
%
delete axes
else
MF_Plot_Base_frame(true);
%
display axes
end
% ------------------------------------function
disp_link_frame_Callback(hObject,
eventdata, handles)
MF_Plot_Links_frame(0, false);
delete axes
else
MF_Plot_Links_frame(0, true);
display axes
end
% ------------------------------------------------------------------function plot_graphs_Callback(hObject,
eventdata, handles)
%Load
cn = S.value{'cn'} - 1;
%get the
last command number
MF_Plot_Graphs(cn);
4 FUNES MISCELNEAS
%==========================================================================
% >>>>>>>>>>>>>>>>> FUNCTION MF-1: INITIALISE SMART GUI <<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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');
set(handles.disp_link_frame, 'Checked',
MF_Load_Structures();
%Load
'off');
structures to base workspace
%Load
%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');

handles.tgroup, 'Title', 'COMMANDS');
handles.tgroup, 'Title', 'PROGRAMS');
handles.tgroup, 'Title', 'DYNAMIC
SIMULATION');
%Place panels into each tab
set(handles.P1,'Parent',handles.tab1);
%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,
'position', P_pos);
%Update sliders and static texts
'position', P_pos);
%Show message about software status
'position', P_pos);
end
'position', P_pos);
set(handles.PF, 'Units', painel_unit,
'position', PF_pos);
%==========================================================================
% >>>>>>>>> FUNCTION MF-2: INITIALISE GRAPHICAL REPRESENTATION <<<<<<<<<<<
%==========================================================================
% 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
%Load
q0 = S.value{'home_q'}; %get home
joint position from SETTINGS
%Load
end
%Load
nl = S.value{'dof'}-1;
%get the
LD = evalin('base', 'LD'); %LD contain
number of links (dof-1)
all the face and vertices points
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()

%
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
);
%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');
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',
ee_vert(:,1:3));

set(ee_patch, 'facec',
end
RP.color{nl+2});% set base color and
end
draw
set(ee_patch,
none
%==========================================================================
% >>>>>>>> FUNCTION MF-3: Creating GUI for Robot Representation <<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will create a new figure to host the axes of
% the graphical animation
%==========================================================================
function MF_Creating_RR_GUI()
%% Configuring size and position
'Callback',@Show_ref_frame_mi_Callback)
;
);
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 =
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);
'Display CG frames', 'Callback',...
@Show_cg_mi_Callback);
'Display Reference frame',...
1',
vm2
2',
vm3
3',
vm4
4',
vm5
5',
vm6
6',
'Callback', @view_Callback);
= uimenu(View_menu,'Label', 'View
%% 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';

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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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
% Load
% Load
Handles (from base workspace)
% Load
sp = H(cn).sp;
%get the
Robot parameters (from base workspace)
number of steps
% Load

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)
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;
%
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')
ax_size -ax_size ax_size]);
end
else
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',
Base_vert(:,1:3));
set(Base_patch, 'facec',
set(Base_patch,
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);
);
else
Tr = plot3(0,0,0,'r',
'LineWidth',2);
end
%% 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,
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',
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

%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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will update the messages displayed in the
% message box.
%==========================================================================
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,'%');
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 <<<<<<<<<<<<<<<<<<<
%==========================================================================

% DESCRIPTION: This function will update the ui controls (handles in the
% commands tab (transformation matrix and other menus)
%==========================================================================
function MF_Update_UI_Controls()
%------%% Load files and variables
%Get
%Update sliders and static text on UI.
handles as HD from base workspace
jn = get(HD.jointn_menu, 'Value');
%Load
qmin = RP.j_range{jn}(1);
Robot Parameters
qmax = RP.j_range{jn}(2);
%Load
%Joint n slider
set(HD.slider_jointn, 'Value', qc(jn));
%Load
set(HD.slider_jointn, 'Min', qmin);
set(HD.slider_jointn, 'Max', qmax);
set(HD.slider_jointn,
'SliderStep',[1/qmax, 10/qmax]);
set(HD.jointn_value, 'String',
a = RP.a';
num2str(round(qc(jn)), 2));
d = RP.d';
%------alpha = RP.alpha';
%Enabling Gripper ui
g_status = 'on';
%gripper will
% get current position
be
enabled
in
Commands
tab
if cn == 1
gn = S.value{'dof'} + 1;
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',
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)));
set(HD.current_joint_pos, 'String',
num2str(round(qc, 2)));
%==========================================================================
% >>>>>>>>>>>>>>>>> FUNCTION MF-7: RESET HISTORY STRUCTURE <<<<<<<<<<<<<<<
%==========================================================================
% version 1.0 - May 14th, 2016.
% DESCRIPTION: This function will clear history structure and reset command
% number. Refer to section 4 of documentation for details.
%==========================================================================
function MF_Clear_History()
end
M = evalin('base', 'M');
%%
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
%Update message
MF_Init_Graph_Rep();
%Update animation window
MF_Load_Settings_table();
%Update table in Settings tab
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 <<<<<<<<<<<<<<<<<
%==========================================================================
% 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} =
num2str(S.param{i});
else
S.value{i};
Settings_table = {};
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})
num2str(S.value{i});
%==========================================================================

% >>>>>>>>>>>> FUNCTION MF-9: LOADING ROBOT PARAMETERS TABLE <<<<<<<<<<<<<
%==========================================================================
% 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.
%==========================================================================
function MF_Load_Robot_param_table()
nc = n; %number of columns in the table
%Load
col_names = {col_names, 'EndSettings from base workspace
Effector'};
n = S.value{'dof'};
nc = nc + 1;
end
%% Configuring size and position
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
%%

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 <<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will load the parameters table when the button
% edit robot parameter in settings tab is pressed.
%==========================================================================
function MF_Load_Program_table(P)
Ptable(row, col) =
% Loading variables
{''};
%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})

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 <<<<<<<<<<<<<<<<
%==========================================================================
% 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);
%full filename
return
end
% Attempt to open the file
try
if size(TP,2) < 6
TP = xlsread(ffname);
catch
end
try
TP = open(ffname);
% saving in base workspace
catch
assignin('base', 'TP', TP);
'warnings');
% update message about load status
return
end
end
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-12: LOAD STRUCTURES FILES <<<<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will ....
%==========================================================================
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.

LD = Robot_data.LD; %Read the variable
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 <<<<<<<<<<<<<<<<
%==========================================================================
% 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]];
% Load
Vby = [[0; 0], [0;
axis_length], [0; 0]];
% Load
Vbz = [[0; 0], [0; 0], [0;
axis_length]];
% show = true: display the axes on the
screen; false: delete the axes
if ~exist('show', 'var')
%
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

%==========================================================================
% >>>>>>>>>>> FUNCTION MF-14: PLOT CENTER OF GRAVITY FRAMES <<<<<<<<<<<<<<
%==========================================================================
% 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)
try
% Load
figure(AF);
catch
% Load
return
end
% Load
%% Display axes
if show
% Load
delete_axes();
show_axes(T, RP, HD, S);
% Load
else
delete it.
cn = S.value{'cn'};
delete_axes()
end
%% Functions
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;
[~, T] =
%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);

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
try
p_cm = Tcmi * r_cm;
'cg_frames');
for j =
Vtx = (Tcmi * Vbx')';
%for each
Vty = (Tcmi * Vby')';
joint
Vtz = (Tcmi * Vbz')';
for c = 1 :
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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
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
%==========================================================================
function MF_Plot_Links_frame(T, show)
% Load
% Load
if strcmp(get(HD.disp_link_frame,
'Checked'), 'on')
% Load
show = true;
else
% Load
show = false;
end
% Load
end
cn = S.value{'cn'};

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] =
end
else
return
end
try
figure(AF);
catch
return
end
%% Display axes
if show
delete_axes();
show_axes(T, RP, HD, S);
else
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;
%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
try
'links_frames');
for j =
%for each
joint
for c = 1 :
coordinate
line =
axes_lines(j,c);
delete(line);
end
end
end
end
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-16: PLOT GRAPHS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will plot the 4 graphs: position, velocity,
% acceleration and torque (each graph will have the n joints)
%==========================================================================
function MF_Plot_Graphs(cn)

%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;
%Load
%% 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;
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
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
ylabel('$\dot\theta(t) [^ \circ
/s]$','interpreter','latex');
);
%3 - Plotting Angular Acceleraion

(ddtheta/dt^2) over time for each joint
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
ylabel('$\ddot\theta(t) [^ \circ
/s^{2}]$','interpreter','latex');
);
%4 - Plotting Torque over time for each
joint
% subplot(plot_r,plot_c,4);
if plot_torque
figure
hold on
grid on
for i = 1:n
try

plot(time,
ylabel('$\tau(t)
torque(:,i),'LineWidth',lw);
[Nm]$','interpreter','latex');
lenged_txt{i} =
strcat('$\tau' , num2str(i),'$');
end
);
end
end
title('Joint Torque');
end
xlabel('$t
%==========================================================================
% >>>>>>>>>>>>>> FUNCTION MF-17: PLOT COMPARISON GRAPHS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will plot the 4 graphs: position, velocity,
% acceleration and torque (each graph will have the n joints)
%==========================================================================
function MF_Plot_Comparison_Graphs(cn)
%cn: command number (cn can be an array
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;
%Load
the figure (2 columns)
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));
% 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
%% Plotting data
ylabel('$\theta(t) [^
figure
\circ]$','interpreter','latex');

end
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-18: UPDATE COMMAND NUMBER <<<<<<<<<<<<<<<<
%==========================================================================
% 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
%==========================================================================
function MF_Update_Cn()
% 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.
end
% Save in base workspace
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-19: UPDATE STOP STATUS <<<<<<<<<<<<<<<<<<
%==========================================================================
% 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
%Load
end
%==========================================================================
% >>>>>>>>>>>>>>>> FUNCTION MF-20: LOAD STRUCTURES FILES <<<<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will load structures to base workspace on
% software opening
%==========================================================================
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
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

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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will save settings to S structure
%==========================================================================
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
not boolean');
%ROW 3: RANGE OF ROTATION OF GRIPPER
not numeric');
%ROW 4: PORT NUMBER (COM)
not numeric');
elseif ~(isboolean(Settings{6, 2}))

%ROW 6: SIMULATION MODE
not boolean');
%ROW 7: TRAJECTORY OPTION****
not boolean');
%ROW 8: SHOW TRAIL
not boolean');
%ROW 9: SHOW AXES
not boolean');
%ROW 10: ENABLE DYNAMIC SIMULATION
not boolean');
%ROW 11: LINEAR SPEED

not boolean');
%ROW 12: MOVE BY INCREMENT: COARSE
not boolean');

RP.alpha'; %DH parameters
n = S.value{'dof'};
max_reach = S.value{'max_reach_p'};
%get max reach distance from Settings

%ROW 13: MOVE BY INCREMENT: FINE
not boolean');
if isempty(max_reach) || max_reach == 0
|| isnan(max_reach)
a_sum = sum(a);
d_sum = sum(d);

%ROW 14: ANGLES SET
not boolean');
DH_reach = norm([a_sum, d_sum]);

%max reach by DH parameters

%ROW 15: NUMBER OF JOINTS
not boolean');
%ROW 16: HOME POSITION
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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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.

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 <<<<<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will clear the trail in the graphical
% representation in each command or when the user click on the button
%==========================================================================
%% 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
5 FUNES DE SUPORTE
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION SPF-1: IMPORT CAD TO MAT <<<<<<<<<<<<<<<<<<<
%==========================================================================
% 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).
%==========================================================================
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
files
Robots_data_path =
% [filename, folder] = uigetfile('*',
'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)

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',
%
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.

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 ==============
%==========================================================================
% 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
% 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

% 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

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:
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:
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('=================================
=============================');

%
%
disp('=================================
% toc
=============================');
%
end
%
disp('TORQUE FOR JOINT:');
%
i
%
EM(1).TORQUE(i)
% end
%==========================================================================
% >>>>>>>>>>>> FUNCTION SF-3: GENERATE JACOBIAN EQUATION <<<<<<<<<<<<<<<<
%==========================================================================
% DESCRIPTION: This function will generate the jacobian linear matrix to be
% used in the Inverse Kinematics function.
%==========================================================================
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
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
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');
% A = sym('A%d',[1,n],'real');
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

%% 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 <<<<<<<<<<<<<<<<
%==========================================================================
% 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)
%==========================================================================
%DESCRIO:
%
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 NO SAEM CORRETAS, E COMO A ORDEM
%DOS PONTOS IMPORTANTE PARA UMA TRAJETRIA, NECESSRIO REORGANIZAR OS
%PONTOS UTILIZANDO O ALGORTIMO ABAIXO.
% O ALGORTMO SUBTRAI O PONTO INICIAL (ESCOLHIDO PELO USURIO) DA TABELA DE
% PONTOS DEPOIS ENCONTRADO O MDULO DESSA SUBTRAO DE VETORES, O MENOR
% MDULO 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 TRAJETRIAS DESENHADAS TAMBM.
%
%
%
%
%
%
%
%
%
%
%
%
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 DESNECESSRIAS (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 NECESSRIO REPETIR O COMANDO VRIAS
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 PRXIMO DA ORIGEM (COORDENADA 0,0) DO ARQUIVO;

% - CLIQUE NO BOTO 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 OPO POINT E DESMARQUE AS DEMAIS, CLIQUE EM NEXT;
NA CAIXA DE OPES 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;

%
%
%
%
%
- MARQUE AS DUAS OPES: 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 VARIVEL 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
APNDICE C
Projeto do Manipulador

Projeto, Controle e Análise de Um Manipulador Robótico Modular

Transféré par

Informations du document

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Projeto, Controle e Análise de Um Manipulador Robótico Modular

Transféré par

Droits d'auteur :

Formats disponibles

UNIVERSIDADE DE MOGI DAS CRUZES

Diego Varalda de Almeida

PROJETO, CONTROLE E ANLISE DE UM

Mogi das Cruzes

UNIVERSIDADE DE MOGI DAS CRUZES

Diego Varalda de Almeida  11121502987

PROJETO, CONTROLE E ANLISE DE UM

Projeto de Graduao apresentado ao curso

Orientador: Prof. Ms. Gilberto Tomz Junior

Universidade de Mogi das Cruzes UMC

Mogi das Cruzes

Almeida, Diego Varalda de

Dedico este trabalho aos meus pais,

Jamais considere seus estudos como uma obrigao,

Compensao de distrbio por torque computado . . . . . . . . . . . .

Figura 56 Exemplo 9 - hlice cnica; esquerda: percurso no espao cartesiano;

Lista de abreviaturas e siglas

Desenho assistido por computador.

Comando Numrico Computadorizado

Anlise de Elementos Finitos

Interface Grfica do Usurio

Graphical User Interface Developer Environment

Administrao Nacional da Aeronutica e Espao

Placa de circuito impresso

Sistema de controle do tipo Proporcional, Integral e Derivado.

Modulao por Largura de Pulso

Robotics Institute of America

Rotaes por minuto

Sistema Internacional de Unidades

nica Entrada - nica Sada

Coordenada de uma junta de revoluo, [rad]

Velocidade da varivel de junta, [rad/s]

Acelerao da varivel da junta, [rad/s2 ]

Coeficiente i da funo polinomial

Matriz de transformao homognea

Foras externas generalizadas

Energia cintica, [kg.m2 s2 ]

Energia potencial, [kg.m2 s2 ]

Tensor de inrcia, [kg.m2 ]

Densidade do corpo, [kg.m3 ]

MODELAMENTO DO MANIPULADOR ROBTICO

Obtendo a matriz de inrcia do elo

PROJETO DO MANIPULADOR . . . . . . . . 223

Histrico dos manipuladores robticos industriais

1.1.1 Manipuladores modulares

Fonte: Robotnik (2016), Corp. (2016), Robots (2016)

Este trabalho tem como objetivo principal desenvolver o projeto de um prottipo de

O trabalho comea com o projeto de um manipulador robtico modular utilizando o

Por fim a interface grfica desenvolvida, utilizando a toolbox GUIDE do MATLAB.

Organizao e escopo do trabalho

Na indstria os robs do tipo manipulador so empregados em uma variedade de aplicaes,

2.1.1 Estrutura e componentes de um rob

Captulo 2. Reviso Bibliogrfica

esforos do atuador para as juntas mecnicas.

Fonte: (DUYSINX; GERADIN, 2004)

A estrutura mecnica detalhada a seguir.

Captulo 2. Reviso Bibliogrfica

Fonte: PLW (2013)

Captulo 2. Reviso Bibliogrfica

2.1.2 Sistemas de acionamento e conguraes do rob

Captulo 2. Reviso Bibliogrfica

Figura 4 Configuraes bsicas de um manipulador robtico: (a: cartesiano; b: cilndrico;

Fonte: Duysinx e Geradin (2004)

2.1.3 Esquema de notao de juntas

Diego Varalda de Almeida 11121502987

2.1.2 Sistemas de acionamento e conguraes do rob