Estamos interessados em fazer o controle de manipuladores robóticos.

Para falar de controle temos que definir qual é o sistema a ser controlado, ou seja, precisamos especificar:

• ... as variáveis manipuladas (entradas),
• ... as variáveis de processo (saída),
• ... o modelo matemático que as relaciona.

Há várias maneiras diferentes de se fazer controle de robôs, com níveis de complexidade diferentes e respectivos sistemas diferentes. Vamos ver um pouco sobre isso antes de prosseguir.

Do ponto de vista da variável manipulada (ou seja, a que podemos comandar para atingir o objetivo de controle), os robôs podem ter interface para os seguintes comandos, do mais baixo nível para o mais alto nível:

• O torque no motor das juntas, \(\tau\),
• A velocidade de configuração desejada \(\dot{q}_d\),
• A configuração desejada \(q_d\),
• A pose do efetuador desejada \(\hat{H}_d\).

Nesse caso, enviamos diretamente qual é o torque do motor em cada junta a cada instante de tempo, \(\tau_i(t)\).

Para controlar dessa forma, temos normalmente como variáveis de processo a configuração e a velocidade de configuração atual, \(q(t), \dot{q}(t)\).

O modelo matemático entrada/saída é dado por um (complicado) sistema de equações diferencias de segunda ordem, não linear, \(\ddot{q} = f(q,\dot{q},\tau)\), dependente dos parâmetros inerciais (massas e momentos de inércia) dos robôs. Aprenderemos a calcular esse modelo na Aula 6.

É (bem) raro robôs industriais terem uma interface de controle para torque, devido à elevada complexidade de se controlar e a possibilidade de causar danos no robô.

Por outro lado, é o que permite a maior liberdade de controle. É especialmente importante se quisermos controlar a força que o robô faz no ambiente (controle de força).

É essencialmente usado por pesquisadores ou pelos fabricantes dos robôs.

Diagrama de blocos:

Nesse caso, assumimos que há um controlador de mais baixo nível que nos permite enviar a velocidade de configuração desejada para cada junta, \(\dot{q}_{i,d}(t)\).

Assumindo que o controlador é bom é que o sinal de velocidade desejada é "razoável", podemos assumir que a velocidade real é igual a desejada, e portanto o modelo matemático é \(\dot{q} = \dot{q}_d\).

Nesse tipo de controle temos como variável de processo a configuração \(q\) (não é necessário medir a velocidade de configuração, a não ser se quisermos verificar que o controlador de baixo nível está seguindo o valor desejado).

É relativamente comum em robôs industriais, nem que seja a partir da modificação de outras interfaces. Em especial, podemos "adaptar" o próximo controle (por configuração) para se tornar este.

Nos permite uma boa flexibilidade para controlar o robô.

É o tema desta aula.

Diagrama de blocos:

Nesse caso, assumimos que há um controlador de mais baixo nível que me permite enviar a configuração desejada para cada junta, \(q_{i,d}(t)\).

Assumindo que o controlador é bom é que o sinal de configuração desejada é "razoável", podemos assumir que a configuração real é igual a desejada, e portanto o modelo matemático é \(q = q_d\).

Nessa interface costumamos a fazer controle em malha aberta (do ponto de vista desse sistema), pois calculamos externamente a configuração desejada \(\dot{q}_d(t)\) para realizar a tarefa, enviamos para o robô, e não precisamos medir nada. Portanto, não há variável de processo.

O controle da Aula 3 é um exemplo de utilização dessa interface: pré-calculamos uma trajetória \(\dot{q}_d(t)\) e "mandamos" para o robô (usando o .add_ani_frame para simular a interface de controle de configuração).

É bem comum em robôs industriais.

Diagrama de blocos:

No nível mais alto, enviamos a pose desejada para o efetuador, \(\hat{H}_d(t)\), escrita como uma MTH ou de outra forma (geralmente 3 números de posição + 3 ângulos de Euler), com relação a um referencial fixo \(\mathcal{F}_0\).

Uma maneira (simplificada) de pensar nesse controlador é pensar que ele faz a cinemática inversa para a pose desejada, obtando a respectiva configuração desejada, \(q_d(t) = \texttt{IK}(\hat{H}_d(t))\), e depois usa a interface anterior.

Na verdade, cada fabricante implementa essa interface de um modo diferente.

Todos os robôs têm, de alguma forma, uma interface desse tipo.

Bem fácil e prático de utilizar. É, de longe, a interface mais utilizada pelos engenheiros/técnicos na indústria.

Por outro lado, te dá um controle menor no robô. Há várias maneiras de fazer o controle, mas a interface te força a usar a que foi implementada pelo fabricante. Pode ser que essa interface não seja aplicável para o problema (ex: há colisão do robô com ele mesmo).

Diagrama de blocos:

Nesta aula, focaremos na interface de controle de velocidade de configuração.

O controle nessa interface é bem mais simples que o controle por torque, e, por outro lado, fornece uma flexibilidade muito boa para fazer controle.

Chamamos o controle nessa interface de controle cinemático pois o controlador não dependerá dos parâmetros inerciais (massas, momentos de inércia) do robô. Ele dependerá apenas das características geométricas da tarefa.

Doravante, chamaremos \(u = \dot{q}_d\), e portanto o sistema que queremos controlar é:

É um integrador simples desacoplado (independente) da configuração.

Parece simples de controlar! Mas veremos que frequentemente usaremos controladores não lineares.

Como nossa variável de processo nesse caso é apenas a configuração \(q\), nosso controlador dependerá dessa variável e (algumas vezes) explicitamente do tempo atual:

Interfaces de configuração são mais comuns do que de velocidade.

Mas é possível utilizar a estratégia de controle cinemático com essa interface também. Seja \(u(t) = \dot{q}_d(t) = h(q(t),t)\) o controlador cinemático que queremos implementar. Então:

em que \(q_0\) é a configuração inicial do robô.

Basta calcular esse \(q_d\) e mandar para a interface de configuração!

Essa ideia é inclusive utilizada na implementação de controles cinemáticos no UAIBot, como veremos.

Definido o sistema que vamos trabalhar e as premissas, vamos falar dos porquê de fazermos controle: a tarefa.

A tarefa nada mais é do que uma especificação do que queremos fazer com o robô.

Consideraremos nesta disciplina apenas tarefas de movimento, ou seja, que não envolvem exercer força no ambiente de maneira controlada.

É um conceito da robótica no geral, não só de manipuladores. No caso de manipuladores de base fixa (nosso caso), essencialmente temos variações de duas tarefas de movimento:

• Fazer com que o efetuador alcance uma determinada pose completa constante ou variante no tempo.
• Fazer com que o efetuador alcance uma pose parcial (ex: a orientação não importa, ou a rotação no eixo \(z\) do efetuador está livre) constante ou variante no tempo.

Em robôs mais complexos, como humanoides, há mais diversidade de tarefas.

Temos duas categorias de tarefas: as invariantes no tempo (alcançar uma pose fixa) e as variantes no tempo (rastrear uma pose variante no tempo).

Para uma tarefa constante, note que podemos especificar uma tarefa como um subconjunto do espaço de configuração para qual queremos que o robô vá.

Por exemplo, se a nossa tarefa é alcançar uma posição desejada constante para o efetuador \(s_d\), temos que

em que \(s_e(q)\) é a posição do efetuador em função de \(q\).

Não necessariamente é fácil calcular todos os membros desse conjunto, e isso (felizmente) não é necessário. Entretanto, é útil considerar uma tarefa invariante no tempo como um conjunto de configurações \(\mathcal{T}\).

Para tarefas variantes no tempo a ideia é similar. Essencialmente para cada \(t\) fixo temos um conjunto \(\mathcal{T}(t)\).

Por exemplo, se a nossa tarefa é alcançar uma posição desejada variante no tempo para o efetuador \(s_d(t)\), temos que

ou seja, para cada \(t\) fixo temos um conjunto.

Nosso objetivo é então que a configuração \(q\) esteja no tempo \(t\) no conjunto \(\mathcal{T}(t)\).

Considere uma tarefa invariante no tempo (ex: alcançar uma pose constante).

Como visto, do ponto de vista da configuração, isso significa que queremos convergir para qualquer configuração \(q\) em uma região \(\mathcal{T}\) do espaço de configurações.

Portanto, devido às propriedades da função de tarefa, encontrar um \(q \in \mathcal{T}\) é equivalente a resolver a equação \(r(q)=0_{g \times 1}\).

Note que a primeira propriedade proibe o seguinte:

• Que haja uma configuração que satisfaça a tarefa em que \(r(q)\) não se anule. Isso faz com que tenhamos a certeza que todas as configurações estejam contempladas em \(r(q)=0_{g \times 1}\).
• Que haja uma configuração que não satisfaça a tarefa em que \(r(q)\) se anule. Isso faz com que ao resolver \(r(q)=0_{g \times 1}\) tenhamos a certeza que nenhuma solução encontrada será "ruim" (não resolverá nossa tarefa).

A propriedade da diferenciabilidade vai se mostrar útil em breve.

Suponha que a tarefa seja fazer a posição do efetuador, \(s_e(q)\), convergir para uma posição constante, \(s_{d}\) não nula.

Suponha que haja mais de uma configuração em que isso seja possível. Seja \(q_d\) uma dessas configurações. Propomos três "candidatas" para a função de tarefa.

• \(r_1(q) = q - q_{d}\),
• \(r_2(q) = s_d \times s_{e}(q)\),
• \(r_3(q) = s_e(q)-s_d\).

As três são funções de tarefa, de acordo com nossa definição?

Podemos notar que as três são funções diferenciáveis em \(q\), já que \(s_e(q)\) vem da cinemática direta para robôs manipuladores (formada por funções infinitamente diferenciáveis como senos e cossenos).

Para testar a primeira propriedade, podemos dividí-la em duas implicações (que devem ser mutuamente verdadeiras):

• (i) \(r(q)=0_{g \times 1} \) implica \(q \in \mathcal{T}\),
• (ii) \(q \in \mathcal{T}\) implica \(r(q)=0_{g \times 1}\).

(i) É fácil ver que primeira candidata, \(r_1(q)\) é zero somente se \(q=q_d\). Como \(q_d\) é uma configuração em que \(s_e(q_d)=s_{d}\), então temos que \(r_1(q) = 0\) implica que \(q \in \mathcal{T}\).

(ii) Mas a recíproca, \(q \in \mathcal{T}\) implica que \(r_1(q) = 0\), não é verdadeira. Pegue outra configuração \(q_d' \not= q_d\) dentro de \(\mathcal{T}\) (nós fizemos a suposição que o conjunto tem mais de um elemento). Nesse caso, \(s(q_d') = q_d'-q_d \not=0\).

Portanto ela não é uma função de tarefa pois exclui algumas configurações de \(\mathcal{T}\) ((ii) falha).

(i) Note que a segunda candidata, \(r_2(q)\), é zero se, por exemplo, \(s_e(q)=2s_d\) (propriedade do produto vetorial). Como \(s_d\) não é o vetor nulo, \(s_d \not= 2s_d\), e portanto não é verdade que \(r_2(q) = 0\) implica que \(q \in \mathcal{T}\).

(ii) É fácil ver que se \(s_e(q) = s_d\) então \(r_2(q)\) é zero. Então \(q \in \mathcal{T}\) implica \(r_2(q)=0\).

Portanto ela não é uma função de tarefa pois produz falsos membros de \(\mathcal{T}\) ((i) falha).

A última é a mais fácil de analisar. É fácil ver que \(r(q)=0\) se e somente se \(s_{e}(q)=s_d\), ou seja, \(q \in \mathcal{T}\).

Portanto ela é a única que é uma função de tarefa!

Existem infinitas funções de tarefa para uma mesma tarefa. Por exemplo, para a tarefa de alcançar uma posição desejada, todas a seguir são tarefas:

• \(r(q) = \alpha (s_e(q)-s_d)\) para \(\alpha \not = 0\),
• \(r(q) = \alpha \|s_e(q)-s_d\|^2\) para \(\alpha \not = 0\),
• \(r(q) = M(s_e(q)-s_d)\) para uma matriz \(M \in \mathbb{R}^{3 \times 3}\) invertível,
• Separando \(s_e\) e \(s_d\) em seus componentes \(x,y,z\): \(r(q) = 3-e^{-(x_e(q)-x_d)^2}-e^{-(y_e(q)-y_d)^2}-e^{-(z_e(q)-z_d)^2}\).

Note que o vetor \(r\) não é necessariamente tridimensional. Por exemplo, no segundo e no quarto exemplo, \(g\) (a dimensão do vetor) é 1.

Quais seriam exemplos de função de tarefa para o controle completo da pose?

Seja \(\phi_e(q) \in \mathbb{R}^{3 \times 1}\) um vetor com os ângulos de Euler da orientação do efetuador na configuração \(q\). Seja \(\phi_d \in \mathbb{R}^{3 \times 1}\) um vetor de ângulos de Euler para orientação desejada do efetuador. Uma "função de tarefa" muito utilizada na literatura, com \(g=6\), é:

Ela não é uma função de tarefa de acordo com nossa definição pois não é diferenciável para todo \(q\) (devido a não-diferenciabilidade dos ângulos de Euler em algumas situações).

Mesmo assim é frequemente utilizada.

Vamos propor uma outra função de tarefa, diferenciável e (bem) mais simples de se calcular. Ela é baseada no seguinte fato: se \(u, v \in \mathbb{R}^{3 \times 1}\) são vetores unitários (\(\|u\|=\|v\|=1\)):

Dessa igualdade, é fácil concluir que se \(u,v\) são vetores unitários, \(1-u^Tv=0\) se e somente se \(u=v\).

Sejam \(x_e(q), y_e(q), z_e(q)\) e \(s_e(q)\) as colunas 1, 2, 3 e 4 da MTH do referencial fixo para o efetuador, excluindo-se a última linha da MTH. Esses vetores tridimensionais (coluna) representam a posição do centro do efetuador e os três eixos da orientação, respectivamente.

Faça o mesmo para a MTH que representa a pose desejada, obtendo \(x_d, y_d, z_d\) e \(s_d\).

Temos então a seguinte função de tarefa, também com \(g=6\), mas diferenciável:

Note que usamos o fato que os vetores \(x,y\) e \(z\) são unitários.

Podemos usar a mesma ideia para funções de tarefa para poses parciais (ex: se não quisermos controlar a orientação dos eixos \(x\) e \(y\), basta remover as respectivas componentes).

Vai ser útil o cálculo da chamada Jacobiana de tarefa:

Mais precisamente, seja \(r_i(q)\), \(i=1,...,g\), a i-ésima componente da função de tarefa e \(q_j\), \(j=1,...,n\), a j-ésima configuração. Então \(J_r(q) \in \mathbb{R}^{g \times n}\), e o elemento na linha i e coluna j é \(\frac{\partial r_i}{\partial q_j}(q)\).

O cálculo da Jacobiana de tarefa é feito caso-a-caso (não há uma "fórmula geral"), entretanto, quase sempre precisamos da Jacobiana geométrica do efetuador para calculá-la.

Um truque útil para calcular a Jacobiana de tarefa é usar a regra da cadeia:

Frequentemente, é mais simples calcular \(\dot{r}\) do que \(J_r(q)\) diretamente. Nesse calculo aparecem, frequentemente, as velocidades lineares e angulares do efetuador, de onde surge a Jacobiana geométrica.

Ao terminar o cálculo de \(\dot{r}\) e comparar os dois lados da equação, podemos inferir \(J_r(q)\).

Considere nossa função de tarefa:

Vamos calcular a derivada temporal de cada uma das 4 sub-tarefas e depois "empilhar" os resultados.

Começamos por \(s_e(q)-s_d\).

Note que \(\frac{d}{dt}(s_e(q)-s_d) = \frac{d}{dt} s_e(q)\) (já que \(s_d\) é constante).

Mas, por definição, \(\frac{d}{dt} s_e(q)\) é a velocidade linear do efetuador! Portanto, usando as três primeiras linhas da Jacobiana geométrica:

Vamos agora trabalhar com \(1-x_d^Tx_e(q)\).

Note que \(\frac{d}{dt}(1-x_d^Tx_e(q)) = -x_d^T \frac{d}{dt}x_e(q)\) (já que \(x_d\) é constante).

Usando o Teorema da Aula 4 (ver também Exercício 3 da Aula 4)

em que \(\omega\) é a velocidade angular do efetuador.

Agora aplicamos as Propriedades 2 e 3 da Aula 4 para concluir que:

Usando a Jacobiana geométrica, \(\omega = J_{\omega}(q)\dot{q}\). Substituindo:

Usando esse fato na fórmula \(\frac{d}{dt}(1-x_d^Tx_e(q)) = -x_d^T \frac{d}{dt}x_e(q)\):

Obviamente, usando um raciocínio análogo, podemos achar fórmulas análogas para \(\frac{d}{dt}(1-y_d^Ty_e(q))\) e \(\frac{d}{dt}(1-z_d^Tz_e(q))\).

Portanto:

Podemos fatorar para fora o produto por \(\dot{q}\), presente em todos os membros, à direita:

Como pela regra da cadeia \(\frac{d}{dt} r(q) = J_r(q) \dot{q}\), temos que, comparando os dois lados e usando o fato que a equação tem que ser verdade para todo \(\dot{q}\):

Usando esse truque e as propriedades da Jacobiana geométrica e da matriz \(S\), podemos calcular a Jacobiana de tarefa para qualquer tarefa.

A título de curiosidade, a Jacobiana de tarefa da "função de tarefa":

recebe um nome especial em robótica: Jacobiana analítica.

Note que (no geral, salvo casos muito partículares) \(\frac{d}{dt} \phi_e \not= \omega\) (ver também Exercício 15 da Aula 4)!

Podemos também considerar tarefas variantes no tempo, \(\mathcal{T}(t)\).

A função de tarefa agora também depende de \(t\) explicitamente, \(r(q,t)\).

A definição da função de tarefa é bem similar ao caso de tarefas constantes:

Por exemplo, para a tarefa de rastrear uma pose variante no tempo, podemos utilizar:

Note que agora, pela regra da cadeia:

Essa equação permite calcular a Jacobiana de tarefa \(J_r(q,t)=\frac{\partial r}{\partial q}(q,t)\), como no caso constante.

Note que agora a Jacobiana de tarefa depende de \(t\).

O termo que "sobra" (que não está multiplicado por \(\dot{q}\), \(\frac{\partial r}{\partial t}(q,t)\)) também vai ser importante, e chamamos de Feedforward de tarefa.

Só existe (não nulo) para tarefas variantes no tempo.

Podemos calcular \(\frac{d}{dt} r(q,t)\) para nossa tarefa de pose variante no tempo.

Grande parte dos cálculos feitos anteriormente para o caso constante se manterão, mas haverá um termo a mais (o Feedforward de tarefa):

Podemos facilmente identificar a Jacobiana e o Feedforward dessa equação.

Para que o Feedforward de tarefa existir, os sinais \(x_d(t), y_d(t), z_d(t)\) e \(s_d(t)\) têm que ser (ao menos) uma vez diferenciáveis no tempo.

O nome Feedforward de tarefa vem do fato que para um caso muito especial em que \(r(q,t) = q - q_d(t)\), o termo vira \(\frac{\partial r}{\partial t}(q,t) = -\dot{q}_d(t)\), que é o (oposto) do termo de feedforward em controle clássico quando a dinâmica do sistema é a de um integrador simples (o que é, no caso do controle cinemático).

O nome, portanto, é primariamente devido a uma analogia.

Vamos supor que, de alguma maneira, escolhemos a velocidade de configuração \(u\) tal que a seguinte equação seja verdadeira:

para algum escalar positivo \(K\).

Seja \(r_i\) a i-ésima função de tarefa. A equação anterior, vetorial, implica as equações escalares para \(i=1,2,...,g\):

Sabemos que a equação diferencial \(\frac{d}{dt}w(t) = -Kw(t)\) com \(K>0\) constante e \(w(t) \in \mathbb{R}\) tem como solução \(w(t) = e^{-Kt}w(0)\).

Então, tomando \(w(t) = r_i(q(t),t)\), temos \(r_i(q(t),t) = e^{-Kt}r_i(q(0),0)\). Portanto, vetorialmente:

Assim, \(r\) cai exponencialmente para zero!

Podemos agora apreciar a primeira propriedade da função de tarefa: é devido a ela que podemos garantir que \(r\) ir para zero implica que a tarefa será alcançada pelo manipulador!

A questão agora é como escolher nossa ação de controle \(\dot{q}=u\) para garantir essa relação.

Usando a regra da cadeia, temos a seguinte equação:

Portanto, se encontrarmos \(\dot{q}\) que satisfaz a equação:

resolvemos nosso problema!

A equação anterior é um sistema linear da forma \(Au=b\), em que \(A = J_r(q,t)\), \(b=-Kr(q,t)-\frac{\partial r}{\partial t}(q,t)\) e \(u=\dot{q}\).

Já estudamos a solução para esse tipo de sistema na Aula 4, inclusive considerando o caso em que há infinitas ou não há solução.

Vamos usar a pseudoinversa amortecida da matriz \(A = J_r(q,t)\) para encontrar \(\dot{q}\).

Então, temos a expressão para nossa ação de controle \(u\):

para uma constante \(\epsilon > 0\).

Há várias coisas a serem ditas sobre esse controlador.

Primeiro, note que o controlador é da forma \(u= h(q,t)\) para uma função \(h(q,t)\) que é, no geral, bastante não linear tanto em \(q\) quanto em \(t\).

A não-linearidade na variável \(q\) insere o controlador na categoria de controle não linear (a não-linearidade na variável independente \(t\) não importa nessa categorização).

Exemplo bem específico em que ele será linear: se quisermos controlar apenas a posição de um robô cartesiano usando a função de tarefa \(r(q,t) = s_e(q)-s_d(t)\).

Note também que não há uma garantia que a equação original que ensejou a expressão do controlador, \(\frac{d}{dt}r = -Kr\), seja perfeitamente satisfeita.

Isso acontece pois a equação linear (em \(\dot{q}\)) pode não ter solução (e portanto a pseudoinversa amortecida encontra uma solução aproximada), ou mesmo por causa do fator de amortecimento \(\epsilon\).

Portanto, o controlador pode, em algumas situações, falhar em chegar na solução desejada.

A boa notícia é que, no geral, funciona muito bem, convergindo para a tarefa mesmo que não seja com a relação exponencial de decaimento da função de tarefa que foi originalmente planejada.

Se falhar? A recomendação mais simples é chavear para um movimento aleatório do manipulador por um tempo e depois rodar o controlador novamente.

Em aplicações reais, algumas restrições no movimento são importantes.

Por exemplo, o robô não pode colidir com ele mesmo ou com o ambiente.

Em nenhum momento isso foi considerado ao calcular o controlador.

Para tarefas relativamente simples em ambientes bem estruturados (como uma indústria), frequentemente essa estratégia vai funcionar sem violar nenhuma restrição. Mas é sempre importante ter um algoritmo supervisor que vai, na pior das hipóteses, pelo menos parar o robô antes que alguma restrição importante (ex: colisão) esteja na iminência de ser violada.

Há como melhorar esse controlador para considerar restrições, mas foge do escopo deste curso.

Se a relação \(\frac{d}{dt}r=-Kr\) for satisfeita, fica claro que a constante \(K\) governa o quão rápido há convergência para tarefa.

Matematicamente falando, só há convergência em \(t \rightarrow \infty\). Mas uma "regra de engenharia" é que podemos considerar convergência em \(t = 5/K\) unidades.

Mesmo se a relação não seja perfeitamente satisfeita, continua sendo verdade que, quanto maior \(K\), mais rápido o controlador, com a diferença em que a relação entre o tempo de convergência e \(K\) não é tão simples.

Evidentemente, há um preço a ser pago: maior \(K\) deixa o robô mais rápido, mas exige mais velocidade dos motores e portanto maior torque. Pode ser que o robô simplesmente não consiga executar a ação de controle enviada.

Quanto maior o fator de amortecimento, \(\epsilon\), mais suave vai ser ação de controle e portanto menos problema o robô terá em executá-la.

Entretanto, mais chance há de o robô falhar em executar a tarefa, como discutido anteriormente.

Falando de outra forma: quando menor o \(\epsilon\), mais chance de resolver a tarefa, mas mais agressivo a ação de controle (exemplo: acelerações de configuração elevadas). Portanto o robô pode ter dificuldades de executá-la.

Como várias coisas na engenharia, há um balanço. Normalmente usamos \(\epsilon = 10^{-3}\).

Suponha que nossa tarefa seja alcançar uma pose constante.

Note que, se o controlador funcionar, o robô irá se mover para uma configuração \(q_{ik}\) cuja respectiva pose no efetuador será justamente a pose desejada.

Ou seja, resolvemos o problema de cinemática inversa!

Em outras palavras, a técnica apresentada pode ser utilizada para (tentar) resolver o problema de cinemática inversa: use o algoritmo de controle apresentado aqui, mas sem executá-lo no robô real. Simule o seu movimento virtualmente no algoritmo usando o modelo \(\dot{q}=u\). Se houver convergência, a configuração final resolve o problema de cinemática inversa.

Isso é, essencialmente, o que a função .ikm do UAIBot faz.

Obtivemos nossa técnica a partir da relação \(\dot{r} = -Kr\), que força que \(r \rightarrow 0_{g \times 1}\).

Há outras alternativas. Podemos escolher outras funções \(F: \mathbb{R}^{g \times 1} \mapsto \mathbb{R}^{g \times 1}\) tal que \(\dot{r} = F(r)\) também garante \(r \rightarrow 0_{g \times 1}\).

Nesse caso, o controlador será:

Isso pode induzir comportamentos interessantes de convergência para o controlador.

Quais funções \(F\) podemos escolher?

Há muitos exemplos desse tipo de função:

• \(f(w) = -Kw\) para um \(K>0\),
• \(f(w) = -K\arctan(Aw)\) para um \(A, K>0\),
• \(f(w) = -A|w|^{n-1} w\) para um \(n, A>0\).

Em vez de usar \(F(r) = -Kr\) podemos usar uma função \(F\) tal que sua i-ésima componente é uma FCE da i-ésima componente de \(r\): \(F_i(r) = f_i(r_i)\).

O decaimento de cada componente de \(r_i\) para zero não mais será exponencial, mas dependerá da FCE escolhida (ver este Exercício da Aula 5).

Por que usar uma função diferente de \(F(r)=-Kr\)? Note que o decaimento exponencial tem a seguinte característica: ele é rápido no início (quando o valor da variável é relativamente grande) e devagar no final.

Portanto, o respectivo controlador pode ter a característica de convergir rápido para próximo da tarefa e depois demorar muito para eliminar essa pequena distância.

Aumentar \(K\) não é uma boa solução, pois aumenta também a velocidade na parte inicial, que já pode ser alta. A solução passa por fazer uma constante \(K\) "dinâmica". Isso implica, essencialmente, na escolha de funções \(F\) diferentes de \(F(r)=-Kr\) para \(K\) constante.

O ideal seria especificar uma velocidade de queda para a função de tarefa que fosse constante (digamos, \(A>0\)) para todo \(w\):

Mas como essa função não é contínua, o controlador tende a ficar muito "agressivo" perto de \(w = r_i=0\). O robô ficará "vibrando" em torno de \(r_i=0\). Para evitar isso, vamos fazer uma versão contínua dessa função:

Nesse caso, \(w_{tol}\) é uma tolerância para a função de tarefa. É um valor pequeno que consideramos que a i-ésima componente da tarefa já está praticamente completa quando \(|r_i| \leq w_{tol}\).

Essa escolha de \(f(w)\) não linear permite que tenhamos uma convergência mais homogênea para cada componente da função de tarefa.

Para ilustrar isso, consideramos duas funções \(f_1\) e \(f_2\) e a evolução das respectivas equações \(\dot{r}_1 = f_1(r_1)\) e \(\dot{r}_2 = f_2(r_2)\).

Foram usados \(f_1(r_1) = -r_1\) e \(f_2(r_2)\) sendo a função do slide anterior com \(A=0{,}5\) e \(w_{tol}=0{,}1\). Ambas começam com o valor de 1.

Note que com \(f_2\) temos uma convergência mais rápida, embora tenhamos no início velocidade de queda da função de tarefa menores do que com \(f_1\)!

Implementaremos agora vários controladores cinemáticos usando o UAIBot.

Vamos definir agora uma estrutura de simulação que será utilizada em todos os exemplos. O objetivo é separar claramente a parte do código que simula o robô real e a parte do código que é o controlador.

Assim, fica bem claro como o código seria portado para o robô real (onde não haveria "simulação" do robô).

Vamos assumir duas interfaces, que existiriam no robô real:

• get_configuration que retorna a configuração (posição das juntas) medida do robô. Assumimos uma medição perfeita.
• set_configuration_speed que seta a velocidade de configuração. Assumimos que o controlador de baixo nível é perfeito e a velocidade de configuração do robô real será exatamente a desejada.

As duas funções serão chamadas uma vez por ciclo de controle. Assumimos que cada ciclo de controle tem a mesma duração, \(dt\), e que entre um ciclo e outro a velocidade de configuração do robô é constante.

Escolheremos \(dt\) em nossas simulações de acordo com nossa conveniência, mas no robô real isso é limitado pela velocidade de comunicação das interfaces.

Ainda, obviamente, o tempo do cálculo do controlador no robô real deve ser menor que \(dt\). Em nossas simulações isso não será um problema.

Esboço da estrutura:

import uaibot as ub
import numpy as np

#O tempo entre um ciclo de controle e o outro. No mundo real
#ele não é constante, mas assumiremos aqui que ele é. Está
#medido em segundos
#0.01 s = controle a 100Hz
dt = 0.01
#O tempo, uma variável global
t = 0
#Tempo máximo de simulação
tmax = 10

def get_configuration(robot):
  #Essa função representa o ato de medir a configuração do robô
  #no tempo atual. Assumimos que medimos a configuração perfeitamente.
  #Devido à qualidade dos sensores, isso é razoavelmente realista.
  return robot.q

def set_configuration_speed(robot, qdot_des):
  #Essa função simula a interface de velocidade de um robô real.
  #A interface assume que entre um ciclo de controle e outro, a velocidade
  #de configuração real do robô vai ser a última enviada.
  #Assumindo um controlador perfeito, a velocidade de configuração real
  #é a desejada.
  #Portanto, a configuração no final do ciclo vai ser:
  q_next = robot.q + qdot_des*dt
  #Atualizamos a simulação para avisar que a configuração do robô mudou.
  robot.add_ani_frame(time = t+dt, q = q_next)


#As inicializações (ex: parâmetros do controlador) virão aqui


#Colocaremos aqui nosso "main" do controlador, que ficará em um laço
#durante um tempo tmax
for i in range(round(tmax/dt)):

  #################################
  # Início da lógica de controle  #
  #################################

  #Colocaremos aqui a lógica de controle

  #################################
  # Fim da lógica de controle     #
  #################################

  #O tempo sempre vai passar no final do ciclo
  t+=dt
                        

Vamos implementar o controle de pose completa constante para o robô Kuka KR5.

A pose desejada é:

em que \(H_e\) é a pose inicial do efetuador.

Vamos usar a função de tarefa apresentada aqui.

Vamos usar primeiro \(F(r) = -Kr\) com \(K=2 s^{-1}\). Usamos \(\epsilon = 10^{-3}\) na pseudoinversa amortecida.

O código e o resultado:

import uaibot as ub
import numpy as np


dt = 0.01
t = 0
tmax = 6

def get_configuration(robot):
  return robot.q

def set_configuration_speed(robot, qdot_des):
  q_next = robot.q + qdot_des*dt
  robot.add_ani_frame(time = t+dt, q = q_next)


#As inicializações (ex: parâmetros do controlador) virão aqui
robot = ub.Robot.create_kuka_kr5()

#Especifica a pose desejada para o efetuador
htm_d = ub.Utils.trn([-0.3,0.2,-0.3]) * robot.fkm()
#Faz a extração dos elementos x_d, y_d, z_d e s_d
x_d = htm_d[0:3,0]
y_d = htm_d[0:3,1]
z_d = htm_d[0:3,2]
s_d = htm_d[0:3,3]

#Cria um referencial no cenário para que seja possível ver se ele alcancou a
#pose
frame_d = ub.Frame(htm = htm_d)

#Cria a simulação
sim = ub.Simulation([robot,frame_d])

#Captura o número de juntas do robô
n = np.shape(robot.q)[0]

#Cria a função F:
def fun_F(r):
  K = 2
  return -K*r

#Cria uma matriz para o histórico de função de tarefa, da ação de controle
#e do tempo
hist_r = np.matrix(np.zeros((6,0)))
hist_u = np.matrix(np.zeros((n,0)))
hist_t = []

#Colocaremos aqui nosso "main" do controlador, que ficará em um laço
#durante um tempo tmax
for i in range(round(tmax/dt)):
  #################################
  # Início da lógica de controle  #
  #################################

  #Mede a configuração dos sensores
  q = get_configuration(robot)

  #Calcula a cinemática direta e Jacobiana para o efetuador nessa
  #configuração
  Jg, fk = robot.jac_geo(q)
  #Faz a extração de x_e, y_e, z_e e s_e
  x_e = fk[0:3,0]
  y_e = fk[0:3,1]
  z_e = fk[0:3,2]
  s_e = fk[0:3,3]

  #Monta o vetor de tarefa
  r = np.matrix(np.zeros((6,1)))
  r[0:3] = s_e - s_d
  r[3] = 1- x_d.T * x_e
  r[4] = 1- y_d.T * y_e
  r[5] = 1- z_d.T * z_e

  #Monta a Jacobiana de tarefa
  Jr = np.matrix(np.zeros((6,n)))

  Jr[0:3,:] = Jg[0:3,:]
  Jr[3,:] = x_d.T * ub.Utils.S(x_e) * Jg[3:6,:]
  Jr[4,:] = y_d.T * ub.Utils.S(y_e) * Jg[3:6,:]
  Jr[5,:] = z_d.T * ub.Utils.S(z_e) * Jg[3:6,:]

  #Calcula a ação de controle
  u = ub.Utils.dp_inv(Jr,0.001)*fun_F(r)

  #Guarda informações no histórico
  hist_r = np.block([hist_r, r])
  hist_u = np.block([hist_u, u])
  hist_t.append(t)

  #Manda a ação de controle para o robô
  set_configuration_speed(robot, u)

  #################################
  # Fim da lógica de controle     #
  #################################

  #O tempo sempre vai passar no final do ciclo
  t+=dt

#Roda a simulação
sim.run()
#plota os gráficos
ub.Utils.plot(hist_t, hist_r, "", "Tempo (s)", "Função de tarefa", "r")
ub.Utils.plot(hist_t, hist_u, "", "Tempo (s)", "Ação (rad/s ou m/s)", "u")
                        

Gráficos para a função de tarefa e ação de controle:

Note que algumas componentes da função de tarefa, como \(r_6\), não seguiram a especificação de decaimento exponencial.

Note também (veja ao lado) que o controle de orientação fica meio "preguiçoso" no final. Isso é devido ao fenômeno que explicamos aqui, advindo da escolha da função \(F(r)=-Kr\).

Vamos tentar a função \(F(r)\) obtida aplicando a função \(f(w)\) vista aqui em cada componente.

Agora, usamos \(f(w)\) igual para todos os componentes para formar \(F(r)\).

Os parâmetros são \(w_{tol} = 0{,}01\) e \(A=0{,}25 s^{-1}\).

Vamos ver o que vai acontecer...

O código e o resultado:

import uaibot as ub
import numpy as np


dt = 0.01
t = 0
tmax = 6

def get_configuration(robot):
  return robot.q

def set_configuration_speed(robot, qdot_des):
  q_next = robot.q + qdot_des*dt
  robot.add_ani_frame(time = t+dt, q = q_next)


#As inicializações (ex: parâmetros do controlador) virão aqui
robot = ub.Robot.create_kuka_kr5()

#Especifica a pose desejada para o efetuador
htm_d = ub.Utils.trn([-0.3,0.2,-0.3]) * robot.fkm()
#Faz a extração dos elementos x_d, y_d, z_d e s_d
x_d = htm_d[0:3,0]
y_d = htm_d[0:3,1]
z_d = htm_d[0:3,2]
s_d = htm_d[0:3,3]

#Cria um referencial no cenário para que seja possível ver se ele alcancou a
#pose
frame_d = ub.Frame(htm = htm_d)

#Cria a simulação
sim = ub.Simulation([robot,frame_d])

#Captura o número de juntas do robô
n = np.shape(robot.q)[0]

#Cria a função F:
def fun_F(r):
    A = [0.25, 0.25, 0.25, 0.25, 0.25, 0.25]
    w_tol = [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
    F = np.matrix(np.zeros((6, 1)))
    for i in range(6):
        if abs(r[i, 0]) < w_tol[i]:
            F[i, 0] = -A[i] * (r[i, 0] / w_tol[i])
        elif r[i, 0] >= w_tol[i]:
            F[i, 0] = -A[i]
        else:
            F[i, 0] = A[i]
    return F

#Cria uma matriz para o histórico de função de tarefa, da ação de controle
#e do tempo
hist_r = np.matrix(np.zeros((6,0)))
hist_u = np.matrix(np.zeros((n,0)))
hist_t = []

#Colocaremos aqui nosso "main" do controlador, que ficará em um laço
#durante um tempo tmax
for i in range(round(tmax/dt)):
  #################################
  # Início da lógica de controle  #
  #################################

  #Mede a configuração dos sensores
  q = get_configuration(robot)

  #Calcula a cinemática direta e Jacobiana para o efetuador nessa
  #configuração
  Jg, fk = robot.jac_geo(q)
  #Faz a extração de x_e, y_e, z_e e s_e
  x_e = fk[0:3,0]
  y_e = fk[0:3,1]
  z_e = fk[0:3,2]
  s_e = fk[0:3,3]

  #Monta o vetor de tarefa
  r = np.matrix(np.zeros((6,1)))
  r[0:3] = s_e - s_d
  r[3] = 1- x_d.T * x_e
  r[4] = 1- y_d.T * y_e
  r[5] = 1- z_d.T * z_e

  #Monta a Jacobiana de tarefa
  Jr = np.matrix(np.zeros((6,n)))

  Jr[0:3,:] = Jg[0:3,:]
  Jr[3,:] = x_d.T * ub.Utils.S(x_e) * Jg[3:6,:]
  Jr[4,:] = y_d.T * ub.Utils.S(y_e) * Jg[3:6,:]
  Jr[5,:] = z_d.T * ub.Utils.S(z_e) * Jg[3:6,:]

  #Calcula a ação de controle
  u = ub.Utils.dp_inv(Jr,0.001)*fun_F(r)

  #Guarda informações no histórico
  hist_r = np.block([hist_r, r])
  hist_u = np.block([hist_u, u])
  hist_t.append(t)

  #Manda a ação de controle para o robô
  set_configuration_speed(robot, u)

  #################################
  # Fim da lógica de controle     #
  #################################

  #O tempo sempre vai passar no final do ciclo
  t+=dt

#Roda a simulação
sim.run()
#plota os gráficos
ub.Utils.plot(hist_t, hist_r, "", "Tempo (s)", "Função de tarefa", "r")
ub.Utils.plot(hist_t, hist_u, "", "Tempo (s)", "Ação (rad/s ou m/s)", "u")
                        

Gráficos para a função de tarefa e ação de controle:

Note que o resultado ficou visivelmente melhor, principalmente para o controle de orientação.

Compare o gráfico da ação de controle para os dois casos: o controlador exigiu velocidades comparáveis com a do outro caso, mas conseguiu um resultado melhor.

Faremos agora o Kuka KR5 rastrear uma trajetória circular, cujo caminho está colocado ao lado.

A posição desejada para o efetuador é:

em que \(R=0{,}2m\), \(y_c=0{,}6m\), \(z_c=0{,}6m\) e \(\omega_d = \frac{\pi}{2} rad/s\).

Queremos que o eixo \(z\) do efetuador esteja alinhado com o eixo \(y\) do mundo (portanto, constante).

Vamos usar a função de tarefa apresentada aqui, mas retirando as componentes de orientação para \(x\) e \(y\), pois não há restrição.

Vamos usar primeiro \(F(r) = -Kr\) com \(K=2 s^{-1}\). Usamos \(\epsilon = 10^{-3}\) na pseudoinversa amortecida.

Note que precisamos do termo de feedforward, conforme mostrado aqui, mas retirando as componentes de orientação para \(x\) e \(y\), pois não há restrição.

O código e o resultado:

import uaibot as ub
import numpy as np

dt = 0.01
t = 0
tmax = 12


def get_configuration(robot):
  return robot.q


def set_configuration_speed(robot, qdot_des):
  q_next = robot.q + qdot_des * dt
  robot.add_ani_frame(time=t + dt, q=q_next)


# As inicializações (ex: parâmetros do controlador) virão aqui
robot = ub.Robot.create_kuka_kr5()

# Especifica a posição desejada para o efetuador
#como uma função de t
#Também fornece a derivada, que vai ser utilizada no feedforward
R = 0.2
y_c = 0.6
z_c = 0.6
omega_d = np.pi/2
s_d = lambda tt: np.matrix([R * np.cos(omega_d * tt), y_c, z_c + R * np.sin(omega_d * tt)]).reshape((3, 1))
s_d_dot = lambda tt: np.matrix([-R * omega_d * np.sin(omega_d * tt), 0, R * omega_d * np.cos(omega_d * tt)]).reshape((3, 1))


# Especifica a orientação desejada para o eixo z do efetuador
z_d = np.matrix([0,1,0]).reshape((3,1))

# Cria uma nuvem de pontos para visualizarmos a parte de posição da referência
pc = np.matrix(np.zeros((3,0)))
for i in range(200):
  pc = np.block([pc, s_d(2*np.pi*i/199)])

target_pos_pc = ub.PointCloud(points=pc, size=0.03, color="purple")

#Cria uma esfera para mostrar que o robô está rastreando a trajetória
ball_tr = ub.Ball(htm = np.identity(4), radius=0.02, color="cyan")

# Cria a simulação
sim = ub.Simulation([robot, target_pos_pc, ball_tr])

# Captura o número de juntas do robô
n = np.shape(robot.q)[0]

# Cria a função F:
def fun_F(r):
  K = 2
  return -K * r

# Cria uma matriz para o histórico de função de tarefa, da ação de controle
# e do tempo
hist_r = np.matrix(np.zeros((4, 0)))
hist_u = np.matrix(np.zeros((n, 0)))
hist_t = []

# Colocaremos aqui nosso "main" do controlador, que ficará em um laço
# durante um tempo tmax
for i in range(round(tmax / dt)):
  #################################
  # Início da lógica de controle  #
  #################################

  # Mede a configuração dos sensores
  q = get_configuration(robot)

  # Calcula a cinemática direta e Jacobiana para o efetuador nessa
  # configuração
  Jg, fk = robot.jac_geo(q)
  # Faz a extração de z_e e s_e
  z_e = fk[0:3, 2]
  s_e = fk[0:3, 3]

  # Monta o vetor de tarefa
  r = np.matrix(np.zeros((4, 1)))
  r[0:3] = s_e - s_d(t)
  r[3] = 1 - z_d.T * z_e

  # Monta a Jacobiana de tarefa
  Jr = np.matrix(np.zeros((4, n)))

  # Monta o termo de feedforward
  ff = np.block([[-s_d_dot(t)],[0]])

  Jr[0:3, :] = Jg[0:3, :]
  Jr[3, :] = z_d.T * ub.Utils.S(z_e) * Jg[3:6, :]


  # Calcula a ação de controle
  u = ub.Utils.dp_inv(Jr, 0.001) * (fun_F(r)-ff)

  # Guarda informações no histórico
  hist_r = np.block([hist_r, r])
  hist_u = np.block([hist_u, u])
  hist_t.append(t)

  # Manda a ação de controle para o robô
  set_configuration_speed(robot, u)

  #################################
  # Fim da lógica de controle     #
  #################################

  # O tempo sempre vai passar no final do ciclo
  t += dt

  #Atualiza a posição da bola (apenas para visualização)
  ball_tr.add_ani_frame(time = t, htm = ub.Utils.trn(s_d(t)))

# Roda a simulação
sim.run()
#plota os gráficos
ub.Utils.plot(hist_t, hist_r, "", "Tempo (s)", "Função de tarefa", "r")
ub.Utils.plot(hist_t, hist_u, "", "Tempo (s)", "Ação (rad/s ou m/s)", "u")
                        

Gráficos para a função de tarefa e ação de controle:

Novamente (veja ao lado) o controle de orientação fica "preguiçoso" durante todo o rastreamento de trajetória.

Vamos, novamente, tentar a função \(F(r)\) obtida aplicando a função \(f(w)\) vista aqui em cada componente. Vamos usar os mesmos parâmetros de antes.

O código e o resultado:

import uaibot as ub
import numpy as np

dt = 0.01
t = 0
tmax = 12


def get_configuration(robot):
  return robot.q


def set_configuration_speed(robot, qdot_des):
  q_next = robot.q + qdot_des * dt
  robot.add_ani_frame(time=t + dt, q=q_next)


# As inicializações (ex: parâmetros do controlador) virão aqui
robot = ub.Robot.create_kuka_kr5()

# Especifica a posição desejada para o efetuador
#como uma função de t
#Também fornece a derivada, que vai ser utilizada no feedforward
R = 0.2
y_c = 0.6
z_c = 0.6
omega_d = np.pi/2
s_d = lambda tt: np.matrix([R * np.cos(omega_d * tt), y_c, z_c + R * np.sin(omega_d * tt)]).reshape((3, 1))
s_d_dot = lambda tt: np.matrix([-R * omega_d * np.sin(omega_d * tt), 0, R * omega_d * np.cos(omega_d * tt)]).reshape((3, 1))


# Especifica a orientação desejada para o eixo z do efetuador
z_d = np.matrix([0,1,0]).reshape((3,1))

# Cria uma nuvem de pontos para visualizarmos a parte de posição da referência
pc = np.matrix(np.zeros((3,0)))
for i in range(200):
  pc = np.block([pc, s_d(2*np.pi*i/199)])

target_pos_pc = ub.PointCloud(points=pc, size=0.03, color="purple")

#Cria uma esfera para mostrar que o robô está rastreando a trajetória
ball_tr = ub.Ball(htm = np.identity(4), radius=0.02, color="cyan")

# Cria a simulação
sim = ub.Simulation([robot, target_pos_pc, ball_tr])

# Captura o número de juntas do robô
n = np.shape(robot.q)[0]

# Cria a função F:
def fun_F(r):
    A = 0.25
    w_tol = [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
    F = np.matrix(np.zeros((4, 1)))
    for i in range(4):
        if abs(r[i, 0]) < w_tol[i]:
            F[i, 0] = -A * (r[i, 0] / w_tol[i])
        elif r[i, 0] >= w_tol[i]:
            F[i, 0] = -A
        else:
            F[i, 0] = A
    return F

# Cria uma matriz para o histórico de função de tarefa, da ação de controle
# e do tempo
hist_r = np.matrix(np.zeros((4, 0)))
hist_u = np.matrix(np.zeros((n, 0)))
hist_t = []

# Colocaremos aqui nosso "main" do controlador, que ficará em um laço
# durante um tempo tmax
for i in range(round(tmax / dt)):
  #################################
  # Início da lógica de controle  #
  #################################

  # Mede a configuração dos sensores
  q = get_configuration(robot)

  # Calcula a cinemática direta e Jacobiana para o efetuador nessa
  # configuração
  Jg, fk = robot.jac_geo(q)
  # Faz a extração de z_e e s_e
  z_e = fk[0:3, 2]
  s_e = fk[0:3, 3]

  # Monta o vetor de tarefa
  r = np.matrix(np.zeros((4, 1)))
  r[0:3] = s_e - s_d(t)
  r[3] = 1 - z_d.T * z_e

  # Monta a Jacobiana de tarefa
  Jr = np.matrix(np.zeros((4, n)))

  # Monta o termo de feedforward
  ff = np.block([[-s_d_dot(t)],[0]])

  Jr[0:3, :] = Jg[0:3, :]
  Jr[3, :] = z_d.T * ub.Utils.S(z_e) * Jg[3:6, :]


  # Calcula a ação de controle
  u = ub.Utils.dp_inv(Jr, 0.001) * (fun_F(r)-ff)

  # Guarda informações no histórico
  hist_r = np.block([hist_r, r])
  hist_u = np.block([hist_u, u])
  hist_t.append(t)

  # Manda a ação de controle para o robô
  set_configuration_speed(robot, u)

  #################################
  # Fim da lógica de controle     #
  #################################

  # O tempo sempre vai passar no final do ciclo
  t += dt

  #Atualiza a posição da bola (apenas para visualização)
  ball_tr.add_ani_frame(time = t, htm = ub.Utils.trn(s_d(t)))

# Roda a simulação
sim.run()
#plota os gráficos
ub.Utils.plot(hist_t, hist_r, "", "Tempo (s)", "Função de tarefa", "r")
ub.Utils.plot(hist_t, hist_u, "", "Tempo (s)", "Ação (rad/s ou m/s)", "u")
                        

Gráficos para a função de tarefa e ação de controle:

Vamos agora ver um exemplo de controle com dois manipuladores. Para isso, vamos usar o robô Darwin Mini.

Vamos considerar que o torso e as pernas estão fixas. Então cada um dos dois braços é um manipulador fixo com 3 graus de liberdade cada.

Os dois braços estão segurando uma bola. Temos que fazer movimentos sincronizados com os dois braços para mover a bola.

Queremos mover os dois braços de modo que o centro da bola faça a trajetória:

para \(x_c = 0{,}35m\), \(z_c=0{,}6m\), \(R=0{,}05m\) e \(\omega_B = \frac{2\pi}{3} rad/s\).

Sejam \(q_{E}, q_{D}\) a configuração dos braços da esquerda e direita, respectivamente. Seja \(q = (q_E^T \ q_D^T)^T\) a configuração total do sistema.

Sejam \(s_E(q_E)\) e \(s_D(q_D)\) a posição do referencial grudado nas mãos esquerda e direita, respectivamente, com relação ao referencial \(\mathcal{F}_0\).

A primeira subtarefa (A) é que o vetor distância entre os pontos \(s_E\) e \(s_D\) deve se manter constante, mantendo o valor inicial \(s_{ED0}\).

Com isso, garantimos que os braços não se separarão (a bola irá cair) e não se aproximarão mais (pode pressionar a bola e ela estourar).

A função de tarefa para essa subtarefa é

Agora vamos escrever a função de tarefa para garantir a segunda subtarefa (B): que o centro da bola faça a trajetória desejada \(s_{bd}(t)\).

Como a subtarefa (A) garante que o vetor distância ficará constante, o que queremos é que:

Então podemos usar a função de tarefa para essa subtarefa:

A função de tarefa (variante no tempo) é:

Precisamos agora calcular a Jacobiana de tarefa e o termo de feedforward.

Vamos usar o truque de derivar no tempo a função de tarefa.

Sejam \(J_{vE}(q_E)\) e \(J_{vD}(q_D)\) as Jacobianas de velocidade linear para os braços da esquerda e direita, respectivamente.

Então:

já que \(s_{ED0}\) é constante.

Com relação a segunda subtarefa:

Então:

já que \(s_{bd}(t)\) não é constante.

Portanto, podemos escrever, matricialmente:

Portanto, podemos extrair pela regra da cadeia (ver este slide):

Vamos usar \(F(r)=-Kr\) com \(K = 5s^{-1}\).

Vamos usar \(\epsilon = 10^{-3}\).

O código e o resultado:

import uaibot as ub
import numpy as np

dt = 0.01
t = 0
tmax = 12


def get_configuration(robot):
  return robot.q


def set_configuration_speed(robot, qdot_des):
  q_next = robot.q + qdot_des * dt
  robot.add_ani_frame(time=t + dt, q=q_next)

#Inicializações
robot = ub.Robot.create_darwin_mini()
ball = ub.Ball(htm=ub.Utils.trn([0.4,0,0.6]), radius=0.15, color="yellow")

#Pega as variáveis referentes aos dois braços
right_arm = robot.list_of_objects[0]
left_arm = robot.list_of_objects[1]

#Coloca as configurações iniciais do cenário
right_arm.add_ani_frame(0, q=[ 0.50289982, -0.12686379, -0.0784082])
left_arm.add_ani_frame(0, q=[ 2.63894549, -0.12686379, -0.0784082])

#Parâmetros da trajetória
radius_mov = 0.05
omega_mov = 2*np.pi/3
s_bd = lambda tt: np.matrix(
    [0.35, radius_mov * np.cos(omega_mov * tt), 0.6 + radius_mov * np.sin(omega_mov * tt)]).reshape((3, 1))
dot_s_bd = lambda tt: np.matrix(
    [0, -omega_mov * radius_mov * np.sin(omega_mov * tt), omega_mov * radius_mov * np.cos(omega_mov * tt)]).reshape(
    (3, 1))

s_ED0 = left_arm.fkm()[0:3, 3] - right_arm.fkm()[0:3, 3]

#Cria a simulação
sim = ub.Simulation.create_sim_factory([robot, ball])

# Cria a função F:
def fun_F(r):
  K = 5
  return -K*r

# Cria uma matriz para o histórico de função de tarefa, da ação de controle
# e do tempo
hist_r = np.matrix(np.zeros((4, 0)))
hist_u = np.matrix(np.zeros((n, 0)))
hist_t = []

# Colocaremos aqui nosso "main" do controlador, que ficará em um laço
# durante um tempo tmax
for i in range(round(tmax / dt)):
  #################################
  # Início da lógica de controle  #
  #################################

# Mede as configurações dos sensores
  q_E = get_configuration(left_arm)
  q_D = get_configuration(right_arm)

  #Calcula as Jacobianas geométricas
  Jg_E, fk_E = left_arm.jac_geo(q_E)
  Jg_D, fk_D = right_arm.jac_geo(q_D)

  s_E = fk_E[0:3, 3]
  s_D = fk_D[0:3, 3]

  #Monta as funções de tarefa e Jacobianas
  r = np.matrix(np.zeros((6, 1)))
  r[0:3, 0] = s_E - s_D - s_ED0
  r[3:6, 0] = (s_E + s_D) / 2 - s_bd(t)

  Jr = np.matrix(np.zeros((6, 6)))
  Jr[0:3, 0:3] = Jg_E[0:3, :]
  Jr[0:3, 3:6] = -Jg_D[0:3, :]
  Jr[3:6, 0:3] = Jg_E[0:3, :] / 2
  Jr[3:6, 3:6] = Jg_D[0:3, :] / 2

  ff = np.matrix(np.zeros((6, 1)))
  ff[3:6, 0] = -dot_s_bd(t)

  #Calcula a velocidade de configuração
  u = ub.Utils.dp_inv(Jr,0.001) * (fun_F(r) - ff)

  u_E = u[0:3, 0]
  u_D = u[3:6, 0]

  # Guarda informações no histórico
  hist_r = np.block([hist_r, r])
  hist_u = np.block([hist_u, u])
  hist_t.append(t)

  # Manda a ação de controle para o robô
  set_configuration_speed(left_arm, u_E)
  set_configuration_speed(right_arm, u_D)

  #################################
  # Fim da lógica de controle     #
  #################################

  # O tempo sempre vai passar no final do ciclo
  t += dt

  #Atualiza a posição da bola
  ball.add_ani_frame(t, htm=ub.Utils.trn((s_E + s_D) / 2))

# Roda a simulação
sim.run()
#plota os gráficos
ub.Utils.plot(hist_t, hist_r, "", "Tempo (s)", "Função de tarefa", "r")
ub.Utils.plot(hist_t, hist_u, "", "Tempo (s)", "Ação (rad/s ou m/s)", "u")
                        

Gráficos para a função de tarefa e ação de controle:

O controle cinemático é uma maneira prática de controlar o robô assumindo que há uma interface de baixo nível que garante uma velocidade de configuração desejada.

É aplicável em situações em que não há altas velocidades nem a necessidade de se controlar precisamente a força que é feita no ambiente.

Há muitas outros aspectos interessantes do controle cinemático que não são abordados neste curso, como:

• Controle com restrições (ex: obstáculos);
• Priorização de tarefas;
• Controle de caminho em vez de controle de trajetória (campo vetorial).