Université Du Québec: Nouvelle Méthode de Génération Des Trajectoires Pour La Commande de Bras de Robots
Université Du Québec: Nouvelle Méthode de Génération Des Trajectoires Pour La Commande de Bras de Robots
Université Du Québec: Nouvelle Méthode de Génération Des Trajectoires Pour La Commande de Bras de Robots
MÉMOIRE
PRÉSENTÉ À
PAR
JUEGOUO JOSIANE
Février 1994
Université du Québec à Trois-Rivières
Service de la bibliothèque
Avertissement
RÉSUMÉ
Le présent travail porte sur l'activation des bras de robots et la génération des
trajectoires par une méthode n'utilisant pas la cinématique inverse. Dans cette optique,
nous avons défini des paramètres dynamiques tels que les variables trinaires et les
des paramètres définis nous permet en outre d'établir à l'avance un nombre fini de
bras. Une trajectoire est donc générée par un choix judicieux de combinaisons
produisant les déplacements élémentaires successifs adéquats. Les simulations qui ont
été réalisées par les algorithmes présentés nous ont permis de conclure que la méthode
donne une bonne approximation des trajectoires et offre d'excellentes possibilités pour
des applications telles que l'assemblage, les manipulations en milieu hostile ... etc
11
REMERCIEMENTS
KADIMA, pOllr tOllt l'enseignement qlle j'ai reçll de lui au cours de ces années
passées à l 'u. Q. T.R., et qui m'a permis d'avoir une meilleure ouverture d 'esprit sur
la recherche scientifique.
RÉSUMÉ
INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 1
1.1 Généralités......... . . . . . . . . . .. . . . . . . . . . . . . . . . . . . 3
4.2 Résultats. .. .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
BIBLIOGRAPHIE . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 95
Annexe A . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
Annexe B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. . . . . .. .. .. .125
VI
Figure Page
Tableau
78
4.1 Paramètre D-H d'un robot PUMA
90
4.2 Erreurs de position en mm
INTRODUCTION
La conséquence est que très souvent, on continue pendant des années à utiliser des
méthodes ou des procédés parce qu 'ils donnent de bons résultats, alors qu ' en les
trouver des solutions plus simples, moins coûteuses et parfois plus performantes.
C ' est dans cette optique que nous avons abordé le problème de la génération
des trajectoires des bras de robots. En effet, la recherche bibliographique montre que
toutes les méthodes de commande pour l'activation des bras articulés utilisent des
algorithmes de cinématique inverse dont les perfomances ont été démontrées mais qui
conservent toute leur complexité. Dans le domaine de la robotique, l' évolution rapide
de la technologie des processeurs et des capteurs focalise la recherche sur des aspects
particuliers tels que la vision et autres contrôles sensoriels. Aussi avons-nous voulu
Pour réaliser nos objectifs, nous nous sommes donné comme méthodologie la
1.1 Généralités
Au cours des siècles, le désir de l'homme de créer des outils de travail de plus
C'est en voulant donner à ces machines une certaine autonomie qu'est apparue l'auto-
matisation. L'homme s'est alors rendu compte qu'en construisant des machines ayant
une structure appropriée, elles pouvaient effectuer plusieurs tâches différentes au lieu
d'une seule . Ainsi est née la première vague de robots industriels. Cependant,
1955, la robotique a connu de nombreux progrès; elle suit d'ailleurs l'évolution des
ces.
Néanmoins, malgré le fait qu'on s'achemine de nos jours vers des robots de
moins variées, les fondements de la robotique restent les mêmes. Seules viennent s'y
4
sophistiquées.
m'apparaît comme tel que dans un environnement donné et par rapport à ce qui
l'entoure; il est conçu pour effectuer une famille de tâches dans un type de
géométrique varie suivant le type des tâches pour lesquelles il a été conçu . Il
est doté d'actionneurs alimentés par la source d' énergie, d'un organe effecteur
5
qui interagit avec l'environnement, et de capteurs qui informent l'organe de
figure 1.2.
Source Source
d'information d'énergie
Environnement
Système ordiné
Interaction Langage
extérioceptives
tés essentielles dont les ·constructeurs essaient de munir au mieux les robots :
1.2.2.1 La versatilité
lité à exécuter une multitude de tâches d'une part et, d'autre part, sa
1.2.2.2 L'adaptativité
L'architecture géométrique du robot varie selon le type des tâches qui lui sont
destinées. L'un des éléments qui caractérise la géométrie du robot est le nombre de
degrés de liberté.
1
8
alors que le solide a six degrés de liberté; sa position, à un instant donné, peut
être décrite par six paramètres indépendants : trois paramètres de rotation défi-
nissant l'orientation du solide par rapport à un trièdre fixe Ro, et trois paramè-
tres décrivant les coordonnées d'un point particulier du solide dans ce même
les dont la mobilité dans l'espace va définir les degrés de liberté du robot. Il
s'agit:
du véhicule;
du porteur;
de l'organe terminal.
1.3.2.1 Le véhicule
n' est pas toujours existant. Dans le cas le plus général où le robot
9
tion pour ne lui laisser que ceux en translation (ou deux des trois)
afin d'assurer son déplacement. Pour les robots fixes, on dira que le
1.3.2.2 Le porteur
C'est sur l' organe terminal qu'est fixé l'outil. Une fois
concourants.
de liberté.
possibles sont répartis entre le porteur et l'organe terminal de manière que les
sieurs axes au niveau du porteur. On obtient ainsi des robots pouvant avoir
sept axes et plus, mais avec six degrés de liberté (ex . : robot peintre ACMA) .
organe
lermlnal
\
\
,,0: \
La structure d'un porteur est caractérisée par des corps rigides, les segments,
susceptibles de se mouvoir par rapport à une base et par des articulations limitant le
Si les segments peuvent avoir des formes variées, les articulations qui sont des
liaisons bilatérales ont par contre des caractéristiques bien précises. On peut ainsi
Elles sont caractérisées par une mobilité unique: c'est-à-dire que la liaison
permet une rotation autour d'un axe commun aux deux segments
adjacents;
permet une translation le long d' un axe commun aux deux segments
adjacents .
1 b) Les articulations complexes
simples.
système de coordonnées dans lequel ils opèrent. On définit ainsi quatre classes prin-
de base comportant deux rotations et une translation; il s ' agit alors d ' un bras
téléscopique pouvant effectuer une rotation autour d' un axe vertical et une
rotation autour d' un axe horizontal, ce qui donne à l' espace de travail du por-
teur la forme d' une coquille sphérique . Comme pour les porteurs cylindriques,
axes de rotation. Néanmoins, l' avantage d' une telle structure est qu ' elle amé-
Les robots articulés ont une structure qui rappelle la plupart du temps
celle d'un bras humain . En général, ils comprennent quatre segments princi-
toutes les articulations du porteur articulé sont donc de type rotoïde et vont
structure articulée est celle qui offre la meilleure flexibilité et permet une
grande souplesse dans l'exécution des tâches, d'où son utilisation fréquente
peuvent être utilisés pour des applications nécessitant une très grande précision .
1.6.1 Généralités
que soit leur structure (rotoïde ou prismatique). Pour ce faire, on utilise des
tions sont réalisés à l'aide d'actionneurs qui peuvent être électriques, hydrau-
des tâches à accomplir. On les appelle des servomécanismes parce que leur
grandeur de sortie est une fonction mécanique. Ils sont caractérisés par la
fluide liquide ou gazeux, mis sous pression à l'aide d ' une pompe ou d'un
ce qui est propice aux fonctionnements intermittents qui caractérisent les ro-
bots; par ailleurs cela perm et de développer des efforts à l'arrêt lorsque cela
rence de pression :
ou à armure rotative.
dant, la nature du fluide et les frottements secs entre les pièces mobi-
servovalves de précision.
l'air parfois retenu dans les canalisations, et ce, quel que soit le type
d'actionneur.
uns sont:
robots sont :
tion.
fait, une électronique plus élaborée peut lui être associée. Cependant,
la commande des moteurs pas à pas exige que l'on puisse contrôler
ment du courant.
du système.
1
1
L _ asservissement .....J
en courant
.... -
1 IR8?OO8 1
Al 1
. 1 81
Thermal 1
Sensino
1 1
Charoe 1
1 Pump CharOe
Current Pump
Trip Onvt
1 $hutOOwn CUllenl Onvt 1 8 Current
Undervonaoe A
Trip Sensino r - - - + - - - - + + - - 7 " " ' O Sense
1 Dvtput
1
Direction 04-.f-'-----'--J
1
Bra"' O-+--1~
~M ~~~_ _ _ _ r-----------r-------__~ 1
1
L ---" ------ - ___ ----1
G,oynd
relativement faible;
25
les moteurs à induit en cloche; ils ne peuvent servIr
soit des vitesses très élevées pour des couples très faibles.
ment à priori illimité. Néanmoins, des limites leur sont imposées par
l'échauffement du moteur;
la démagnétisation;
la commutation;
trices suivantes :
ment en position.
27
R L
U0 M
..
ie=cte
MI1t~~' r-- ....
_L-
ee 1- es
ue Us
mances, à savoir:
la qualité de la régulation.
mande).
c = C + J dU) (1.1)
m r dt
ramenée au moteur.
U = Ri + L
di + e (1.2)
dt
où
moteur
thèse que
donc C r = 0 et J m
= O·'
C = J dw = km i
e 0 dt
Uo = Ri + k mw
dw (1.3)
on en déduit que :
dt
dw = 0 et i = 0
dt
dw k m2
= (w - w) (l.4)
dt J o .R 0
(1.5)
au niveau des articulations , tandis que la tâche à exécuter, et par ce fait, la position
Rappelons néanmoins que l' on peut décrire un bras de robot comme étant une
chaîne cinématique de segments rigides. D eux segments adjacents sont liés par une
Considérons un repère fixe Ro, d ' axes Xo> Y0> Za' et un corps rigide
dans l'espace représenté par un point Op' La position du point Op est décrite
(2.1)
centré en Op (fig. 2.1), alors, ce repère qui bouge avec le corps permet de
définir son orientation par rapport au repère fixe Ro' En effet, soient n, 0 et
a, les vecteurs unitaires liés respectivement aux axes x p, Yp' zp; les composan-
tes de chacun de ces vecteurs sont les cosinus directeurs obtenus en les proje-
La rotation du corps rigide est ainsi décrite par les neuf cosinus direc-
(2.3)
(2.4)
avec A = [~ ~l
La matrice A représente à la fois la position et l'orientation du repère
Op-xpypzp.
tion.
(2.5)
où
La plupart des robots industriels ou des bras manipulateurs usuels ont une
structure assimilable à une chaîne cinématique ouverte, ayant en général une base
fixe. Il est donc aisé de décrire pour ces robots la position et l'orientation de l'or-
Pour cela, Denavit et Hartenberg ont développé en 1955 une approche mathé-
repère Ri lié au i-ème segment du robot, l'axe z du repère R; étant aligné avec l'axe
art. i+ 1
segmen t i
.\,,.
\ 1
r,
ai : il décrit la distance entre les axes Zi_l et Zi' mesurée le long de l'axe
ai : c'est l'angle que fait l'axe Zi_l avec l'axe Zi' mesuré suivant la règle
de la main droite;
Bi : c'est l'angle entre l'axe Xi. l et Xj, mesuré par rapport à Zj.l selon la
que pour les articulations prismatiques, ce sont ai' ai et 8 i qui sont constants . La
transformations.
i par rapport au segment i-l s ' écrit, pour une articulation rotoïde :
(2.7)
articulaires .
Lorsqu ' on établit au préalable le tableau des paramètres (fig. 2.4), les matrices
nx 0x ax Px
ny 0y ay Py
T=
nz Oz az Pz
0 001
Le vecteur p
(P~x:l décrit la position de l'organe terminal dans l'espace carté-
sien, tandis que les composantes des vecteurs 11, 0 et a sont les cosinus directeurs qui
d'orientation R .
39
Paramètres
N° a·1 a ·1 d·1 0 1·
d'articulation
1
2
... .. . . .. . .. . ..
du robot qui peut résulter soit de la résolution du problème cinématique direct, soit
de celle du problème cinématique inverse . Dans l'un ou l'autre cas, cette résolution
des équations qui permettent d ' exprimer les coordonnées spatiales de position
laires.
40
X = F(q) (2.8)
le vecteur X correspondant.
Pour les robots ayant une géométrie simple (2 à 3 axes), il est assez
envisageable. Cependant, elle est assez longue à établir à cause des nombreu-
l'organe terminal vers une position donnée et avec une orientation précise .
connues; il faut alors calculer les déplacements devant être imposés aux articu-
quation.
(2.9)
Deux approches sont utilisées pour résoudre cette équation; une ap-
en temps réel.
L ' équation (2.9) qui décrit le problème inverse se traduit en réalité par un
cinématique inverse très complexes, ce qui constitue un sérieux problème lorsqu'on désire
,
D'autre part, à cause de la non linéarité des équations, l'équation cinématique
inverse admet souvent un nombre de solution supérieur à 1; et même si toutes les solu-
tions ne sont pas nécessairement accessibles, leur multiplicité (lorsqu'elles sont en nombre
lités avec les possibilités mécaniques du bras, ce qui a pour effet de rallonger le temps de
calcul.
toutes, d'une manière générale, des algorithmes de cinématique inverse avec succès, elles
très perfonnants et d'utiliser des calculateurs puissants et coûteux afin de pouvoir opérer
1
Face à toute cette complexité induite par la cinématique inverse, une question
s'impose à l'esprit: pourquoi n'essaie-t-on pas de générer les trajectoires sans passer par
problème cinématique des systèmes articulés en se basant sur la cinématique directe pour
laquelle la solution est toujours unique, quelle que soit la structure considérée ?
CHAPITRE III
suivante : le bras occupe une configuration quelconque mais possible de son espace
avec une orientation précise par rapport au référentiel de base. Il s'agit donc de
formuler, de façon très explicite, les relations qui lient les coordonnées spatiales et les
coordonnées articulaires.
C'est cette formulation qui fait l'objet des algorithmes de cinématique inverse
dont la complexité nous a amenés à rechercher une solution plus simple à la résolva-
1) lui imprimer soit une rotation dans un sens défini positif, soit un allongement
2) lui imprimer soit une rotation dans le sens négatif, soit un raccourcissement
3) la maintenir immobile.
comme hypothèse que toute commande appliquée à une articulation produit au niveau
effectue une rotation si c'est une articulation rotoïde, une translation si c'est
Partant de cette constatation, nous pouvons donc définir un pas articulaire qui
résulte que :
re, toute variation articulaire devient un incrément qui peut être négatif, positif
ou nul.
46
k. STEP
l ' outil.
lation reçoit une commande, elle doit donc se mouvoir dans un sens
Par analogie à une variable binaire qui n ' admet que deux valeurs,
nous appelons variable "trinaire" une variable définie dans la base trois, à
laquelle on ne peut attribuer que trois valeurs possihles. Ainsi, pour un nom-
47
bre donné Il de variables trinaires, nous pouvons établir 3 n combinaisons diffé-
ristiques d' incrément et on peut donc lui associer comme une variable trinaire .
ment qui peut être réajusté par la suite. Par exemple, considérons un bras articulé,
géométriquement tendu de façon à former une droite; on peut penser à priori que le
effectuent simultanément une rotation dans le même sens. Ceci permet, en fixant
Les variables articulaires étant des variables trinaires, toutes les articulations
fini 3/1 de combinaisons de variables trinaires et par conséquent 3/1 commandes possi-
bles. Toutes les possibilités de commande étant fixées à l'avance, les valeurs des
x= F(q)
devant être appliquées aux articulations comme l'imposent les algorithmes de cinéma-
tique inverse.
ne peuvent donc pas être utilisées intégralement. Elles sont au préalable ramenées à
l'articulation numérotée j. Le vecteur des caractéristiques d ' incrément est par consé-
base fixe (souvent lié au châssis du robot) ainsi qu'un référentiel outil lié au
(3.1)
telle que
dans le référentiel de base sont données par Px, P y , Pz et <Px, <Py, <Pz
respectivement.
où J = [aF]
aq est la matrice Jacobienne.
Le vecteur des accroissements spatiaux est: flX(R o ) = [flPx flPy flPz fl~x fl~y fl~ z ] t
px. = Px + IlPx
PY. = Py + IlPy
pz. = Pz + IlPz
<l> x. = <l> x + Il <l> x
(3.3)
<Il Y. = <Il y + Il <Il y
<Il z. = <Il z + Il <Il z
(3.4)
ré.
coordonnées articulaires .
!:J.ql
!:J.%
!:J.%
!:J.q,
!:J.qs
!:J.qô
Nous pouvons alors calculer pour chaque point de l ' espace accessible
1 = 1, ... 11
54
Sélection et calcul
de la matrice jacobienne correspondant au nombre
n d'articulations du bras considéré
( FIN)
56
P o =P"+pw
0 0
(3 .6)
Ma
rique (poignet triaxial à axes concourants) qui offre une plus grande
matrice de transformation.
six axes.
et (P wn' Pwy' P"'Z) les coordonnées du vecteur P ow, il est possible de les
de transformation globale T .
nx °x ax Px
ny Oy ay Py
T
nz Oz az Pz
0 0 0 1
o >'0
(3.7)
on obtient ainsi :
En réécrivant (3.7) on a :
soit Pu = Px - Pwn
paz = pz - pwz
(3.8)
60
PAX • Px - d 6 dx
ce qui donne : P. y • Py - d 6 dy
Pu .. p. - d 6 d.
teurs ax' a.y, élz sont connus. Ainsi la donnée des vecteurs Po et Po·
gnet.
orientation.
A% (3.9)
A%
A%
Aq,
Aqs
Aq6
61
position finale
posiU on
risée
Ainsi, on a :
D/2
sinll
cr;
d ' où (3.10)
n.
f'i
_
-
•
SlD
-1[2"1 [(
a X " 'i
_
a x"
)" + (
aY"'i
_
a yw
) 2+ (a
zloli
_ a )
zw
:']1/2]
avec Pi < P; l'orientation est terminée lorsqu 'on obtient Pi < co, Co
Il doit sélectionner parmi les commandes déjà définies , celles qui sont
d'un jalon est réalisé après un certain nombre d' itérations. La préci-
lateur car cbaque point est atteint avec une très bonne précision.
POSITIONNEMENT DU BRAS
* Calcul de la matrice jacobienne de position du bras
* variable à contrôler à chaque itération: distance entre
position actuelle et position visée (après évaluation)
* Tests de sélection de la combinaison appropriée
* Bouclage jusqu'à l'arrêt du positionnement
66 .
ORIENTATION DU POIGNET
* évaluation à chaque itération de l'angle bêta
entre orientaion actuelle et orientation visée
* vérification de l'immobilité du bras positionné
* contrôle de la décroissance de bêta
* bouclage jusqu'à l'arrêt
. négatif
~------------~
3 .5 Stratégie de commande
même façon, à une vitesse déterminée par l'asservissement. Il n ' est donc pas
que d'incrément qui lui est désignée, puis verrouille son moteur. Le mouve-
l'effecteur à une vitesse instantanée V,., qui ne dépend que de l'ensemble des
(fig. 3.6).
68
T oom t
diatement exécutées les unes à la suite des autres. Ainsi, pour pouvoir faire
variable.
verrouillés.
entre deux combinaisons non nulles, nous réalisons une période d'attente TA
des commandes T com est alors égale à (Tx + TA) et varie selon la durée de TA
effectue un pas angulaire sous l'action d'une impulsion de courant envoyée sur
pas .
pas , il est moins aisé de faire varier la période, T rom' d ' échantillonnage des
seur principal.
met de réaliser les pas angulaires tel que définis . De plus , nous considérons
que le courant dans chaque mot eur est constant afin d ' assurer à sa charge un
couple constant. En d' autres termes nous prenons pour acquis que chaque
lui appliquons .
71
doit lui permettre d 'effectuer ses caractéristiques d ' incrément avec la meilleure
compteur, la valeur qu'on y met détermine le nombre de fois qu ' il est néces-
saire d ' envoyer aux articulations la combinaison nulle entre deux combinaisons
tion , une phase de vitesse constante et une phase de décélération , tout en réali-
déplacement du centre de l' effecteur évolue d'abord très vite, puis de manière
à peu près constante et enfin plus lentement jusqu ' à l' arrêt. La stratégie déve-
la commande automatique.
d'une trajectoire.
lations.
position visée
position finale
bien défini , les valeurs permettant d 'att eindre les accélérations ou les
servo - Articulation 1
MÉMOIRE ~ Processeur 1
ampli. actionneur 1 , capteurs
l'
-Traitement
-Commande
- t
1
1
• 1
1
-Interpolation
• 1
1
• 1
servo - Articulation n
coprocesseur '-- Processeur n
ampli. actionneur n , capteurs
mathématique ., 1
l SYST~ME MÉCANIQUE
Interfaces
No de la combinaison
Articulation
Microprocesseur
codeur
mêm e, le nombre d ' itérations pour parcourir deux jalons de même longu eur est
vitesse du système; il s' agit d 'évaluer le temps moyen nécessaire pour parcou-
rir une certaine longueur de trajectoire , ainsi que le temps moy en nécessaire
pour effectuer une certaine rotation du poignet. C 'est en tenant compte de ces
RÉSULTATS DE SIMULATIONS
Pour vérifier la théorie développée, nous av ons effectué des simulations sur un
bras de type PUMA dont les paramètres D-H son t donnés dans la table 4.1 :
Paramètres
N° a(O) a(mm) d(mm)
d' arti cula ti on
1 91 - 90 0 660.4
2 92 0 431.8 149.09
3 93 90 - 20.32 0
4 94 - 90 0 433 .07
5 95 90 0 0
6 96 0 0 56.25
La position et l'orientation de départ du bras sont fixées par les valeurs angu-
laires suivantes :
81 8, = 174.126°
8 .. = 122.134°
jalons égaux.
80
R = 250mm et centré en un point C tel que C = (-200, -416 .2983, 730.81) Nous
définissons le long de cet arc de cercle, dix points équidistants tels que chaque jalon
m esure environ 8mm. L ' orientation du poignet est assez arbitraire; sa variation d ' un
L 'a nalyse du passage du bras le long de cet arc de cercle est faite pour deux
pas articulaires différents ; nous pouvons ainsi observer l 'i nfluence du pas articulaire
a) dans le premier cas , le pas angulaire est STEP = 0.000175 rad soit
di <0.075mm
b) dans le deuxième cas, le pas angulaire est STEP = 0.000525 rad soit
di <0.2mm
l 'outil que nous noterons Epj lors du passage par chaque point avec
81
dx i = Xi finl!l - X i visé
dYi = Yifindl - Yi vi s~
dZ i = Zifinal - Zivi5~
orienter le poignet.
divergent dès que le pas articulaire est trop grand pour le critère
d'arrêt défini . Dans cette simulation, les valeurs des variables articu-
différence entre le point final d' une étape et le point initial de l'autre.
82
fin""l:
r~ ...... .''-''~
-Ç\"':,,=, . 4"':" 1 736.604 -23~.6~2 730.803
Coordonnees du Doin~ ac tu e l :
-f,é,]. ?7 1~
Coordonnees du point vi s~ :
-1 7 6.23 -629.529 737.92 7
Coordonnees du po i nt actuel:
- 63Sl. ::·7 5
C0Grdon~ees du point vi se :
- H i ..: ·1 9 737.9.27 - 2 17.1 98 73 0 .22
175
86
4.2.2 Deuxi ème cas STEP 0.000525 rad
- C" i .. -,.-
- ':1"_ -+ . .1 / Q
87
a·:tuel:
-227. :::28 -628.595 736.301 -277.121 - 654 . 177 730 . 798
t
89
.
Co o rdonnees du po int Ilna-L:
~ ,
-167.497 -64 0 .263 737.927 -217.151 -665.732 7 3 0.863
D ' après ces résultats, on peut observer qu'en multipliant le pas arti-
culaire par trois,on réduit d'environ 1/3 le nombre d' itérations néces-
tâche, et ce d'autant plus que l'erreur de position Epi ne subit pas une
trop grande variation lors du passage par les points spécifiés. Il faut
Ce projet a été réalisé dans le but d ' élaborer une nouvelle méthode de
génération des trajectoire pour l'activation des bras de robot; pour cela nous avons
linéaires. Dès lors , il devient possible d'adopter une méthode itérative comme celle
1. Caractéristiques principales
L ' élaboration de cette nouvelle méthode ne s'est pas faite sans difficultés,
matrice jacobienne évolue très lentement et peut par conséquent n ' être réévaluée
trajectoire ont été spécifiés, le bras ne peut pas s'écarter de celle-ci car, comme
nous pouvons le voir dans la table 4.2, les erreurs de position ne sont pas
cumulatives.
Cependant, bien que les résultats obtenus concordent avec les objectifs que
nous nous étions fixés , cette nouvelle méthode nécessite une étude plus élaborée
vitesse moyenne de déplacement du bras. Pour avoir une bonne idée des vitesses
conséquent d ' utiliser des processeurs bien précis ainsi que toutes les
l'objet d'une étude plus approfondie dans le cadre d'un autre projet.
de robot ainsi que les types de tâches pour lesquels la méthode offre les meilleures
performances d'une part, et, d'autre part d'établir , si besoin est , les limites
pwduit directement utilisable sur le marché: cela n 'aurait pas été possible compte
94
tenu de nos moyens logiciels et matériels limités; ni de faire une comparaison
immédiate avec des méthodes ou des logiciels existant déjà. Cette comparaison
méthode présentée.
mis à l'écart par les chercheurs, et qui pourtant semblait offrir une résolution plus
des bras manipulateurs peut se faire autrement que par des algorithmes complexes
int rep,m;
int i,n,Nb comb,j,k,s , A,B,N, Nb trans,*Trans;
float *t,*a,*al,*d,*Tt,*Ttmax ,STEP,*pas bal;
float *J[3]; -
int *Ct,*Cr;
float **Q,*Tab;
float stepR,stepT;
main ( )
{
printf("INTRODUCTION DES PARAMETRES CONSTANTS\n");
scanf("%d",&n);
Nb comb=(pow(3,n));
prIntf(lI\n Nombre d'articulations: %d\n\n",n);
printf(" Nombre de combinaisons: %d\n\n",Nb_comb);
printf(" Y a-t-il des articulations qui sont des translations?\n");
printf("\n Si oui taper l, si non taper 0: ");fflush(stdout);
scanf("%d",&rep)i
switch(rep)
{
case 1: printf("\n Entrer le nombre de translations: ")ifflush(stdout)i
scanf(l%d",&Nb trans)i
printf("\n Entrer le pas lineaire: stepT ");fflush(stdout);
scanf ("%g", &stepT);
printf("\n Entrer le pas angulaire: stepR ");fflush(stdout);
scanf(l%g",&stepR)i
breaki
case 0: Nb trans=O;
printf(lI\n Entrer le pas angulaire: stepR ");fflush(stdout)i
scanf (" %g", &stepR) ;
if (Nb_trans !=O)
for (i=O; i<Nb_trans; i++)
{
printf("\n Entrer le numero de la translation ");fflush(stdout);
scanf( " %d", &Trans[i] ) ;
}
,.
99
1* INTRODUCTION DES PARAMETRES DE D-H *1
1* Le vecteur "T" correspond aux angles thetai le vecteur "a" aux angles alpha* /
1* le vecteur "al" aux constantes ai et le vecteur "d" aux constantes di . *1
t=Calloc(n,TRUC)i
a=Calloc(n,TRUC)i
al=Calloc(n,TRUC)i
d=Calloc(n,TRUC)i
Ct=Calloc(n,int);
Cr=Calloc(n,int);
Tab=Calloc«n*Nb comb),TRUC)i
Q=Calloc(Nb comb~TRUC *);
if (Tab==NUEL Il Q==NULL) exit(l)i
for (i=Oi i<Nb_combi i++)
{
Q[i]=Tab+i*n;
}
matq() i
switch(n)
{
case 3: t[3]=Oi d[3]=0;
t[4]=0; d[4]=0;
t[5]=0; d[5]=Oi
jacobi3();
break;
case 4: t[4]=0; d[4]=0;
t [5] =0 ; d [5] =0 ;
jacobi4();
break;
case 5: t[5)=0; d[S)=O;
jacobiS()i
break;
case 6: jacobi6();
1
101
1* =========================================================================
* DEFINITION DES FONCTIONS DEVANT SERVIR A ETABLIR *
* LA MATRICE JACOBIENNE *
========================================================================= *1
#include <rnath.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(rn,type) (type *)calloc«rn),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
float fx(t,a)
float *t,*a;
{
return«C(t[O)*C(t[1)*C(t[2)-(S(t[O)*C(a[O))*S(t[1)*C(t[2))
-(C(t[O)*S(t[1)*C(a[1)*S(t[2)-(S(t[O)*C(a[O))*C(t[l)
*C(a[1)*S(t[2)+(S(t[O)*S(a[O)*S(a[1)*S(t[2)));
float gx(t,a)
float *t , *a;
{
return«-C(t[O)*C(t[1)*S(t[2)*C(a[2)+(S(t[O))*C(a[O)*S(t[l)
*S(t[2)*C(a[2)-(C(t[O)*S(t[1)*C(a[1)*C(t[2))*C(a[2))
-(S(t[O)*C(a[O)*C(t[1)*C(a[1)*C(t[2)*C(a[2))+(S(t[O)
*S(a[O)*S(a[1)*C(t[2)*C(a[2))+(C(t[O) )*S(t[l)*S(a[l)
*S(a[2)+(S(t[O)*C(a[O)*C(t[1)*S(a[1)*S(a[2)))+(S(t[O)
*S(a[O)*C(a[1)*S(a[2));
float hx(t,a)
float *t,*a;
{
return«C(t[O)*C(t[1)*S(t[2)*S(a[2)-(S(t[O))*C(a[O)*S(t[l)
*S(t[2)*S(a[2)+(C(t[O)*S(t[1)*C(a[1)*C(t[2))*S(a[2))
+(S(t[O)*C(a[O)*C(t[1)*C(a[1)*C(t[2)*S(a[2))-(S(t[O)
*S(a[O)*S(a[1)*C(t[2)*S(a[2)+(C(t[O)*S(t[1))*S(a[1)
*C(a[2)+(S(t[O)*C(a[O)*C(t[1)*S(a[1)*C(a[2)))+(S(t[O)
*S(a[O)*C(a[1)*C(a[2));
float fy(t,a)
float *t,*a;
{
return«S(t[O)*C(t[1)*C(t[2)+(C(t[O)*C(a[O))*S(t[1)*C(t[2))
-(S(t[O)*S(t[1)*C(a[1)*S(t[2)+(C(t[O)*C(a[O))*C(t[l)
*C(a[1)*S(t[2)-(C(t[O)*S(a[O)*S(a[1)*S(t[2)));
float gy(t,a) 102
float *t,*ai
{
return((-S(t[O)*C(t[1)*S(t[2)*C(a[2)-(C(t[O))*C(a[O)*S(t[l)
*S(t[2)*C(a[2)-(S(t[O)*S(t[1))*C(a[1) )*C(t[2)*C(a[2))
+(C(t[O)*C(a[O)*C(t[1)*C(a[1)*C(t[2)*C(a[2))-(C(t[O)
*S(a[O)*S(a[1)*C(t[2)*C(a[2)+(S(t[O))*S(t[1) )*S(a[l)
*S(a[2)-(C(t[O)*C(a[O)*C(t[1)*S(a[1))*S(a[2) »-(C(t[O)
*C(a[O)*C(a[1)*S(a[2))i
float hy(t,a)
float *t,*ai
{
return((S(t[O)*C(t[1)*S(t[2)*S(a[2)+(C(t[O))*C(a[O)*S(t[l)
*S(t[2)*S(a[2)+(S(t[O)*S(t[1)*C(a[1)*C(t[2))*S(a[2))
-(C(t[O)*C(a[O)*C(t[l)*C(a[l) )*C(t[2)*S(a[2)+(C(t[O))
*S(a[O)*S(a[1)*C(t[2)*S(a[2)+(S(t[O)*S(t[1))*S(a[1)
*C(a[2)-(C(t[O)*C(a[O)*C(t[1)*S(a[1)*C(a[2)))-(C(t[O)
*S(t[O)*S(a[1)*C(a[2))i
float fz(t,a)
float *t,*ai
{
return((S(a[O)*S(t[1)*C(t[2)+(S(a[O)*C(t[1))*C(a[1)*S(t[2))
+(C(a[O)*S(a[1)*S(t[2))i
float gz(t,a)
float *t,*ai
{
return((-S(a[O)*S(t[1)*S(t[2)*C(a[2)+(S(a[O))*C(t[l)*C(a[l)
*C(t[2)*C(a[2)+(C(a[O)*S(a[1)*C(t[2))*C(a[2) »-(S(a[O)
*C(t[1)*S(a[1)*S(a[2)+(C(a[O))*C(a[1) )*S(a[2))i
float hz(t,a)
float *t,*ai
{
return((S(a[O)*S(t[1)*S(t[2)*S(a[2)-(S(a[O))*C(t[1) )*C(a[l)
*C(t[2)*S(a[2)-(C(a[O)*S(a[1)*C(t[2))*S(a[2) »-(S(a[O)
*C(t[1)*S(a[1)*C(a[2)+(C(a[O)*C(a[1))*C(a[2) »);
float fp(t,a,d,al)
float *t,*a,*al,*d;
{
return((d[S)*((C(t[3)*S(t[4)*S(a[4)+(S(t[3))*C(a[3)*C(t[4)
*S(a[4)+(S(t[3)*S(a[3)*C(a[4)))+(al[4)*((C(t[3))
*C(t[4)-(S(t[3)*C(a[3)*S(t[4)))+(d[4)*S(t[3))
*S(a[3)+(al[3)*C(t[3)))i
float gp(t,a,d,al)
float *t,*a,*d,*ali 103
{
return«d[S)*«S(t[3)*S(t[4)*S(a[4) »-(C(t[3)*C(a[3)*C(t[4)
*S(a[4)-(C(t[3)*S(a[3)*C(a[4)))+(al[4)*«S(t[3))
*C(t[4)+(C(t[3)*C(a[3)*S(t[4)))-(d[4)*C(t[3))
*S(a[3)+(al[3)*S(t[3)))i
float hp(t,a,d,al)
float *t,*a,*d,*ali
{
return«-d[S)*«S(a[3)*C(t[4)*S(a[4) »+(C(a[3)*C(a[4)))
+(al[4)*S(a[3)*S(t[4)-(d[4)*C(a[3))+d[3))i
float dfxl(t,a)
float *t,*ai
{
return«-S(t[O)*C(t[1)*C(t[2)-(C(t[O)*C(a[O))*S(t(1)
*C(t[2)+(S(t[O)*S(t[l)*C(a[l)*S(t[2))-(C(t[O))
*C(a[O)*C(t[1)*C(a[1)*S(t[2)+(C(t[O)*S(a[O))*S(a[l)*S(t[2))i
float dgxl(t,a)
float *t,*ai
{
return«S(t[O)*C(t[1)*S(t[2)*C(a[2))+(C(t[O) )*C(a[O)*S(t[l)
*S(t[2)*C(a[2)+(S(t[O)*S(t[l)*C(a[l)*C(t[2))*C(a(2))
-(C(t[O)*C(a[O)*C(t[l)*C(a[l)*C(t[2)*C(a[2))+(C(t[O)
*S(a[O)*S(a[l)*C(t[2)*C(a[2) »-(S(t[O)*S(t[l)*S(a[l)
*S(a[2)+(C(t[O)*C(a[O)*C(t[1))*S(a[1) )*S(a[2)+(C(t[O))
*S(a[O)*C(a[1)*S(a[2))i
float dhxl(t,a)
float *t,*ai
{
return«-S(t[O)*C(t[1)*S(t[2)*S(a[2)-(C(t[O))*C(a[O)*S(t[l)
*S(t[2)*S(a[2)-(S(t[O)*S(t[1)*C(a[1)*C(t[2))*S(a[2))
+(C(t[O)*C(a[O)*C(t[1)*C(a[l)*C(t[2)*S(a[2))-(C(t[O)
*S(a[O)*S(a[1)*C(t[2)*S(a[2))-(S(t[O) )*S(t[l)*S(a[l)
*C(a[2)+(C(t[O)*C(a[O)*C(t[1))*S(a[1) )*C(a[2)+(C(t[O))
*S(a[O)*C(a[1)*C(a[2))i
float dxl(t,a,d,al)
float *t,*a,*d,*a1i
{
return«al[2)*«-S(t[O)*C(t[l)*C(t[2)-(C(t[O))*C(a[O)*S(t[l)
*S(t[2)+(S(t[O)*S(t[l)*C(a[l)*S(t[2))-(C(t[O))*C(a[O)
*C(t[l)*C(a[l)*S(t[2)+(C(t[O)*S(a[O)*S(a[l))*S(t(2)))
+(d[2)*«-S(t[O)*S(t[l)*S(a[l)+(C(t[O)*C(a[O))*C(t[l)
*S(a[l)+(C(t[O)*S(a[O)*C(a[l))))+(al[l)*«-S(t[O))
*C(t[l)-(C(t[O)*C(a[O)*S(t[l)))+(d[l)*C(t[O))*S(a[O))
-(al[O)*S(t[O))i
float dfx2(t,a) 104
float *t,*ai
{
return«-C(t[O)*S(t[l)*C(t[2))-(S(t[O) )*C(a[O)*C(t[l) )*C(t[2))
-(C(t[O)*C(t[l)*C(a[l)*S(t[2)+(S(t[O))*C(a[O) )*S(t[l)
*C(a[l)*S(t[2))i
float dgx2(t,a)
float *t,*ai
{
return«C(t[O)*S(t[l)*S(t[2)*C(a[2) »+(S(t[O) )*C(a[O)*C(t[l)
*S(t[2)*C(a[2)-(C(t[O)*C(t[l))*C(a[l) )*C(t[2)*C(a[2))
+(S(t[O)*C(a[O)*S(t[l)*C(a[l)*C(t[2)*C(a[2))+(C(t[O)
*C(t[l)*S(a[l) )*S(a[2)-(S(t[O))*C(a[O) )*S(t[l)*S(a[l)*S(a[2))i
float dhx2(t,a)
float *t,*ai
{
return«-C(t[O)*S(t[l)*S(t[2)*S(a[2))-(S(t[O) )*C(a[O)*C(t[l)
*S(t[2)*S(a[2)+(C(t[O)*C(t[1)*C(a[l)*C(t[2))*S(a(2))
-(S(t[O)*C(a[O)*S(t[l)*C(a[l)*C(t[2)*S(a[2))+(C(t[O)
*C(t[l)*S(a[l)*C(a[2)-(S(t[O)*C(a[O)*S(t[l))*S(a [l)*C(a[2))i
float dx2(t,a,d,al)
float *t,*a,*al,*di
{
return«al[2)*«-C(t[O)*S(t[l)*C(t[2)-(S(t[O))*C(a[O)*C(t[l)
*C(t[2)-(C(t[O)*C(t[1)*C(a[1)*S(t[2))+(S(t[O))*C(a[O)
*S(t[l)*C(a[l)*S(t[2)+(d[2)*«C(t[O))*C(t[l))*S(a[l))
-(S(t[O)*C(a[O)*S(t[l)*S(a[l))+(al[l)*«-C(t[O))*S(t[l]»
-(S(t[O)*C(a[O])*C(t[l]»»)i
float dfx3(t,a)
float *t,*ai
{
return«-C(t[O])*C(t[l)*S(t[2]»+(S(t[O)*C(a[O])*S(t[l)*S(t[2))
-(C(t[O])*S(t[1)*C(a[l])*C(t[2)-(S(t[O))*C(a[O])*C(tEl])
*C(a[l)*C(t[2)+(S(t[O)*S(a[O)*S(a[l)*C(t[2)))i
float dgx3(t,a)
float *t,*ai
{
return«-C(t[O)*C(t[l)*C(t[2)*C(a[2))+(S(t[O) )*C(a[O)*S(t[l)
*C(t[2)*C(a[2)+(C(t[O)*S(t[l)*C(a[l))*S(t[2) )*C(a[2))
+(S(t[O)*C(a[O)*C(t[l)*C(a[1)*S(t[2) )*C(a[2)-(S(t[O))
*S(a[O)*S(a[1)*S(t[2)*C(a[2))i
float dhx3(t,a) 105
float *t,*ai
{
return«C(t[O))*C(t[l))*C(t[2))*S(a[2)))-(S(t[O))*C(a[O))*S(t[l))
*C(t[2))*S(a[2)))-(C(t[O))*S(t[l))*C(a[l))*S(t[2))*S(a(2)))
-(S(t[O))*C(a[O))*C(t[l))*C(a[l) )*S(t[2))*S(a[2)))+(S(t[O))
*S(a[O))*S(a[l))*S(t[2))*S(a[2))))i
float dx3(t,a,al)
float *t,*a,*a1i
{
return(al[2)*«-C(t[O))*C(t[l) )*S(t[2)))+(S(t[O) )*C(a[O))*S(t[l))
*S(t[2)))-(C(t[O))*S(t[l))*C(a[l))*C(t[2) ))-(S(t[O))*C(a[O))
*C(t[l))*C(a[l))*C(t[2)))+(S(t[O) )*S(a[O))*S(a[l))*C(t[2)))))i
float dfy1(t,a)
float *t,*ai
{
return«C(t[O))*C(t[l))*C(t[2) ))-(S(t[O) )*C(a[O))*S(t[l))*C(t[2)))
-(C(t[O))*S(t[l))*C(a[l))*S(t[2)))-(S(t[O))*C(a[O))*C(t[l))
*C(a[l))*S(t[2)))+(S(t[O))*S(a[O))*S(a[l))*S(t[2))))i
float dgy1(t,a)
float *t,*ai
{
return«-C(t[O)) *C(t[l))*S(t[2))*C(a[2)))+(S(t[O))*C(a[O))*S(t[l))
*S(t[2))*C(a[2)))-(C(t[O))*S(t[l))*C(a[l))*C(t[2))*C(a(2)))
-(S(t[O))*C(a[O))*C(t[l))*C(a[l))*C(t[2))*C(a[2)))+(S(t[O))
*S(a[O))*S(a[l))*C(t[2))*C(a[2)))+(C(t[O))*S(t[l))*S(a(1))
*S(a[2)))+(S(t[O))*C(a[O))*C(t[l))*S(a[l))*S(a[2)))+(S(t[O))
*C(a[O))*C(a[l))*S(a[2))))i
float dhy1(t,a)
float *t,*ai
{
return«C(t[O))*C(t[l))*S(t[2))*S(a[2)))-(S(t[O))*C(a[0))*S(t[l))*S(t[2))
*S(a[2)))+(C(t[O))*S(t[l))*C(a[l))*C(t[2))*S(a[2)))+(S(t[O))
*C(a[O))*C(t[l))*C(a[l))*C(t[2))*S(a[2)))-(S(t[O))*S(a(0))
*S(a[l))*C(t[2))*S(a[2)))+(C(t[O))*S(t[l))*S(a[l))*C(a(2)))
+(S(t[O))*C(a[O))*C(t[l))*S(a[l))*C(a[2)))+(S(t[O))*S(a[O))
*C(a[l))*C(a[2))))i
float dy1(t,a,d,al)
float *t,*a,*al,*di
{
return«al[2]*«C(t[O])*C(t[l))*C(t[2]))-(S(t[O])*C(a[O])*S(t[l])
*C(t[2]))-(C(t[O])*S(t[l])*C(a[l])*S(t[2]))-(S(t[O])*C(a[O])
*C(t[l])*C(a[l])*S(t[2]))+(S(t[O])*S(a[O])*S(a[l))*S(t[2]))))
+(d[2]*«C(t[O))*S(t[l))*S(a[l] ))+(S(t[O])*C(a[O])*C(t[l])
1 *S(a[l]))+(S(t[O])*S(a[O])*C(a[l]))))+(al[l]*«C(t[O])*C(t[l]))
-(S(t[O])*C(a[O])*S(t[l]))))+(d[l]*S(t[O])*S(a[O]))+(a l[O]*C(t[O])))i
float dfy2(t,a)
float *t,*ai 106
{
return«-S(t[O])*S(t[l])*C(t[2]))+(C(t[O])*C(a[O])*C(t[l])*C(t[2]))
-(S(t[O])*C(t[l])*C(a[l])*S(t[2]))-(C(t[0])*C(a[0])*S(tel])
*C(a[l])*S(t[2])))i
float dgy2(t,a)
float *t,*ai
{
return«S(t[0])*S(t[1])*S(t[2])*C(a[2]))-(C(t[0])*C(a[0] )*C(t[l])
*S(t[2])*C(a[2]))-(S(t[0])*C(t[1])*C(a[1] )*C(t[2])*C(a[2]))
-(C(t[0])*C(a[0])*S(t[1])*C(a[1])*C(t[2))*C(a[2)))+(S(t[O])
*C(t[1])*S(a[1))*S(a[2)))+(C(t [ 0))*C(a[0))*S(t[1))*S(a(1))*S(a[2))))i
float dhy2(t,a)
float *t,*ai
{
return«-S(t[0))*S(t[1))*S(t[2))*S(a[2)))+(C(t [ 0))*C(a[O])*C(t[l])
*S(t[2])*S(a[2]))+(S(t[0])*C(t[1] )*C(a[1])*C(t[2])*S(a[2]))
+(C(t[0))*C(a[0])*S(t[1])*C(a[1])*C(t[2))*S(a[2]))+(S(t[O])
*C(t[1])*S(a[l])*C(a[2)))+(C(t[O])*C(a[O] )*S(t[l])*S(a[l])*C(a[2))))i
float dy2(t,a,d,al)
float *t,*a,*al,*di
{
return«al[2)*«-S(t[O])*S(t[l))*C(t[2)))+(C(t[0))*C(a[O])*C(t[l])
*C(t[2]))-(S(t[0])*C(t[1])*C(a[l] )*S(t [ 2 ] ))-(C(t[0])*C(a[O])
*S(t[l])*C(a[1))*S(t[2)))))+(d[2]*«S(t[0))*C(t[l))*S(a[l)))
+(C(t[O))*C(a[O])*S(t[l])*S(a[l)))))+(al[l)*«-S(t[O))*S(t[l)))
+(C(t[O])*C(a[O))*C(t[l))))))i
float dfy3(t,a)
float *t,*ai
{
return«-S(t[O))*C(t[l])*S(t[2)))-(C(t[O))*C(a[0])*S(t[l))*S(t[2]))
-(S(t[0))*S(t[1))*C(a[1])*C(t[2)))+(C(t[0))*C(a[O])*C(t[l))
*C(t[2]))-(C(t[0])*S(a[0])*S(a[1])*C(t[2])))i
float dgy3(t,a)
float *t,*ai
{
return«-S(t[0))*C(t [ 1])*C(t[2))*C(a[2] ))-(C(t[O))*C(a[O))*S(t[l))
*C(t[2))*C(a[2)))+(S(t[0])*S(t[1])*C(a[1 ] )*S(t[2) )*C(a[2)))
-(C(t[O))*C(a[0))*C(t[1))*C(a[1])*S(t[2))*C(a[2)))-(C(t[O))
*S(a[O))*S(a[1))*S(t[2))*C(a[2))))i
"
float dy3(t,a,al)
float *t,*a,*ali
{
return(al[2)*«-S(t[O)*C(t[l)*S(t[2)-(C(t[O))*C(a[O)*S(t[l)
*S(t[2)-(S(t[O)*S(t[l)*C(a[l)*C(t[2))+(C(t[O))*C(a[O)
*C(t[l)*C(a[l)*C(t[2)-(C(t[O)*S(a[O)*S(a[1))*C(t (2)))i
float dfz2(t,a)
float *t,*ai
{
return«S(a[O)*C(t[l)*C(t[2)-(S(a[O)*S(t[l))*C(a[1)*S(t[2))i
float dgz2(t,a)
float *t,*ai
{
return( (-S(a[O) )*C(t[l) )*S(t[2) )*C(a[2) )-(S(a[O) )*S(t[l) )*C(a[l)
*C(t[2)*C(a[2)+(S(a[O)*S(t[l)*S(a[1)*S(a[2)))i
float dhz2(t,a)
float *t,*ai
{
return«S(a[O)*C(t[l)*S(t[2)*S(a[2)+(S(a[O))*S(t[l)*C(a[l)
*C(t[2)*S(a[2)+(S(a[O)*S(t[l)*S(a[l)*C(a[2)))i
float dz2(t,a,d,al)
float *t,*a,*al,*di
{
return«al[2)*«S(a[O)*C(t[l)*C(t[2)-(S(a[O))*S(t[l)*C(a[l)
*S(t[2)+(d[2)*S(a[O)*S(t[l))*S(a[l)))+(al[l)*S(a[O)*C(t[l))i
float dfz3(t,a)
float *t,*ai
{
ret urn ( (- S ( a [ 0 ) ) * S (t [ 1) ) * S ( t [ 2 ) ) ) + ( S ( a [ 0) ) * C ( t [ 1) ) * C (a [ 1 ) ) * C ( t [ 2 ) ) )
+(C(a[O)*S(a[1)*C(t[2))i
float dgz3(t,a)
float *t,*ai
{
return( (-S(a[O) )*S(t[l) )*C(t[2) )*C(a[2) )-(S(a[O) )*C(t[l) )*C(a[l)
*S(t[2)*C(a[2)-(C(a[O)*S(a[1)*S(t[2)*C(a[2)))i
float dhz3(t,a) 108
float *t,*ai
{
return«S(a[O])*S(t[1])*C(t[2])*S(a[2]»+(S(a[O])*C(t[1] )*C(a[1])
*S(t[2])*S(a[2]»+(C(a[O])*S(a[1])*S(t[2] )*S(a[2] »)i
float dZ3(t,a,al)
float *t,*a,*ali
{
return(al[2]*«-S(a[O])*S(t[1)*S(t[2)+(S(a[O))*C(t[1)*C(a[1])
*C(t[2)+(C(a[O)*S(a[l)*C(t[2))))i
float dfp4(t,a,d,al)
float *t,*a,*al,*di
{
return«d[5)*«-S(t[3)*S(t[4)*S(a[4)+(C(t[3))*C(a[3)*C(t[4)
*S(a[4)+(C(t[3)*S(a[3)*C(a[4))))+(al[4)*«-S(t[3])
*C(t[4)-(C(t[3)*S(a[3)*S(t[4]»))+(d[4]*C(t[3])
*S(a[3)-(al[3]*S(t[3])))i
float dgp4(t,a,d,al)
float *t,*a,*al,*di
{
return«d[5)*«C(t[3)*S(t[4])*S(a[4]»+(S(t[3)*C(a[3])*C(t[4])
*S(a[4)+(S(t[3)*S(a[3])*C(a[4))))+(al[4]*«C(t[3])
*C(t[4)-(S(t[3)*C(a[3)*S(t[4)))+(d[4)*S(t[3))
*S(a[3]»+(al[3)*C(t[3]»)i
float dfp5(t,a,d,al)
float *t,*a,*al,*di
{
return«d[S)*«C(t[3)*C(t[4)*S(a[4]»-(S(t[3])*C(a[3)*S(t[4)
*S(a[4)+(al[4)*«-C(t[3))*S(t[4)))-(S(t[3])*C(a[3])*C(t[4)))i
float dgp5(t,a,d,al)
float *t,*a,*al,*di
{
return«d[5)*«S(t[3])*C(t[4)*S(a[4]»+(C(t[3])*C(a[3)*S(t[4)
*S(a[4)+(al[4)*«-S(t[3)*S(t[4)))+(C(t[3))*C(a[3))*C(t[4]»»)i
float dhpS(t,a,d,al)
float *t,*a,*al,*di
{
return«d[S)*S(a[3])*S(t[4])*S(a[4]»+(al[4)*S(a[3])*C(t[4]»)i
float ft6(t,a)
float *t,*ai
{
return«C(t[3])*S(t[4] )*S(a[4)+(S(t[3))*C(a[3] )*C(t[4])*S(a[4]»
+(S(t[3)*S(a[3)*C(a[4));
109
float gt6(t,a)
float *t,*a;
{ ret urn ( ( S ( t [3) ) * S ( t [ 4 ) ) * s (a [ 4 ) ) ) - (c (t [3] ) * C ( a [3] ) * C ( t [ 4 ) ) * S ( a [ 4 ) ) )
_(C(t[3)*S(a[3)*C(a[4));
float ht6(t,a)
float *t,*a;
{ return«_S(a[3)*C(t[4)*S(a[4)+(C(a[3)*C(a[4)));
110
/* ======================================================================
* SOUS-PROGRAMME DE CALCUL LA MATRICE JACOBIENNE
POUR TROIS ARTICULATIONS
*
* *
====================================================================== */
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(m,type) (type *)calloc«m),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
jacobi3 ( )
{
FILE *resu13i
float Dx,Dy,Dz,R,Rmax,r,maxRi
R=li
maxR=Oi
for (Tt[2)=Oi Tt(2)<=Ttmax[2)i Tt(2)+=pas_bal[2))
{
for (Tt[O)=Oi Tt[O)<=Ttmax[O)i Tt[O)+=pas_bal[O))
J[O)[O)=Cr[O)*dx1(t,a,d,al)i 112
J[O)[l)=(Cr[l)*dx2(t,a,d,al))+(Ct[l)*S(t[O))*S(a[O) ))i
J[O) [2)=(Cr[2)*dx3(t,a,al))+(Ct[2)*((C(t[O))*S(t[1))*S(a[1)))
+(S(t[O))*C(a[O))*C(t[l))*S(a[l)))+(S(t[O))*S(a[O))
*C(a[l]))))i
J[l) [0)=Cr[O)*dy1(t,a,d,al)i
J[1)[1)=(Cr[l)*dy2(t,a,d,al))-(Ct[1)*C(t[0))*S(a[O)))i
J[l)[2)=(Cr[2)*dy3(t,a,al))+(Ct[2)*((S(t[O))*S(t[1))*S(a[l)))
-(C(t[O))*C(a[O))*C(t[l))*S(a[l)))-(C(t[O))*S(a[O))
*C(a[l)))) )i
J [ 2) [0) =Ct [ 0 ) i
J[2)[l)=(Cr[1)*dz2(t,a,d,al))+(Ct[l)*C(a[OJ))i
J[2J[2J=(Cr[2J*dz3(t,a,al))+(Ct[2J*((-S(a[0))*C(t[lJ)*S(a[l)))
+(C(a[O))*C(a[l)))))i
Rmax=Oi
Dy= ( (J [ 1 J [0) * Q [ i) [0 ) ) + ( J [ 1 J [ 1 ) * Q [ i J [ 1) ) + ( J [ 1) [2 J * Q [ i J [2 J ) ) i
Dz=( (J[2J [O)*Q[iJ [OJ )+(J[2J [lJ*Q[iJ [lJ )+(J[2J [2J*Q[iJ [2J)) i
R=sqrt((pow(Dx,2))+(pow(Dy,2))+(pow(Dz,2)))i
test: if (R>Rmax)
Rmax=Ri
}
if (Rmax>maxR)
maxR=Rmaxi
}
}
}
resu13=fopen("resu13","w+")i
fprintf(resu13,"maxR = %g",maxR)i
printf("\n Deplacement elementaire maximum= %g",maxR)i
fclose(resu13);
}
113
j* ======== ==============================================================
* SOUS-PROGRAMME DE CALCUL LA MATRICE JACOBIENNE *
* POUR QUATRE ARTICULATIONS *
====================================================================== *j
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(m,type) (type *)calloc«m),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
jacobi4()
{
FILE *resu14;
float Dx4,Dy4,Dz4,R4,Rmax4,r,maxR;
R4=1;
maxR=O;
for (Tt[3)=O; Tt(3)<=Ttmax[3); Tt(3)+=pas_bal[3))
{
for (Tt[O]=O; Tt[O)<=Ttmax[O]; Tt[O)+=pas_bal[O])
J[O][l]=Cr[l]*«dfx2(t,a)*fp(t,a,d,al»+(dgx2(t,a)*gp(t,a,d,al»
+(dhx2(t,a)*hp(t,a,d,al»+dx2(t,a,d,al»+(Ct[l]*S(t[O])*S(a[O]»i
J[O][2]=Cr[2]*«dfx3(t,a)*fp(t,a,d,al»+(dgx3(t,a)*gp(t,a,d,al»
+(dhx3(t,a)*hp(t,a,d,al»+dx3(t,a,al»+(Ct[2]*«C(t[O])
*S(t[l])*S(a[l]»+(S(t[O])*C(a[O])*C(t[l])*S(a[l]»+(S(t[O])
*S(a[O])*C(a[l]»»i
J[O][3]=Cr[3]*«fx(t,a)*dfp4(t,a,d,al»+(gx(t,a)*dgp4(t,a,d,al»)
+(Ct[3]*hx(t,a»i
J[l] [O]=cr[O]*«dfY1(t,a)*fp(t,a,d,al»+(dgy1(t,a)*gp(t,a,d,al»
+(dhy1(t,a)*hp(t,a,d,al»+dy1(t,a,d,al»i
J[l][l]=Cr[l]*«dfy2(t,a)*fp(t,a,d,al»+(dgy2(t,a)*gp(t,a,d,al»
+(dhy2(t,a)*hp(t,a,d,al»+dy2(t,a,d,al»+(Ct[l]*(-C(t[O])*S(a[O]»);
J[l][2]=Cr[2]*«dfy3(t,a)*fp(t,a,d,al»+(dgy3(t,a)*gp(t,a,d,al»
+(dhy3(t,a)*hp(t,a,d,al»+dy3(t,a,al»+(Ct[2]*«S(t[O])
*S(t[l])*S(a[l]»-(C(t[O])*C(a[O])*C(t[l])*S(a[l]»-(C(t[O])
*S(a[O] )*C(a[l]»»;
J[l][3]=Cr[3]*«fy(t,a)*dfp4(t,a,d,al»+(gy(t,a)*dgp4(t,a,d,al»)
+(Ct[3]*hy(t,a»;
J[2][l]=Cr[l]*«dfz2(t,a)*fp(t,a,d,al»+(dgz2(t,a)*gp(t,a,d,al»
+(dhz2(t,a)*hp(t,a,d,al»+dz2(t,a,d,al»+(Ct[l]*C(a[O]»;
J[2][2]=Cr[2]*«dfz3(t,a)*fp(t,a,d,al»+(dgz3(t,a)*gp(t,a,d,al»
+(dhz3(t,a)*hp(t,a,d,al»+dz3(t,a,al»+(Ct[2]*«-S(a[O])
*C(t[l])*S(a[l]»+(C(a[O])*C(a[l]»»;
J[2][3]=Cr[3]*«fz(t,a)*dfp4(t, a,d,al»+(gz(t,a)*dgp4(t,a,d,al»)
+(Ct[3]*hz(t,a»;
Rmax4=Oi
Dx4=( (J[O] [O]*Q[i] [0] )+(J[O] [l]*Q[i] [1] )+(J[O] [2]*Q[i] [2])
+(J[0][3]*Q[i][3]»i
Dy4=( (J[l] [O]*Q[i] [0] )+(J[lJ [l)*Q[i] [1) )+(J[lJ [2]*Q[i] [2])
+(J[lJ [3)*Q[i) [3J»i
Dz4=( (J[2) [O)*Q[i) (0) )+(J[2) [1)*Q[i) [1) )+(J[2) (2)*Q[i) (2)
+(J[2)[3)*Q[i)[3)); 116
R4=sqrt((pow(Dx4,2»+(pow(Dy4,2»+(poW(Dz4,2»);
test2: if (R4>Rrnax4)
Rrnax4=R4;
}
if (Rrnax4>maxR)
maxR=Rrnax4;
}
}
}
resu14=fopen( Iresu14", "w+");
fprintf (resu14, "maxR= %g", maxR) ;
printf("\n Deplacement elementaire maximum %g" ,maxR);
fclose(resu14);
117
/* ======================================================================
* SOUS-PROGRAl111E DE CALCUL LA 11ATRICE JACOBIENNE *
* POUR CINQ ARTICULATIONS
======================================================================* */
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#includ~ <alloc.h>
#define Calloc(m,type) (type *)calloc«m),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
jacobi5()
{
FILE *resu15i
float DX5,Dy5,Dz5,R5,Rmax5,r,maxR;
R5=li
maxR=O i
for (Tt[4]=Oi Tt[4]<=Ttmax[4]i Tt[4]+=pas_bal[4])
{
for (Tt[O]=Oi Tt[O]<=Ttmax[O]i Tt[O]+=pas_bal[O])
J[O) [1)=Cr[l)*«dfx2(t,a)*fp(t,a,d,al))+(dgx2(t,a)*gp(t,a,d,al))
+(dhx2(t,a)*hp(t,a,d,al))+dx2(t,a,d,al))+(Ct[l)*S(t[O))*S(a[O)))i
J[O)[2)=Cr[2)*«dfx3(t,a)*fp(t,a,d,al))+(dgx3(t,a)*gp(t,a,d,al))
+(dhx3(t,a)*hp(t,a,d,al))+dx3(t,a,al))+(Ct[2)*«C(t[O))
*S(t[l))*S(a[l)))+(S(t[O))*C(a[O))*C(t[l))*S(a[l)))
+(S(t[O))*S(a[O))*C(a[l)))))i
J[O) [3)=Cr[3)*«fx(t,a)*dfp4(t,a,d,al))+(gx(t,a)*dgp4(t,a,d,al)))
+(Ct[3)*hx(t,a))i
J[O) [4)=Cr[4)*«fx(t,a)*dfp5(t,a,d,al))+(gx(t,a)*dgp5(t,a,d,al))
+(hx(t,a)*dhp5(t,a,d,al)))+(Ct[4)*«fx(t,a)*S(t[3))*S(a(3)))
-(gx(t,a)*C(t[3))*S(a[3)))-(hx(t,a)*C(a[3)))));
J[l)[O)=Cr[O)*«dfyl(t,a)*fp(t,a,d,al))+(dgyl(t,a)*gp(t,a,d,al))
+(dhyl(t,a)*hp(t,a,d,al))+dyl(t,a,d,al))i
J[1)[1)=Cr[1)*«dfy2(t,a)*fp(t,a,d,al))+(dgy2(t,a)*gp(t,a,d,al))
+(dhy2(t,a)*hp(t,a,d,al))+dy2(t,a,d,al))+(Ct[1)*(-C(t[ O))*S(a[O))))i
J[1)[2)=Cr[2)*«dfy3(t,a)*fp(t,a,d,al))+(dgy3(t,a)*gp(t,a,d,al))
+(dhy3(t,a)*hp(t,a,d,al))+dy3(t,a,al))+(Ct[2)*«S(t[O))
*S(t[l))*S(a[l)))-(C(t[O))*C(a[O))*C(t[l))*S(a[l)))
-(C(t[O))*S(a[O))*C(a[l)))));
J[l) (3)=Cr[3)*«fy(t,a)*dfp4(t,a,d,al))+(gy(t,a)*dgp4(t,a,d,al)))
+(Ct[3)*hy(t,a));
J[l)[4)=Cr[4)*«fy(t,a)*dfp5(t,a,d,al))+(gy(t,a)*dgp5(t,a,d,al))
+(hy(t,a)*dhp5(t,a,d,al)))+(Ct[4)*«fy(t,a)*S(t[3))*S(a(3)))
-(gy(t,a)*C(t[3))*S(a[3)))-(hy(t,a)*C(a[3)))));
J(2) [l)=Cr[l)*«dfz2(t,a)*fp(t,a,d,al))+(dgz2(t,a)*gp(t,a,d,al))
+(dhz2(t,a)*hp(t,a,d,al))+dz2(t,a,d,al))+(Ct[l)*C(a[O)));
J(2)[2)=Cr[2)*«dfz3(t,a)*fp(t,a,d,al))+(dgz3(t,a)*gp(t,a,d,al))
+(dhz3(t,a)*hp(t,a,d,al))+dz3(t,a,al))+(Ct[2)*«-S(a[O))
*C(t[l))*S(a[l)))+(C(a[O))*C(a[l)))));
J[2) [4)=Cr[4)*«fz(t,a)*dfp5(t,a,d,al))+(gz(t,a)*dgp5(t,a,d,al))
+(hz(t,a)*dhp5(t,a,d,al)))+(Ct[4)*«fz(t,a)*S(t[3))*S(a[3)))
-(gz(t,a)*C(t[3))*S(a[3)))-(hz(t,a)*C(a[3)))))i
Rmax5=Oi
::or (i=Ci i«po\,,(3,n»i i++)
{
for (j=Oi j<ni j++)
't
i f « t [ j J +Q [i J [ j J ) >Ttmax [ j J )
{
R5=Oi
goto test3i
Dx5=( (J(OJ [OJ'-<Q[iJ [0) )+(J[O) [lj*Q[iJ [lJ )+(J[OJ [2J*Q[iJ [2J)
+(J~OJ ~3J"'Q[i] ~3; )+{J[OJ [4J*Q[iJ [4J» i
Dz5=( (J[2] [OJ*Q[iJ [OJ )+(J[2J [l;xQ(i] [lJ )+(J(2j ~2j*Q[i} [2J)
-:-(J~2J [3;:<Q[i] [3J )+(J[2: :4~>:Q[ij [4;)) i
Rs=sqrt«pOW(DX512»)+(pOW(Dy5,2»~(pOW(DZS/2»))i
~es~3: if (R5>Rrnax5)
Rrnax5=R5i
,
if (Rr.1ax5 >ma:"R)
rnaxR=Rr.1ax5i
printi ("\n %g\n" 1 maxR) i
}
resu15=fopen( "resu15", "w+") i
fprintf (resu15 l "maxR= %g" 1 maxR) ; %g" 1 maxR) i
printf("\n Deplacement elementaire maximum
fclose(resu15)i
}
121
j* ======================================================================
* SOUS-PROGRAMME DE CALCUL LA MATRICE JACOBIENNE *
* POUR SIX ARTICULATIONS
======================================================================* *j
#include <rnath.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(rn,type) (type *)calloc«rn),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
jacobi6()
{
float Dx6,Dy6,Dz6,R6,Rmax6,r,maxRi
FILE *resu16i
R6=li
maxR=Oi
for (Tt[5)=Oi Tt[5)<=Ttmax[5)i Tt[5)+=pas_bal[5))
{
for (Tt[O)=O; Tt[O)<=Ttmax[O)i Tt[O)+=pas_bal[O))
J[O] [O]=Cr[O]*«dfxl(t,a)*fp(t,a,d,al»+(dgxl(t,a)*gp(t,a,d,al»
+(dhxl(t,a)*hp(t,a,d,al» +dxl(t,a,d,al»i
J[O][1]=Cr[l]*«dfx2(t,a)*fp(t,a,d,al»+(dgx2(t,a)*gp(t,a,d,al»
+(dhx2(t,a)*hp(t,a,d,al»+dx2(t,a,d,al»+(Ct[l]*S(t[0])*S(a[O]»i
J[O][2]=Cr[2]*«dfx3(t,a)*fp(t,a,d,al»+(dgx3(t,a)*gp(t,a,d,al»
+(dhx3(t,a)*hp(t,a,d,al»+dx3(t,a,al»+(Ct[2]*«C(t[O])
*S(t[l])*S(a[l]»+(S(t[O])*C(a[O])*C(t[l])*S(a[l]»
+(S(t[O])*S(a[O])*C(a[l]»»i
J[O] [3]=Cr[3]*«fx(t,a)*dfp4(t,a,d,al»+(gx(t,a)*dgp4(t,a,d,al»)
+(Ct[3]*hx(t,a»i
J[O][4]=Cr[4]*«fx(t,a)*dfpS(t,a,d,al»+(gx(t,a)*dgpS(t,a,d,al»
+(hx(t,a)*dhpS(t,a,d,al»)+(Ct[4]*«fx(t,a)*S(t[3])*S(a[3]»
-(gx(t,a)*C(t[3])*S(a[3]»-(hx(t,a)*C(a[3]»»i
J[O][S]=Ct[S]*«fx(t,a)*ft6(t,a»+(gx(t,a)*gt6(t,a»+(hx(t,a)*ht6(t,a»)i
J[l][O]=Cr[O]*«dfyl(t,a)*fp(t,a,d,al»+(dgyl(t,a)*gp(t,a,d,al»
+(dhyl(t,a)*hp(t,a,d,al»+dyl(t,a,d,al»i
J[l][l]=Cr[l]*«dfy2(t,a)*fp(t,a,d,al»+(dgy2(t,a)*gp(t,a,d,al»
+(dhy2(t,a)*hp(t,a,d,al»+dy2(t,a,d,al»+(Ct[l]*(-C(t[O])*S(a[O]»)i
J[l][2]=Cr[2]*«dfy3(t,a)*fp(t,a,d,al»+(dgy3(t,a)*gp(t,a,d,al»
+(dhy3(t,a)*hp(t,a,d,al»+dy3(t,a,al»+(Ct[2]*«S(t[O])
*S(t[l])*S(a[l]»-(C(t[O])*C(a[O])*C(t[l] )*S(a[l]»
-(C(t[O])*S(a[O])*C(a[l]»»i
J[l][3]=Cr[3]*«fy(t,a)*dfp4(t,a,d,al»+(gy(t,a)*dgp4(t,a,d,al»)
+(Ct[3]*hy(t,a»i
J[1][4]=Cr[4]*«fy(t,a)*dfpS(t,a,d,al»+(gy(t,a)*dgpS(t,a,d,al»
+(hy(t,a)*dhpS(t,a,d,al»)+(Ct[4]*«fy(t,a)*S(t[3])*S(a[3]»
-(gy(t,a)*C(t[3])*S(a[3]»-(hy(t,a)*C(a[3]»»i
J[l][S]=Ct[S]*«fy(t,a)*ft6(t,a»+(gy(t,a)*gt6(t,a»+(hy(t,a)*ht6(t,a»)i
J[2][O]=Ct[O]i
J[2][l]=Cr[l]*«dfz2(t,a)*fp(t,a,d,al»+(dgz2(t,a)*gp(t,a,d,al»
+(dhz2(t,a)*hp(t,a,d,al»+dz2(t,a,d,al»+(Ct[l]*C(a[O] »i
J[2][2]=Cr[2]*«dfz3(t,a)*fp(t,a,d,al»+(dgz3(t,a)*gp(t,a,d,al» .
+(dhz3(t,a)*hp(t,a,d,al»+dz3(t,a,al»+(Ct[2]*«-S(a[O])
*C(t[l)*S(a[l]»+(C(a[O])*C(a[l]»»i
J[2][3]=Cr[3]*«fz(t,a)*dfp4(t, a,d,al»+(gz(t,a)*dgp4(t,a,d,al»)
+(Ct[3]*hz(t,a»i
J[2][4]=Cr[4]*«fz(t,a)*dfp5(t,a,d,al»+(gz(t,a)*dgp5(t,a,d,al»
+(hz(t,a)*dhp5(t,a,d,al»)+(Ct[4]*«fz(t,a)*S(t[3])*S(a[3]»
-(gz(t,a)*C(t[3])*S(a[3]»-(hz(t,a)*C(a[3]»»i
J[2] [S]=Ct[S]*«fz(t,a)*ft6(t,a»+(gz(t,a)*gt6(t,a»+(hz(t,a)*ht6(t,a»)i
Rmax6=Oi 124
Dz6=( (J[2] [O]*Q[i] [0] )+(J[2] [l]*Q[i] [1] )+(J[2] [2]*Q[i] [2])
+(J[2] [3]*Q[i] [3] )+(J[2] [4]*Q[i] [4] )+(J[2] [5]*Q[i] [5]»i
R6=sqrt«pow(Dx6,2»+(pow(Dy6,2»+(pow(Dz6,2»)i
test4: if (R6>Rmax6)
Rmax6=R 6 i
}
if (Rmax6>maxR)
maxR=Rmax6 i
printf( "\n %g\n" ,maxR) i
}
}
}
resu16=fopen ( "resu16" , "w+" ) i
fprintf(resu16,"maxR= %g",maxR)i
printf("deplacement elementaire maximum %g" , maxR) i
fclose(resu16)i
ANNEXE B
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(m,type) (type *)calloc«m),sizeof(type))
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
int rep,m,iter;
int i,n,Nb comb,j,k,s,A,B,N,Nb trans,*Trans;
float *t,*a,*al,*d,*Tt,*Ttmax,STEP,*pas bal;
float *J[6J,DX[729J,Dy[729J,DZ[729J,DOX[729J,Doy[729J,DOZ[729J;
float Epx,Epy,Epz,Eox,Eoy,Eoz; .
int Ct,Cr,ind,indl,ind2;
float **Q,*Tab,T[4J[4J,Erx,Ery,Erz,Erox,Eroy,Eroz,ex,ey,ez,eox,eoy,eoz;
float Mx,My,Mz,Mox,Moy,Moz,Mlx,Mly,Mlz,Mlox,Mloy,Mloz;
float M2x,M2y,M2z,M2ox,M2oy,M2oz;
float stepR,stepT,obl,ob2,ob3,ob4,obS,ob6,ar,br,cr,RO,Rl,Ri;
float ml,m2,m3,m4,mS,m6,Ar,Br,Dr;
double ul,vl,wl,u2,v2,w2,ui,vi,wi;
int fact,test3;
double ECR,THETA=.2S,Tetl,Teti;
float epsl=0.07S; j*Critere d'arret de positionnement *j
float eps2=0.001SO; j*Critere d'arret d'orientation*j
main ( )
{
printf("INTRODUCTION DES PARN1ETRES CONSTANTS\n");
j***************************************************** *********
* Il s'agit d'introduire le nombre d'articulations du bras, *
* de preciser leur nature et d'entrer les parametres du bras *
****************************************************** ********j
ind=O;
printf("entrer le nombre d'articulations n: ")ifflush(stdout)i
scanf("%d",&n)i
Nb comb=(pow(3,n))i
prIntf(lI\n Nombre d'articulations: %d\n\n",n)i
printf(" Nombre de combinaisons: %d\n\n",Nb_comb)i
printf(" Y a-t-il des articulations qui sont des translations?\n")i
printf("\n Si oui taper l, si non taper 0: ")ifflush(stdout)i
scanf ("%d", &rep) i
switch(rep) 127
{
case 1: printf("\n Entrer le nombre de translations: "); fflush (stdout);
scanf("%d",&Nb trans);
printf("\n Entrer le pas lineaire: stepT ");fflush(stdout);
scanf ( "%g" , &stepT) ;
printf("\n Entrer le pas angulaire: stepR ");fflush(stdout);
scanf("%g",&stepR);
break;
case 0: Nb trans=O;
printf("\n Entrer le pas angulaire: stepR ");fflush(stdout);
scanf ( "%g" , &stepR) ;
if (Nb_trans !=O)
j***************************************************** *************************
*INTRODUCTION DES PARAMETRES DE D-H *
*Le vecteur "t" correspond aux angles theta; *
*le vecteur "a" aux angles alpha (il n'est pas utilis dans les simulations);*
*le vecteur "al" aux constantes ai et le vecteur "d" aux constantes di . *
****************************************************** ************************j
t=Calloc(n,TRUC);
al=Calloc(n,TRUC);
d=Calloc(n,TRUC);
Tab=Calloc«n*Nb comb),TRUC)i
Q=Calloc(Nb comb~TRUC *)i
if (Tab==NULL Il Q==NULL) exit(l)i
for (i=Oi i<Nb_combi i++)
{
Q[i]=Tab+i*ni
}
i ter=l i
matq() i
u2=(M2ox-M2X)jd[S]i
v2=(M2oy-M2y)jd[S]i
w2=(M2oz-M2z)jd[S]i
j*u2,v2,w2 sont les cosinus directeurs de l'orientation visee. *j
BOUCLE pt_courant()i
jacobpos()i
ECR=sqrt(pow(ex,2)+pow(ey,2)+pow(ez,2»i
jacobori();
goto boucle2;
/************************************************************
* Fonction definissant l'angle beta qui permet d'effectuer *
* l'orientation du poignet. *
************************************************************/
double fonc(double a2,double al,double b2,double bl,double c2,double cl)
{
return(2*asin(O.5*(sqrt(pow((a2-al),2)+pow((b2-bl),2)+pow((c2-cl),2»»);
/* ============;~======================================================== *
* SOUS-PROGRAMME DE CALCUL DU POINT ACTUEL *
* *
* Par Josiane JUEGOUO. 1993 *
/* ====================================================================== */
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(m,type) (type *)calloc((m),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(m,type) (type *)calloc«m),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
matq( )
{
FILE * matricej
j* Generation de la matrice *j
if« matrice=fopen("matrice","w+"»==NULL)
{
printf("Ouverture du fichier impossible \n")j
exit(l) j
}
N=n-lj
switch(rep)
{
case 1: for(j=N; j>=Oj j--)
{
for (m=O; m<Nb transi m++)
{ if (j==Trans[m)
{
STEP=stepT;
break;
}
else
{
if (m==Nb trans-1)
STEP=stepR;
s=O;
A=pow(3,j);
for(k=li k<=pow(3, (N-j»j k++)
{ for(i=s; i<=(s+(pow(3,j)-1»j i++)
Q[i) [j)=-STEPi
131
Q(i+A) (j)=Oi
Q(i+(2*A)) (j)=STEPi
s=k*pow(3,j+l)i
}
breaki
case 0: for (j=Ni j>=Oi j--)
{
s=Oi
A=pow(3,j)i
for (k=li k<=pow(3, (N-j)i k++)
{ for(i=si i<=(s+(pow(3,j)-1)i i++)
{ Q[i)[j)=-stepRi
Q[i+A) [j)=Oi
Q(i+(2*A»)[j)=step R i
}
s=k*pow(3,j+l)i
}
B=Nb combi
for(I=Oi i<Bi i++)
{
}
fprintf(matrice,"\n")i
}
fclose(matrice)i
132
j* ====================================================================== *
* SOUS-PROGRAMME DE CALCUL DU POINT ACTUEL *
* *
* Par Josiane JUEGOUO. 1993 *
j* ====================================================================== *j
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(m,type) (type *)calloc«m),sizeof(type))
#define TRUC float
#define CCx) cos(x)
#define S(x) sin(x)
pt courant ()
T[O][O]=C(t[O])*(C(t[1]+t[2])*«C(t[3])*C(t[4])*C(t[5]))-(S(t[3])
*S(t[5])))-(S(t[1]+t[2])*S(t[4])*C(t[S])))-(S(t[O])*«S(t[3])
*C(t[4])*C(t[5]))+(C(t[3])*S(t[S]))))i
T[O][1]=C(t[O])*(-C(t[1]+t[2])*«C(t[3])*C(t[4])*S(t[5]))+(S(t[3])
*C(t[5])))-(S(t[1]+t[2])*S(t[4])*S(t[5])))-(S(t[O])*«-S(t[3])
*C(t[4])*S(t[5]))+(C(t[3])*C(t[5]))))i
T[O] [2]=C(t[O])*«C(t[1]+t[2])*C(t[3])*S(t[4]))+(S(t[1]+t[2])*C(t[4])))
-(S(t[Q])*S(t[3])*S(t[4]))i
T[O] [3]=C(t[O])*(d[5]*«C(t[1]+t[2])*C(t[3])*S(t[4]))+(S(t[1]+t[2])
*C(t[4])))+(d[3]*S(t[1]+t[2]))+(al[2]*C(t[l]+t[2]))+(a1[1]
*C(t[1])))-(S(t[O])*(d[5]*S(t[3])*S(t[4])+d[l]))i
T[l][Q]=S(t[Q])*(C(t[l]+t[2])*«C(t[3])*C(t[4])*C(t[5]))-(S(t[3])*
S(t[5])))-(S(t[l]+t[2])*S(t[4])*C(t[S])))+(C(t[O])*«S(t[3])
*C(t[4])*C(t[S]))+(C(t[3])*S(t[S]))))i
133
T[1)[1)=S(t[0))*(-C(t[1)+t[2))*((C(t[3))*C(t[4))*S(t[S)))+(S(t[3))
*C(t[S))))-(S(t[1)+t[2))*S(t[4))*S(t[S))))+(C(t[0))*((-S(t[3))
*C(t[4))*S(t[S)))+(C(t[3))*C(t[S)))));
T[1)[2)=S(t[0))*((C(t[1)+t[2))*C(t[3))*S(t[4)))+(S(t[1)+t[2))*C(t[4])))
+(C(t[0])*S(t[3))*S(t[4)));
T[l] [3)=S(t[0))*(d[S]*((C(t[1]+t[2])*C(t[3])*S(t[4]))+(S(t[1]+t[2])
*C(t[4))))+(d[3)*S(t[1)+t[2)))+(al[2)*C(t[1)+t[2)))+(a1[1]
*C(t[1))))+(C(t[0])*(d[S]*S(t[3))*S(t[4])+d[1)));
T[2][0]=-C(t[1)+t[2))*S(t[4])*C(t[S))-(S(t[1]+t[2])*((C(t[3))*C(t[4])
*C(t[S)))-(S(t[3))*S(t[S)))));
T[2)[2]=-(S(t[1]+t[2])*C(t[3])*S(t[4]))+(C(t[1]+t[2])*C(t[4)));
T[2] [3]=d[0)-(al[1]*S(t[1]))-(al[2]*S(t[1]+t[2]))+(d[3]*C(t[1]+t[2]))
-(d[S]*((S(t[1]+t[2])*C(t[3])*S(t[4)))-(C(t[1]+t[2])*C(t[4]))));
T[3][0]=0;
T [ 3 ] [ 1 ] =0 ;
T [3] (2) =0 i
T[3J [3J=1;
Mlx=T[O] [3J-(d[S)*T[0)[2));
Mly=T[l] [3]-(d[S]*T[1] [2])i
Mlz=T[2] (3)-(d[S] *T[2] [2]) i
Mlox=T[O] [3];
Mloy=T[1][3];
Mloz=T[2) [3] i
if(iter==l)
{
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(m,type) (type *)calloc«m),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
jacobpos ( )
{
J[0)[0)=-S(t[0)*«d[3)*S(t[1)+t[2))+(al[2)*C(t[1)+t[2))+(al[1)
*C(t[l)-(d[l)*C(t[O)))i
J[0)[1)=-C(t[0)*«-d[3)*C(t[1)+t[2))+(al[2)*S(t[1)+t[2))
+(al[l)*S(t[l))i
J[O) [2)=-C(t[0)*«-d[3)*C(t[1)+t[2)+(al[2)*S(t[1)+t[2)) »i
J [0) [3) =0 i
J [0) [ 4 ) =0 i
J[O) [5)=Oi
J[l) [0)=C(t[0)*«d[3)*S(t[1)+t[2)+(al[2)*C(t[1)+t[2)))+(al[l)
*C(t[l)-(S(t[O)*d[l))i
J[l) [1)=-S(t[0)*«-d[3)*C(t[1)+t[2)+(al[2)*S(t[1)+t[2)))+(al[l)
*S(t[l))i
J[l) (2)=-S(t[0)*«-d[3)*C(t[1)+t[2)+(al[2)*S(t[l)+t[2)) »i
135
J[I) [3)=0;
J[I)[4)=0;
J[1)[5)=0;
J [ 2) [0) =0 ;
J[2)[1)=-«d[3)*S(t[1)+t[2)))+(al[2)*C(t[1)+t[2)))+(al [1)*C(t[1))))i
J[2)[2)=-«d[3)*S(t[1)+t[2)))+(al[2)*C(t[1)+t[2))));
J[2)[3)=Oi
J [2) [ 4 ) =0 i
J [2) [5) =0 i
Erx=fabs(M2x-Mlx)i
Ery=fabs(M2y-MlY)i
Erz=fabs(M2z-M1z)i
Ar2=fabs(M2x-ob1)i
Br2=fabs(M2y-ob2)i
Dr2=fabs(M2z-ob3)i
ar2=fabs(M2ox-ob4)i
br2=fabs(M2oy-ob5)i
cr2=fabs(M2oz-ob6)i
RO=Oi
Ct=Oi
for (i=Oi i<Nb_combi i++)
{
Dx[i)=( (J[O) [0) *Q[i) [0) )+(J[O) [1) *Q[i) [1) )+(J[O) [2)*Q[i) [2))
+(J[O) [3)*Q[i) [3) )+(J[O) [4)*Q[i) [4) )+(J[O) [5)*Q[i) [5) ))i
Dy[i)=( (J[1) [O)*Q[i) [0) )+(J[1) [1)*Q[i) [1) )+(J[1) [2)*Q[i) [2))
+ (J [1) [3) *Q [i) [3) ) + (J [1) [4) *Q [i) [4) ) + (J [1) [5) *Q [i) [5) ) ) ;
Dz[i)=( (J[2) [O)*Q[i) [0) )+(J[2) [1)*Q[i) [1) )+(J[2) [2)*Q[i) [2))
+(J[2) [3)*Q[i) [3) )+(J[2) [4)*Q[i) [4) )+(J[2) [5)*Q[i) [5)))i
/* tem est une variable permettant de verifier si,
dans la combinaison utilisee, une au moins des
articulations d'orientation recevra une commande
non nulle. */
tem=(fabs(Q[i) [3)) )+(fabs(Q[i) [4)) )+(fabs(Q[i) [5))) i
m12=M2x-(M1x+Dx[i))i
m22=M2y-(M1y+Dy[i))i
m32=M2z-(M1z+Dz[i) )i
136
/* Ri represente la distance entre la position actuelle augmentee
d'un deplacement elementaire et la position visee. */
Ri=sqrt(pow(m12,2)+pow(m22,2)+pow(m32,2»i
Epx=fabs(m12)i
Epy=fabs (m22) i
Epz=fabs(m32)i
if(tem==O)
{
if(Ri<Rl)
{
Ct+=l i
Rl=Rii
Erx=Epxi
Ery=EpYi
Erz=Epzi
indl=ii
}
else
{
if«Ri>O)&&(Epx<Erx)&&(Epy<Ery)&&(Epz<Erz»
{
Rl=Rii
Erx=Epxi
Ery=EpYi
Erz=Epz i
ind2=ii
}
if«Ri==Rl)&&(Ri<epsl»
{
ind=ii
goto egali
}
if(Ct>O)
{
ind=indl i
}
else
{
ind=ind2i
}
137
egal: 11x=M1x+Dx [ind) i
My=M1y+Dy[ind)i
11z=M1z+Dz[ind) i
ex=fabs(M2x-Mx)i
ey=fabs(M2y-MY)i
ez=fabs(M2z-Mz)i
int signe(float x)
{
if(x>=O)
{
return(l)i
}
else
{
return(O)i
}
}
138
/* ======================================================================
* SOUS-PROGRN1ME DE CALCUL DE L'ORIENTATION *
* *
* Par Josiane JUEGOUO. 1993. *
====================================================================== */
#include <math .h>
#include <stdarg.h>
#include <stdio .h>
#include <stdlib.h>
#include <stddef.h>
#include <alloc.h>
#define Calloc(m,type) (type *)calloc«m),sizeof(type»
#define TRUC float
#define C(x) cos(x)
#define S(x) sin(x)
jacobori()
{
ul=T[O] [2];
vl=T[l] (2);
wl=T(2) (2);
Tet1=THETA; /*fonc(u2,u1,v2,v1,w2,w1);*/
J[O) [O]=-S(t[O])*«d[3)*S(t[l]+t[2)+(al[2]*C(t[l]+t[2]))+(al[l]
*C(t[l]»)-(d[l]*C(t[O]»;
J[O][l]=-C(t[O])*«-d[3]*C(t[l]+t[2]»+(al[2]*S(t[l]+t[2]»+(al[l]*S(t[l]»);
J[O] [2]=-C(t[O])*«-d[3]*C(t[l]+t[2]»+(al[2]*S(t[1]+t[2])»;
J[O][3]=0;
J[O] [4]=0;
J[O] [5]=0;
139
J[l) [O)=C(t[O)*«d[3)*S(t[l)+t[2))+(al[2 ) *C ( t[l) + t [2 ) »+ (al[l)
*C(t[l)-(S(t[O)*d[l));
J[l)[2)=-S(t[O)*«-d[3)*C(t[l)+t[2))+(al[2)*S(t[l)+t(2));
J [ 1) (3) =0 ;
J [ 1) [ 4 ) =0 ;
J [ 1) (5) =0 ;
J [ 2) (0) =0 ;
J (2) (3) =0 ;
J(2) (4)=0;
J [ 2) (5) =0 ;
Dx[i)=( (J[O) [O)*Q[i) (0) )+(J[O) [l)*Q[i) (1) )+(J[O) (2)*Q[i) (2)
+(J[O) (3)*Q[i) (3) )+(J[O) (4)*Q[i) (4) )+(J[O) (5)*Q[i) ( 5 ) ) i
Dy[i)=( (J[l) (0) *Q[i) (0) )+(J[l) (1) *Q[i) (1) )+(J[l) (2) *Q[i) (2)
+(J[l) (3)*Q[i) (3) )+(J[l) (4)*Q[i) [4) )+(J[l) [5)*Q[i) (5)) i
Dz[i)=( (J[2) [O)*Q[i) (0) )+(J[2) [l)*Q[i ) [1) )+(J[2) [2)*Q[i) [2)
+(J[2) [3)*Q[i) [3) ) + (J[2) [4)*Q[i) [4) )+(J[2) [5)*Q[i) [5)) i
Ri=sqrt(pow(DX[i),2)+pow(Dy[i),2)+pow(Dz[i),2»i
if(Ri==O)
{
for (j=O; j<ni j++)
{
tp [ j ) =t [ j ) +Q [i) [ j ) i
}
T[l) [2)=S(tp[0) )*( (C(tp[1)+tp[2) )*C(tp[3) )*S(tp[4) )+(S(tp[1) + tp[2) )*C(tp [4)
ui =T [ 0 ) [2 ) i
v i=T [ 1) [2) i
wi=T[2) [2) i
140
Teti=fonc(u2,ui,v2,vi,w2,wi);
if( (fabs(Teti)<fabs(Tetl) )&&( (Q(i] (3] !=O) Il (Q(i] (4] !=O) Il (Q(i] (5] !=O»)
{
Tetl=Teti;
ar3=ui;
br3=vi;
cr3=wi;
ind=i;
THETA=fonc(u2,ar3,v2,br3,w2,cr3);
m13=Mx+(d(5]*ar3);
m23=My+(d(5]*br3);
m33=Mz+(d(5]*cr3);