Já sabemos como descrever o movimento de um corpo rígido separadamente.

Nosso robô é formado pela junção de corpos rígidos (os elos e o efetuador). Mas eles não movem de maneira independente! É o movimento das juntas do robô que proporciona esse movimento.

Relacionar o movimento da junta com o movimento dos elos/efetuador é o objetivo da Cinemática Direta.

Começamos com um conceito/definição importante: configuração.

No contexto de robótica de manipuladores de base fixa, elos rígidos e de cadeia aberta, a configuração do robô em um determinado instante de tempo, \(q\), nada mais é do que um vetor coluna com \(n\) elementos, em que \(n\) é o número de juntas, que tem a rotação (junta rotativa)/ translação (junta linear) da junta naquele instante de tempo.

Qual é o sentido de rotação/translação da junta quando o valor de \(q_i\) cresce e qual é a rotação/translação correspondente ao valor nulo da junta (\(q_i=0\)) é uma questão puramente de convenção.

Isso é comum na física (ao criar um referencial, você define qual é o ponto zero, qual é o sentido positivo do eixo \(x\), etc...).

Normalmente, isso já vem definido no robô, mas pode ser facilmente mudado se lhe for conveniente.

Veja as diversas configurações ao lado, onde é possível determinar o sentido de rotação escolhido (anti-horário) e a rotação nula para as duas juntas.

Note que, em posse da configuração, podemos determinar a posição de qualquer partícula do robô.

A definição geral de configuração na Mecânica é essa: a informação necessária para, naquele instante de tempo, determinar a posição de qualquer partícula de um sistema.

No nosso caso, essa informação é a rotação/translação das juntas rotativas e lineares, respectivamente.

Vamos calcular um primeiro exemplo de cinemática direta. Considere o robô planar ao lado, com duas juntas rotativas.

Os dois elos têm 0,5m.

Calcule \(T^{e}_0(q)\), ou seja, a MTH do referencial que está grudado no efetuador, \(\mathcal{F}_e\) em relação ao referencial \(\mathcal{F}_0\), em função da configuração \(q\).

Considere que o sentido de rotação positivo das duas juntas é o anti-horário. Ainda, que em \(q_1=q_2=0\), o robô está todo esticado para à direita.

Vamos resolver o problema por partes. Primeiro, calcularemos o vetor \(s(q)\), que é o centro do referencial \(\mathcal{F}_e\) escrito no referencial \(\mathcal{F}_0\).

Começamos traçando o vetor \(v_1\) que vai do centro de \(\mathcal{F}_0\), que é o centro da primeira junta, até o centro da segunda junta...

Com uma trigonometria básica, descobrimos que a componente \(x_0\) desse vetor é \(0,5\cos(q_1)\), e a componente \(z_0\) é \(0,5\sin(q_1)\). A componente \(y_0\) é \(0\).

Então: $$v_1(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} 0,5\cos(q_1) \\ 0 \\ 0,5\sin(q_1) \end{array}\Large{\Bigg)}\normalsize{}.$$

Vamos traçar agora o vetor \(v_2\), que vai do centro da segunda junta até o centro de \(\mathcal{F}_e\).

Com um pouco de geometria, descobrimos que o ângulo que esse vetor faz com o eixo \(x_0\) é \(q_1+q_2\).

Novamente, com uma trigonometria básica, descobrimos que a componente \(x_0\) desse vetor é \(0,5\cos(q_1+q_2)\), e a componente \(z_0\) é \(0,5\sin(q_1+q_2)\). A componente \(y_0\) é \(0\).

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

Portanto, o vetor \(s(q)\) é \(v_1(q)+v_2(q)\):

Então: $$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{}.$$

Precisamos agora achar a matriz de rotação \(Q(q)\).

Note, pela imagem ao lado, que, desconsiderando a translação (já resolvida quando calculamos \(s(q)\)), o referencial \(\mathcal{F}_e\) é o referencial \(\mathcal{F}_0\) rodado de \(q_1+q_2\) no eixo \(y_0\) no sentido horário! Como definimos a rotação positiva como sendo no sentido no sentido anti-horário, então será uma rotação de \(-(q_1+q_2\)) em \(y_0\).

Portanto:

$$ 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{}$$

em que usamos o fato que \(\cos(-\theta)=\cos(\theta)\) e \(\sin(-\theta)=-\sin(\theta)\).

Finalmente, usando as notações \(c_1=\cos(q_1)\), \(s_1=\sin(q_1)\), \(c_{12}=\cos(q_1+q_2)\) e \(s_{12}=\sin(q_1+q_2)\):

em que \(q=(q_1 \ q_2)^T\).

O próximo exemplo é similar ao primeiro, mas temos uma junta linear antes do efetuador.

Duas juntas rotativas e uma linear.

A terceira junta tem como sentido positivo o sentido apontado por \(x_e\). O valor \(q_3=0\) corresponde a um elo de \(0,25m\) (ver imagem ao lado).

A solução é muito parecida com a anterior.

A diferença é o vetor \(v_2\). No Exemplo 1, usamos o valor do tamanho do elo como \(0,5m\). Agora usaremos \(0,25+q_3\). O vetor ficará então:

Então: $$v_2(q) = \Large{\Bigg(}\normalsize{}\begin{array}{c} (0,25+q_3)\cos(q_1+q_2) \\ 0 \\ (0,25+q_3)\sin(q_1+q_2) \end{array}\Large{\Bigg)}\normalsize{}.$$

A MTH final será:

$$ H_0^e(q) = \LARGE{\Bigg(}\normalsize{}\begin{array}{cccc} c_{12} & 0 & -s_{12} & 0,5c_1+(0,25+q_3)c_{12} \\ 0 & 1 & 0 & 0 \\ s_{12} & 0 & c_{12} & 0,5s_1+(0,25+q_3)s_{12} \\ 0 & 0 & 0 & 1 \end{array}\LARGE{\Bigg)}\normalsize{}$$

em que \(q=(q_1 \ q_2 \ q_3)^T\).

Note que, nos dois exemplos anteriores, há combinações de configuração \(q\) que seriam inválidas.

Por exemplo, em uma determinada configuração pode haver a colisão entre elos.

Ou mesmo, no caso da junta linear do Exemplo 2, há claramente limites \(q_{3,min} \leq q_3 \leq q_{3,max}\), pois a junta não pode se estender indefinidamente para um lado ou para o outro.

Juntas rotativas também podem ter limites de fábrica, criados para evitar colisões.

De fato, para cada robô existe um subconjunto de valores \(q\) que são proibidos, devido a esses fatores.

Nesta disciplina, vamos essencialmente ignorar isso. Vamos assumir que nossos controladores não guiam a configuração para esses valores proibidos.

Isso é razoável, na verdade, pois os robôs são construídos para minimizar esse problema e as tarefas que tratamos no ambiente industrial não são extremamente complexas.

Há técnicas para considerar explícitamente esse problema no controlador, mas fogem do escopo desta disciplina.

Para robôs planares, onde não é necessário preocupar com movimentos 3D, podemos usar técnicas de geometria analítica para calcular a cinemática direta para o efetuador.

Mas para robôs gerais isso não é muito prático, e a análise ficaria muito ad-hoc .

Para uniformizar o processo de se calcular a cinemática direta, foi criada a convenção de Denavit-Hartenberg (DH).

A ideia é simples: a convenção de DH diz como você deve grudar referenciais em cada um dos corpos rígidos do robô.

Com essa convenção, há uma maneira sistemática e simples de ir calculando a MTH de um referencial até o próximo, até que tenhamos a MTH do referencial \(\mathcal{F}_0\) para o referencial do efetuador, \(\mathcal{F}_e\).

Para um robô com \(n\) juntas, a convenção de DH cria \(n+1\) referenciais.

Vamos dar o nome \(\mathcal{F}_{DHi}\) para esses referenciais, \(i=0,1,...,n\).

Há uma "receita de bolo" para criar esses referenciais. Faça o seguinte para \(i=0,...,n\), em que as juntas são numeradas de \(1\) até \(n\)

O referencial \(\mathcal{F}_{DHi}\) está grudado no i-ésimo elo, sendo o elo 0 a base do robô (imóvel).

Há alguns casos em que a regra é mal-definida, e o engenheiro tem a liberdade de escolher:

• Para \(i=0\), não existe junta₀. Então, para \(i=0\), simplesmente escolhemos o centro arbitrariamente na linha₁, e o eixo \(x_{DH0}\) arbitrariamente, contanto que seja ortogonal ao eixo \(z_{DH0}\).
• Se as retas linha_i e linha_i+1 são paralelas, existem infinitas escolhas possíveis para o par de pontos ponto_i,i+1 e ponto_i+1,i. Escolha um desses pares arbitrariamente.
• Se ponto_i,i+1 =ponto_i+1,i, a regra de escolha de \(x_{DHi}\) é mal-definida. Escolha qualquer um dos dois vetores que são mutuamente ortogonais às retas linha_i e linha_i+1.
• Se \(i=n\), não existe junta_n+1. Nesse caso, escolhemos a reta linha_n+1 arbitrariamente. Geralmente usamos a reta anterior, linha_n.

Após grudarmos \(n+1\) referenciais no robô (um deles na base fixa), há uma maneira sistemática de calcular a MTH \(H_{DH0}^{DHn}(q)\), que é o último referencial DH (\(i=n\)) escrito no primeiro referencial DH (\(i=0\)).

Dada as duas interpretações para uma MTH, também podemos interpretar \(H_{DH0}^{DHn}(q)\) como a transformação rígida que temos que fazer do primeiro referencial ao último.

De toda forma, para isso temos que extrair os Parâmetros de Denavit-Hartenberg do robô, que são \(4n\) números que nos permitem saber como cada um dos referenciais está em um determinado instante de tempo.

Há 4 parâmetros para transformar do referencial \(\mathcal{F}_{DH(i-1)}\) para o próximo, o referencial \(\mathcal{F}_{DHi}\). Isso para \(i=1,...,n\), totalizando \(4n\) parâmetros.

São os 4 parâmetros:

A maneira como os eixos DH são grudados garantem a existência desses parâmetros, ou seja, que existem números \(\theta_i,d_i,\alpha_i,a_i\) em que essas transformações parametrizadas por ele transformam um referencial no próximo.

Todos os três parâmetros restantes de um referencial para o outro são constantes estruturais do robô, decididas no momento de sua concepção.

Note que as transformações sempre usam o referencial atual, não o o referencial inicial...

Note que nos dois exemplos anteriores, o robô era o mesmo, mas em configurações diferentes.

Os parâmetros estruturais (em branco) eram os mesmos, mas os variáveis (em amarelo) mudaram.

No caso, como todas as juntas eram rotativas, todos os \(\theta_i\) são variáveis, em função da configuração \(q_i\).

Em posse dos parâmetros, é muito fácil calcular a transformação do referencial \(\mathcal{F}_{DH(i-1)}\) para o \(\mathcal{F}_{DHi}\), para \(i=1,2,...,n\).

A transformação é a seguir: $$H_{DH(i-1)}^{DHi}(q_i) = R_z(\theta_i)D_z(d_i)R_x(\alpha_i)D_x(a_i).$$

Note que a transformação deve ser lida da esquerda para à direita, pois usamos o referencial atual, e não o inicial, para interpretar as transformações!

Então primeiro é a rotação em \(z\), depois o deslocamento em \(z\), depois a rotação em \(x\), e finalmente o deslocamento em \(x\).

A MTH \(H_{DH(i-1)}^{DHi}(q_i)\) pode ser interpretada ou como a transformação de \(\mathcal{F}_{DH(i-1)}\) para \(\mathcal{F}_{DHi}\) interpretada em \(\mathcal{F}_{DH(i-1)}\) (intepretação como transformação rígida) ou como o referencial \(\mathcal{F}_{DHi}\) escrito em \(\mathcal{F}_{DH(i-1)}\) (interpretação como referencial).

Note que \(H_{DH(i-1)}^{DHi}(q_i)\) depende de \(q_i\), pois ou \(\theta_i\) (junta rotativa) ou \(d_i\) (junta linear) depende de \(q_i\).

A MTH \(H_{DH0}^{DHn}(q)\) entre o primeiro referencial (fixo) e o que está grudado no último elo (efetuador) é: $$H_{DH0}^{DHn}(q) = H_{DH0}^{DH1}(q_1)H_{DH1}^{DH2}(q_2)...H_{DH(n-1)}^{DHn}(q_n).$$

Depende de toda a configuração \(q\).

Nem sempre o primeiro referencial criada pela DH, \(\mathcal{F}_{DH0}\), é o referencial que você queria que fosse "o" referencial \(\mathcal{F}_0\).

Da mesma forma, nem sempre o último referencial da DH, \(\mathcal{F}_{DHn}\), é o referencial que você gostaria que fosse o do efetuador \(\mathcal{F}_e\).

Felizmente, nos dois casos vão existir transformações constantes (independente de \(q\)), \(H_{0}^{DH0}\) e \(H_{DHn}^{e}\), respectivamente, que nos permitem fazer esse ajuste.

A convenção de DH, embora não tenha resolvido o problema completamente nesses casos, resolveu a parte mais complexa do problema, que é achar a parte da transformação que é variável, dependendo de \(q\).

Com essas duas matrizes de ajuste, temos: $$H_{0}^e(q) = H_0^{DH0} H_{DH0}^{DHn}(q) H_{DHn}^{e}.$$

Os parâmetros de DH podem ser obtidos medindo o robô diretamente, ou em seu manual.

Infelizmente, há outras convenções além da DH. Assim, algumas vezes o manual fornece os parâmetros para outra convenção.

De toda forma, é a definição mais usada.

No problema de cinemática inversa temos uma MTH desejada para o efetuador, \(\hat{H}_{0}^{e}\) e queremos encontrar \(q\) tal que \(H_0^{e}(q) =\hat{H}_{0}^{e}\).

Esse problema pode ter uma, nenhuma ou múltiplas soluções.

Temos que resolver um complexo sistema de equações não-lineares.

Para exemplificar essa complexidade, vamos resolver um problema que é, aparentemente, bem simples.

Considere o robô planar com duas juntas rotativas ao lado. Queremos que a posição do efetuador seja uma posição desejada \(x=x_a\) e \(z=z_a\).

Não nos preocupamos com a orientação. É um problema de cinemática inversa "parcial".

Mesmo assim, veremos que não é simples.

Usando a notação de \(c_1,s_1,c_{12}\) e \(s_{12}\) definida anteriormente, temos as igualdades:

$$L_1c_1 + L_2c_{12} = x_a,$$

$$L_1s_1 + L_2s_{12} = z_a$$

obtidas pela cinemática direta (ver Exemplo 1 de Cinemática Direta).

Eleve ao quadrado as duas igualdades e some.

Use o fato que \(c_1^2+s_1^2 = c_{12}^2+s_{12}^2 = 1\), e que \(c_1c_{12} + s_1s_{12} = \cos(q_2)\):

$$L_1^2+L_2^2 + 2L_1L_2\cos(q_2) = x_a^2+z_a^2.$$

E portanto:

$$\cos(q_2) = \frac{x_a^2+z_a^2-(L_1^2+L_2^2)}{2L_1L_2}.$$

Para a equação

$$\cos(q_2) = \frac{x_a^2+z_a^2-(L_1^2+L_2^2)}{2L_1L_2}$$

ter solução, temos que o lado direito deve estar entre -1 e 1. Reescrevendo essa condição, temos:

$$(L_1-L_2)^2 \leq x_a^2+z_a^2 \leq (L_1+L_2)^2$$

Reflita um pouco e perceberá que essa condição faz todo sentido, geometricamente.

Se a condição for satisfeita, temos duas soluções:

$$q_2 = \pm \cos^{-1}\left(\frac{x_a^2+z_a^2-(L_1^2+L_2^2)}{2L_1L_2}\right).$$

Vamos assumir de agora em diante que escolhemos uma dessas duas soluções. Substituindo no sistema original e usando \(c_{12} = c_1c_2-s_1s_2\) e \(s_{12} = c_1s_2+c_2s_1\) (em que \(c_2=\cos(q_2)\) e \(s_2=\sin(q_2)\)):

$$(L_1+L_2c_2)c_1 - L_2s_2s_1 = x_a,$$

$$L_2s_2 c_1 + (L_1+L_2c_2)s_1 = z_a$$

Temos então um sistema linear com duas equações e incógnitas para as variáveis \(s_1\) e \(c_1\):

$$\left(\begin{array}{cc} L_1+L_2c_2 & -L_2s_2 \\ L_2s_2 & L_1+L_2c_2\end{array}\right)\left(\begin{array}{c} c_1 \\ s_1 \end{array}\right) = \left(\begin{array}{c} x_a \\ z_a \end{array}\right).$$

Isolando:

$$c_1 = U = \frac{(L_1+L_2c_2)x_a + L_2s_2z_a}{G}$$

$$s_1 = V = \frac{-L_2s_2x_a + (L_1+L_2c_2)z_a}{G}$$

em que \(G = L_1^2+L_2^2 + 2L_1L_2c_2\).

Usando a expressão para \(c_2\) que temos, é fácil ver que \(G=x_a^2+z_a^2\). Portanto, se assumirmos que o ponto alvo não está na origem, temos que \(G \not=0\).

Para que um sistema de equações da forma \(\cos(q_1)=U\) e \(\sin(q_1)=V\) tenha solução, é necessário e suficiente que \(U^2+V^2=1\). De fato, podemos mostrar que isso é verdade nesse caso.

Nesse caso, a solução para \(q_1\) é única. Geralmente, em sistemas computacionais, a função atan2(V,U) dá esse valor.

Temos o código em Python para calcular a cinemática inversa:

import numpy as np

def find_q1(L1,L2,xa,za,q2):
  G =  xa**2+za**2
  c2 = np.cos(q2)
  s2 = np.sin(q2)
  U = ( (L1+L2*c2)*xa + L2*s2*za )/G
  V = ( -L2*s2*xa + (L1+L2*c2)*za )/G
  return np.arctan2(V,U)

def solve_ik(L1,L2,xa,za):
  G =  xa**2+za**2
  if G >= (L1+L2)**2 or G <= (L1-L2)**2:
    raise Exception("Não tem solução!")
  else:

    q2_A = np.arccos( (G - (L1**2+L2**2))/(2*L1*L2) )
    q2_B = -q2_A

    return [find_q1(L1,L2,xa,za,q2_A), q2_A], [find_q1(L1,L2,xa,za,q2_B), q2_B]


#Testa:
L1 = 1
L2 = 1.5
xa = 0.8
za = 0.5

q_A, q_B = solve_ik(L1,L2,xa,za)

print(q_A)
print(q_B)
                        

Considerando \(G \not=0\), quando temos solução, temos duas soluções (ver ao lado).

A exceção é quando \(q_2=0\), em que só temos uma solução.

O caso em que \(G=0\) pode ser tratado separadamente.

Resolver o problema de cinemática inversa para um robô tridimensional com vários graus de liberdade de maneira análitica, como fizemos aqui, é muitas vezes inviável

Há pesquisas dedicadas somente ao problema de cinemática inversa analítica para robôs específicos.

Entretanto, os métodos numéricos são mais práticos para robôs gerais.

A técnica de controle cinemático que veremos em breve pode ser fácilmente adapdatada para esse propósito.

No UAIBot, podemos acessar a configuração atual do robô como robot.q, em que robot é o nome da variável do robô:

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
#Mostra a configuração atual
print(robot.q)
#Mostra a configuração "base"
print(robot.q0)
                        

Ambos são vetores numpy com shape (n,1) (n linhas e 1 colunas).

Podemos especificar qual é a configuração no instante de tempo \(t\) com .add_ani_frame.

Quando ela é chamada, mudamos a configuração atual do robô (robot.q).

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
sim = ub.Simulation([robot])

#Vamos fazer a primeira junta variar de 0 até 2*pi, enquanto as outras continuam em
# zero. O tempo irá variar de 0,01 em 0,01 segundos, até 10 segundos.
dt=0.01
tmax=10
for i in range(round(tmax/dt)):
    t = i*dt
    robot.add_ani_frame(time = t, q = [ (2*np.pi)*t/tmax, 0, 0, 0, 0, 0])

#Vamos ver o resultado
sim.run()
                        

A cinemática direta pode ser calculada com a função .fkm (de Forward KineMatics).

Todas as MTHs são calculadas com relação ao referencial do cenário de simulação:

import numpy as np
import uaibot as ub

#Apenas configura como as matrizes serão mostradas
np.set_printoptions(precision=4, suppress=True)

robot = ub.Robot.create_kuka_kr5()

#Sem parâmetros, calcula para a configuração atual (robot.q) e o efetuador
print(robot.fkm())

#Podemos especificar a configuração
print(robot.fkm(q=[0,1,2,3,4,5]))

#Podemos escolher se queremos calcular a MTH para o efetuador (padrão) ou se
#para os referenciais de DH. Nesse último caso, temos uma lista com n matrizes
#(para o primeiro referencial, FDH0, não é calculado, pois ele é fixo)

#Calcula para o efetuador
print(robot.fkm(q=robot.q, axis='eef'))
#Calcula para os referenciais de DH
htm_dh = robot.fkm(q=[0,1,2,3,4,5], axis='dh')
for i in range(len(htm_dh)):
    print(htm_dh[i])
                        

No UAIBot podemos acessar os parâmetros DH do robô:

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()

#As juntas vão de i=0 até i=n-1
for i in range(len(robot.links)):
    #Tipo: 0 (rotativa), 1 (linear)
    print("Tipo"+str(i+1)+": "+str(robot.links[i].joint_type))
    print("theta"+str(i+1)+": "+str(robot.links[i].theta)+" rad")
    print("d"+str(i+1)+": "+str(robot.links[i].d)+" m")
    print("alpha"+str(i+1)+": "+str(robot.links[i].alpha)+" rad")
    print("a"+str(i+1)+": "+str(robot.links[i].a)+" m")
    print("\n")


                        

A cinemática inversa pode ser calculada com a função .ikm (de Inverse KineMatics):

import numpy as np
import uaibot as ub

robot = ub.Robot.create_kuka_kr5()
sim = ub.Simulation([robot])

#Vamos criar uma mth alvo para o efetuador que é igual à inicial, mas deslocada
#um pouco para baixo
target =  ub.Utils.trn([0,0,-0.3]) * robot.fkm()

#Vamos descobrir qual é a configuração que dá essa mth para o efetuador:
q_ikm = robot.ikm(htm_target = target)

#Vamos fazer uma animação suave do robô indo da configuração inicial até
#a configuração alvo
dt=0.01
tmax = 10
for i in range(round(tmax/dt)):
    t = i*dt
    q_c = (1-t/tmax) * robot.q0 + (t/tmax) * q_ikm
    robot.add_ani_frame(time=t, q=q_c)

sim.run()