Sabemos como descrever a pose do efetuador com relação a um referencial \(\mathcal{F}_0\).

A cinemática direta mapeia a "posição" das juntas (configuração) na "posição" do corpo rígido (pose).

Estamos agora interessado em mapear não as "posições", mas as "velocidades". Esse é o objetivo da cinemática diferencial.

Antes de prosseguir, temos que apresentar um conceito importante:

O nome "S" vem de skew-symmetric, que é o termo inglês para "anti-simétrica".

Essa matriz tem propriedades interessantes:

Há uma profunda relação entre o operador/função \(S\) e matrizes de rotação variantes no tempo:

Pelo teorema, \( S\big(\omega(t)\big) = \dot{Q}(t)Q(t)^T \).

Considere a rotação a seguir (rotação no eixo \(z\) de um ângulo \(2t\)). Calcule \(\omega(t)\):

$$Q(t) = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} \cos(2t) & -\sin(2t) & 0 \\ \sin(2t) & \cos(2t) & 0 \\ 0 & 0 & 1 \end{array}\Large{\Bigg)}\normalsize{}. $$

Então:

$$\dot{Q}(t) = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} -2\sin(2t) & -2\cos(2t) & 0 \\ 2\cos(2t) & -2\sin(2t) & 0 \\ 0 & 0 & 0 \end{array}\Large{\Bigg)}\normalsize{}. $$

Usando a propriedade que \(\cos(\theta)^2+\sin(\theta)^2=1\):

$$\dot{Q}(t)Q(t)^T = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} 0 & -2 & 0 \\ 2 & 0 & 0 \\ 0 & 0 & 0 \end{array}\Large{\Bigg)}\normalsize{}. $$

Da definição de \(S\), podemos extrair que:

$$\omega(t) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0 \\ 0 \\ 2 \end{array}\Large{\Bigg)}\normalsize{}. $$

Neste exemplo o vetor \(\omega(t)\) é constante, mas isso nem sempre acontece, como veremos.

Considere a rotação a seguir (uma rotação complexa). Calcule \(\omega(t)\):

$$Q(t) = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} \cos(2t) & -\sin(4t)/2 & \sin(2t)^2 \\ \sin(2t) & \cos(2t)^2 & -\sin(4t)/2 \\ 0 & \sin(2t) & \cos(2t) \end{array}\Large{\Bigg)}\normalsize{}. $$

Com um pouco de trigonometria, podemos mostrar que:

$$\dot{Q}(t)Q(t)^T = \Large{\Bigg(}\normalsize{}\begin{array}{ccc} 0 & -2 & 2\sin(2t) \\ 2 & 0 & -2\cos(2t) \\ -2\sin(2t) & 2\cos(2t) & 0 \end{array}\Large{\Bigg)}\normalsize{}. $$

Portanto, usando a definição do operador \(S\), podemos ver que:

$$\omega(t) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 2\cos(2t) \\ 2\sin(2t) \\ 2 \end{array}\Large{\Bigg)}\normalsize{}. $$

Agora o vetor \(\omega(t)\) não é constante.

O vetor \(\omega(t)\) tem uma interpretação importante na mecânica, como veremos em breve.

No UAIBot usamos a função Utils.S para calcular a matriz \(S\) de um vetor.

import numpy as np
import uaibot as ub

print(ub.Utils.S([1,2,3]))
                        

Agora vamos aprender a descrever a velocidade de corpos rígidos, sem ainda estar vinculado ao contexto de manipuladores robóticos.

Uma partícula no espaço 3D com vetor posição \(p(t)\) escrita em um referencial \(\mathcal{F}_0\) tem sua velocidade descrita pelo vetor \(v(t) = \dot{p}(t)\), outro vetor 3D.

Uma descrição de velocidade para corpos rígidos tem que conter as informações necessárias para descrever a velocidade de todas as partículas que formam um corpo rígido.

Considere um corpo rígido com um referencial \(\mathcal{F}_{obj}\) grudado nele.

Fato importante: toda partícula \(P\) que forma esse corpo rígido tem suas coordenadas descritas no referencial \(\mathcal{F}_{obj}\) por um vetor tridimensional constante \(p^{P}_{obj}\).

Esse vetor é, de certa forma, uma "carteira de identidade" da partícula, pois é único para cada partícula e, independente do movimento do corpo rígido, esse vetor será constante ao longo do tempo para cada partícula.

Seja \(H_{0}^{obj}(t)\) a MTH que representa a pose do objeto com relação a um referencial \(\mathcal{F}_0\) no tempo \(t\).

Podemos extrair dela as matrizes de rotação \(Q(t)\) e vetor de deslocamento \(s(t)\).

Então, a posição da partícula \(P\) no referencial \(\mathcal{F}_{0}\), \(p^{P}_0(t)\), é variável no tempo, e é dada por:

$$p^{P}_0(t) = Q(t)p^{P}_{obj} + s(t)$$

Só aplicamos a transformação rígida respectiva à MTH \(H_{0}^{obj}(t)\) na sua descrição local.

Vamos derivar os dois lados dessa equação no tempo:

$$\dot{p}^{P}_0(t) = \dot{Q}(t)p^{P}_{obj} + \dot{s}(t).$$

Usando o teorema que relaciona a derivada da matriz de rotação à matriz \(S\):

$$\dot{p}^{P}_0(t) = S\big(\omega(t)\big)Q(t)p^{P}_{obj} + \dot{s}(t).$$

Mas \(Q(t)p^{P}_{obj} = p^{P}_0(t)-s(t)\), então:

$$\dot{p}^{P}_0(t) = S\big(\omega(t)\big)\big(p^{P}_0(t)-s(t)\big) + \dot{s}(t).$$

Usando a Propriedade 2 da matriz \(S\), chegamos em um resultado muito importante:

$$\dot{p}^{P}_0(t) = \omega(t) \times \big(p^{P}_0(t)-s(t)\big) + \dot{s}(t).$$

Você provavelmente já viu uma versão simplificada dessa fórmula no ensino médio...

Um disco de raio \(R\) roda no sentido horário com velocidade angular constante \(\Omega\) enquanto se desloca para direita com velocidade linear constante \(V\). Calcule o módulo do vetor velocidade instantânea da partícula \(P\) no instante de tempo mostrado ao lado.

O vetor velocidade da partícula é resultante da soma de duas componentes (vetores): uma devido à velocidade do disco transladando, um vetor para direita, e outra devido à rotação do disco, que, no instante mostrado ao lado, na partícula em questão, tem a mesma direção e sentido do vetor velocidade devido à translação.

O módulo do vetor velocidade devido à translação é V.

O módulo do vetor velocidade devido à rotação é \(\Omega R\).

Como os dois vetores estão na mesma direção e sentido, o módulo do vetor velocidade resultante é: $$\Omega R + V$$

Compare as duas fórmulas:

$$\Omega R + V$$

$$ \omega \times \big(p^{P}_0-s\big) + \dot{s}$$

A segunda é uma generalização da primeira!

• O vetor \(\omega\) faz o papel do escalar \(\Omega\), e é chamado de vetor velocidade angular;
• O vetor \(\dot{s}\) faz o papel do escalar \(V\), e é chamado de vetor velocidade linear, usaremos a notação \(v\);
• O vetor \(p^{P}_0-s\) faz o papel do escalar \(R\), e é o vetor distância entre a posição da partícula e o centro do referencial do objeto.

Todas essas quantias estão medidas com relação ao referencial \(\mathcal{F}_0\)!

Assim, a fórmula para o vetor velocidade da partícula \(P\), medida em \(\mathcal{F}_0\):

$$v^P_0 = \omega \times \big(p^{P}_0-s\big) + v$$

pode ser usado em situações genéricas, com inclusive os três elementos variantes no tempo.

A fórmula:

$$v^P_0 = \omega \times \big(p^{P}_0-s\big) + v$$

deixa claro que para caracterizarmos a velocidade de qualquer partícula, precisamos da velocidade linear \(v(t)\) e velocidade angular \(w(t)\) do corpo rígido no instante de tempo \(t\).

Importante: note que esses dois vetores são propriedades do corpo rígido como um todo, e não de partículas separadas. Assim, não faz sentido falar "velocidade linear da partícula" ou "velocidade angular da partícula" . A partícula tem o seu vetor velocidade, calculado a partir de \(v\), \(\omega\) e o vetor distância usando a fórmula acima.

Como calcular os vetores \(v(t)\) e \(\omega(t)\) de um corpo rígido?

Simples, em posse da sua descrição do movimento como uma MTH, \(H_{0}^{obj}(t)\), extraímos as respectivas rotação \(Q(t)\) e translação \(s(t)\).

A velocidade linear é \(v(t) = \dot{s}(t)\).

A velocidade angular é encontrada pela equação \(S(\omega(t)) = \dot{Q}(t)Q(t)^T\).

Repito que os dois vetores estarão medidos no referencial \(\mathcal{F}_0\).

O vetor velocidade angular independe de como o referencial do objeto, \(\mathcal{F}_{obj}\), está grudado nele.

Já a velocidade linear depende de como isso é feito.

Nas duas animações a seguir temos um corpo rígido com um mesmo movimento, mas com referenciais do objeto grudados de maneira diferentes. Você pode observar que a velocidade angular é a mesma, mas a linear muda...

De toda forma, a fórmula para velocidade de uma partícula no corpo dará o mesmo resultado, independente de como o referencial está grudado.

Exemplo: o ponto no centro do paralelepípedo do exemplo a seguir tem velocidade zero nos dois casos...

Interpretar o vetor velocidade linear \(v\) não é muito difícil.

Entretanto, o vetor velocidade angular \(\omega\) é um pouco mais complexo.

Podemos pensar nele da seguinte maneira:

• Sua direção e sentido formam um "eixo de rotação" instantâneo em torno do centro do referencial do objeto.
• Seu módulo dá a velocidade de rotação (em rad/s, por exemplo) instantânea naquele momento.

Então, se um corpo rígido gira em um único eixo o tempo todo com uma velocidade constante \(\Omega\) (ver o Exemplo anterior, à direita), seu vetor velocidade angular é um vetor constante com a direção e sentido do eixo de rotação e intensidade \(\Omega\).

Finalmente, podemos resolver nosso problema de cinemática diferencial.

O objetivo é calcular o \(v\) e \(\omega\) (escritos no referencial \(\mathcal{F}_0\)) do efetuador em um determinado instante de tempo.

Como dito na cinemática direta, como todo movimento do manipulador é fruto de um respectivo movimento em suas juntas, ou seja, em sua configuração \(q\), deve ser possível relacionar a velocidade de configuração \(\dot{q}=\frac{d}{dt}q\) com \(v\) e \(\omega\).

Na verdade, embora possa parecer um pouco contra-intuitivo, precisamos também saber qual é a configuração atual \(q\) para esse calculo. Ou seja, no geral, fornecer apenas qual é a velocidade instantânea de configuração em um momento não é suficiente para calcular \(v\) e \(\omega\).

Procuramos então funções \(g_v\) e \(g_{\omega}\) tal que

$$v = g_v(q,\dot{q}),$$

$$\omega = g_\omega(q,\dot{q}).$$

Vamos ter um sentimento/intuição de como isso é possível com um exemplo.

Na aula de cinemática direta, calculamos a cinemática direta para o robô ao lado.

Concluímos que o vetor \(s\), a posição do centro do referencial do efetuador com relação ao referencial, \(\mathcal{F}_0\), é:

$$s(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0,5\cos(q_1)+0,5\cos(q_1+q_2) \\ 0 \\ 0,5\sin(q_1)+0,5\sin(q_1+q_2) \end{array}\Large{\Bigg)}\normalsize{}.$$

Conforme vimos, para calcular a velocidade linear \(v\) basta derivar \(s\) no tempo.

Temos que usar a regra da cadeia, pois vamos derivar em \(t\), \(s=s(q)\) depende de \(q\) e a configuração \(q\) depende de \(t\), \(q=q(t)\):

$$v = \frac{d}{dt} s(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} -0,5\sin(q_1)\dot{q}_1-0,5\sin(q_1+q_2)(\dot{q}_1+\dot{q}_2) \\ 0 \\ 0,5\cos(q_1)\dot{q}_1+0,5\cos(q_1+q_2)(\dot{q}_1+\dot{q}_2) \end{array}\Large{\Bigg)}\normalsize{}.$$

Usando a notação \(c_1,s_1,c_{12},s_{12}\) já usada anteriormente, podemos escrever a última equação de uma maneira matricial:

$$v =\Large{\Bigg(}\normalsize{}\begin{array}{cc} -0,5s_1-0,5s_{12} & -0,5s_{12} \\ 0 & 0 \\ 0,5c_1+0,5c_{12} & 0,5c_{12} \end{array}\Large{\Bigg)}\normalsize{} \Large{\Big(}\normalsize{}\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \end{array}\Large{\Big)}\normalsize{}.$$

Vamos chamar essa matriz \(3 \times 2\) de \(J_v(q)\). Note que ela depende apenas de \(q\) (não de \(\dot{q}\)) e que é uma função não linear de \(q\).

Com essa notação:

$$v = J_v(q)\dot{q}.$$

Vamos agora para a velocidade angular. Lembramos que a matriz de rotação respectiva à MTH da cinemática direta é:

$$ Q(q) = \LARGE{\Bigg(}\normalsize{}\begin{array}{ccc} \cos(q_1+q_2) & 0 & -\sin(q_1+q_2) \\ 0 & 1 & 0 \\ \sin(q_1+q_2) & 0 & \cos(q_1+q_2) \end{array}\LARGE{\Bigg)}\normalsize{}$$

Então:

$$ \frac{d}{dt} Q(q) = \LARGE{\Bigg(}\normalsize{}\begin{array}{ccc} -\sin(q_1{+}q_2)(\dot{q}_1{+}\dot{q}_2) & 0 & -\cos(q_1{+}q_2)(\dot{q}_1{+}\dot{q}_2). \\ 0 & 0 & 0 \\ \cos(q_1{+}q_2)(\dot{q}_1{+}\dot{q}_2) & 0 & \sin(q_1{+}q_2)(\dot{q}_1{+}\dot{q}_2) \end{array}\LARGE{\Bigg)}\normalsize{}.$$

Usamos a relação \(S(\omega) = (\frac{d}{dt} Q(q))Q(q)^T\). Com um pouco de trigonometria, temos que:

$$ S(\omega) = \LARGE{\Bigg(}\normalsize{}\begin{array}{ccc} 0 & 0 & -(\dot{q}_1+\dot{q}_2) \\ 0 & 0 & 0 \\ (\dot{q}_1+\dot{q}_2) & 0 & 0 \end{array}\LARGE{\Bigg)}\normalsize{}.$$

E portanto:

$$\omega = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0 \\ -(\dot{q}_1+\dot{q}_2) \\ 0 \end{array}\Large{\Bigg)}\normalsize{}.$$

Podemos reescrever a última equação como:

$$\omega =\Large{\Bigg(}\normalsize{}\begin{array}{cc} 0 & 0 \\ -1 & -1 \\ 0 & 0 \end{array}\Large{\Bigg)}\normalsize{} \Large{\Big(}\normalsize{}\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \end{array}\Large{\Big)}\normalsize{}.$$

Vamos chamar essa matriz \(3 \times 2\) de \(J_\omega(q)\). Note que ela não depende de \(\dot{q}\) e nem de \(q\), mas não há erro matemático em dizer que ela é uma função (constante) de \(q\)...

Com essa notação:

$$\omega = J_\omega(q)\dot{q}.$$

Suponha que em um determinado instante de tempo tenhamos \(q_1=\frac{\pi}{4}\), \(q_2=0\) e \(\dot{q}_1=\dot{q}_2=1 rad/s\).

Usando as duas fórmulas:

$$ v = \Large{\Bigg(}\normalsize{}\begin{array}{c} -\sqrt{2} \\ 0 \\ \sqrt{2} \end{array}\Large{\Bigg)}\normalsize{} m/s \ , \ \omega = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0 \\ -2 \\ 0 \end{array}\Large{\Bigg)}\normalsize{} rad/s.$$

Ambos os vetores estão escritos no referencial \(\mathcal{F}_0\).

Veja o vetor velocidade linear ao lado, em amarelo, e observe que está coerente com o que esperaríamos em termos de velocidade linear instantânea para o efetuador nesta situação.

E o efetuador vai girar 2rad/s no eixo \(y\) no sentido anti-horário (sentido negativo de \(y\), neste caso).

Vamos calcular a mesma relação para o segundo exemplo da aula anterior...

O procedimento é o mesmo. Começamos derivando \(s\) no tempo para obter:

$$v = \frac{d}{dt} s(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} -0,5s_1\dot{q}_1{-}(0,25{+}q_3)s_{12}(\dot{q}_1{+}\dot{q}_2)+c_{12}\dot{q}_3 \\ 0 \\ 0,5c_1\dot{q}_1{-}(0,25{+}q_3)c_{12}(\dot{q}_1{+}\dot{q}_2)+s_{12}\dot{q}_3 \end{array}\Large{\Bigg)}\normalsize{}.$$

Assim como no exemplo anterior, podemos reescrever de maneira matricial:

$$v =\Large{\Bigg(}\normalsize{}\begin{array}{ccc} -0,5s_1{-}(0,25{+}q_3)s_{12} & -(0,25{+}q_3)s_{12} & c_{12} \\ 0 & 0 & 0 \\ 0,5c_1{+}(0,25{+}q_3)c_{12} & (0,25{+}q_3)c_{12} & s_{12} \end{array}\Large{\Bigg)}\normalsize{} \Large{\Bigg(}\normalsize{}\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{array}\Large{\Bigg)}\normalsize{}.$$

Vamos chamar essa matriz \(3 \times 3\) de \(J_v(q)\). Note que ela depende apenas de \(q\) (não de \(\dot{q}\)) e que é uma função não linear de \(q\).

Com essa notação:

$$v = J_v(q)\dot{q}.$$

A velocidade angular é a mesma do exemplo anterior, sendo independente da terceira configuração:

$$\omega =\Large{\Bigg(}\normalsize{}\begin{array}{ccc} 0 & 0 & 0 \\ -1 & -1 & 0\\ 0 & 0 & 0\end{array}\Large{\Bigg)}\normalsize{} \Large{\Bigg(}\normalsize{}\begin{array}{c} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{array}\Large{\Bigg)}\normalsize{}.$$

Vamos chamar essa matriz \(3 \times 3\) de \(J_\omega(q)\). Note que ela não depende de \(\dot{q}\) e nem de \(q\), mas não há erro matemático em dizer que ela é uma função (constante) de \(q\)...

Com essa notação:

$$\omega = J_\omega(q)\dot{q}.$$

Então, para ambos exemplos, existem matrizes \(3 \times n\) \(J_v(q)\) e \(J_{\omega}(q)\) (em que \(n\) é o número de juntas do respectivo exemplo) tal que:

$$v = J_v(q)\dot{q},$$

$$\omega = J_\omega(q)\dot{q}.$$

Será que algo similar ocorre para um manipulador geral?

Sim! no caso geral para um manipulador de \(n\) juntas, podemos mostrar que existem matrizes \(3 \times n\) \(J_v(q)\), \(J_{\omega}(q)\), funções no geral não lineares de \(q\), tal que:

$$v = J_v(q)\dot{q},$$

$$\omega = J_\omega(q)\dot{q}.$$

Quando "empilhamos" as duas matrizes em uma única matrix \(6 \times n\) temos a (importantíssima) Jacobiana Geométrica:

$$J_{geo}(q) = \left(\begin{array}{c} J_v(q) \\ J_{\omega}(q) \end{array}\right).$$

Das equações:

$$v = g_v(q,\dot{q}) = J_v(q)\dot{q},$$

$$\omega = g_\omega(q,\dot{q}) = J_\omega(q)\dot{q}.$$

podemos concluir que as duas velocidades são (no geral) funções não lineares da configuração atual, \(q\), mas sempre lineares em \(\dot{q}\).

Há uma "receita de bolo" para construir a Jacobiana geométrica do referencial do efetuador usando a convenção de Denavit-Hartenberg.

Para isso, usamos as matrizes \(H_0^e(q)\) (MTH do referencial \(\mathcal{F}_0\) para \(\mathcal{F}_e\)) e \(H_0^{DHi}(q) = H_0^{DH0} H_{DH0}^{DHi}(q)\) (MTH do referencial \(\mathcal{F}_0\) para \(\mathcal{F}_{DHi}\)).

Sejam \(s_{DHi}(q)\) e \(z_{DHi}(q)\) a translação e eixo \(z\) da MTH \(H_0^{DHi}(q)\), respectivemente (última e penúltima coluna das matrizes, respectivamente, sem o último elemento).

Seja \(s_{e}(q)\)a translação da MTH \(H_0^{e}(q)\).

Montaremos as duas matrizes coluna-a-coluna, para \(j=1,2,...,n\):

No UAIBot usamos a função .jac_geo para calcular a Jacobiana geométrica.

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
#Calcula a Jacobiana geométrica para um dada configuração
#A MTH da cinemática direta (fk) vem como sub-produto
Jg, fk = robot.jac_geo(q=[0,1,-1,2,3,4])
#Se não especificarmos a configuração, ele assume a atual (robot.q)
Jg, fk = robot.jac_geo()
#Assumindo que o robô tem na configuração atual uma velocidade instantânea
#de 1 rad/s para cada junta, vamos calcular os vetores velocidade linear
#e angular
qdot = np.matrix([1,1,1,1,1,1]).reshape((6,1)) #Ele tem que ser um vetor coluna
v = Jg[0:3,:] * qdot
w = Jg[3:6,:] * qdot
                        

A matriz Jacobiana geométrica em uma configuração \(q\) nos permite fazer uma série de análises interessantes acerca da manipulabilidade do manipulador nessa configuração.

Essencialmente, ela pode nos dizer o quão difícil (ou se é impossível) fazer um dado movimento naquela configuração \(q\).

A primeira pergunta é a seguinte: dado que estamos em uma configuração \(q\), é possível fazer (simultanemente) qualquer velocidade linear \(v_{d}\) e angular \(\omega_d\) desejada manipulando a velocidade de junta \(\dot{q}\)?

Do ponto de vista matemático, queremos saber se o sistema linear de equações:

tem uma solução \(\dot{q}\) para qualquer vetor \(\xi_d \in \mathbb{R}^6\).

Um vetor com a velocidade linear e angular "empilhada", como \(\xi_d\), recebe o nome simplesmente de velocidade.

Vamos relembrar um pouco de Álgebra Linear:

Seja \(A\) uma matriz com \(m\) linhas e \(n\) colunas. Queremos saber se para todo \(b \in \mathbb{R}^m\) a equação \(Au=b\) tem uma solução \(u \in \mathbb{R}^n\).

Para responder essa pergunta, temos que lembrar o conceito de posto (rank) de uma matriz:

Alguns resultados:

Outra definição importante:

Outras definições importante:

Obs: ele se chama "\(\texttt{Ker}\)" pois em inglês damos o nome "Kernel" (núcleo).

Temos então um resultado importante:

Portanto, a equação \(J_{geo}(q)\dot{q} = \xi_d\) tem solução para qualquer velocidade \(\xi_d \in \mathbb{R}^6\) se e somente se \(n \geq 6\) (o robô tem no mínimo 6 juntas) e \(J_{geo}(q)\) tem posto completo (no caso, 6).

Além disso, o Teorema 4 nos diz que quanto menor o posto de \(J_{geo}(q)\), menor a dimensão de velocidades \(\xi_d\) "realizáveis" na configuração \(q\), ou seja, em que é possível escolher \(\dot{q}\) que nos dê essa velocidade \(\xi_d\).

Portanto, o espaço de velocidades alcancáveis na configuração \(q\) é \(\texttt{Im}(J_{geo}(q))\).

Considerando um robô com no mínimo 6 juntas, dizemos que uma configuração \(q\) é singular se o posto de \(J_{geo}(q)\) não é completo.

Em uma configuração singular não é possível executar qualquer velocidade \(v_d, \omega_d\), o que limita a movimentação do robô.

São configurações difíceis de se trabalhar, e os engenheiros de controle devem evitá-las sempre que possível. O porquê ficará mais claro quando discutirmos melhor o controle de manipuladores.

Se um manipulador não puder executar qualquer velocidade \(\xi_d\) em uma configuração \(q\), quais ele não pode executar nessa configuração?

Para encontrar algumas velocidades não executáveis, podemos encontramos vetores \(\xi_d\) tal que \(J_{geo}(q)^T\xi_d = 0_{n \times 1}\).

O Teorema 7 fornece maneiras de encontrar algumas velocidades que não são executáveis, mas há velocidades não executáveis que não satisfazem \(J_{geo}(q)^T\xi_d = 0_{n \times 1}\).

O espaço de velocidades não executáveis não é um espaço vetorial!

Considerar apenas as configurações singulares como problemáticas não é prático.

Na prática, há configurações \(q\) que não são exatamente singulares, mas são praticamente singulares, pois para executar uma velocidade \(\xi_d\) precisamos de uma velocidade de configuração \(\dot{q}\) muitíssimo alta (e portanto inexequível, na prática).

São aquelas configurações em que a matriz \(J_{geo}(q)\) tem valores singulares muito próximos de zero, mas não exatamente zero.

Devido à continuidade da matriz \(J_{geo}(q)\) na configuração \(q\), as configurações quase singulares são configurações próximas das configurações singulares.

Considere o robô Kuka KR5 ao lado. A configuração ao lado (que é a inicial dele) é singular.

Ele não consegue executar os vetores (em cyan e em amarelo, respectivamente) \(v_d = (1,78 \ 0 \ 0)^T\) e \(\omega_d = (0 \ 0 \ 1)^T\) simultaneamente, por exemplo.

Há muitas outras que ele não consegue executar...

De fato, se \(h_{ne}\) é uma velocidade não executável e \(h_{e}\) é executável, então \(h_{ne}+h_{e}\) não é executável.

Vamos fazer agora uma análise que é, de certa forma, complementar à análise de alcançabilidade.

Suponha que em uma configuração \(q\) exista um vetor não-nulo \(\dot{q}\) tal que \(J_{geo}(q)\dot{q} = 0_{6 \times 1}\).

Isso significa que existe um "movimento instantâneo" (relativo à velocidade de junta instantânea \(\dot{q}\)) que, embora mexa partes do robô, mantém o efetuador exatamente como ele está!

Portanto, o espaço de velocidades de configuração inócuas ao efetuador na configuração \(q\) é \(\texttt{Ker}(J_{geo}(q))\).

Se o robô tem mais de 6 juntas, para toda configuração \(q\) existem vetores não-nulo \(\dot{q}\) tal que \(J_{geo}(q)\dot{q} = 0_{6 \times 1}\).

Veja o KUKA LBR IIWA ao lado, que tem 7 juntas. Ele sempre consegue fazer movimentos que não mudam a pose do efetuador...

Em resumo, para um robô com \(n\) juntas:

• O espaço de velocidades alcancáveis na configuração \(q\) é \(\texttt{Im}(J_{geo}(q))\), e tem dimensão posto(\(J_{geo}(q)\))...
• O espaço de velocidades de configuração inócuas ao efetuador na configuração \(q\) é \(\texttt{Ker}(J_{geo}(q))\), e tem dimensão \(n\)-posto(\(J_{geo}(q)\))...

Há também intepretações para \(\texttt{Im}(J_{geo}(q)^T)\) e \(\texttt{Ker}(J_{geo}(q)^T)\), mas serão vistas em outra oportunidade.

Estamos interessados agora em, efetivamente, resolver a equação

dado uma configuração \(q\) e uma velocidade desejada \(\xi_d\).

Esse é o problema da Cinemática Diferencial Inversa!

Esse problema, e problemas similares, é uma peça fundamental para grande parte das estratégias de controle de manipuladores.

Só pode acontecer uma das três possibilidades com um sistema linear \(Au=b\) da forma:

• ...ele tem uma solução única...
• ...ele tem infinitas soluções...
• ...ele não tem solução.

Não pode ocorrer dele ter, por exemplo, exatamente 4 soluções (e não mais)!

É razoável pensar o seguinte:

• Se \(Au=b\) só tem uma solução, queremos encontrá-la.
• Se \(Au=b\) tem infinitas soluções, queremos encontrar aquela que seja a melhor de acordo com algum critério.
• Se \(Au=b\) não tem solução, queremos encontrar o vetor \(u\) que mais se aproxima de resolver a equação.

Vamos formalizar o conceito de "melhor" no segundo e terceira situação.

Há várias maneiras de formalizar o conceito de "melhor", mas a maneira a seguir é muito conveniente matematicamente:

• Se \(Au=b\) só tem uma solução, queremos encontrá-la.
• Se \(Au=b\) tem infinitas soluções, queremos encontrar a que minimiza \(\|u\|^2\) (solução de menor norma).
• Se \(Au=b\) não tem solução, queremos encontrar a que minimiza \(\|Au-b\|^2\) (melhor solução em termos de mínimos quadrados).

O que procuramos é um algoritmo que considere os três casos e obtenha a respectiva solução.

Há uma maneira elegante de implementar essa solução. Considere o seguinte problema de otimização:

Iremos nos convencer agora que resolver esse problema para um \(\epsilon\) muito pequeno, mas positivo (\(\lim_{\epsilon \rightarrow +0}\)) é equivalente a implementar a solução de três partes que queríamos...

O "truque" consiste em observar que a função objetivo \(\|Au-b\|^2 + \epsilon \|u\|^2\) pondera dois objetivos, que podem ser conflitantes: o de minimizar \(\|Au-b\|^2\) e o de minimizar \(\|u\|^2\).

Considerando \(\epsilon\) muito pequeno, minimizar \(\|Au-b\|^2\) tem muito mais prioridade do que minimizar \(\|u\|^2\), e a segunda parcela só será considerada se conseguirmos minimizar totalmente \(\|Au-b\|^2\) , ou seja, se \(Au=b\).

• Se \(Au=b\) só tiver uma solução, minimizaremos \(\|Au-b\|^2\) completamente, sem se preocupar com minimizar \(\|u\|^2\). O resultado será o único \(u\) que resolve \(Au=b\).
• Se tivermos infinitas soluções, minimizaremos \(\|Au-b\|^2\) completamente. Dentre aqueles \(u\) em que \(Au=b\), o problema escolherá o que minimiza a função objetivo de menor prioridade, \(\|u\|^2\), justamente como queríamos.
• Se não tivermos nenhuma, o problema só minimizará \(\|Au-b\|^2\) da melhor maneira possível, sem se preocupar com minimizar \(\|u\|^2\).

Sendo bem preciso, o terceiro caso tem uma análise um pouco mais complicada, pois podem existir infinitos \(u\) que minimizam \(\|Au-b\|^2\) sem ter \(Au=b\). Nesse caso, a segunda função objetivo atua novamente.

O problema de otimização:

pode ser resolvido analíticamente, tirando o gradiente da função objetivo com relação a variavél \(u\) e igualando a zero.

A solução é:

Podemos mostrar que, para \(\epsilon > 0\), a matriz \(A^TA + \epsilon I_{n \times n}\) é sempre invertível.

Vamos introduzir a notação:

Então, nossa solução, com \(\epsilon \rightarrow +0\), é:

$$u = \lim_{\epsilon \rightarrow +0} A^{\dagger(\epsilon)} b = \left(\lim_{\epsilon \rightarrow +0} A^{\dagger(\epsilon)}\right)b.$$

A matriz \(A^{\dagger} = \left(\lim_{\epsilon \rightarrow +0} A^{\dagger(\epsilon)}\right)\) recebe o nome pseudoinversa da matriz \(A\), enquanto \(A^{\dagger(\epsilon)}\) o nome pseudoinversa amortecida da matriz \(A\).

Finalmente, temos a nossa solução:

Infelizmente, essa solução não é muito boa.

Quando \(A\) está próxima de ser singular, \(A^{\dagger}\), e portanto \(u\), tende a "explodir" (ter valores muito grandes).

Não é prático quando nosso \(u\) representa a velocidade de junta, por exemplo.

Por esse motivo, trabalhamos com a pseudoinversa amortecida, \(u = A^{\dagger(\epsilon)}b\), com \(\epsilon\) pequeno, mas não nulo.

Temos uma troca (trade-off) entre nosso desejo original para \(u\) e termos uma solução mais razoável (com valores pequenos) para o problema.

Dado uma velocidade alvo na configuração \(q\) e no tempo \(t\), \(\xi_d(q,t)\), e um fator de amortecimento \(\epsilon\), podemos calcular a respectiva velocidade de junta:

Se o robô tiver uma interface para enviar a velocidade de configuração, podemos usar essa equação para fazer controle!

O desafio é justamente escolher \(\xi_d\) que vai nos permitir realizar a tarefa desejada.

No UAIBot usamos a função Utils.dp_inv para calcular a pseudoinversa amortecida:

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
#Calcula a Jacobiana geométrica para a configuração atual
Jg, fk = robot.jac_geo()
#Define a velocidade desejada
xi_d = np.matrix([1,0,0,0,0,0]).reshape((6,1))
#Calcula a velocidade de junta
eps=0.001
qdot = ub.Utils.dp_inv(Jg, eps) * xi_d
                        

O nome vem do fato de em inglês chamarmos a matriz de damped pseudoinverse.

Vamos fazer o efetuador ficar indo para cima e para baixo, sem mudar sua orientação. Para isso, basta escolher algo como \(v_d(t) = (0 \ 0 \ 0{,}2\cos(t))^T\) e \(\omega_d(t) = (0 \ 0 \ 0)^T\).

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
sim = ub.Simulation([robot])
#Parâmetros
dt=0.01
eps=0.001

for i in range(2000): #Simula por 20 segundos
    #Calcula o tempo discreto a partir do tempo contínuo
    t = i*dt
    #Calcula a velocidade desejada
    xi_d = np.matrix([0,0,0.2*np.cos(t),0,0,0]).reshape((6,1))
    #Calcula a Jacobiana geométrica (na configuração atual)
    Jg, fk = robot.jac_geo()
    #Calcula qual deve ser a velocidade de junta
    qdot = ub.Utils.dp_inv(Jg, eps) * xi_d
    #Aqui, simulamos o movimento do robô de acordo com essa velocidade de junta
    #instantânea.
    #Usamos uma integração de Euler de primeira ordem:
    #próxima configuração = configuração atual + velocidade de configuração *dt
    qprox = robot.q + qdot * dt
    robot.add_ani_frame(t, q = qprox)

sim.run()




                        

Em posse da Jacobiana geométrica, podemos mapear velocidades em velocidades de junta.

Isso é muito útil pois frequentemente é mais fácil planejar o controle na velocidade, mas o robô recebe o comando em velocidade de junta. Esse mapeamento resolve o problema.

Na próxima aula veremos que a Jacobiana geométrica será utilizada como ingrediente principal para o cálculo de outras Jacobianas, relativas a um espaço onde planejar as velocidades é ainda mais simples do que no espaço de velocidades.

Nesse espaço, planejaremos nosso controlador.