See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/337955144

Modélisation et simulation cinématique du robot Stäubli TX2 -90 sur Matlab

avec Robotics Toolbox

Article · December 2019

CITATIONS READS

0 300

2 authors:

Vincent Bagaïni Loic Baum

École de Technologie Supérieure École de Technologie Supérieure
1 PUBLICATION   0 CITATIONS    1 PUBLICATION   0 CITATIONS   

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Cinematic study of the TX2-90 robot View project

All content following this page was uploaded by Vincent Bagaïni on 17 December 2019.

The user has requested enhancement of the downloaded file.

 1

Modélisation et simulation cinématique du robot StäubliⓇ TX2 – 90

sur Matlab avec Robotics Toolbox

Vincent BAGAÏNI 1, Loïc BAUM 1

1
Ecole de Technologie Supérieure, Département Génie Électrique, Canada

Résumé — Cet article propose une modélisation et une simulation cinématique du robot industriel
6 axes StäubliⓇ TX2 – 90 entièrement réalisées sous MatlabⓇ avec Robotics Toolbox. Le but est
d’évaluer la performance du toolbox et d’expliquer son utilisation à travers une étude simple sur
un robot complexe. La dynamique du robot n’est pas traitée. Cet article vise également à expliquer
les principes fondamentaux de la robotique. Finalement, les fonctionnalités développées avec l’aide
du toolbox ont été intégrées à une IHM.

Mots clés — Robotique, MatlabⓇ, modélisation, simulation, cinématique, génération de trajectoires [A]

I. INTRODUCTION II. MODÉLISATION DU ROBOT

D’ici 2020, le nombre de robots industriels dans le La première étape de la modélisation d’un robot consiste
monde est estimé à plus de 3 millions [1]. Dans un contexte où à placer les repères de chaque articulation du robot. Pour ce
la robotique est désormais essentielle à l’industrie (quel que soit faire, il faut placer les repères de la manière suivante :
le domaine d’application), il est important d’identifier et de
comprendre les concepts de base. Les robots les plus utilisés • L’axe Z doit être placé de sorte à être l’axe de
dans la robotique industrielle sont des robots polaires à 6 axes, rotation/translation de l’articulation selon sa nature
on parle alors de robots à 6 degrés de liberté (DDL). Grâce à rotative ou prismatique.
leurs 6 liaisons rotatives, ces robots peuvent déplacer et orienter • L’axe X doit être placé de sorte à faciliter le passage
l’effecteur (terminaison du bras) dans toutes les directions de d’un repère à l’autre, car pour ce faire on ne peut que
l’espace. Ils présentent une large enveloppe de travail et réaliser des rotations et translations en fonction des
peuvent atteindre des positions dans l’espace selon plusieurs axes X et Z.
configurations alternatives.
Voici ci-contre une représentation 3D du robot avec le
Pour qualifier les performances d’un robot, diverses
placement des différents repères des articulations :
études sont conduites pour traiter tous les aspects. Parmi ces
aspects, on distingue : la cinématique (étude des mouvements
sans prendre en compte leurs causes), les forces statiques
(forces nécessaires pour réaliser le mouvement), les forces
dynamiques (forces de l’environnement se répercutant sur le
système), la commande (génération de trajectoire par exemple)
ainsi que la stabilité du système. X5
Notre but ici est de traiter l’aspect cinématique d’un
robot industriel 6 DDL. Nous avons choisi un robot très utilisé
mais peu représenté dans la littérature de la robotique : le robot
StaübliⓇ TX2 – 90. Pour simuler ce robot, nous utiliserons le
logiciel MatlabⓇ ainsi que l’extension Robotics toolbox de Peter
Corke [2]. Cet article vise à expliquer notre utilisation du
toolbox lors de la simulation 3D du robot. Tout d’abord, nous
décrirons les articulations du robot pour modéliser le robot sous
forme d’une matrice. Puis, nous étudierons les fondements de
la cinématique directe et inverse. Enfin, nous analyserons la
génération de trajectoires et développerons une Interface
Homme Machine (IHM) pour simuler et afficher notre robot
selon diverses configurations.

Figure 1 - Représentation des repères liés à chaque articulation

Tous droits réservés, la reproduction de ce document sans l’autorisation écrite des auteurs est interdite.
 2

A noter que sur la Figure 1, les repères {1} et {2} ne Une fois la matrice de paramètres MDH connue, on
sont pas confondus. En effet, il y a un offset au niveau de peut modéliser le robot avec le logiciel Matlab Ⓡ et le Robotics
« l’épaule » du robot (voir partie de droite de la Figure 2). Toolbox. Pour cela, plusieurs fonctions sont utilisées. La
Cependant, les origines des repères {4}, {5} et {6} sont première étape consiste à définir chaque articulation du robot.
confondues. Pour définir des articulations rotatives, on utilise la fonction
RevoluteMDH qui prend pour arguments les différents
La prochaine étape consiste à déterminer les différents paramètres MDH du robot. Les limites physiques du robot
paramètres de chaque articulation du robot selon la convention (angles minimum et maximum indiqués dans la documentation
Denavit-Hartenberg modifiée : constructeur) peuvent être spécifiées avec l’argument'qlim'.
La fonction renvoie un objet de type Link. En utilisant cette
• 𝜶𝒊−𝟏 : Angle de la rotation autour de l’axe 𝑋, pour que fonction on peut donc construire un tableau de Link qui
l’axe 𝑍𝑖−1 soit parallèle à l’axe 𝑍𝑖 comprend toutes les articulations du robot :
• 𝒂𝒊−𝟏 : Distance de la translation sur l’axe 𝑋, pour
ramener l’axe 𝑍𝑖−1 à l’axe 𝑍𝑖 L(1) = RevoluteMDH('d', 0,'a', 0,'alpha', 0)
L(2) = RevoluteMDH('d', 0,'a', 50,'alpha', -pi/2)
• 𝜽𝒊 : Angle de la rotation autour de l’axe 𝑍, pour que L(3) = RevoluteMDH('d', 50,'a', 425,'alpha', 0)
l’axe 𝑋𝑖−1 soit parallèle à l’axe 𝑋𝑖 L(4) = RevoluteMDH('d', 425,'a', 0,'alpha', pi/2)
• 𝒅𝒊−𝟏 : Distance de la translation sur l’axe 𝑍, pour L(5) = RevoluteMDH('d', 0,'a', 0,'alpha', -pi/2)
L(6) = RevoluteMDH('d', 0 ,'a', 0,'alpha', pi/2)
ramener l’axe 𝑋𝑖−1 à l’axe 𝑋𝑖
Le robot est ensuite construit en utilisant la fonction
Pour déterminer les valeurs de chaque paramètre, on
SerialLink, qui prend en argument le tableau d’objets Link et
s’aide de la documentation fournie par le constructeur. Les
renvoie une classe de type SerialLink qui constituera notre
dimensions du robot TX2-90 sont montrées dans la Figure 2.
robot.
Les dimensions importantes sont entourées en rouge. Une fois
tous les paramètres connus, on établit la matrice de paramètres Robot = SerialLink(L)
Modified Denavit Hartenberg (MDH) du robot. (Tableau 1).
On peut maintenant utiliser plusieurs fonctions de la
classe Robot, notamment la fonction suivante :

Robot.tool = transl(0,0,100)

Cette fonction permet de définir la position de

l’effecteur du robot par rapport à la dernière articulation. Dans
notre cas, on souhaite que l’effecteur du robot soit situé à
l’extrémité de la dernière articulation. On effectue donc une
translation de 100mm en fonction de l’axe Z. (voir Figure 2)

Enfin, on peut afficher le squelette (stick figure) du

robot sur un repère 3D en spécifiant une configuration d’angle
pour chaque articulation. Pour cela, on utilise la commande ci-
dessous avec 𝑞 le vecteur de configuration d’angles 1 × 6 :

Robot.plot(q)

Figure 2 - Cotes du robot [B]

𝜽𝒊 𝒅𝒊 𝒂𝒊−𝟏 𝜶𝒊−𝟏
Link 1 𝜃1 0 0 0
Link 2 𝜃2 0 50 -90°
Link 3 𝜃3 50 425 0
Link 4 𝜃4 425 0 90°
Link 5 𝜃5 0 0 -90°
Link 6 𝜃6 0 0 90°

Tableau 1 – Matrice de paramètres du robot Figure 3 – Squelette du robot selon la configuration « zero angle »
 3

Pour la configuration « zero angle » les 6 angles sont T = Robot.fkine(q)

nuls. On constate cependant que sur la Figure 3 le bras
robotique n’est pas « droit ». Son effecteur n’est pas aligné
verticalement avec la base. Il y a des rotations de +90° au
niveau des articulations 2 et 3. Lors du placement des repères
on a effectué des rotations sur l’axe Z de 𝜃 + 90° pour
l’articulation 2 et 3. On ne les a pas spécifiées dans la matrice
de paramètres car elles n’impactent en rien l’étude cinématique
du robot. Ces rotations de +90° sont visibles seulement lorsque
l’on affiche le squelette avec MatlabⓇ.
III. CINÉMATIQUE DIRECTE
Le but de cette partie est d’exprimer la position de
l’effecteur par rapport à la base du robot en fonction des angles
de chaque articulation.
𝑇 est une matrice de transformation homogène 4 × 4
Remarque : dans la version 10.4 du toolbox, 𝑇 est considéré
comme un objet de type SE3 (de type matrice de
transformation homogène) et non pas de type matrice
classique MatlabⓇ
𝑅𝑜𝑏𝑜𝑡 est un objet de type robot (SerialLink) défini par un
vecteur d’objet de type articulation (Link)
𝑞 est un vecteur de configuration d’angles 1 × 𝑁 avec 𝑁 le
nombre d’articulations
Voici donc la ligne de commande est le résultat pour
une configuration avec tous les angles à 0°

Angles des Position de l’effecteur

articulations dans l’espace cartésien

Avec notre matrice de paramètres définie dans la

partie précédente, nous pouvons calculer les matrices de
transformations homogènes de chaque articulation. Une matrice
de transformation homogène (de dimension 4 × 4) permet de
mettre en équation les translations et rotations à effectuer entre
le repère {𝑖 − 1} et le repère {𝑖} (selon la convention MDH).
Voici sa forme générale en fonction des paramètres Le principal avantage de ce toolbox est la facilité
∝𝑖−1 , 𝑎𝑖−1 , 𝑑𝑖 , 𝜃𝑖 : d’utilisation et la rapidité d’exécution de la commande une fois
que le robot est correctement défini. Cependant, il est
𝑐𝜃𝑖 −𝑠𝜃𝑖 0 𝑎𝑖−1 impossible d’obtenir une expression symbolique exploitable de
la matrice (en fonction de 𝑞1, 𝑞2, 𝑞3, 𝑞4, 𝑞5 𝑒𝑡 𝑞6). De plus, la
𝑠𝜃𝑖 𝑐∝𝑖−1 𝑐𝜃𝑖 𝑐∝𝑖−1 −𝑠∝𝑖−1 −𝑠∝𝑖−1 𝑑𝑖
𝑇𝑖𝑖−1 = transformation est toujours effectuée de la base à l’effecteur du
𝑠𝜃𝑖 𝑠∝𝑖−1 𝑐𝜃𝑖 𝑠∝𝑖−1 𝑐∝𝑖−1 𝑐∝𝑖−1 𝑑𝑖 robot. On ne peut pas choisir de calculer la matrice entre des
[ 0 0 0 1 ] repères intermédiaires (par exemple 𝑇42 ).

Les expressions cos (𝜃) et sin (𝜃) sont notées
relativement 𝑐𝜃 et 𝑠𝜃. La sous matrice composée des éléments
allant de 𝑥1,1 à 𝑥3,3 est appelée matrice de rotation. Le vecteur
composé des éléments allant de 𝑥1,4 à 𝑥3,4 est appelé vecteur de
translation. Une fois que les matrices relatives à chaque
articulation sont calculées, il suffit de les multiplier entre elles
(dans l’ordre des articulations) pour obtenir la matrice de
transformation homogène finale exprimant la relation entre la
base {0} et l’effecteur {6} du robot.
𝑇60 = 𝑇10 𝑇21 𝑇32 𝑇43 𝑇54 𝑇65
La matrice obtenue est exprimée sous la forme
suivante (les coefficients sont donnés en annexes) :
𝑅11 𝑅12 𝑅13 𝑃𝑥
𝑅21 𝑅22 𝑅23 𝑃𝑦
𝑇60 =[ ]
𝑅31 𝑅32 𝑅33 𝑃𝑧
0 0 0 1
Nous avons calculé cette matrice complexe grâce au
calcul matriciel de MatlabⓇ. Le toolbox de Peter Corke permet
également de calculer cette matrice très rapidement une fois que
le robot est défini. Pour cela, on utilise la fonction :
IV. CINÉMATIQUE INVERSE
Contrairement à la cinématique directe, la cinématique
inverse consiste à déterminer les angles de chaque articulation
pour une position et orientation donnée de l’effecteur. Une
position unique de l’effecteur peut être atteinte selon plusieurs
configurations d’angles.
Position de l’effecteur Angles des
dans l’espace cartésien articulations
La cinématique inverse d’un robot consiste à établir
des systèmes d’équations algébriques ou trigonométriques et les
résoudre pour obtenir les angles des articulations en fonction
des éléments de la matrice de transformation homogène qui
caractérise la position et l’orientation de l’effecteur.
Le Robotics Toolbox propose la fonction ikine6s qui
permet de calculer les différentes solutions analytiques pour la
cinématique inverse d’un robot 6 axes. L’utilisation de cette
fonction semble correspondre à notre robot. Son exécution est
rapide et elle permet d’obtenir les 8 solutions possibles en
spécifiant les configurations suivantes : droitier / gaucher,
coude orienté vers le haut / bas et poignet retourné ou non.

Cependant, nous ne pouvons pas l’utiliser car la
fonction n’est compatible qu’avec des robots définis en
convention DH standard, ce qui n’est pas notre cas.
Une autre fonction proposée par le toolbox est la
fonction ikine. Cette fonction permet de résoudre la
cinématique inverse d’un robot sans conditions sur le nombre
de degrés de liberté et sur la convention utilisée pour la
déclaration du robot.
Elle diffère de la fonction ikine6s car la solution n’est
pas trouvée analytiquement mais numériquement. En effet,
l’algorithme de la fonction consiste à tester tous les angles
possibles des articulations du robot pour trouver la solution à la
cinématique inverse de manière itérative. Voici la fonction :
q = Robot.ikine(T)
Voici les lignes de commandes utilisées pour calculer
la cinématique inverse d’une position et orientation 𝑇𝑖𝑛𝑣𝑒𝑟𝑠𝑒 du
robot. Les valeurs des angles q sont affichées en radians.
Pour vérifier la validité de la solution, on calcule la
cinématique directe du robot à partir des angles donnés par q :
On obtient bien que 𝑇𝑓𝑜𝑟𝑤𝑎𝑟𝑑 = 𝑇𝑖𝑛𝑣𝑒𝑟𝑠𝑒 et que la
solution trouvée pour la cinématique inverse est correcte. En
revanche, il faut garder en tête que les solutions ne sont pas
uniques et qu’il en existe probablement d’autres. Pour trouver
les autres solutions de cinématique inverse, l’utilisation de la
fonction ikine est plus complexe de par son fonctionnement
itératif. C’est pourquoi dans ce document nous nous limiterons
à la première solution trouvée.
4
V. GÉNÉRATION DE TRAJECTOIRE
Grâce à la partie IV, la génération de trajectoire
devient possible. Par trajectoire on entend l’ensemble des
positions successives occupées par l’effecteur du robot en
fonction du temps. Pour générer une trajectoire, au moins 3
conditions sont nécessaires : la position initiale, la position
finale et la période.
Habituellement, d’autres conditions doivent être
considérées telles que la vitesse et l’accélération initiale / finale,
l’enveloppe du robot, la présence d’obstacles (détection de
collision), la vitesse maximale des moteurs de chaque
articulation et le courant maximum accepté par les moteurs.
Pour notre étude qui porte uniquement sur l’aspect cinématique,
nous ne prendrons pas en compte les conditions citées
précédemment. Nous considérerons seulement que la vitesse et
l’accélération initiale / finale sont nulles.
Une trajectoire est définie par une équation
polynomiale. Plus le degré 𝑛 est haut, plus la précision est
élevée et donc plus le contrôle du système est robuste.
𝑞(𝑡) = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 2 + ⋯ 𝑎𝑛 𝑡 𝑛
La génération de trajectoire complexe à la main est
fastidieuse mais avec le toolbox de Peter Corke, l’opération
devient simple. Pour cela, on utilise la fonction :
[q,qd,qdd] = jtraj(q0,qf,T)
𝑞 est un tableau composé de 𝑁 colonnes contenant les angles
pour chaque pas de temps 𝑚
𝑞𝑑 est un tableau composé de 𝑁 colonnes contenant les
vitesses des articulations pour chaque pas de temps 𝑚
𝑞𝑑𝑑 est un tableau composé de 𝑁 colonnes contenant les
accélérations des articulations pour chaque pas de temps 𝑚
𝑞0 𝑒𝑡 𝑞𝑓 sont respectivement les angles des articulations de
la position initiale et finale (des opérations de cinématique
inverse sont nécessaires si les positions sont exprimées dans
l’espace cartésien)
𝑇 = [𝑡0 : 𝑚: 𝑡𝑓 ]′ est un vecteur de configuration de la
période calculée par 𝑡𝑓 − 𝑡0 avec un pas de temps de 𝑚
Remarque : par défaut la trajectoire calculée est d’ordre 5.
Voici le script MatlabⓇ d’une génération de trajectoire
entre les positions définies par les matrices 𝑇1 et 𝑇2. Ce script
affiche également les tracés des angles, vitesses et accélérations
de chaque articulation en fonction du temps.
T1 = transl(475,50, 425); %Tool pos 1
T2 = transl(-400,600,20)*trotx(60); %Tool pos2
q1 = Robot.ikine(T1) %reverse kinematics
q2 = Robot.ikine(T2)
t = [0:0.05:2]'; %time segment
[q, qd, qdd] = jtraj(q1, q2, t); %Trajectory
 5

VI. IHM ET ANIMATION 3D

Nous avons désormais codé 3 fonctionnalités sous
MatlabⓇ : calcul de la cinématique directe, de la cinématique
inverse et génération d’une trajectoire entre 2 positions. Afin de
les regrouper et de les utiliser facilement, nous avons développé
une IHM sous MatlabⓇ. Sur cette IHM, l’utilisateur peut entrer
une configuration d’angle, une position dans l’espace cartésien
et une trajectoire via une position initiale et une position finale.
Les valeurs des angles ou de la position sont mises à jour après
chaque calcul et le robot est affiché sur le côté de l’interface.
Cette IHM est très utile pour vérifier l’existence d’une position
ou d’une configuration d’angle rapidement.

Figure 6 - Aperçu de l'IHM

Nous avons également animé le robot sous un logiciel

de Conception Assistée par Ordinateur (CAO). Pour cela, une
trajectoire a été générée sur MatlabⓇ. Puis le tableau des angles
en fonction du temps a été exporté sur Catia v5Ⓡ. Nous avons
pu ainsi visualiser notre trajectoire avec le modèle 3D du robot.
Figure 4 - Angles, vitesses et accélérations de chaque articulation en
fonction du temps

La trajectoire générée étant de degré 5, les courbes de

la Figure 4 apparaissent lisses. Avec une telle trajectoire,
l’utilisation des moteurs est optimisée car elle ne sollicite aucun
pic de courant. De plus, les courbes de la Figure 5 indiquent que
le parcours de l’effecteur est souple et contrôlé.

Figure 7 - Position finale de la trajectoire sous Catia v5Ⓡ

Figure 5 - Position de l'effecteur selon les plans XY, YZ et XZ en

fonction du temps
 6

VII. CONCLUSION RÉFÉRENCES

Dans cet article la modélisation et la simulation
cinématique d’un robot à 6 DDL a été conduite. Grâce à
l’extension Robotics Toolbox de Peter Corke aucune équation
n’a été nécessaire pour cette étude. La puissance et la rapidité
de ce toolbox a été confirmée. L’exploitation des données du
constructeur nous a permis de décrire chaque articulation et de
dresser une matrice de paramètres. La cinématique inverse est
fonctionnelle mais n’est pas complète et ne présente pas toutes
les solutions possibles. La génération de trajectoire est
fonctionnelle et a permis de présenter divers tracés de position,
vitesse et accélération de chaque articulation en fonction du
temps. Une IHM a également été développée pour nous
permettre de tester rapidement des positions d’effecteur ou des
configurations d’angle. Certains aspects tels que la dynamique,
la commande et l’étude de l’enveloppe du robot n’ont pas été
traités mais pourraient l’être dans une prochaine étude.
ANNEXES
Coefficients de la matrice de transformation homogène 𝑇60 :
𝑅1,1 = − 𝑐𝑜𝑠(𝑞6) ∗ (𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗
𝑠𝑖𝑛(𝑞2)) − 𝑐𝑜𝑠(𝑞4) ∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗
𝑠𝑖𝑛(𝑞3))) − 𝑠𝑖𝑛(𝑞4) ∗ 𝑠𝑖𝑛(𝑞6) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗
𝑠𝑖𝑛(𝑞3))
𝑅2,1 = 𝑐𝑜𝑠(𝑞4) ∗ 𝑠𝑖𝑛(𝑞6) + 𝑐𝑜𝑠(𝑞5) ∗ 𝑐𝑜𝑠(𝑞6) ∗ 𝑠𝑖𝑛(𝑞4)
Dernière visite des liens le 5 Décembre 2019
[1] Grégoire Normand, “Industrie : la robotisation avance à grand pas dans le
monde”, Mai 2018 à
https://www.latribune.fr/entreprises-finance/industrie/industrie-la-
robotisation-avance-a-grand-pas-dans-le-monde-780261.html
[2] Site officiel de Peter Corke et documentation du toolbox :
http://petercorke.com/wordpress/toolboxes/robotics-toolbox
Livre officiel du toolbox : robot.pdf
[3] Farzad Cheraghpour, IEEE Member, Masoud Vaezi, Resam Eddin
Shoori Jazeh, S. Ali A. Moosavian, “Dynamic Modeling and Kinematic
Simulation of Staubli© TX40 Robot Using MATLAB/ADAMS Co-
simulation”, Mai 2011 à
https://www.researchgate.net/publication/224251145_Dynamic_modelin
g_and_kinematic_simulation_of_StaubliC_TX40_robot_using_MATLA
BADAMS_co-simulation
[4] Hiba Hage1, Philippe Bidaud, and Nicolas Jardin “Simulation of a Stäubli
TX90 Robot during Milling using SimMechanics”, Mars 2017 à
https://www.researchgate.net/publication/258576045_Simulation_of_a_
Staubli_TX90_Robot_during_Milling_Using_SimMechanics
Référence des images :
[A] Brochure constructeur gamme TX2 – 90, Groupe Staübli, page 5
disponible sur le site http://www.staubli.com
[B] Brochure constructeur gamme TX2 – 90, Groupe Staübli, page 7
disponible sur le site http://www.staubli.com

𝑅3,1 = 𝑠𝑖𝑛(𝑞4) ∗ 𝑠𝑖𝑛(𝑞6) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)) −

𝑐𝑜𝑠(𝑞6) ∗ (𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) +
𝑐𝑜𝑠(𝑞4) ∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)))

𝑅1,2 = 𝑠𝑖𝑛(𝑞6) ∗ (𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)) −

𝑐𝑜𝑠(𝑞4) ∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3))) −
𝑐𝑜𝑠(𝑞6) ∗ 𝑠𝑖𝑛(𝑞4) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3))

𝑅2,2 = 𝑐𝑜𝑠(𝑞4) ∗ 𝑐𝑜𝑠(𝑞6) − 𝑐𝑜𝑠(𝑞5) ∗ 𝑠𝑖𝑛(𝑞4) ∗ 𝑠𝑖𝑛(𝑞6)

𝑅3,2 = 𝑠𝑖𝑛(𝑞6) ∗ (𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) +

𝑐𝑜𝑠(𝑞4) ∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2))) +
𝑐𝑜𝑠(𝑞6) ∗ 𝑠𝑖𝑛(𝑞4) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2))

𝑅1,3 = 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)) + 𝑐𝑜𝑠(𝑞4) ∗

𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3))

𝑅2,3 = 𝑠𝑖𝑛(𝑞4) ∗ 𝑠𝑖𝑛(𝑞5)

𝑅3,3 = 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) − 𝑐𝑜𝑠(𝑞4) ∗

𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2))

𝑃𝑥 = 425 ∗ 𝑐𝑜𝑠(𝑞2) + 425 ∗ 𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 425 ∗ 𝑐𝑜𝑠(𝑞3) ∗

𝑠𝑖𝑛(𝑞2) + 100 ∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2)) +
100 ∗ 𝑐𝑜𝑠(𝑞4) ∗ 𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) +
50

𝑃𝑦 = 100 ∗ 𝑠𝑖𝑛(𝑞4) ∗ 𝑠𝑖𝑛(𝑞5) + 50

𝑃𝑧 = 425 ∗ 𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 425 ∗ 𝑠𝑖𝑛(𝑞2) − 425 ∗ 𝑠𝑖𝑛(𝑞2) ∗

𝑠𝑖𝑛(𝑞3) + 100 ∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3)) −
100 ∗ 𝑐𝑜𝑠(𝑞4) ∗ 𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗ 𝑠𝑖𝑛(𝑞2))

View publication stats