Vous êtes sur la page 1sur 7

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

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 IV. CINÉMATIQUE INVERSE
relativement 𝑐𝜃 et 𝑠𝜃. La sous matrice composée des éléments
Contrairement à la cinématique directe, la cinématique
allant de 𝑥1,1 à 𝑥3,3 est appelée matrice de rotation. Le vecteur inverse consiste à déterminer les angles de chaque articulation
composé des éléments allant de 𝑥1,4 à 𝑥3,4 est appelé vecteur de pour une position et orientation donnée de l’effecteur. Une
translation. Une fois que les matrices relatives à chaque position unique de l’effecteur peut être atteinte selon plusieurs
articulation sont calculées, il suffit de les multiplier entre elles configurations d’angles.
(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. Position de l’effecteur Angles des
dans l’espace cartésien articulations
𝑇60 = 𝑇10 𝑇21 𝑇32 𝑇43 𝑇54 𝑇65
La cinématique inverse d’un robot consiste à établir
La matrice obtenue est exprimée sous la forme des systèmes d’équations algébriques ou trigonométriques et les
suivante (les coefficients sont donnés en annexes) : résoudre pour obtenir les angles des articulations en fonction
des éléments de la matrice de transformation homogène qui
𝑅11 𝑅12 𝑅13 𝑃𝑥 caractérise la position et l’orientation de l’effecteur.
𝑅21 𝑅22 𝑅23 𝑃𝑦
𝑇60 =[ ]
𝑅31 𝑅32 𝑅33 𝑃𝑧 Le Robotics Toolbox propose la fonction ikine6s qui
0 0 0 1 permet de calculer les différentes solutions analytiques pour la
cinématique inverse d’un robot 6 axes. L’utilisation de cette
Nous avons calculé cette matrice complexe grâce au fonction semble correspondre à notre robot. Son exécution est
calcul matriciel de MatlabⓇ. Le toolbox de Peter Corke permet rapide et elle permet d’obtenir les 8 solutions possibles en
également de calculer cette matrice très rapidement une fois que spécifiant les configurations suivantes : droitier / gaucher,
le robot est défini. Pour cela, on utilise la fonction : coude orienté vers le haut / bas et poignet retourné ou non.
4

Cependant, nous ne pouvons pas l’utiliser car la V. GÉNÉRATION DE TRAJECTOIRE


fonction n’est compatible qu’avec des robots définis en Grâce à la partie IV, la génération de trajectoire
convention DH standard, ce qui n’est pas notre cas. devient possible. Par trajectoire on entend l’ensemble des
positions successives occupées par l’effecteur du robot en
Une autre fonction proposée par le toolbox est la fonction du temps. Pour générer une trajectoire, au moins 3
fonction ikine. Cette fonction permet de résoudre la conditions sont nécessaires : la position initiale, la position
cinématique inverse d’un robot sans conditions sur le nombre finale et la période.
de degrés de liberté et sur la convention utilisée pour la
déclaration du robot. Habituellement, d’autres conditions doivent être
considérées telles que la vitesse et l’accélération initiale / finale,
Elle diffère de la fonction ikine6s car la solution n’est l’enveloppe du robot, la présence d’obstacles (détection de
pas trouvée analytiquement mais numériquement. En effet, collision), la vitesse maximale des moteurs de chaque
l’algorithme de la fonction consiste à tester tous les angles articulation et le courant maximum accepté par les moteurs.
possibles des articulations du robot pour trouver la solution à la Pour notre étude qui porte uniquement sur l’aspect cinématique,
cinématique inverse de manière itérative. Voici la fonction : nous ne prendrons pas en compte les conditions citées
q = Robot.ikine(T) précédemment. Nous considérerons seulement que la vitesse et
l’accélération initiale / finale sont nulles.
Voici les lignes de commandes utilisées pour calculer
la cinématique inverse d’une position et orientation 𝑇𝑖𝑛𝑣𝑒𝑟𝑠𝑒 du Une trajectoire est définie par une équation
robot. Les valeurs des angles q sont affichées en radians. 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
Pour vérifier la validité de la solution, on calcule la accélérations des articulations pour chaque pas de temps 𝑚
cinématique directe du robot à partir des angles donnés par q : 𝑞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.
On obtient bien que 𝑇𝑓𝑜𝑟𝑤𝑎𝑟𝑑 = 𝑇𝑖𝑛𝑣𝑒𝑟𝑠𝑒 et que la
solution trouvée pour la cinématique inverse est correcte. En T1 = transl(475,50, 425); %Tool pos 1
revanche, il faut garder en tête que les solutions ne sont pas T2 = transl(-400,600,20)*trotx(60); %Tool pos2
uniques et qu’il en existe probablement d’autres. Pour trouver
les autres solutions de cinématique inverse, l’utilisation de la q1 = Robot.ikine(T1) %reverse kinematics
fonction ikine est plus complexe de par son fonctionnement q2 = Robot.ikine(T2)
itératif. C’est pourquoi dans ce document nous nous limiterons t = [0:0.05:2]'; %time segment
à la première solution trouvée. [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 Dernière visite des liens le 5 Décembre 2019
cinématique d’un robot à 6 DDL a été conduite. Grâce à
l’extension Robotics Toolbox de Peter Corke aucune équation [1] Grégoire Normand, “Industrie : la robotisation avance à grand pas dans le
monde”, Mai 2018 à
n’a été nécessaire pour cette étude. La puissance et la rapidité
https://www.latribune.fr/entreprises-finance/industrie/industrie-la-
de ce toolbox a été confirmée. L’exploitation des données du robotisation-avance-a-grand-pas-dans-le-monde-780261.html
constructeur nous a permis de décrire chaque articulation et de
dresser une matrice de paramètres. La cinématique inverse est [2] Site officiel de Peter Corke et documentation du toolbox :
http://petercorke.com/wordpress/toolboxes/robotics-toolbox
fonctionnelle mais n’est pas complète et ne présente pas toutes
Livre officiel du toolbox : robot.pdf
les solutions possibles. La génération de trajectoire est
fonctionnelle et a permis de présenter divers tracés de position, [3] Farzad Cheraghpour, IEEE Member, Masoud Vaezi, Resam Eddin
vitesse et accélération de chaque articulation en fonction du Shoori Jazeh, S. Ali A. Moosavian, “Dynamic Modeling and Kinematic
Simulation of Staubli© TX40 Robot Using MATLAB/ADAMS Co-
temps. Une IHM a également été développée pour nous
simulation”, Mai 2011 à
permettre de tester rapidement des positions d’effecteur ou des https://www.researchgate.net/publication/224251145_Dynamic_modelin
configurations d’angle. Certains aspects tels que la dynamique, g_and_kinematic_simulation_of_StaubliC_TX40_robot_using_MATLA
la commande et l’étude de l’enveloppe du robot n’ont pas été BADAMS_co-simulation
traités mais pourraient l’être dans une prochaine étude.
[4] Hiba Hage1, Philippe Bidaud, and Nicolas Jardin “Simulation of a Stäubli
TX90 Robot during Milling using SimMechanics”, Mars 2017 à
ANNEXES https://www.researchgate.net/publication/258576045_Simulation_of_a_
Staubli_TX90_Robot_during_Milling_Using_SimMechanics
Coefficients de la matrice de transformation homogène 𝑇60 :
Référence des images :
𝑅1,1 = − 𝑐𝑜𝑠(𝑞6) ∗ (𝑠𝑖𝑛(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑠𝑖𝑛(𝑞3) + 𝑐𝑜𝑠(𝑞3) ∗
𝑠𝑖𝑛(𝑞2)) − 𝑐𝑜𝑠(𝑞4) ∗ 𝑐𝑜𝑠(𝑞5) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗
[A] Brochure constructeur gamme TX2 – 90, Groupe Staübli, page 5
𝑠𝑖𝑛(𝑞3))) − 𝑠𝑖𝑛(𝑞4) ∗ 𝑠𝑖𝑛(𝑞6) ∗ (𝑐𝑜𝑠(𝑞2) ∗ 𝑐𝑜𝑠(𝑞3) − 𝑠𝑖𝑛(𝑞2) ∗
disponible sur le site http://www.staubli.com
𝑠𝑖𝑛(𝑞3))
[B] Brochure constructeur gamme TX2 – 90, Groupe Staübli, page 7
𝑅2,1 = 𝑐𝑜𝑠(𝑞4) ∗ 𝑠𝑖𝑛(𝑞6) + 𝑐𝑜𝑠(𝑞5) ∗ 𝑐𝑜𝑠(𝑞6) ∗ 𝑠𝑖𝑛(𝑞4) 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