Documenti di Didattica
Documenti di Professioni
Documenti di Cultura
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.
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
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
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
CNC
DC
Corrente contnua.
FEA
GDL
Graus de liberdade
GUI
GUIDE
NASA
PCI
PID
PWM
RIA
RPM
SI
SISO
Lista de smbolos
J
Matrix Jacobiana
Tempo [s]
Constante de amortecimento
ai
Lagrangiano
qi
Coordenada generalizada
Qi
Vetor distncia
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
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
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
17
Captulo 1
Introduo
1.1
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.
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
1.4
Metodologia
Captulo 1. Introduo
20
1.5
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
22
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
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.
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
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).
26
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
28
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
30
31
Nm. de iteraes
Pseudoinversa
32
96,4
Pseudoinversa
46
104,1
35
89,2
Transposta
59
83,3
Transposta
163
84,8
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
Pseudoinversa
Pseudoinversa
74
81,5
Transposta
117
73,8
Transposta
98
77,8
32
2.3
Dinmica e controle
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.
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
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)
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.
Captulo 3. Metodologia
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)
Captulo 3. Metodologia
37
e os ngulos das juntas inicial e final correspondem ao ngulo em que a junta est e o
ngulo calculado final respectivamente, portanto:
(0) = 0 ,
(tf ) = f ,
(0) = 0,
(tf ) = 0.
(3.1)
As quatro 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)
Captulo 3. Metodologia
38
(3.9)
(3.10)
(3.11)
a2 =
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)
A expresso ponto se refere sistemas de referncia que fornecem tanto a posio quanto a orientao
do efetuador.
Captulo 3. Metodologia
39
f 0 = lim
(3.15)
f (x0 + h) f (x0 )
h
(3.16)
(3.17)
Captulo 3. Metodologia
40
Captulo 3. Metodologia
41
Captulo 3. Metodologia
42
Captulo 3. Metodologia
43
Captulo 3. Metodologia
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
Captulo 3. Metodologia
45
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)
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)
Captulo 3. Metodologia
46
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.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
Captulo 3. Metodologia
47
(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
Captulo 3. Metodologia
48
(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
como segue:
Captulo 3. Metodologia
49
(3.33)
J = JT J
1
JT
(3.35)
Captulo 3. Metodologia
50
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)
J T J + 2 I = J T e
(3.37)
Captulo 3. Metodologia
51
= J T J + 2 I
1
JT e
(3.38)
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
(3.41)
(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).
Captulo 3. Metodologia
52
d
= J (q) q
X
+
J (q) q
dt
(3.44)
(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)
Captulo 3. Metodologia
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
Captulo 3. Metodologia
54
1985)
q(k+1) = q(k) + (k)
(3.49)
rj q (k) +
n
X
(k) (k)
Jji i
= 0,
j = 1, ..., 6
(3.50)
i=1
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)
3.3
Dinmica do Manipulador
Z
m
r2 dm
(3.53)
Captulo 3. Metodologia
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)
Para os eixos y e z :
Iyy =
Z
(3.56)
(3.57)
x2 + z 2 dV
Izz =
Z
x2 + y 2 dV
(xy) dV
Iyz = Izy =
(yz) dV
(3.58)
Ixz = Izx =
(xz) dV
I
I
I
xy
xz
xx
Captulo 3. Metodologia
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)
Captulo 3. Metodologia
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)
(3.65)
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.
Captulo 3. Metodologia
58
(K U )
(K U )
d
=
qi
qi
dt
(K)
d
qi
dt
(U )
(K) (U )
+
= Qi (3.67)
qi
qi
qi
(K)
(K) (U )
+
= Qi
qi
qi
qi
(3.68)
d
dt
(K)
(U )
(K)
qi +
qi = Qi qi
qi
qi
qi
(3.69)
i = 1, 2, ..., n
(3.70)
n
X
Ki
(3.71)
i=1
Captulo 3. Metodologia
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)
(3.74)
M=
n
X
m1 JLi
JLi
JA
i
I i JA
i
(3.75)
i=1
K=
n X
n
1X
Mij qi qj
2 i=1 j=1
(3.76)
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)
(3.78)
Captulo 3. Metodologia
60
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)
hi =
n X
n
X
Cijk qj qk
(3.80)
j=1 k=1
Mij
1 Mjk
qk
2 qi
(3.81)
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
Gi =
n
n
X
X
U
r0,cj
=
mj gT
=
mj gT JLj,i
qi
q
i
j=1
j=1
(3.84)
Captulo 3. Metodologia
61
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)
3.4
Sistemas de controle
Captulo 3. Metodologia
62
q (s)
c (s)
a (s)
H (s) =
F (s) =
p (s)
d (s)
b (s)
(3.88)
Captulo 3. Metodologia
63
Uma simples multiplicao entre os blocos do diagrama mostra que a funo de transferncia
(s)
T (s) = YR(s)
dada por:
T (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
Captulo 3. Metodologia
64
q (s) d (s)
D (s)
p (s) d (s) + q (s) c (s)
(3.90)
djk q d qjd +
(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).
Captulo 3. Metodologia
65
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.
67
17
16
15
12
4.2
Detalhamento do mecanismo
68
Valor
Unidade
Graus de liberdade
1,2
350
200
150
Preciso unidirecional
+/-1,0
mm
Repetibilidade unidirecional
+/-0,4
mm
1,65
m/s
50
Hz
Massa total
26,8
kg
69
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
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
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
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
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.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
4.3
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
78
79
80
81
82
Captulo 5
Anlise e Resultados
5.1
Cinemtica
83
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 [ ]
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
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)
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:
(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
86
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)
(5.5)
(5.6)
onde:
a0 = 0
a1 = 0
a3 =
a4 =
0
2
20f 200 8f + 120 tf 30 f t2f
2t3f
a5 =
(5.7)
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 =
(5.8)
v
Ztf u
u
t
0
dx
dt
!2
dy
+
dt
!2
dz
+
dt
!2
dt
(5.9)
np 1
X
i=1
kpi+1 pi k =
np 1 q
i=1
(5.10)
88
Figura 47 Pontos de passagem com as velocidades desejadas nos pontos indicados pelas
tangentes
J 1 v, n = 6
=
J v, n 6= 6
(5.11)
vx = vy = vz
v=
(5.12)
T
1 [v, v, v]
3
89
(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):
derivada numrica (3.16)
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
(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):
derivada numrica3.17
(t) : calculado pela
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
S
v
Especificado pelo
usurio
(t):
derivada numrica3.16
tabela de pontos no
espao cartesianos;
velocidade linear
constante, (v)
(t):
Equao 5.11
S
v
91
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
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
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
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]
(5.13)
95
Parametrizada: elipse
=c
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
=c
= b sin (kz t) + e
y = a cos (ky t) + d
t [0, 2]
(5.15)
97
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)
98
= et + f
= c + dt sin (t)
y = a + bt cos (t)
t [0, 8]
(5.17)
99
5.4
Dinmica
Cmkj
(5.18)
100
onde,
Hmkj = zm .J mk . (zj zk )
Emkj =
Dmkj =
J mk =
n
P
i=max(m,k,j)
n
P
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
101
Elo 2
Elo 3
Elo 4
Elo 5
Efetuador
Tensor de inrcia[kg.m2 ]
0, 02862 0, 00013
0, 01413
I2,CG = 0, 00013 0, 76379 0, 00001
0, 01413 0, 00001 0, 75654
0, 00845 0, 00001
0, 00000
I4,CG = 0, 00001 0, 00776 0, 00056
0, 00000 0, 00056 0, 00292
Massa [kg]
[0 69,12 139,70]
7,40
9,17
6,736
2,64
2,64
0,94
102
103
104
105
106
107
108
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
110
6.1
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.
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 confer- ence 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
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
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
118
119
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 ].
121
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.
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
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
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.
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 .
124
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
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.
126
127
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
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
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
O que resultaria em um crculo de raio 500. O tempo do trajeto tambm deve ser
passado ao programa.
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.
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.
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.
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.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
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
Descrio
dq
ddq
lv
la
tq
time
sp
matrizes de transformao
homognea de cada junta
qc
dqc
ddqc
tqc
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
Descrio
settings
commands
programs
notice
warnings
animW
robotP
simulation
controlS
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
commands
program_table
robot_param
validation
stop
cmd_state
co
cn
139
APNDICE B
R do Software
Rotinas em Matlab
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);
for i=1:length(p_o(:,1));
[~, T_m] =
PF_Forward_Kinematics(q0, d, a, alpha);
T_d = Goal_Transform(p_o(i, 4:end),
p_o(i,1:3));
error = error_f(T_m{n}, T_d,
s);%initial error (posit. and
orientation)
q = q0;
lambda = 20;
iter = 0;
counter
we = 50;
%lambda factor;
%iteration
%
Method 1: Dumped least square,
used to approximate the end-effector
%
position to the neighbourhood of
the goal position (position only,
%
orientation not considered)
while any(abs(error(1:3)) >
abs(m_error(1:3)))
J = PF_Jacobian(T_m);
%Computes the Jacobian
[~, T_m] =
PF_Forward_Kinematics(q, d, a, alpha);
error = error_f(T_m{n}, T_d,
s);
iter = iter + 1;
%Increment
of iteration number.
if iter > max_iter
%
disp('REACHED
MAXIMUM NUMBER OF ITERATIONS');
nci = [nci; i];
break
end
end
Tmatrix(:,:,i) = T_m{n}- T_d;
%#delete this (for checking errors
only)
%Update initial theta angles
q0 = q;
qm(i,:) = q;
end
end
%% METHODS
function q = DSL(q, J, lambda, we, n,
error)
%
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
% DESCRIPTION: This function will generate the trajectory for each joint
% based on the user's input. The trajectory will be interpolated by the use
% of a 5th order polynom.
% Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
%% LOAD FILES
PF_Interpolated_Traj(varargin)
S = evalin('base', 'S');
% varargin: {1}time, {2}either 'cart'
Settings (from base workspace)
or 'joint', {3}either p or q vetors
%Load
%Load
n = size(qt, 2);
%number of
joints (scalar)
% Computing step points (sp)
sp = ceil(ti * fps);
%==========================================================================
% >>>>>>> FUNCTION PF-C.2: INTERPOLATED TRAJECTORY WITH VIA POINTS <<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will generate the trajectory for each joint
% based on the user's input. The trajectory will be interpolated by the use
% of a 5th order polynom, for each via point. If the vecity is not given
% for the via points, the function will try to compute by the use of
% Jacobian or heuristic method.
% Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
% ot = orientation of each via point
PF_Interpolated_wvp_Traj(pt, tvp,
% vl = linear velocity via point (endvl,va,qt)
effector) [mm/sec]
% tvp = time of via points
% va = angular velocity via point (end% pt = cartesian via points
effector) [deg/sec]
%get
for vp = 1:size(pt,1)
if any(vl ~= 0) || any(va ~= 0)
[~, T_m] =
PF_Forward_Kinematics(qt(vp,:), d, a,
A);
J = PF_Jacobian(T_m);
%Computes the Jacobian
vlt = (1/sqrt(3)) * (ones(1,3)
* vl(vp));%linear velocity terms
vat = (1/sqrt(3)) * (ones(1,3)
* va(vp));%angular velocity terms
dqt(vp,:) = (pinv(J) * [vlt,
vat]')';
else
%Apply heuristic in the joint
space
%get previous theta
if vp == 1
q_p = q0;
else
q_p = qt(vp-1, :);
end
%get next theta
if vp == size(pt,1)
dqt(vp, :) =
zeros(1,n);%the last via point has
angular veloc 0
else
q_n = qt(vp+1,:);
end
mp = qt(vp,:) - q_p; %angular
coefficient of the line
mn = qt(vp,:) - q_n; %angular
coefficient of the line
for j = 1:n
if sign(mp(j)) ~=
sign(mn(j))
dqt(vp, j) = (mp(j) +
mn(j)) / 2;
else
dqt(vp, j) = 0;
end
end
end
end
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
end
%Load
%Load
va = varargin{1};
% Get joint variable target or
cartesian target position from
arguments in
if nargin ~= 3
return
else
if strcmp(varargin{2}, 'cart')
pt = varargin{3};
elseif strcmp(varargin{2}, 'joint')
qt = varargin{3};
end
if exist('pt', 'var') &&
~exist('qt', 'var')
qt = PF_Inverse_Kinematics(pt,
q0); %compute target q if not passed
end
end
n = size(qt,2); %number of joints
(scalar)
if i==1
spp = 0;
tvj(1:spi, i) = linspace(0,
ti(i), spi)';
tvj(spi:sp, i) = ti(i);
else
tvj(1:spp, i) = 0;
tvj((spp : spp+spi-1), i) =
linspace(0, ti(i), spi)';
tvj(spp+spi-1:sp, i) = ti(i);
end
spp = spp+spi;
end
%% Generate the trajectory
q = zeros(sp,n);
dq = zeros(sp,n);
ddq = zeros(sp,n);
for i=1:n
%Assembling joint position
coefficients
if qt(i) > q0(i)
a1 = dqc;
elseif qt(i) < q0(i)
a1 = -dqc;
elseif qt(i) == q0(i)
a1 = 0;
end
q_cf = [a1, q0(i)];
% Computing theta, d_theta,
dd_theta (column vector)
q(:, i) = polyval(q_cf, tvj(:, i));
dq(:,i) = linspace(dqc, dqc, sp);
ddq(:,i) = linspace(0, 0, sp);
%for a constant velocity the accel is 0
end
end
%Load
%Load
ti = varargin{1};
% Get joint variable target or
cartesian target position from
arguments in
if nargin ~= 3
return
else
if strcmp(varargin{2}, 'cart')
pt = varargin{3};
elseif strcmp(varargin{2}, 'joint')
qt = varargin{3};
end
if exist('pt', 'var') &&
~exist('qt', 'var')
qt = PF_Inverse_Kinematics(pt, q0);
%compute target q if not passed
end
end
n = size(qt, 2);
joints (scalar)
%%
% Computing step points (sp)
sp = sum(ceil(ti * fps));
%number of
tvj(1:spp, i) = 0;
tvj((spp : spp+spi-1), i) =
linspace(0, ti(i), spi)';
tvj(spp+spi-1:sp, i) = ti(i);
end
spp = spp+spi;
end
%% Generate the trajectory
q = zeros(sp,n);
dq = zeros(sp,n);
ddq = zeros(sp,n);
% Fifth order polynomial function
for i=1:n
a0 = q0(i);
a1 = 0;
a2 = 0;
a3 = (10*(qt(i) - q0(i))) /
ti(i)^3;
%==========================================================================
% >>>>>>>> FUNCTION PF-C.6: LINEAR TRAJECTORY W/ CST LINEAR VEL <<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will generate the trajectory for each joint
% based on the user's input. A linear trajectory is generated using the
% Settings value linear increment distance. The time is computed based on
% the end-effector to goal distance and velocity provided.
% Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
PF_StraightL_clv_Traj(pt, vl)
n = size(qt, 2);
%number of
% pt: targe position and
joints (scalar)
orientation(for uncoordinated traj: pt
=> 1x6)
[p0, T] = PF_Forward_Kinematics(q0, d,
% vl = constant linear velocity;
a, A);
%% Load structures and calculate
necessary variables
S = evalin('base', 'S');
Settings (from base workspace)
H = evalin('base', 'H');
History (from base workspace)
RP = evalin('base', 'RP');
%Load Robot Parameters
p0(4:6) = PF_Transform_to_Orient(T{n});
%get the current orientation
%Load
%Load
%D-
deltap = pt - p0;
L = norm(deltap(1:3)); %Linear distance
between initial and target points
ti = L/vl;
%Time for
completing the traj. with linear
velocity.
nbp = ceil(L / liv); %number of break
points in the line
sp = nbp;
% Time vector from 0 to final time,
with sp rows
tv = linspace(0, ti, nbp)';
%% Generating Trajectory
% break point positions (coordinates
xyz)
pbp = zeros(nbp, 6); %posit. & orient.
of each break point(x,y,z,rx,ry,rz)
for i=1:6
pbp(:,i) = linspace(p0(i),pt(i),
nbp)';
end
qbp = PF_Inverse_Kinematics(pbp(2:end,
:), q0);
%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;
% DESCRIPTION: This function will generate the trajectory for each joint
% based on the user's input. A linear trajectory is generated using the
% Settings value linear increment distance.
% Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
PF_StraightL_ti_Traj(pt, ti)
[p0, T] = PF_Forward_Kinematics(q0, d,
% pt: targe position and
a, A);
orientation(for uncoordinated traj: pt
p0(4:6) = PF_Transform_to_Orient(T{n});
=> 1x6)
%get the current orientation
% dqc = constant angular velocity;
%% Load structures and calculate
necessary variables
S = evalin('base', 'S');
Settings (from base workspace)
H = evalin('base', 'H');
History (from base workspace)
RP = evalin('base', 'RP');
%Load Robot Parameters
%Load
%Load
%D-
deltap = pt - p0;
nbp = ceil(norm(deltap(1:3)) / liv);
%number of break points in the line
sp = nbp;
% Time vector from 0 to final time,
with sp rows
tv = linspace(0, ti, nbp)';
%% Generating Trajectory
% break point positions (coordinates
xyz)
pbp = zeros(nbp, 6); %posit. & orient.
of each break point(x,y,z,rx,ry,rz)
for i=1:6
pbp(:,i) = linspace(p0(i),pt(i),
nbp)';
end
qbp = PF_Inverse_Kinematics(pbp(2:end,
:), q0);
q = [q0; qbp]; %joint displacement
%joint velocity and acceleration are
computed by numerical differentiation
% dq = diff([zeros(1,n); q]);%/liv;
% ddq = diff([zeros(1,n); dq]);%/liv;
tm = [zeros(1,n); tv * ones(1, n)];
dq = diff([zeros(1,n); q])./diff(tm);
dq(1,:) = 0; dq(end,:) = 0;
ddq = diff([zeros(1,n); dq])./diff(tm);
ddq(1,:) = 0; %ddq(end,:) = 0;
end
n = size(qt, 2);
%number of
joints (scalar)
%==========================================================================
% >>>>>>>> FUNCTION PF-C.8: PARAMETRISED TRAJECTORY W/ CST LIN VEL <<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will generate the trajectory for each joint
% based on the user's input. This function makes use of MATLAB ability to
% work with symbolic variables and functions to create a path based on the
% expression inputted.
% Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
xv = symvar(pex);
PF_Parametrised_clv_Traj(pex, pey, pez,
yv = symvar(pey);
...
zv = symvar(pez);
if ~isempty(xv)
vl, fvv, orient)
pex = symfun(pex, xv);
%convert
% pex, pey, pez: parametric equation in
symbolic expr. to symb. function
x y and z respectively
dpex = diff(pex,
% lv: linear velocity
symvar(pex));%taking the derivative of
% fvv: final variable value: Because
the param eq.
the parametric equation is in function
else
% of time it is needed a final value to
dpex = 0;
determine where the parametric
end
% curve will stop in space. For the
C.10 where the user input is time and
if ~isempty(yv)
% not a constant linear velocity, this
pey = symfun(pey, yv);
value (fvv) is the final time, here
dpey = diff(pey, yv);
% is used only to know where the curve
else
stops, and the time is computed
dpey = 0;
% using the linear velocity
end
%% LOAD FILES
S = evalin('base', 'S');
Settings (from base workspace)
H = evalin('base', 'H');
History (from base workspace)
%Load
%Load
~isempty(zv)
pez = symfun(pez, zv);
dpez = diff(pez, symvar(pez));
else
dpez = 0;
if
%get
%get
end
try
%Parametric curve length
S = int(sqrt(dpex^2 + dpey^2 +
dpez^2), fvv(1), fvv(2));
S = double(S);
ti = S / vl; %Computing trajectory
time
catch
ti = abs(fvv(2) - fvv(1));
%If
not possible to compute S, set fvv as
time
end
sp = ceil(ti * fps);
% Computing
step points (sp)
% Time vector from 0 to final time,
with sp rows
tv = linspace(0,ti,sp)';
%time
vector used for plots
%time vector used for solving the
parametric equation
tvfpe = linspace(0, fvv(2), sp)';
%% Generate the trajectory
xp = []; yp = []; zp = [];
if ~isempty(xv)
%==========================================================================
% >>>>>>>> FUNCTION PF-C.9: PARAMETRISED TRAJECTORY W/ CST ANG VEL <<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will generate the trajectory for each joint
% based on the user's input. This function makes use of MATLAB ability to
% work with symbolic variables and functions to create a path based on the
% expression inputted.
% Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
PF_Parametrised_cav_Traj(pej, jn, va,
%make a vector of time if passed a
fvv)
scalar
% pej: joint parametric equation
if size(fvv, 2) == 1
% jn = joint number to apply the
fvv(2) = fvv;
parametrisation
fvv(1) = 0;
% va: angular velocity
end
% fvv: final variable value: Because
the parametric equation is in function
% of time it is needed a final value to
determine where the parametric
% curve will stop in space.
%% LOAD FILES
S = evalin('base', 'S');
%Load
Settings (from base workspace)
H = evalin('base', 'H');
%Load
History (from base workspace)
cn = S.value{'cn'}; %get the command
number from Settings structure
fps = S.value{'fps'}; %get the number
of frames per second
if cn == 1
q0 = S.value{'home_q'};
%get current theta angles
else
q0 = H(cn - 1).q(end,:);
current theta angles
end
n = size(q0, 2);
%Computing S
pej = sym(pej); %converting the string
expression to symbolic
jv = symvar(pej);
if
~isempty(jv)
pej = symfun(pej, jv);
%convert symbolic expr. to symb.
function
dpej = diff(pej, jv); %taking the
derivative of the parametric eq.
else
dpej = 0;
end
try
%get
%==========================================================================
% >>>>> FUNCTION PF-C.10: PARAMETRISED JOINT TRAJECTORY W/ TIME INPUT <<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will generate the trajectory for the joint
% selected based on the user's input. This function makes use of MATLAB
% ability to work with symbolic variables and functions to create a path
% based on the expression inputted. The expression generates a joint curve
% not a cartesian curve like the others, so the inverse kinematics function
% is not used here. Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
PF_Parametrised_J_ti_Traj(pej, jn,
deltat)
% pej: joint parametric equation
% jn = joint number to apply the
parametrisation
% deltat: delta time: Because the
parametric equation is in function of
% time it is needed an initial and
final value to determine where the
% parametric curve starts and stops in
space.
if cn == 1
q0 = S.value{'home_q'};
%get current theta angles
else
q0 = H(cn - 1).q(end,:);
current theta angles
end
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
expression to symbolic
sp = ceil(ti * fps);
% Computing
step points (sp)
% Time vector from 0 to final time,
with sp rows
tv = linspace(0,ti,sp)';
end
% all joints will remain q0 but the
joint jn
%==========================================================================
% >>>>>>>> FUNCTION PF-C.11: PARAMETRISED TRAJECTORY W/ TIME INPUT <<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will generate the trajectory for each joint
% based on the user's input. This function makes use of MATLAB ability to
% work with symbolic variables and functions to create a path based on the
% expression inputted.
% Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
%converting the string expression to
PF_Parametrised_ti_Traj(pex, pey, pez,
symbolic
deltat, orient)
pex = sym(pex);
% pex, pey, pez: parametric equation in
pey = sym(pey);
x y and z respectively
pez = sym(pez);
% lv: linear velocity
% deltat: delta time: Because the
if size(deltat, 2) == 1
parametric equation is in function of
ti = deltat;
% time it is needed an initial and
else
final value to determine where the
ti = abs(deltat(2) - deltat(1));
% parametric curve starts and stops in
end
space.
%% LOAD FILES
sp = ceil(ti * fps);
% Computing
S = evalin('base', 'S');
%Load
step points (sp)
Settings (from base workspace)
% Time vector from 0 to final time,
H = evalin('base', 'H');
%Load
with sp rows
History (from base workspace)
tv = linspace(0,ti,sp)';
cn = S.value{'cn'}; %get the command
number from Settings structure
fps = S.value{'fps'}; %get the number
of frames per second
if cn == 1
q0 = S.value{'home_q'};
%get current theta angles
else
q0 = H(cn - 1).q(end,:);
current theta angles
end
n = size(q0, 2);
%get
= symvar(pex);
= symvar(pey);
= symvar(pez);
~isempty(xv)
pex = symfun(pex, xv);
%convert symbolic expr. to symb.
function
xp = [xp; double(pex(tv))];
%solving for tv entries
else
xp = [xp; pex * ones(sp, 1)];
end
if ~isempty(yv)
end
s = [1 1 1];
if any(orient == 0)
s = [0 0 0];
%then the
orientation is not specified in IK
%==========================================================================
% >>>>>>>>> FUNCTION PF-C.12: TRAJECTORY BY TABLE W/ TIME INPUT <<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will generate the trajectory for each joint
% based on the user's input. This function receives a matrix with cartesian
% coordinates and a time (in seconds) for completing the trajectory. The
% function converts each coordinate to joint position by inverse kinematics
% Refer to section 4 of documentation for details.
%==========================================================================
function [q, dq, ddq, tv, sp] =
n = size(q0, 2);
PF_Table_ti_Traj(p, ti, s)
% p: position matrix (n x 6: x, y, z,
sp = size(p, 1);
% Computing step
rx, ry, rz)
points (sp)
% ti: time for completing the
trajectory
% Time vector from 0 to final time,
% s: orientation specification (vector
with sp rows
1x3), if [1 1 1] the inverse
tv = linspace(0,ti,sp)';
% kinematics will attempt to satisfy
%% Generate the trajectory
the orientation presented in the table
if ~exist('s', 'var')
%PS: The inputs check is made in the
if (size(p,2) < 6) || (n < 6)
Secondary function that calls this one
s = [0 0 0]; % no need to
%% LOAD FILES
satisfy orientation in IK
S = evalin('base', 'S');
%Load
p(:, 4:6) = zeros(size(p, 1),
Settings (from base workspace)
3);
H = evalin('base', 'H');
%Load
else
History (from base workspace)
s = [1 1 1];% satisfy
cn = S.value{'cn'}; %get the command
number from Settings structure
if cn == 1
q0 = S.value{'home_q'};
current theta angles
else
q0 = H(cn - 1).q(end,:);
current theta angles
end
%get
%get
orientation in IK
end
end
q = PF_Inverse_Kinematics(p, q0, s);
tm = [zeros(1,n); tv * ones(1, n)];
dq = diff([zeros(1,n); q])./diff(tm);
dq(1,:) = 0; dq(end,:) = 0;
ddq = diff([zeros(1,n); dq])./diff(tm);
ddq(1,:) = 0; %ddq(end, :) = 0;
end
% Computing step
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION PF-D: FORWARD DYNAMICS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Peter I. Corke,
% Modified by Diego Varalda de Almeida with permission under the terms of
% the GNU.
% Date: June 05th, 2016.
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will compute the compute the forwards dynamics
% given the torque inputted by the user in tab 4: Dynamics simulation. The
% solution is done by the use of the ode45 function and the inverse
% dynamics function using the Recursive Newton Euler method (also
% implemented by Peter I. Corke in his Robotics Matlab Toolbox). Refer to
% section 4 of documentation for details.
%==========================================================================
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 PF-E: INVERSE DYNAMICS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% Date: March 30th, 2016.
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will compute the torque for each joint based
% on the trajectory provided.
% Refer to section 4 of documentation for details.
%==========================================================================
function Torque =
%center of mass vectors converted to
PF_Inverse_Dynamics_Lagrange(q, dq,
metres
ddq, g)
for i=1:length(r_cm); r_cm(i) =
% q: matrix sp x n (step points per
{r_cm{1} / 1000}; end
dof) with the joints positions
% dq: matrix sp x n (step points per
if ~exist('g', 'var')
dof) with the joints velocities
g = [0 0 9.08665];
%Gravity
% ddq: matrix sp x n (step points per
acceleration in metres/s^2
dof) with the joints accelerations
end
%% Loading neccessary files
RP = evalin('base', 'RP');
%Load
% mass = RP.mass;
Robot Parameters (from base workspace)
% m = cat(1,mass(:));
%% Assembling variables
n = size(q,2);
alpha = RP.alpha';
a = RP.a' / 1000;
%DH parameter
converted to metres
d = RP.d' / 1000;
%DH parameter
converted to metres
r_cm =
cellfun(@transpose,RP.r_cm,'UniformOutp
ut',false);
m =
Ixx
Ixy
Ixz
Iyy
Iyz
Izz
RP.mass;
= RP.Ixx';
= RP.Ixy';
= RP.Ixz';
= RP.Iyy';
= RP.Iyz';
= RP.Izz';
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
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_i_idx = (p_vecs{idx} p_vecs{i}) + r_cm{idx}; %#verificar se
indices estao certos
rv_j_idx = (p_vecs{idx} p_vecs{j}) + r_cm{idx}; %#verificar se
indices estao certos
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
zi_1 = R0_i_1 * k;
term i equation XX
% z
2 FUNES SECUNDRIAS
%==========================================================================
% >>>>>>>>>> FUNCTION SF-1: MOVE END-EFFECTOR TO HOME POSITION <<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 2.0 - March 30th, 2016.
% DESCRIPTION: This function will load the home position from Settings
% structure and process this to move the end-effector to home position
% orientation in interpolated trajectory. This function works as a
% intermediate for the GUI (user inputs) and the primary function
% responsible for generating the trajectory. Refer to section 4 of
% documentation for details.
%==========================================================================
function SF_Move_EE_to_Home()
% If dynamics is enabled, compute
%% Load files and variables
torque
S = evalin('base', 'S');
%Load
enable_dynamics =
Settings (from base workspace)
S.value{'enable_dynamics'};
H = evalin('base', 'H');
%Load
if enable_dynamics
History (from base workspace)
tq = PF_Inverse_Dynamics(q, dq,
RP = evalin('base', 'RP');
%Load
ddq);
%get the computed torque
Robot Parameters (from base workspace)
% If control system is enabled,
cn = S.value{'cn'}; %get the command
simulate the result of the control
number from Settings structure
enable_control =
home_q = S.value{'home_q'}; %get home
S.value{'enable_control'};
joint variables
if enable_control
max_speed = RP.max_speed;
tqc = PF_Robot_Control(q, dq,
%%
ddq, tq);
%get the control system
%PS: No need to check if home position
torque
is reached or allowed (already done)
[qc, dqc, ddqc] =
PF_Forward_Dynamics(tqc);
else
% Get current joint variables to
qc = []; dqc = []; ddqc = [];
compute time
end
if cn == 1 %End-effector already in
else
home position
qc = []; dqc = []; ddqc = []; tq =
MF_Update_Message(16, 'warnings');
[];
tqc = [];
return
end
else
%% Save outputs into the History
q0 = H(cn - 1).q(end,:);
structure
end
MF_Save_Structures('H', 'q', q, 'dq',
dq, 'ddq', ddq, 'qc', qc, ...
if abs(home_q - q0) == 0 %End-effector
'dqc', dqc, 'ddqc', ddqc, 'sp', sp,
already in home position
'time',tv,
'tq', tq);
MF_Update_Message(16, 'warnings');
%%
return
% Animate command
else
%
Note: First the user will see the
%Compute time using half of the
command
animation on the screen and
maximum joint speed
%
then
the motion is performed in
time = max(abs(home_q - q0)) /
the
robot.
For motion and animation
(min(max_speed) / 4);
%
occuring
at the same time, amend
end
this code using Multithreading
MF_Animate_Commands(cn);
%Call the respective trajectory
function
[q, dq, ddq, tv, sp] =
% Drive Servos (Send command to robot
PF_Interpolated_Traj(time, 'joint',
it connected)
home_q);
simulation_only =
S.value{'simul_only'};
case 3
[q, dq, ddq, tv, sp] =
PF_Uncoodinated_Traj(In.av, In.space,
In.pos);
case 4
[q, dq, ddq, tv, sp] =
PF_Sequential_cav_Traj(In.av, In.space,
In.pos);
case 5
[q, dq, ddq, tv, sp] =
PF_Sequential_ti_Traj(In.time,
In.space, In.pos);
case 6
[q, dq, ddq, tv, sp] =
PF_StraightL_clv_Traj(In.pos, In.lv);
case 7
[q, dq, ddq, tv, sp] =
PF_StraightL_ti_Traj(In.pos, In.time);
otherwise
MF_Update_Message(7,
'warnings');
return
end
% If dynamics is enabled, compute
torque
%
then the motion is performed in
the robot. For motion and animation
%
occuring at the same time, amend
this code using Multithreading
MF_Animate_Commands(cn)
end
versions.
if collision_detected
MF_Update_Message(6, 'warnings');
MF_Update_Message(9, 'warnings');
return
end
%Call the respective trajectory
function
switch In.trajopt
case 1
[q, dq, ddq, tv, sp] =
PF_Interpolated_Traj(In.time, 'joint',
qt);
case 3
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.
%To do list:
%
When the path is loaded, show all
points of the path in the graphical
%
window (independet of trail
option).
if In.trajopt == 12 &&
~isempty(In.time)
ti = In.time;
[q, dq, ddq, tv, sp] =
PF_Table_ti_Traj(TP, ti);
elseif In.trajopt == 11 &&
~isempty(In.lv)
clv = In.lv;
[q, dq, ddq, tv, sp] =
PF_Table_clv_Traj(TP, clv);
else
MF_Update_Message(23, 'warnings');
return
end
%%
%PS: The parametric equation given by
the user may result in a start
%position very far away from the
current position, so it is necessary to
%first move the end-effector to the
first point given by the parametric
%equation and then start the trajectory
itself, it is done by a coordinated
%trajectory using the medium speed of
the joints (such as in MOVE_HOME fcn)
%
Create a smooth trajectory between
first point of parametric trajectory
%
and current position.
max_speed = RP.max_speed;
% Get current joint variables to
compute time
if cn == 1 %End-effector already in
home position
q0 = S.value{'home_q'};
else
q0 = H(cn - 1).q(end,:);
end
else
MF_Update_Message(12,
'warning');
return
end
end
%%
%PS: The parametric equation given by
the user may result in a start
%position very far away from the
current position, so it is necessary to
%first move the end-effector to the
first point given by the parametric
%equation and then start the trajectory
itself, it is done by a coordinated
%trajectory using the medium speed of
the joints (such as in MOVE_HOME fcn)
%
Create a smooth trajectory between
first point of parametric trajectory
%
and current position.
max_speed = RP.max_speed;
% Get current joint variables to
compute time
if cn == 1 %End-effector already in
home position
q0 = S.value{'home_q'};
else
q0 = H(cn - 1).q(end,:);
end
if any(abs(q(1,:) - q0) ~= 0) % joints
not in starting point
%Compute time using half of the
maximum joint speed
time = max(abs(q(1,:) - q0)) /
(min(max_speed) / 4);
%Call interpolated trajectory and
get pre-trajectory values
[qp, dqp, ddqp, tvp, spp] =
PF_Interpolated_Traj(time,'joint',
q(1,:));
% Update variables (q, dq, ddq, tv,
sp): merge both trajectories
q = [qp; q]; dq = [dqp; dq]; ddq =
[ddqp; ddq];
sp = spp + sp;
tv = linspace(0, (tvp(end,1) +
tv(end, 1)), sp)';
end
%%
% Check if coordinate is reachable and
configuration mode used
[is_allowed, is_reachable] =
PF_Conf_Const_Reach('q', q);
if ~is_reachable
end
%% Save outputs into the History
structure
MF_Save_Structures('H', 'q', q, 'dq',
dq, 'ddq', ddq, 'qc', qc, ...
'dqc', dqc, 'ddqc', ddqc, 'sp', sp,
'time',tv, 'tq', tq);
%%
% Animate command
%
Note: First the user will see the
command animation on the screen and
%
then the motion is performed in
the robot. For motion and animation
%
occuring at the same time, amend
this code using Multithreading
MF_Animate_Commands(cn)
enable_control =
S.value{'enable_control'};
if enable_control
tqc = PF_Robot_Control(q, dq,
ddq, tq);
%get the control system
torque
[qc, dqc, ddqc] =
PF_Forward_Dynamics(tqc);
else
qc = []; dqc = []; ddqc = [];
end
else
qc = []; dqc = []; ddqc = []; tq =
[]; tqc = [];
end
%% Save outputs into the History
structure
MF_Save_Structures('H', 'q', q, 'dq',
dq, 'ddq', ddq, 'qc', qc, ...
'dqc', dqc, 'ddqc', ddqc, 'sp', sp,
'time',tv, 'tq', tq);
%%
% Animate command
%
Note: First the user will see the
command animation on the screen and
%
then the motion is performed in
the robot. For motion and animation
%
occuring at the same time, amend
this code using Multithreading
MF_Animate_Commands(cn)
%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
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION SF-11: PAUSE PROGRAM <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will pause the program by a determined time
sp = ceil(time * ctrl_rate);
Computing step points (sp)
tv = linspace(0,time,sp)';
%%
% Animate command
%
Note: First the user will see the
command animation on the screen and
%
then the motion is performed in
the robot. For motion and animation
%
occuring at the same time, amend
this code using Multithreading
MF_Animate_Commands(cn, 'full')
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
%%
%% ========================================================================
% >>>>>>>>>>>>>>>>>>>>>>>>>>> TAB 1: SETTINGS <<<<<<<<<<<<<<<<<<<<<<<<<<<<
%==========================================================================
%% #ok
function
%%
settings_table_CellSelectionCallback(hO
function
bject, eventdata, handles)
load_settings_bt_Callback(hObject,
indice = eventdata.Indices();
eventdata, handles)
selec_row = double(indice(:,1)');
%Transpose of the indice matrix
(Tranform
%%
%the first
function
column of the indice matrix into a row)
reset_settings_bt_Callback(hObject,
MF_Update_Message(selec_row,
eventdata, handles)
'settings');
%>>> END OF FUNCTION
%% #ok
function
function
edit_robot_param_bt_Callback(hObject,
settings_table_CellEditCallback(hObject
eventdata, handles)
, eventdata, handles)
MF_Load_Robot_param_table();
%%
%% #ok
function
function
save_settings_bt_Callback(hObject,
clear_history_bt_Callback(hObject,
eventdata, handles)
eventdata, handles)
settings_data =
MF_Clear_History();
get(handles.settings_table, 'Data');
MF_Save_Settings(settings_data);
%>>> END OF FUNCTION
%% ========================================================================
% >>>>>>>>>>>>>>>>>>>>>>>>>>> TAB 2: COMMANDS <<<<<<<<<<<<<<<<<<<<<<<<<<<<
%==========================================================================
x_pos = round(str2num(get(handles.p_x,
'string')),3);
function undock_bt_Callback(hObject,
y_pos = round(str2num(get(handles.p_y,
eventdata, handles)
'string')),3);
z_pos = round(str2num(get(handles.p_z,
%% #ok
'string')),3);
function move_coord_Callback(hObject,
orient_angle =
eventdata, handles)
round(str2num(get(handles.orientation_a
% Get the values from the edit box (X,
ngle, 'string')),3);
Y and Z);
set(hObject,'BackgroundColor',[.9
.9 .9]);
end
if
isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'
))
set(hObject,'BackgroundColor',[.9
.9 .9]);
end
if
isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'
))
set(hObject,'BackgroundColor',[.9
.9 .9]);
end
%% #ok
function
move_ee_neg_x_Callback(hObject,
eventdata, handles)
% Get the increment option (coarse,
fine)
coarse =
get(handles.coarse_rb,'Value');
fine = get(handles.fine_rb,'Value');
% Assemble the inputs (In)
if coarse ==1 && fine == 0
In.inc_opt = 'coarse';
else
In.inc_opt = 'fine';
end
In.space = 'cart'; %cartesian space
In.axes = [-1 0 0]; %increment only in
X (negative).
In.trajopt = get(handles.traj_opt,
'Value');
In.lv = str2num(get(handles.v_linear,
'String'));
In.av = str2num(get(handles.v_angular,
'String'));
In.time = str2num(get(handles.time,
'String'));
if isempty(In.time) && isempty(In.av)
&& isempty(In.lv)
MF_Update_Message(23, 'warnings');
return
end
SF_Move_EE_by_Increment(In);
%>>> END OF FUNCTION
%% #ok
function
move_ee_pos_x_Callback(hObject,
eventdata, handles)
% Get the increment option (coarse,
fine)
SF_Move_EE_by_Increment(In);
%>>> END OF FUNCTION
%% #ok
function
move_ee_pos_y_Callback(hObject,
eventdata, handles)
% Get the increment option (coarse,
fine)
coarse =
get(handles.coarse_rb,'Value');
fine = get(handles.fine_rb,'Value');
% Assemble the inputs (In)
if coarse ==1 && fine == 0
In.inc_opt = 'coarse';
else
In.inc_opt = 'fine';
end
In.space = 'cart'; %cartesian space
In.axes = [0 1 0]; %increment only in
Y (positive).
In.trajopt = get(handles.traj_opt,
'Value');
In.lv = str2num(get(handles.v_linear,
'String'));
In.av = str2num(get(handles.v_angular,
'String'));
In.time = str2num(get(handles.time,
'String'));
if isempty(In.time) && isempty(In.av)
&& isempty(In.lv)
MF_Update_Message(23, 'warnings');
return
end
SF_Move_EE_by_Increment(In);
%>>> END OF FUNCTION
%% #ok
function
move_ee_neg_z_Callback(hObject,
eventdata, handles)
% Get the increment option (coarse,
fine)
coarse =
get(handles.coarse_rb,'Value');
fine = get(handles.fine_rb,'Value');
% Assemble the inputs (In)
if coarse ==1 && fine == 0
In.inc_opt = 'coarse';
else
In.inc_opt = 'fine';
end
In.space = 'cart'; %cartesian space
In.axes = [0 0 -1]; %increment only in
Z (negative).
In.trajopt = get(handles.traj_opt,
'Value');
In.lv = str2num(get(handles.v_linear,
'String'));
In.time = str2num(get(handles.time,
'String'));
In.av = str2num(get(handles.v_angular,
'String'));
In.lv = str2num(get(handles.v_linear,
'String'));
SF_Move_Joints(In);
%% #ok
function
move_joints_bt_Callback(hObject,
eventdata, handles)
% Get the values from the edit box
(Joints and A);
joints =
round(str2num(get(handles.joints_move,
'String')),3);
qvalue =
round(str2num(get(handles.angle_move_jo
ints, 'string')),3);
%% #ok
function
gripper_slider_Callback(hObject,
eventdata, handles)
%>>>1: Get the joint angle;
gripper_value = round(get(hObject,
'Value'),1);
%%
function
gripper_slider_CreateFcn(hObject,
eventdata, handles)
if
isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'
))
set(hObject,'BackgroundColor',[.9
.9 .9]);
end
%% #ok
function jointn_menu_Callback(hObject,
eventdata, handles)
MF_Update_UI_Controls();
%update
maximum and minimum values of slider
%% #ok
function
transformation_options_Callback(hObject
, eventdata, handles)
MF_Update_UI_Controls();
%update UI
controls
%% #ok
function home_pos_Callback(hObject,
eventdata, handles)
SF_Move_EE_to_Home();
%% #ok
function stop_robot_Callback(hObject,
eventdata, handles)
MF_Update_Stop_status(true);
%%
function
update_joints_Callback(hObject,
eventdata, handles)
Update_Theta(eventdata, handles);
set(handles.orientation_angle,
'String', '');
set(handles.time, 'String', '');
if ispc &&
isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'
))
%% #ok
function clear_field_Callback(hObject,
set(hObject,'BackgroundColor','white');
eventdata, handles)
end
set(handles.p_x, 'String', '');
%>>> END OF FUNCTION
set(handles.p_y, 'String', '');
set(handles.p_z, 'String', '');
%% ========================================================================
% >>>>>>>>>>>>>>>>>>>>>>>>>>> TAB 3: PROGRAMS <<<<<<<<<<<<<<<<<<<<<<<<<<<<
%==========================================================================
function
'program_pos_z', 'prg_orient_txt',
program_cmd_opts_Callback(varargin)
'prg_orient','inst_time_txt',...
if nargin == 3
'inst_time','prg_linvel_txt','prg_linve
handles = varargin{3};
l','prg_angvel_txt','prg_angvel'};...
elseif nargin == 1
handles = varargin{1};
%'Move Joints'
end
%Get the selected command from the pop{'inst_path_option_txt','program_traj_o
up menu.
pts', 'inst_time_txt',...
cmd_sel = get(handles.program_cmd_opts,
'inst_time','inst_joints_txt',
'Value');
'inst_joints', 'inst_A_txt',
'inst_A',...
handle_obj = {'inst_path_option_txt',
'prg_angvel_txt','prg_angvel'};...
'program_traj_opts',...
'inst_save_grip_pos','inst_torque_cb','
%'Parametric Cart. Traj.'
program_xpos_txt','program_pos_x',...
'program_ypos_txt','program_pos_y','pro
{'inst_path_option_txt','program_traj_o
gram_zpos_txt','program_pos_z',...
pts','program_xpos_txt',...
'prg_orient_txt','prg_orient','inst_tim
'program_pos_x', 'program_ypos_txt',
e_txt','inst_time','inst_joints_txt',..
'program_pos_y','program_zpos_txt',...
.
'program_pos_z', 'prg_orient_txt',
'inst_joints','inst_A_txt','inst_A','pr
'prg_orient','inst_time_txt',...
g_linvel_txt','prg_linvel',...
'inst_time', 'prg_linvel_txt',
'prg_angvel_txt','prg_angvel','program_
'prg_linvel'};...
cn_txt','program_cn'};
%'Pametric Joint Traj.'
cmd_enabled_field = {
{};... %'Home Position'
%'Move to Coordinate'
{'inst_path_option_txt','program_traj_o
pts','program_xpos_txt',...
'program_pos_x', 'program_ypos_txt',
'program_pos_y','program_zpos_txt',...
'program_pos_z', 'prg_orient_txt',
'prg_orient','inst_time_txt',...
'inst_time', 'prg_linvel_txt',
'prg_linvel'};...
{'inst_path_option_txt','program_traj_o
pts', 'inst_time_txt',...
'inst_time','inst_joints_txt',
'inst_joints', 'inst_A_txt',
'inst_A',...
'prg_angvel_txt','prg_angvel'};...
%'Save actual Joints position'
{'inst_path_option_txt','program_traj_o
pts', 'inst_time_txt',...
'inst_time',
'prg_angvel_txt','prg_angvel'};...
%'Move by Increment'
{'inst_path_option_txt','program_traj_o
pts','program_xpos_txt',...
'program_pos_x', 'program_ypos_txt',
'program_pos_y','program_zpos_txt',...
%'Close Gripper'
%'Add a Pause'
{''};...
{'inst_path_option_txt','program_traj_o
pts', 'inst_time_txt',...
'inst_time',
'prg_angvel_txt','prg_angvel'};...
%'Add a Pause'
{'inst_time_txt', 'inst_time'};...
%'Repeat commands in History'
{'program_cn_txt', 'program_cn'}};
cmd_trajopts = {
%'Home Position'
{''};
%'Move to Coordinate'
{'Interpolated', 'Interpolated w/
via pts', 'Uncoordinated',...
'Sequential (CST angular vel.)',
'Sequential (time)',...
'Straight line (CST linear vel.)',
'Straight line (time)'};...
%'Move by Increment'
{'Interpolated', 'Uncoordinated',
'Sequential (CST angular vel.)',...
'Sequential (time)','Straight line (CST
linear vel.)','Straight line
(time)'};...
%'Move Joints'
{'Interpolated', 'Interpolated w/
via pts', 'Uncoordinated',...
'Sequential (CST angular vel.)',
'Sequential (time)'};...
%'Parametric Cart. Traj.'
{'Parametrised (CST linear
vel.)','Parametrised (time)'};...
%'Pametric Joint Traj.'
{'Parametrised (CST angular vel.)',
'Parametrised (time)'};...
%'Save actual Joints position'
{'Interpolated',
'Uncoordinated','Sequential (CST
angular vel.)',...
'Sequential (time)'};...
%'Load Table of Coordinates'
%%
function add_inst_Callback(hObject,
eventdata, handles)
HD = handles;
%create a copy of
handles (just for shortening the name)
AS = evalin('base', 'AS');
% Load
Additional Settings
ptvar = AS.program_table;
%Program
table variables
%Get P table from base workspace
try
P = evalin('base', 'P');
% Load
Program table (P) from base workspace
if isempty(P{1,1}{:})
prow = 1;
else
prow = size(P,1) + 1;
% get
the first empty rows.
end
catch
P = table();
% create
an empty table
P.(ptvar{6}){prow,1} = hv{8};
P.(ptvar{7}){prow,1} = hv{9};
P.(ptvar{8}){prow,1} = hv{10};
% Saving P in base workspace
assignin('base', 'P', P);
%Displaying P structure in uitable
MF_Load_Program_table(P);
function
program_traj_opts_CreateFcn(hObject,
eventdata, handles)
if ispc &&
isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'
))
set(hObject,'BackgroundColor','white');
end
function prg_clear_Callback(hObject,
eventdata, handles)
% Create a new empty P table
AS = evalin('base', 'AS');
% Load
Additional Settings
ptvar = AS.program_table;
%Program
table variables
P = evalin('base', 'P');
% Load P
from base workspace
% Ask user if the table should be saved
before cleaned
try
if ~isempty(P) || size(P,1)>1
answer = questdlg('Do you want
to save the program?',...
'Save confirmation');
if strcmp(answer,'Yes')
uisave('P', 'Program_');
end
end
end
%Updating uiTable
MF_Load_Program_table(P);
%>>> END OF FUNCTION
%% #ok
function save_inst_Callback(hObject,
eventdata, handles)
%>>>1: Open window to select folder for
saving file
P = evalin('base', 'P');
%get
Program table from base workspace
uisave('P', 'Program_');
%>>> END OF FUNCTION
%% #ok
function load_inst_Callback(hObject,
eventdata, handles)
%Open dialog box for selecting file to
load into workspace
%'load' will display only .mat files
uiopen('load')
%Validate the table loaded (using
P.Properties.Description)
try
valid_msg =
P.Properties.Description;
%compare both strings
if strcmp(valid_msg, 'Valid Smart
GUI program table');
isvalidfile = true;
else
isvalidfile = false;
end
catch
isvalidfile = false;
end
% Save program table in workspace for
future use
if isvalidfile
assignin('base', 'P', P);
%save
program table (P) in base workspace
else
MF_Update_Message(3, 'warnings');
return
end
% Display the loaded file in the
program table handle
MF_Load_Program_table(P);
%>>> END OF FUNCTION
%% #ok
function run_inst_Callback(hObject,
eventdata, handles)
SF_Run_Program();
%>>> END OF FUNCTION
%% #ok
function delete_inst_Callback(hObject,
eventdata, handles)
%Get the table content from base
workspace
try
P = evalin('base', 'P');
catch
MF_Update_Message(29, 'warnings');
return
end
try
pgr_ind = getappdata(0,
'program_table_indice');
pgr_row = pgr_ind(:, 1)';
catch
return
end
P(pgr_row, :) = []; % Delete selected
rows;
%Saving in base workspace
assignin('base', 'P', P);
%Updating uiTable
MF_Load_Program_table(P);
%>>> END OF FUNCTION
function
inst_torque_cb_Callback(hObject,
eventdata, handles)
status = get(hObject, 'Value');
Torque_On_Off(handles.id, status,
handles);
%>>> END OF FUNCTION
function stop_inst_Callback(hObject,
eventdata, handles)
global stop_state
%stop_state needs to be a global
variable because Function 10: Run
%Instructions will update this value,
and it cannot update handles
%0: Let the instructions Run; 1:Prevent
from running.
stop_state = 1;
%>>> END OF FUNCTION
%% #ok
function traj_opt_Callback(hObject,
eventdata, handles)
MF_Enable_Commands(hObject, handles);
if ispc &&
isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'
))
set(hObject,'BackgroundColor','white');
% ------------------------------------------------------------------function plot_graphs_Callback(hObject,
eventdata, handles)
S = evalin('base', 'S');
%Load
Settings (from base workspace)
cn = S.value{'cn'} - 1;
%get the
last command number
MF_Plot_Graphs(cn);
4 FUNES MISCELNEAS
%==========================================================================
% >>>>>>>>>>>>>>>>> FUNCTION MF-1: INITIALISE SMART GUI <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 2.0 - March 30th, 2016.
% DESCRIPTION: This function will load the joints trajectory from History
% and animate it in the Animation window by rotating the LD matrices that
% contain the vertices and patches got from SF_Importing_CAD. The rotation
% is made using the Transform matrix; the animation will have fps frames
% per second (From Settings). Refer to section 6.4.1 for details.
%==========================================================================
function MF_Init_Smart_GUI(hObject,
% >>>> Tab 4: Dynamics Simulation
eventdata, handles, varargin)
set(handles.sim_joint, 'String',
joints_list);
clc
%Clear Command
Window
% >>>> Context Menu: Tools
% close all;
%Close all
set(handles.display_cg, 'Checked',
opened figures
'off');
set(handles.disp_ref_frames, 'Checked',
'off');
%% Loading and defining variables
set(handles.disp_link_frame, 'Checked',
MF_Load_Structures();
%Load
'off');
structures to base workspace
S = evalin('base', 'S');
%Load
Settings from base workspace
AS = evalin('base', 'AS');
%Load
Additional Settings from base workspace
%%
%% %Atributing values to pop-up menus
% >>>> Tab 2: Commands
joints_list = {}; T_list = {};
n = S.value{'dof'};
for i = 1:n
joints_list = [joints_list,
num2str(i)];
T_list = [T_list, ['T_0_',
num2str(i)]];
end
set(handles.jointn_menu, 'String',
joints_list);
set(handles.joint_param_eq_menu,
'String', joints_list);
set(handles.transformation_options,
'String', T_list);
set(handles.transformation_options,
'Value', n);
% >>>> Tab 3: Programs
cmd_list = AS.commands;
traj_opts = AS.traj_opts;
set(handles.program_cmd_opts, 'String',
cmd_list);
set(handles.program_traj_opts,
'String', traj_opts);
%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();
figure(RR_fig);
%light('Position',[-1 0 0]);
try
figure(AF);
clf('reset');
catch
return;
end
hold on;
grid('on');
light
%Add a default light
% Clearing the trail
setappdata(0,'xtrail',[]); % used
for trail tracking.
setappdata(0,'ytrail', []); % used
for trail tracking.
setappdata(0,'ztrail',[]); % used
for trail tracking.
Tr = plot3(0,0,0,'r',
'LineWidth',2); %'Color', [0.5 0 0.5]);
% holder for trail paths
set(Tr,'xdata',[],'ydata',[],'zdata',[]
);
%
line5 = plot3([-ax_size,
ax_size],[-ax_size,ax_size],[ax_size,ax_size],'k');
%
line6 = plot3([-ax_size,ax_size],[ax_size,ax_size],[ax_size,ax_size],'k')
;
%
GR.boundary = [line1, line2,
line3, line4, line5, line6];
Base_vert = LD(nl+1).Ve;
%get the
base vertices for patch
%the base is
the immediate item after the joints
% Setting and drawing base
Base_patch = patch('faces',
LD(nl+1).Fa, 'vertices',
Base_vert(:,1:3));
set(Base_patch, 'facec',
[.8,.8,.8]);% set base color and draw
set(Base_patch,
'EdgeColor','none');% set edge color to
none
%
***************************************
************
[~, T_m] =
PF_Forward_Kinematics(q0, d, a, alpha);
for j = 1:nl %number of links (dof1)
L_vert = (T_m{j} * LD(j).Ve')';
L_patch = patch('faces',
LD(j).Fa, 'vertices', L_vert(:,1:3));
set(L_patch, 'facec',
RP.color{j},'EdgeColor','none');% set
link color and draw
%
set(L_patch, );% set edge
color to none
end
%
Setting and drawing endeffector if it exists
if S.value{'enable_ee'}
%if the
robot has end-effector
ee_n = nl + 1;
End_effector = LD(nl+2).Ve;
%get the end-effector vertices for
%
patch the end-effector is the
immediate item after the base
ee_vert = (T_m{ee_n} *
End_effector')';
ee_patch = patch('faces',
LD(nl+2).Fa, 'vertices',
ee_vert(:,1:3));
1',
vm2
2',
vm3
3',
vm4
4',
vm5
5',
vm6
6',
'Callback', @view_Callback);
= uimenu(View_menu,'Label', 'View
'Callback', @view_Callback);
= uimenu(View_menu,'Label', 'View
'Callback', @view_Callback);
= uimenu(View_menu,'Label', 'View
'Callback', @view_Callback);
= uimenu(View_menu,'Label', 'View
'Callback', @view_Callback);
= uimenu(View_menu,'Label', 'View
'Callback', @view_Callback);
%% Configuring Toolbar
% RR_fig.Toolbar = 'figure'; % Display
the standard toolbar
Toolbar =
findall(AF,'Type','uitoolbar');
%% Creating axes
% R_GR =
axes('Units','pixels','Position',[0,0,
Position(3:4)]);
R_GR = axes();
AF.Visible = 'on';
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)
end
function
Show_links_frame_mi_Callback(hObject,ev
entdata)
end
'View
'View
'View
'View
3'
4'
5'
6'
end
end
end
function
Plot_graphs_mi_Callback(hObject,eventda
ta)
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-4: ANIMATE COMMANDS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 2.0 - March 30th, 2016.
% DESCRIPTION: This function will load the joints trajectory from History
% and animate it in the Animation window by transforming (rotaion and
% translation) the LD matrices that contain the vertices and patches got
% from SF_Importing_CAD. The transformation is made by using the Transform
% matrix (primary function Forward_Kinematics); the animation will have fps
% frames per second (From Settings). Refer to section 4 of documentation
% for details.
%==========================================================================
function MF_Animate_Commands(cn,
LD = evalin('base', 'LD');
% LD
axessize)
contain all the face and vertices
% cn: command number (a scalar or a
points
vector with all commands the user wants
AF = evalin('base', 'AF');
% Load
% to animate).
figure (Animation Figure)
%% Loading structures and collecting
variables
HD = evalin('base', 'HD');
% Load
S = evalin('base', 'S');
% Load
Handles (from base workspace)
Settings (from base workspace)
RP = evalin('base', 'RP');
% Load
sp = H(cn).sp;
%get the
Robot parameters (from base workspace)
number of steps
H = evalin('base', 'H');
% Load
History (from base workspace)
children = get(gca,
'children');
delete(children);
end
hold on;
grid('on');
light
%Add a default light
max_r = S.value{'max_reach_p'};
%Get maximum reach distance
ax_size = max_r+200;
%set the axes dimensions
%
daspect([1 1 1])
%Setting the aspect ratio
%
view(135,25)
%Adjust the view orientation.
%
xlabel('X'),ylabel('Y'),zlabel('Z');
if exist('axessize', 'var')
if strcmp(axessize, 'full')
%set the axes dimensions
axis([-ax_size ax_size -ax_size
ax_size -ax_size ax_size]);
end
else
%set the axes dimensions
axis([-ax_size ax_size -ax_size
ax_size -0 (ax_size + RP.d(1))]);
end
%% Setting and drawing base
Base_vert = LD(nl+1).Ve;
%get the
base vertices for patch
%The base is
the immediate item after the joints
Base_patch = patch('faces',
LD(nl+1).Fa, 'vertices',
Base_vert(:,1:3));
set(Base_patch, 'facec',
[.8,.8,.8]);% set base color and draw
set(Base_patch,
'EdgeColor','none');% set edge color to
none
if clear_trail_after_command
%Clear trail
setappdata(0,'xtrail',[]);
setappdata(0,'ytrail', []);
setappdata(0,'ztrail',[]);
Tr = plot3(0,0,0,'r',
'LineWidth',2);
set(Tr,'xdata',[],'ydata',[],'zdata',[]
);
else
Tr = plot3(0,0,0,'r',
'LineWidth',2);
end
set(EE_patch,
'EdgeColor','none');% set edge color to
none
end
trail
MF_Update_Message(8,'warnings');
MF_Update_Stop_status(false);
%reseting stop status
break
end
%Delete previous patches
try
for idx = 1:nl
delete(L_patch{idx});
end
delete(EE_patch);
end
q_row = q(i,:);
[p_xyz, T_m] =
PF_Forward_Kinematics(q_row, d, a,
alpha);
% Setting and drawing links
for j = 1:nl %number of links (dof1)
L_vert = (T_m{j} * LD(j).Ve')';
% get the vertices transformed
L_patch{j} = patch('faces',
LD(j).Fa, 'vertices', L_vert(:,1:3));
% set link color and draw
set(L_patch{j}, 'facec',
RP.color{j},'EdgeColor','none');
end
% Setting and drawing end-effector
if it exists
ee_enabled = S.value{'enable_ee'};
if ee_enabled %if the robot has
end-effector
ee_n = nl + 1;
End_effector = LD(nl+2).Ve;
%get the end-effector vertices for
% patch the end-effector is the
immediate item after the base
EE_vert = (T_m{ee_n} *
End_effector')';
EE_patch = patch('faces',
LD(nl+2).Fa, 'vertices',
EE_vert(:,1:3));
set(EE_patch, 'facec',
RP.color{nl+2});% set base color and
draw
x_trail =
getappdata(0,'xtrail');
y_trail =
getappdata(0,'ytrail');
z_trail =
getappdata(0,'ztrail');
%
xdata = [x_trail p_xyz(1)];
ydata = [y_trail p_xyz(2)];
zdata = [z_trail p_xyz(3)];
%
setappdata(0,'xtrail',xdata); % used
for trail tracking.
setappdata(0,'ytrail',ydata); % used
for trail tracking.
setappdata(0,'ztrail',zdata); % used
for trail tracking.
%
set(Tr,'xdata',xdata,'ydata',ydata,'zda
ta',zdata);
end
if show_link_frame
MF_Plot_Links_frame(T_m);
elseif show_cg_frame
MF_Plot_CG_frames(T_m,
true, q_row);
end
drawnow
if save_video
F(i) = getframe;
%get
frame if save video is set to true
%
%save images each 5
frames (in jpeg)
%
if i==1 || rem(i,17) == 0
%
Image =
frame2im(F(i));
%
imagename =
strcat('image', num2str(cn), '-',
num2str(i),'.jpg');
%
imwrite(Image,
imagename);
%
end
end
if save_video
%convert F frames to avi video
videoname = strcat('command',
num2str(cn),'.avi');
movie2avi(F, videoname, 'quality',
75, 'fps', S.value{'fps'});
end
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-5: UPDATE MESSAGES <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will update the messages displayed in the
% message box.
% Refer to section 6.4.1 for details.
%==========================================================================
function MF_Update_Message(mn, fname,
inputs = {};
inputs)
%create an empty cell array
% mn: messge number (M(mn).fname) end
integer number
end
% fname: field name in M structure (eg:
else
robotP, or settings) - string
inputs = {};
% inputs: message inputs (cell array),
end
applicable for some messages only
%
in the message use (%0.2f) for
msg_txt = getfield(M(mn), fname);
input numbers and (%s) for input text
%get message from M strucute
% M structure:
previous_msg = get(HD.messages_txt,
% M(mn).settings
'String');
% M(mn).commands
divider = ' ';
% M(mn).programs
if ~isempty(msg_txt)%if message exists
% M(mn).notice
in M structure
% M(mn).warnings
%
msg_ass =
% M(mn).animW
strcat(previous_msg,'\n', divider,
% M(mn).robotP
'\n\n', msg_txt);
% M(mn).simulation
msg_ass = strcat('\n\n', msg_txt);
% M(mn).controlS
%check message number of inputs
ni = strfind(msg_txt,'%');
HD = evalin('base', 'HD');
if size(inputs, 2) > size(ni, 2)
%Load message structure from base
inputs = inputs(1:ni);
workspace
end
M = evalin('base', 'M');
msg = sprintf(msg_ass, inputs{:});
%assembled message (with inputs)
%Check if the inputs variables was
passed to the function
set(HD.messages_txt, 'String',
if exist('inputs', 'var')
msg); %set message in handle
if ~iscell(inputs)
%
set(handles.messages_txt,
try
'String', msg); %set message in handle
inputs = num2cell(inputs);
end
%convert to cell array if not
%>>>END OF FUNCTION
catch
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-6: UPDATE UI CONTROLS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will clear history structure and reset command
% number. Refer to section 4 of documentation for details.
%==========================================================================
function MF_Clear_History()
end
%% Loading variables
M = evalin('base', 'M');
H = evalin('base', 'H');
S = evalin('base', 'S');
%%
answer =
questdlg(M(28).warnings,'Confirmation')
;
if strcmp(answer,'No')
return
elseif strcmp(answer, 'Yes')
History_path =
which('HISTORY.mat');
%get file path
settings_path =
which('SETTINGS.mat');
H_fields = fieldnames(H);
H_size = size(H);
if H_size(2) > 1
H(2:H_size(2)) = [];
%deleting 'layers' (indices nonscalar) of H
end
%Clear GR_Data
% Deleting fields of layer 1;
for i = 1:length(H_fields)
end
H(1).(H_fields{i}) = [];
%==========================================================================
% >>>>>>>>>>>>>>>> FUNCTION MF-8: LOADING SETTINGS TABLE <<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 2.0 - March 30th, 2016.
% DESCRIPTION: This function will load the settings table to the GUI in settings tab.
Refer to section 4 of documentation for details.
%==========================================================================
function MF_Load_Settings_table()
Settings_table{i,1} =
HD = evalin('base', 'HD');
num2str(S.param{i});
S = evalin('base', 'S');
else
Settings_table{i,2} =
S.value{i};
Settings_table = {};
Settings_table{i,1} =
S.param{i};
for i = 1:1:height(S)
%Each row
end
of Settings
catch
try
end
%Use str2num to convert string
end
to a list of numbers
set(HD.settings_table,'Data',
%It is necessary because
Settings_table);
Instructions section send angles as a
end
%
string not as numbers.
if isnumeric(S.value{i})
Settings_table{i,2} =
num2str(S.value{i});
%==========================================================================
'ROBOT PARAMETERS');
RPT_fig.MenuBar = 'none';
standard menu bar menus.
% Hide
%% Callback Functions
%%
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
% Reconverting to UItable to
table array
for r_idx = 1 : size(table, 1)
for c_idx = 1 : size(table,
2)
%==========================================================================
% >>>>>>>>>>>> FUNCTION MF-10: LOADING PROGRAM TABLE <<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will load the parameters table when the button
% edit robot parameter in settings tab is pressed.
% Refer to section 4 of documentation for details.
%==========================================================================
function MF_Load_Program_table(P)
Ptable(row, col) =
% Loading variables
{''};
HD = evalin('base', 'HD');
%Load
length(Pcell{row,col})
handles from base workspace
for i =
Pcell = table2cell(P);
%table
1:length(Pcell{row,col})
converted to cell
if
%% Converting to string (cts: converted
isnumeric(Pcell{row,col}{i})
table to string)
Ptable{row,
for row = 1:size(P,1)
col} = [Ptable{row, col},' ', ...
for col=1:size(P,2)
if isnumeric(Pcell{row,col})
num2str(Pcell{row, col}{i})];
Ptable(row, col) =
else
{num2str(Pcell{row, col})};
Ptable(row,
else
col) = [Ptable{row, col},' ', ...
if iscell(Pcell{row, col})
end
end
end
end
else
set(HD.program_table,'Data',Ptable);
Ptable(row, col) =
%Display in the table.
{num2str(Pcell{row, col})};
end
end
%==========================================================================
% >>>>>>>>>>>>>> FUNCTION MF-11: LOAD TABLE OF COORDINATES <<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will open a window for the user to choose the
% excel spreadsheet with the coordinates, then the function will check the
% coordinates in the table and if valid, it will be saved in base workspace
% under the variable name TB. Refer to section 4 of documentation for
% details.
%==========================================================================
function MF_Load_Table_Coord()
% Make sure all rows are number (it
%open an window to the user choose the
does not check here if the points are
table (excel)
% reacheable or not)
[filename, folder] = uigetfile('*');
if ~isnumeric(TP)
ffname = fullfile(folder, filename);
MF_Update_Message(14, 'warnings');
%full filename
return
end
% Attempt to open the file
try
if size(TP,2) < 6
TP = xlsread(ffname);
MF_Update_Message(24, 'warnings');
catch
end
try
TP = open(ffname);
% saving in base workspace
catch
assignin('base', 'TP', TP);
MF_Update_Message(13,
'warnings');
% update message about load status
return
MF_Update_Message(7, 'notice');
end
end
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-12: LOAD STRUCTURES FILES <<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 2.0 - March 30th, 2016.
% DESCRIPTION: This function will ....
% Refer to section 6.4.1 for details.
%==========================================================================
function MF_Load_Structures()
History =
Settings_path = which('SETTINGS.mat');
matfile(History_path,'Writable',true);
Rdata_path = which('ROBOT_DATA.mat');
%load as matfile
GR_data_path = which('GR_DATA.mat');
Settings =
History_path = which('HISTORY.mat');
matfile(Settings_path,'Writable',true);
Messages_path = which('MESSAGES.mat');
%load as matfile
Messages =
matfile(Messages_path,'Writable',true);
Robot_data =
%load as matfile
matfile(Rdata_path,'Writable',true);
%load as matfile
RP = Robot_data.RP; %Read the variable
topo from the MAT-file.
assignin('base',
assignin('base',
assignin('base',
assignin('base',
assignin('base',
assignin('base',
'RP', RP);
'H', H);
'S', S);
'AS', AS);
'LD', LD);
'M', M);
end
%==========================================================================
% >>>>>>>>>>>>>> FUNCTION MF-13: PLOT BASE REFERENCE FRAME <<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will plot base frame on the robot
% representation in animation window. The function checks if
% Tools>Reference Frames is checked and then create the axes frames in
% robot 0 coordinate. The color for the axes follow a standar for
% coordinates frames: X: red; Y: green; Z: blue. Refer to section 4 of
% documentation for details.
%==========================================================================
function MF_Plot_Base_frame(show)
Vbx = [[0; axis_length], [0;
%Load and assemble variables
0], [0; 0]];
RP = evalin('base', 'RP');
% Load
Vby = [[0; 0], [0;
Robot parameters (from base workspace)
axis_length], [0; 0]];
AF = evalin('base', 'AF');
% Load
Vbz = [[0; 0], [0; 0], [0;
figure (Animation Figure)
axis_length]];
% show = true: display the axes on the
screen; false: delete the axes
if ~exist('show', 'var')
HD = evalin('base', 'HD');
%
Load Handles (from base workspace)
if strcmp(get(HD.disp_ref_frames,
'Checked'), 'on')
show = true;
else
show = false;
end
end
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
%Position of CG of link i to
function delete_axes()
try
p_cm = Tcmi * r_cm;
axes_lines = getappdata(0,
'cg_frames');
for j =
Vtx = (Tcmi * Vbx')';
1:size(axes_lines,1)
%for each
Vty = (Tcmi * Vby')';
joint
Vtz = (Tcmi * Vbz')';
for c = 1 :
size(axes_lines,2) %for each
coordinate
p_cm = p_cm(1:3)';
line =
axes_lines(j,c);
Vtx = Vtx(:, 1:3) + [p_cm;
delete(line);
p_cm];
end
Vty = Vty(:, 1:3) + [p_cm;
end
p_cm];
end
Vtz = Vtz(:, 1:3) + [p_cm;
end
p_cm];
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-15: PLOT LINKS FRAME <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
base.
% DESCRIPTION: This function will plot the links reference frames on the
% robot representation in animation window. The function checks if the user
% selected the option Display Links Frames and transform a reference frame
% from the base to the selected link (in Transformation Matrices menu % Commands tab). If the Settings tab option: Show all axes are set to true,
% then all links(joints) reference frames are plotted at once. Refer to
% section 4 of documentation for details.
%==========================================================================
function MF_Plot_Links_frame(T, show)
%Load and assemble variables
% show = true: display the axes on the
S = evalin('base', 'S');
% Load
screen; false: delete the axes
Settings (from base workspace)
if ~exist('show', 'var')
H = evalin('base', 'H');
% Load
if strcmp(get(HD.disp_link_frame,
History (from base workspace)
'Checked'), 'on')
HD = evalin('base', 'HD');
% Load
show = true;
Handles (from base workspace)
else
RP = evalin('base', 'RP');
% Load
show = false;
Robot parameters (from base workspace)
end
AF = evalin('base', 'AF');
% Load
end
figure (Animation Figure)
cn = S.value{'cn'};
function delete_axes()
try
axes_lines = getappdata(0,
'links_frames');
for j =
1:size(axes_lines,1)
%for each
joint
for c = 1 :
size(axes_lines,2) %for each
coordinate
line =
axes_lines(j,c);
delete(line);
end
end
end
end
end
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-16: PLOT GRAPHS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 2.0 - March 30th, 2016.
% DESCRIPTION: This function will plot the 4 graphs: position, velocity,
% acceleration and torque (each graph will have the n joints)
% Refer to section 6.4.1 for details.
%==========================================================================
function MF_Plot_Graphs(cn)
xlabel('$t
[s]$','interpreter','latex');
ylabel('$\theta(t) [^
\circ]$','interpreter','latex');
legend(lenged_txt,'interpreter','latex'
);
%2- Plotting Angular Velocity
(dtheta/dt) over time for each joint
subplot(plot_r,plot_c,2);
hold on
grid on
for i = 1:n
try
plot(time,
theta_dot(:,i),'LineWidth',lw);
lenged_txt{i} =
strcat('$\dot\theta' , num2str(i),'$');
end
end
title('Joint velocity');
xlabel('$t
[s]$','interpreter','latex');
ylabel('$\dot\theta(t) [^ \circ
/s]$','interpreter','latex');
legend(lenged_txt,'interpreter','latex'
);
% S = evalin('base', 'S');
%Get
Settings structure (S) from base
workspace
% RP = evalin('base', 'RP'); %Get
Settings structure (S) from base
workspace
for i = 1:size(varargin, 2)
if isfield(H,
varargin{i})
%update field with input
given
H(cn).(varargin{i})
= varargin{i+1};
end
end
end
save(History_path, 'H', 'append');
%Save the mat file.
assignin('base', 'H', H);
%Save in base workspace
case 'S'
switch sname
case 'H'
%History'
end
H = evalin('base', 'H'); %Get H
end
structure from base workspace
if nargin > 1
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION MF-21: SAVE SETTINGS <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will save settings to S structure
% Refer to section 4 of documentation for details.
%==========================================================================
function
elseif ~(isnumeric(Settings{5, 2}))
MF_Save_Settings(settings_data)
%ROW 5: ROBOT NUMBER
settings_path = which('SETTINGS.mat');
msgbox('the number you entered is
load (settings_path);
not numeric');
for i=1:length(settings_data)
try
S.value = str2num(S.value{i});
end
end
%% Check entries for inconsistencies
Settings((2:length(settings_data)+1),2)
= settings_data(:,2);
%>>Check each row for inconsistent
numbers;
if ~(isboolean(Settings{2, 2}))
%ROW 2: ENABLE GRIPPER
msgbox('the number you entered is
not boolean');
elseif ~(isnumeric(Settings{3, 2}))
%ROW 3: RANGE OF ROTATION OF GRIPPER
msgbox('the number you entered is
not numeric');
elseif ~(isnumeric(Settings{4, 2}))
%ROW 4: PORT NUMBER (COM)
msgbox('the number you entered is
not numeric');
if isempty(max_reach) || max_reach == 0
|| isnan(max_reach)
a_sum = sum(a);
d_sum = sum(d);
%%
%% Compute either home_q or home_p
given one of them
[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
5 FUNES DE SUPORTE
%==========================================================================
% >>>>>>>>>>>>>>>>>> FUNCTION SPF-1: IMPORT CAD TO MAT <<<<<<<<<<<<<<<<<<<
%==========================================================================
% Created by Diego Varalda de Almeida
% version 1.0 - March 30th, 2016.
% DESCRIPTION: This function will convert the cad robot file (saved in STL)
% in a MAT file called ROBOT_DATA that will be used to generate the
% graphical representation of the robotic arm and perform the simulations.
% Each link should be saved in a different stl file. For a 6dof robot with
% end effector, there should be 7 files: base.stl, link1.stl,
% link2.stl,..., link5.stl, end_effector.stl (user must enable end-effector
% in Settings. The files should be inserted in the directory OTHER_FILES).
% Refer to section 4 of documentation for details.
%==========================================================================
function SF_Importing_CAD()
filenames(n_links+2) =
% n: robot degrees of freedom (number
strcat(file(3), extension); %adding the
of joints)
end-effector name
% end_effector: 1 for robots with
end
end_effector, 0 without end_effector
clc
% %prompt the user to select the stl
Settings_path = which('SETTINGS.mat');
files
Robots_data_path =
% [filename, folder] = uigetfile('*',
which('ROBOT_DATA.mat');
'MultiSelect','on');
% fname = fullfile(folder, filename);
load(Settings_path);
%
%#release in future version: open
R_data =
window to select the stl files and
matfile(Robots_data_path,'Writable',tru
%
then create a figure with a table
e); %load as matfile
so then the user can inform the
R_info = R_data.RP; %load the table
%
program which of the file is
that contains the D-H parameters
base, link and end-effector
n_links = S.value{'dof'} - 1; %number
of links = number of dof - 1
end_effector = true;%S.value{2}; %true
for robot w/ end-effector, false
otherwise
d = R_info.d';
%get parameter d
from L structure (ROBOT_DATA)
a = R_info.a';
%get parameter a
Alpha = R_info.alpha'; %get parameter
alpha
%The
% figure
% ax_size = 1350;
% axis([-ax_size/5 ax_size -ax_size/2
ax_size/2 -0 ax_size/3]);
% view(135,25) %Adjust the view
orientation.
% hold on;
% grid('on');
% light
%
% for i=1:length(filenames)
% AAA{i} = patch('faces', LD(i).Fa,
'vertices', LD(i).Ve(:,1:3));
%
set(AAA{i}, 'facec',
[.8,.8,.8]);% set base color and draw
%
set(AAA{i}, 'EdgeColor','none');%
set edge color to none
%
input('Press a key to continue');
%
try
%
children = get(gca,
'children');
%
delete(children);
%
end
%
% end
Pcm = collect(Pcm);
Pcm = combine(Pcm);
Pcm = simplify(Pcm);
S(i).P = Pcm (1:3);
c_ijk = simplify(c_ijk);
C_terms = C_terms + c_ijk *
dq(j) * dq(k);
end
C_row = C_row + C_terms;
end
EM(1).C(i,1) = C_row;
%EM:
Equations of Motion Structure
end
disp('Second term: (MATRIX C . dq)
computed');
%% THIRD TERM OF THE EQUATION OF
MOTION: MATRIX G
%Vector nx1
for i=1:n
G_cell = zeros(1,1);
for j=1:n
G_cell = G_cell + (m(j) * g' *
S(j).J_L(:,i));
end
EM(1).G(i,1) = G_cell;
%EM:
Equations of Motion Structure
end
disp('Third term: matrix G computed');
%% FINAL RESULT: EQUATIONS OF MOTION
TORQUE = EM(1).Mq + EM(1).C + EM(1).G;
%TORQUE = simplify(TORQUE);
TORQUE = collect(TORQUE);
disp('terms collected');
TORQUE = combine(TORQUE);
disp('terms combined');
% TORQUE = simplify(TORQUE);
disp('terms simplifyed');
EM(1).TORQUE = TORQUE;
D = R_data.D;
EM = TORQUE;
save(Robot_data_path, 'EM', '-append');
toc
% save('EQUATIONS_MOTION.mat', 'EM');
% disp('EQUATIONS OF MOTION FOR THE
ROBOT GENERATED SUCCESSFULLY');
%
% %
=======================================
===================================
% % ================================
RESULTS
===============================
% %
=======================================
===================================
% for i=1:n
%
disp('=================================
=============================');
% d = sym('d%d',[1,n],'real');
%Symbolic Theta angles (joint variable)
% a = sym('a%d',[1,n],'real');
%Symbolic Theta angles (joint variable)
% A = sym('A%d',[1,n],'real');
%Symbolic Theta angles (joint variable)
T = sym('T%d',[1,n],'real'); %Symbolic
Theta angles (joint variable)
%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);
%==========================================================================
% 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