Travail Pratique #3

ELE8203 - Robotique

Cinématique inverse du manipulateur Kinova Gen3 Lite

Département de génie électrique

Polytechnique Montréal

Automne 2023
Travail à remettre avant le début du TP4

Présentation de votre rapport : il est conseillé mais pas requis d’écrire votre rapport à
l’ordinateur. Si vous l’écrivez à la main, il faudra scanner le document de façon à ce qu’il
reste bien lisible. Dans tous les cas, soignez votre présentation, car des points pourront être
retirés à un document mal présenté (voir le plan de cours). À la limite, un document illisible
où pour lequel la correction demanderait un effort exagéré pourrait se voir attribuer la note
0. Vous devriez tenir compte des conseils généraux suivants.
— Page ou section de présentation : Le nom et le matricule de chacun des membres de
l’équipe (2 étudiants par équipe, sauf exception), le numéro du cours, le numéro de
la section et le nom du laboratoire effectué sont correctement indiqués.
— Qualité des figures et tableaux : La présentation des figures et des tableaux est
claire et soignée. Chaque figure ou tableau est accompagné d’un titre clair et sa
présence est justifiée par une discussion y renvoyant dans le texte du rapport. Les
différents éléments présentés sur une figure ou dans une image doivent être claire-
ment différentiables.
— Qualité des développements mathématiques : Les développements mathématiques sont
clairs et complets, simples à comprendre et à suivre.
— Qualité du texte et des explications : Le rapport est facile à lire et les idées sont
clairement énoncées, dans un français de qualité.
— Utilisation de variables et symboles : La signification des variables ou des symboles
utilisés ainsi que leurs unités respectives sont clairement indiquées dans le rapport.
— Professionnalisme : Le rapport comprend une introduction ainsi qu’une conclusion
résumant les objectifs à atteindre et commentant leur réalisation. Dans l’ensemble le
rapport fait preuve d’une démarche professionnelle.

ELE8203 - Travail Pratique #3 Page de couverture Automne 2023

 Figure
Figure 65: DH
1 – Géométrie parameters
du robot frames de
Gen3 lite [reproduction definitions
la figure 64 du manuel utilisateur], incluant les repères
de Denavit-Hartenberg. Les dimensions indiquées sont en mm. Notez la légère erreur sur le dessin de droite pour la
position des points O5 et O6 , qui devraient se trouver à la hauteur de l’axe de rotation “Actuator 5”.

1 Objectifs
Ce TP met en application les notions introduites dans les chapitres 4 à 6 des notes de cours. Il permet aux
étudiants
— d’établir de manière analytique la cinématique inverse d’une version simplifiée du manipulateur à six degrés
de liberté du TP 2.
— de développer une méthode numérique itérative pour résoudre le problème de cinématique inverse pour le
modèle géométrique exact de ce manipulateur.
N.B. : vous devez écrire votre code essentiellement “à partir de zéro”. L’emploi de libraires de robotique pour ce
TP n’est pas autorisé dans votre code, mais vous pouvez les utiliser à des fins de validation. Vous devez développer
votre code par défaut à l’aide de Matlab.

Table 51: Selected singularity configurations description

ELE8203 - Travail Pratique #3 Page 1 of 5 Automne 2023

2 Structure géométrique du robot Gen3 lite
Le robot Gen3 lite a été introduit dans les sujets de TP précédents, qui contiennent aussi des pointeurs vers
l’information nécessaire pour établir la cinématique directe de ce manipulateur. La cinématique directe différentielle
a aussi été établie. En cas de difficulté avec le TP 2, si vous avez besoin pour ce TP des fonctions de cinématique
directe, vous pouvez utiliser la “Robotics toolbox” de P. Corke, comme expliqué dans le TP 2, avec les paramètres
DH donnés dans le manuel utilisateur.
La figure 1 était déjà incluse dans le TP 2 et montre la géométrie du robot dans sa position de repos (variables
articulaires nulles) avec ses diverses dimensions. Sur cette figure, on notera notamment la position du repère de la
base (de centre O1 sur la figure, O dans notre notation), que l’on dénotera par la suite {s}, et du repère de l’outil ou
effecteur (de centre Otool sur la figure, E dans notre notation), que l’on dénotera par la suite {e}. On rappelle aussi
que les axes z des repères DH indiquent les axes de rotation des différentes articulations et le sens positif de rotation
pour chacune.
N.B. 1 : Pour être consistent, toutes les longueurs, positions, etc. devront être exprimées en mm, et
tous les angles devront être exprimés en radians et ramenés dans l’intervalle (−π, π]. Ceci est valable en
particulier pour les entrées et sorties des fonctions que l’on vous demande de développer. La fonction
wrapangle.m fournie pourra être utilisée pour respecter cette convention.
N.B. 2 : Dans les solutions de cinématique inverse retournées par vos fonctions, certaines pourraient
ne pas vérifier les limites articulaires spécifiées dans le manuel utilisateur. Pour ce TP, vous ne devez
pas tenir compte de ces limites ; il faut retourner toutes les solutions mathématiquement possibles.

(a) Structure géométrique du robot (b) Structure géométrique simplifiée avec dwo = 0.

Figure 2 – Structure géométrique du robot Gen3 lite pour établir sa cinématique inverse.

1. [2 points] Afin d’établir la cinématique inverse permettant de placer l’effecteur dans une pose voulue, justifiez
brièvement que l’on peut considérer que le robot possède la structure géométrique montrée sur la figure 2a. Le
point W correspond à l’intersection des axes de rotation 4 et 5. Identifiez en particulier les valeurs des paramètres
ℓ1 , ℓ2 , ℓ3 , d1 , dwo , dwe sur cette figure, à partir des dimensions du robot données sur la figure 1.

ELE8203 - Travail Pratique #3 Page 2 of 5 Automne 2023

 2. [1 point] Supposons que l’on modifie la structure de la figure 2a en prenant dwo = 0, pour obtenir la structure
de la figure 2b. En quoi cela simplifie-t-il le problème du calcul de cinématique inverse ?
3. [1 point] Dans la relation de cinématique directe (par la méthode utilisant les déplacements) pour le manipulateur
de la figure 2, on peut écrire les produit des 3 dernières rotations R4:6 sous la forme d’un produit de matrices de
rotation élémentaires
R4:6 = Ra,θ4 Rb,θ5 Rc,θ6 ,
avec trois vecteurs unitaires a, b, c. Pour chacun de ces vecteurs, expliquez s’il doit être égal à x, y ou z. À
quelle version des angles d’Euler les variables articulaires (θ4 , θ5 , θ6 ) correspondent-elles ?

3 Calculs de cinématique inverse pour le modèle simplifié

Dans cette section, on cherche à calculer la cinématique inverse du modèle simplifié de la figure 2b.
4. [3 points] Pour les angles d’Euler du type identifié à la question 3, écrivez une fonction que vous nommerez
dcm2eul(R,solnumber), qui prend en entrée une matrice DCM R (donc de taille 3 × 3) ainsi qu’une variable
binaire solnumber, et qui retourne un vecteur angles de taille 1 × 3 (angles(1) correspondra à θ4 , angles(2)
à θ5 et angles(3) à θ6 ). Dans le cas générique, cette fonction doit retourner l’une des deux solutions possibles
(ϕ, θ, ψ) pour les angles d’Euler, selon la valeur de solnumber. Plus précisément, si solnumber=0 la fonction
retournera la solution avec θ ∈ (0, π), si solnumber=1 elle retournera la solution avec θ ∈ (−π, 0). Votre fonction
devra aussi traiter les cas singuliers où sin θ = 0 : dans ce cas elle retournera, parmi l’infinité de solutions
possibles, la solution telle que ϕ = 0 (sans se soucier dans ce cas de solnumber).
N.B. : Il est utile ici de vous reporter à la section 5, qui suggère une stratégie de validation de votre travail.
5. [5 points] Implémentez une fonction ikine_anthroRRR(Wdes,soltype) qui calcule la cinématique inverse pour
les 3 premières articulations RRR du manipulateur de la figure 2. Cette fonction prend en entrée la position
désirée Wdes pour le point W (Wdes est un vecteur de taille 1 x 3 contenant les coordonnées de W dans s) et
retourne un vecteur thetas de taille 1 x 3 contenant les valeurs des variables articulaires θ1 , θ2 , θ3 qui permettent
d’atteindre ce point W . D’autre part, soltype est une chaine de caractère, qui peut être "rd", "ru", "ld" ou
"lu", et qui permet de retourner la solution “right-down”, “right-up”, “left-down” ou “left-up”, correspondant
à la position de l’épaule (droite ou gauche) et du coude (haut ou bas).
Pour concevoir votre fonction, vous vous reporterez aux formules de la section 6.1.2. des notes de cours. Toutefois,
vous ferez attention au fait que les valeurs nulles des angles correspondant à la position de repos ainsi que le
sens positif de la direction des angles peuvent être différents pour le robot de la figure 2 par rapport aux notes
de cours.
N.B. : Tout d’abord, votre fonction doit être capable de retourner les 4 solutions distinctes (pour une configuration
générique), simplement en changeant la valeur de soltype. Ensuite, votre code devrait fournir des réponses
cohérentes avec la nomenclature "rd", "ru", "ld" ou "lu". Pour vous assurer que les solutions retournées
correspondent bien au bon type, vous vous reporterez au fichier test_tp3.m, qui contient un exemple avec les
types attendus.
6. [3 points] Implémentez maintenant une fonction ikine_gen3_simplifie(T,armsoltype,wristsolnumber) qui
calcule calcule la cinématique inverse du bras simplifié de la figure 2b. Cette fonction prend comme argument une
matrice T dans SE(3), spécifiant une pose désirée pour l’effecteur. Elle retourne un vecteur de variables articulaires
de taille 1 x 6, dont les composantes sont, dans l’ordre, les angles θ1 à θ6 . La variable wristsolnumber peut
prendre les valeurs 0 ou 1, afin de fournir les 2 solutions possibles pour les 3 dernières variables articulaires,
en correspondance avec la question 4. La variable armsoltype est une chaine de caractère, qui peut être "rd",
"ru", "ld" ou "lu", et permet de sélectionner une des 4 configurations possibles pour les 3 premières variables
articulaires, en correspondance avec la question 5.
Votre méthode doit donc combiner les fonctions des questions précédentes pour calculer n’importe laquelle des 8
solutions possibles pour le bras de la figure 2b.

4 Un algorithme itératif pour le modèle réel

On retourne maintenant au modèle de la figure 2a avec dwo ̸= 0. Dans ce cas, il n’est à priori pas possible
d’obtenir la cinématique inverse par des formules analytiques exactes. La section 6.2 des notes de cours décrit

ELE8203 - Travail Pratique #3 Page 3 of 5 Automne 2023

dans les grandes lignes un algorithm itératif de Newton pour essayer de trouver numériquement une solution du
problème de cinématique inverse pour une architecture générale de manipulateur, étant données des valeurs initiales
estimées pour chaque valeur articulaire. Cet algorithme fait intervenir toutes les variables articulaires ainsi que la
matrice jacobienne du manipulateur et est généralement délicat à ajuster, notamment au voisinage des singularités
cinématiques. Ici, nous allons essayer une approche plus simple qui prend en compte le fait que le modèle exact ne
dévie que légèrement de celui de la figure 2b.
7. [1 point] Pour le modèle de la figure 2a, avec θ6 l’angle de l’articulation 6, montrez que
W s = E s + dwo (sin θ6 yes − cos θ6 xse ) − dwe zes . (2)
Soit Td une pose désirée, et θ ∗ une solution de la cinématique inverse, c’est-à-dire telle que Tes (θ ∗ ) = Td . Si l’angle
θ6∗de l’articulation 6 était connu, on pourrait calculer les coordonnées du point W par (2), puis en déduire tous les
angles θ comme par la méthode standard de la section précédente. En particulier, le calcul basé sur W s (θ6∗ ) devrait
produire une valeur θ6 égale à la valeur θ6∗ de laquelle on était parti, autrement dit θ6∗ est un point fixe d’un certain
opérateur θ6′ = f (θ6 ) = f2 ◦ f1 (θ6 )
f1 f2
θ6 −→ W s (θ6 ) −→ θ6′ ,
où f1 est simplement (2) et f2 est le calcul de la section précédente qui produit θ ′ , et donc en particulier θ6′ , lorsque
la position W s et l’orientation Res sont spécifiées.
Cette remarque motive la stratégie suivante, qui cherche une solution du problème de cinématique inverse en
itérant uniquement sur θ6 jusqu’à ce que l’on tombe sur un point fixe de f . Elle implémente essentiellement les
(k+1) (k)
itérations θ6 = f (θ6 ).
(0)
1. On part d’une valeur estimée θ6 pour l’angle θ6 , par exemple une valeur obtenue pour l’architecture simplifiée
par la fonction de la section précédente.
(k)
2. A l’itération k, on a une estimée actuelle θ6 de θ6 :
— on calcule les coordonnés du point W (k) par (2).
— on calcule les angles θ (k+1) en partant de W (k) et en appliquant la méthode de la question précédente (qui
utilise aussi Res ).
(k+1) (k)
— Si θ6 est suffisamment proche de θ6 , on s’arrête. Sinon on retourne à l’étape précédente mais en
(k+1)
partant maintenant de la valeur θ6 .
(k+1) (k)
Les itérations θ6 = f (θ6 ) convergent vers une valeur θ6∗ satisfaisant θ6∗ = f (θ6∗ ) sous certaines conditions sur f
(lorsque f est une contraction notamment). On ne vous demande pas de vérifier ici ces conditions, et en fait elles
pourraient ne pas être toujours satisfaites : en d’autres termes, il est possible que la méthode ne converge pas, au moins
(k+1) (k+1)
pour certaines poses Td et certaines des 8 configurations. D’autre part, l’étape de calcul des angles θ1 , θ2 ,
(k+1) (k)
θ3 peut échouer si le point W candidat n’est pas atteignable, donc en pratique obtenir une méthode robuste
(k−1) (k) (k) (k−1)
demanderait de contrôler en plus la taille du pas entre θ6 et θ6 au lieu de toujours prendre θ6 = f (θ6 ).
Notez cependant que les problèmes de convergence sont aussi présents pour la méthode de Newton et qu’ici, il est
de plus possible de forcer la recherche d’une configuration particulière parmi 8 configurations possibles 1 .
8. [3 points] Implémentez une fonction ikine_gen3(T,theta6_guess,armsoltype,wristsolnumber) qui suit la
(0)
stratégie ci-dessus en partant d’une valeur theta6_guess pour θ6 . Les autres variables d’entrée sont les mêmes
que pour la fonction ikine_gen3_simplifie. Comme avant, votre fonction devra produire en sortie un vecteur de
taille 1 x 6 contenant les valeurs θ1 à θ6 des variables articulaires permettant d’atteindre la pose T voulue. Lorsque
votre fonction ne converge pas ou détecte qu’une pose n’est pas atteignable dans une certaine configuration, elle
devra retourner la valeur [NaN,NaN,NaN,NaN,NaN,NaN]. On devra pouvoir sélectionner différentes configurations
(jusqu’à 8) à l’aide des variables armsoltype et wristsolnumber.
N.B. : Une solution parfaite n’est pas nécessairement possible pour cette question 8. La robustesse de votre
solution et sa précision seront des facteurs pris en compte dans la notation. Contrôlez le temps que vous passez
sur cette question, et commentez sur les difficultés et limites que vous aurez pu constater pour votre fonction.
9. [1 point] Validez votre fonction ikine_gen3 à l’aide des paires (configuration, pose) retournées par l’API Kinova
que vous avez relevées lors du TP 2 pour le robot Gen3 lite physique. Commentez sur les difficultés potentielles
rencontrées par votre fonction et comparez les résultats.
1. On a identifié 8 solutions possibles pour le modèle simplifié, mais nous n’avons pas montré que la cinématique inverse du modèle
exact possède encore 8 solutions. Le fait suivant est connu : la cinématique inverse d’un manipulateur 6R général possède au plus 16
solutions.

ELE8203 - Travail Pratique #3 Page 4 of 5 Automne 2023

5 Validation
Si y = f (x) est une fonction de cinématique directe et que l’on cherche à valider une fonction de cinématique
inverse {x1 , . . . , xp } = f −1 (y) qui peut avoir plusieurs solutions x1 , . . . , xp , il est très utile de commencer par
implémenter la fonction f elle-même. On validera ensuite f (f −1 (y)) = y, relation qui est toujours vérifiée lorsque
f −1 (y) a au moins une solution. Pour générer des y valides, de nouveau on pourra partir de vecteurs x et appliquer
f . On devra alors avoir x ∈ f −1 (f (x)).
Concrètement, pour valider vos réponses au fur et à mesure dans les questions précédentes, il sera utile d’im-
plementer une fonction eul2dcm, ainsi que la cinématique directe fkine_anthroRRR du manipulateur RRR (les
équations se trouvent dans les notes de cours avec des conventions d’angles un peu différentes, ou se retrouvent
rapidement à la main). Pour calculer la cinématique directe du Gen3 lite (avec ou sans dwo ) sans vous tromper, vous
pourrez utiliser la Robotics Toolbox pour Matlab, comme expliqué au TP 2.
La toolbox fournit aussi des fonctions de cinématique inverse, notamment ikine6s et ikine, que vous pourrez
tester pour comparer. Il n’y a pas de garantie que cela fonctionne (note : il pourra être plus robuste d’exprimer les
distances en m au lieu de mm avec la toolbox).
Un fichier test_tp3.m est fourni avec quelques tests de base pour démarrer et illustrer le prototype des fonctions.

6 Travail à remettre
Votre rapport est à remettre avant le début du TP4 sur Gradescope. Il doit répondre aux questions ci-dessus, et
brièvement décrire votre approche en ce qui concerne les questions demandant d’écrire du code.
De plus, vous devrez fournir votre code sous la forme d’un répertoire contenant toutes vos fonctions, remis dans
l’espace prévu à cet effet dans Moodle. Ce répertoire devra être compressé sous un format standard (ex : zip, gz,
etc.) et le fichier archive final devra être nommé TP3_ELE8203_nom1_nom2.***, où nom1, nom2 sont les noms de
famille des membres de votre groupe, et l’extension *** du fichier dépend de votre format de compression. Il devra
être possible d’appeler vos fonctions principales dcm2eul, ikine_anthroRRR, ikine_gen3_simplifie, ikine_gen3
directement depuis ce répertoire afin d’exécuter les tests unitaires de correction. Vous devrez donc en particulier
respecter ces noms de fonction et leur prototype.

ELE8203 - Travail Pratique #3 Page 5 of 5 Automne 2023