Controle de força do estudo da orientação do espaço da tarefa para drogas da artrite do lobo para cães

Então você quer usar o controle de força para controlar a orientação do seu efetor, eh? Que esforço nobre. Eu também queria controlar a orientação do efetor. Embora a jornada tenha sido longa e árdua, o código resultante é curto e rápido de implementar. Todo o código para reproduzir os resultados mostrados aqui está no meu github e no repositório de controle do ABR.

Ao descrever a orientação do efetor, há três métodos primários diferentes usados: ângulos de euler, matrizes de rotação e quaternions. Os ângulos euler e denotam roll, pitch e yaw, respectivamente. As matrizes de rotação especificam 3 vetores unitários ortogonais, e eu descrevo em detalhes como calculá-los neste post em transformações para frente.

E os quatérnios são vetores de 4 dimensões, usados ​​para descrever a orientação tridimensional, que fornecem estabilidade e requerem menos memória e computação, mas são mais complicados de entender.

A maioria dos controles robóticos modernos é feita usando quaternions porque eles não têm singularidades, e é direto para convertê-los em outras representações. Curiosidade: as trajetórias de quaternion também interpolam bem, onde os ângulos de euler e as matrizes de rotação não o fazem, então eles são usados ​​em computação gráfica para gerar uma trajetória para um objeto seguir no espaço de orientação.

Embora você não precise de um entendimento completo dos quatérnios para usar o código de controle de orientação, definitivamente ajuda se algo der errado. Se você quer saber mais sobre os quaternions, ou até mesmo se não estiver, mas ainda não viu esse recurso, você deve definitivamente conferir esses vídeos interativos com o auxílio de sanderson e beneberg. Eles fizeram um incrível trabalho desenvolvendo módulos para dar às pessoas uma intuição de como os quaternions funcionam, e eu não posso recomendar seu trabalho o suficiente. Há também uma versão de vídeo não interativa que abrange o mesmo material.

No controle de posição, em que estamos apenas preocupados com a posição da mão, as dimensões de velocidade angular são removidas do jacobiano, de modo que é uma matriz de 3 x n uniões, em vez de uma matriz de 6 x n uniões. Portanto, a orientação do efetor não é controlada de maneira alguma. Quando fizemos isso, não precisávamos nos preocupar ou saber nada sobre a forma das velocidades angulares.

Agora que estamos interessados ​​no controle de orientação, no entanto, precisaremos aprender (e agradecer a Yitao ding por esclarecer este ponto nos comentários sobre a versão original deste post). O jacobiano para a orientação descreve as velocidades de rotação em torno de cada eixo em relação às velocidades de junção. A velocidade de rotação de cada eixo “acontece” ao mesmo tempo. É importante notar que isso não é o mesmo que os ângulos euler, que são aplicados sequencialmente.

É importante notar que os ângulos de euler podem vir em 12 formatos diferentes. Você tem que saber com que tipo de ângulos euler você está lidando (e.G. Gire em torno de X, em seguida Y, depois Z, ou gire em torno de X, Y, X etc) para que tudo isso funcione corretamente. Ele deve estar bem documentado em algum lugar, por exemplo, a página da VREP API nos diz que ele retornará ângulos correspondentes às rotações x, y e z.

Os eixos de rotação podem ser estáticos (rotações extrínsecas) ou rotativos (rotações intrínsecas). OBSERVAÇÃO: a página da VREP diz que eles giram em torno de um quadro absoluto de referência, o que considero estático, mas acredito que seja um erro de digitação na página deles. Se você calcular a orientação do efetor final do UR5 usando matrizes de transformação, e depois convertê-lo para ângulos de euler com eixos = ‘rxyz’, você obterá uma correspondência com os ângulos de euler exibidos, mas não com eixos = ‘sxyz’.

Agora vamos ter que ser capazes de transformar entre ângulos de euler, matrizes de rotação e quaternions. Existem métodos bem estabelecidos para fazer isso, e um monte de pessoas tem codificado as coisas para fazê-lo de forma eficiente. Aqui eu uso o módulo de transformações muito útil de christoph gohlke na universidade da Califórnia. É importante ressaltar que, ao converter para quaterniões, não se esqueça de normalizar os quatérnios em tamanho de unidade.

OBSERVAÇÃO: você terá problemas quando o ângulo for cruzado, onde o braço “percorre o caminho longo”. Para explicar isso, use q_r [1:] * np.Sign (q_r [0]). Isso fará com que você sempre gire ao longo de uma trajetória < 180 graus em direção ao ângulo alvo. A razão pela qual isto surge é porque existem vários quaterniões diferentes que podem representar a mesma orientação.

De brincar com isso brevemente, parece que esse método também funciona. Os autores observam na discussão que ele pode “sofrer no caso de grandes erros de orientação”, mas eu não consegui obter um comportamento inadequado ao brincar com ele no VREP. A outra desvantagem que eles mencionam é que a carga computacional é mais pesada com esse método do que com o feedback de quatérnio.

Aqui, a bola verde é o alvo, e o efetor está sendo controlado para combinar com a orientação da bola. A caixa azul é apenas uma ajuda de visualização para exibir a orientação do efetor. E essa mão está lá apenas de outro projeto que eu estava trabalhando, então esqueci de remover, mas já fiz os vídeos, então aqui estamos nós. Está definido para não afetar a dinâmica, então não se preocupe. O alvo muda a orientação uma vez por segundo. O ganho de orientação para esses ensaios é ko = 200 e kv = np.Sqrt (600).

Se você se lembra do meu artigo sobre jacobianos, havia uma seção sobre a análise do jacobiano. Tudo se resume a dois pontos principais: 1) o jacobiano especifica qual espaço de tarefa (DOF) pode ser controlado. Se houver uma linha de zeros, por exemplo, o espaço de tarefa correspondente DOF (i.E. Não pode ser controlado. 2), a classificação do jacobiano determina quantos DOF ​​podem ser controlados ao mesmo tempo.

Por exemplo, em um braço planar de duas articulações, as variáveis ​​podem ser controladas, mas não podem ser controladas porque suas linhas correspondentes são todas zeros. Assim, 3 variáveis ​​podem potencialmente ser controladas, mas como o jacobiano é o rank 2, apenas duas variáveis ​​podem ser controladas de cada vez. Se você tentar controlar mais de 2 DOF de cada vez, as coisas vão ficar mal. Aqui estão algumas animações de tentar controlar 3 DOF vs 2 DOF em um braço articulado 2:

Em seguida, descascamos o jacobian e o sinal de controle do espaço de tarefas até as linhas relevantes com J = robot_config. (‘EE’, q) [ctrlr_dof] e u_task = (current-target) [ctrlr_dof]. Isso significa que tanto a corrente quanto o alvo devem ser vetores de 6 dimensões, especificando os valores atual e de destino, respectivamente, independentemente de quantas dimensões estamos realmente controlando.

O UR5 tem 6 graus de liberdade, por isso podemos controlar totalmente a posição e a orientação do espaço de tarefa. Para fazer isso, no script acima, apenas ctrl_dof = np.Array ([verdadeiro, verdadeiro, verdadeiro, verdadeiro, verdadeiro, verdadeiro]), e lá vai você! Nas seguintes animações, os valores de ganho usados ​​foram kp = 300, ko = 300 e kv = np.Sqrt (kp + ko) * 1.5. O script completo pode ser encontrado no meu github.

OBSERVAÇÃO: ao implementar esse script de exemplo de código mínimo, me deparei com um problema causado pelo cálculo da matriz de inércia do espaço de tarefas. Acontece que usando np.Linalg.Pinv dá resultados muito diferentes do que np.Linalg.Inv, e eu não percebi isso. Eu vou ter que explorar isso mais tarde, mas basicamente afirma que você quer usar o np.Linalg.Inv o máximo possível. Então você notará no código acima que eu verifico a dimensionalidade do mx_inv e primeiro tento usar o np.Linalg.Inv antes de recorrer ao np.Linalg.Pinv.

NOTA: se você começar a brincar com apenas um ou dois dos ângulos de orientação, deve ter em mente: porque estamos usando eixos rotativos, se você configurar false, false, true, então não parecerá o efetor está se alinhando com o alvo. Isso é porque e não foram definidos primeiro. No entanto, se você gerar um gráfico das orientações do alvo versus as orientações do finalizador, verá que você está em contato direto com a orientação do alvo.

Então é isso! Muitas advertências, notas e muito mais trabalho a ser feito, mas esperamos que este seja um recurso útil para qualquer outro que embarque na mesma jornada. Você pode baixar o código, experimentá-lo e brincar com os ganhos e metas. Deixe-me saber abaixo se você tiver alguma dúvida ou gostou do post, ou quiser compartilhar quaisquer outros recursos no controle de força da orientação do espaço de tarefas.