Petit projet sympa, permettant de jouer avec :

• une imprimante 3D
• OpenCV (avec C#)
• les servomoteurs Dynamixel
• un Arduino (uniquement pour l’électroaimant)

Vous trouverez une présentation vidéo de ce robot ici : https://youtu.be/Hbkp3n0x0qE.

Le but de cet article est de donner des détails techniques complémentaires à cette vidéo.

Fabrication

Plastique (ça pollue mais… ça s’imprime)

Les parties blanches sont :

• dessinées (patiemment) avec Fusion 360
• « slicées » (= « découpées en tranches pour l’imprimante ») avec Cura
• imprimées sur une Artillery Sidewinder-X1

Profilés aluminium

Les profilés sont du MakerBeam XL. Ce n’est pas indispensable ici… C’est pratique, et c’est beau, mais c’est un peu cher…

Les moteurs

Ceux-là sont entièrement numériques.
On leur envoie l’angle de destination, ils y vont, et ils s’y tiennent malgré la charge.
Ils peuvent atteindre cette destination avec un profil « doux » (=phase d’accélération, puis vitesse maxi, puis phase de décélération) ce qui rend les mouvements très « fluides » et « pro ».
A tout moment il est possible de lire leur vitesse, position, température… (doc. constructeur ici)
Bref, un plaisir à commander !

Et, comble du bonheur:

• ils se connectent « les uns derrières les autres » (2 prises sur chaque moteur) ce qui limite drastiquement le câblage à effectuer ! (câblage « daisy chain »)
• ils ne nécessitent que 3 fils (pour la version « TTL ») qui transportent le courant de puissance, et le BUS TTL (la commande)

C’est juste un peu cher (environ 55 EUR), mais il en existe de moins chers (ex: Dynamixel XL-320, 37 EUR) et aussi de beaucoup plus chers (plus puissants, solides, rapides…)

Les « liaisons pivot », ou « articulation »

Je me suis posé beaucoup de questions à ce sujet. Finalement je m’en suis sorti avec 3 types de liaisons pivot :

• roulement à bille + axe acier
• palier (recherche Google : « Manchon de roulement autolubrifiant douilles en bronze fritté ») + axe acier
• alésage dans le plastique + axe acier

La vidéo mentionnée plus haut montre ces 3 montages.

Ce qui reste aléatoire : les sites internet qui vendent des axes en acier et des roulements pour le « grand public » (=en petite quantité) ne permettent pas de choisir des diamètres aux dimensions très précises incluant une indication de tolérance. On ne sait donc jamais si la bague du roulement sera serrée ou pas sur l’axe.
Cela dit, en pratique les charges étant faibles, ce n’est pas trop gênant.

Schéma de principe

Les calculs de cinématique, et le code qui va avec

Ici le but est de déterminer les angles des 3 moteurs en fonction de la position (x, y, z) souhaitée du centre de la « plateforme » (celle qui supporte l’électroaimant), et des dimensions du robot.
J’ai tenté de trouver les équations. J’y étais presque, mais il me manquait quelques « astuces » de mathématicien pour finir. Heureusement il y a toujours un article qui traine sur internet.
Celui qui m’a aidé est celui-là :
https://www.ohio.edu/mechanical-faculty/williams/html/PDF/DeltaKin.pdf.

Les calculs reviennent à déterminer l’intersection de 3 sphères.

Vous trouverez ci-dessous le code (une fonction c#) correspondant aux équations de cet article (chapitre « Inverse Position Kinematics (IPK) Solution » ).
Explication des variables et des entrées / sorties de cette fonction :

• x, y, z : coordonnées du centre de la plateforme triangulaire (cf. PDF), qui sont les entrées de cette fonction.
• teta1, teta2, teta3 : les angles des moteurs, en radian, calculés par cette fonction, et définis comme dans le PDF
• teta1Dyna, teta2Dyna, teta3Dyna : ces mêmes angles, mais traduits en unité « dynamixel » : de 0 à 4095. Ces sont les nombres qui seront envoyés au contrôleur. Ils sont les sorties de cette fonction.
• Constantes : L, l, Up, Wb : les dimensions du robot, telles que définies dans le PDF.

Pour ma part j’ai pu mesurer Up, Wb et L sur Fusion 360.

Remarque : l’unité de mesure des coordonnées (x, y, z) déterminées par cette fonction est la même que celle des dimensions du robot (ex: le millimètre)

//
// Calculs de cinématique inverse.
// Source de ces calculs : https://www.ohio.edu/mechanical-faculty/williams/html/PDF/DeltaKin.pdf
//
private void calculeEtAffiche(double x, double y, double z, ref int teta1Dyna, ref int teta2Dyna, ref int teta3Dyna) {
	// Vérification des minimums / maximums
	if ((x < -150) || (x > 150) || (y < -150) || (y > 150) || (z < -260) || (z > -130)) {
		textBoxCalculs.Text = "ERREUR DE VALEUR !!\r\nx=" + x + ", y=" + y + ", z=" + z;
		return;
	}

	// Constantes
	double L = 90.316;
	double l = 230;
	double Up = 35;
	double Wb = 70.502;
	double Racine3 = Math.Sqrt(3);

	// Calculs
	double Wp = Up / 2;
	double Sp = 3 * Up / Racine3;
	double a = Wb - Up;
	double b = Sp / 2 - Racine3 / 2 * Wb;
	double c = Wp - Wb / 2;
	double E1 = 2 * L * (y+a);
	double F1 = 2 * z * L;
	double G1 = x * x + y * y + z * z + a * a + L * L + 2 * y * a - l * l;
	double E2 = -L * (Racine3 * (x + b) + y + c);
	double F2 = 2 * z * L;
	double G2 = x * x + y * y + z * z + b * b + c * c + L * L + 2 * (x * b + y * c) - l * l;
	double E3 = L * (Racine3 * (x - b) - y - c);
	double F3 = 2 * z * L;
	double G3 = x * x + y * y + z * z + b * b + c * c + L * L + 2 * (-x * b + y * c) - l * l;
	double t1 = (-F1 - Math.Sqrt(E1 * E1 + F1 * F1 - G1 * G1)) / (G1 - E1);
	double t2 = (-F2 - Math.Sqrt(E2 * E2 + F2 * F2 - G2 * G2)) / (G2 - E2);
	double t3 = (-F3 - Math.Sqrt(E3 * E3 + F3 * F3 - G3 * G3)) / (G3 - E3);
	double teta1 = 2 * Math.Atan(t1);
	double teta2 = 2 * Math.Atan(t2);
	double teta3 = 2 * Math.Atan(t3);
	teta1Dyna = radToDyna(teta1);
	teta2Dyna = radToDyna(teta2);
	teta3Dyna = radToDyna(teta3);
}

OpenCV

Une fois que l’on a compris comment installer OpenCV (avec Visual Studio c’est en fait assez simple : packages NuGet OpenCvSharp4, OpenCvSharp4.Extensions, OpenCvSharp4.runtime.win, OpenCvSharp4.WpfExtension), et sa philosophie, c’est un plaisir d’utiliser cette librairie de « computer vision ».

Pour déterminer les positions des rondelles dans le repère de la caméra, je procède ainsi :

• au lancement de l’application, j’affiche l’image de la caméra à l’écran. Avec la souris je clique sur une rondelle rouge, puis verte, puis bleue (l’ordre n’a pas d’importance) : cela me permet de récupérer les valeurs HSV des 3 couleurs, qui varient selon la lumière présente dans la pièce.
• J’efface les zones « inutiles » de l’image, avec 2 rectangles plein noirs, puis je convertis en HSV

Cv2.Rectangle(frameInitale, new Rect(0, 0, 640, 125), Scalar.FromRgb(0, 0, 0), -1);   // haut
Cv2.Rectangle(frameInitale, new Rect(0, 0, 200, 480), Scalar.FromRgb(0, 0, 0), -1);   // gauche
Mat frameHSV = frameInitale.CvtColor(ColorConversionCodes.BGR2HSV);

• Je crée une « frame » ne contenant que des pixels blancs et noir : les blancs correspondent à des pixels ayant la couleur rouge, avec une certaine tolérance (entre « low » et « high »)

frameInRangeRouge = frameHSV.InRange(new Scalar(low_H, low_S, low_V), new Scalar(high_H, high_S, high_V));

• Je recherche dans cette image des « zones » de pixels blancs dont la taille est comprises entre 80 et 260 pixels (dans mon cas), et au minimum distants de 25 pixels:

paramsBlob.MinDistBetweenBlobs = 25f;
paramsBlob.MinArea = 80f;
paramsBlob.MaxArea = 260f;
detector = SimpleBlobDetector.Create(paramsBlob);
keypoints = detector.Detect(frameInRange);

Et voilà, avec un peu d’organisation dans le code, on récupère les coordonnées de toutes nos rondelles (contenues dans keypoints)

Changement de repère : caméra -> robot

Le but ici est de transformer les coordonnées des rondelles obtenues dans le repère de la caméra par OpenCV, en coordonnées dans le repère du robot (en 2D, au niveau de la « planche »)

Comme expliqué dans la vidéo, la caméra, même placée parfaitement perpendiculairement au support, « déforme » l’image : voilà à quoi ressemble une feuille de papier quadrillée vue par la caméra :

On comprend ici clairement que la déformation n’est pas « linéaire », mais bien plus complexe.

Plutôt que de tenter de retrouver les calculs exacts de déformations (liés à la géométrie de la lentille de la caméra), j’ai préféré effectuer une approximation (avec une méthode très personnelle) :

• je découpe la surface en 9 zones (quadrillage rouge)
• je considère que les déformations sont « linéaires » dans les 9 rectangles ainsi formés.

J’estime que sur ces 9 surfaces réduites, l’erreur commise par cette approximation est raisonnable pour mon besoin.

J’appelle « linéaire » la déformation suivante, dont les calculs sont détaillés plus bas :

Pour effectuer ce changement de repère, voilà donc comment j’ai procédé :

• sur l’image de la caméra j’ai ajouté un quadrillage (rouge) :

• sur une feuille fixée au support (planche) j’ai tracé des croix aux intersections des lignes rouges (en me « guidant » avec l’écran) On voit ces croix ci-dessus et ci-dessous.

• j’ai déterminé les coordonnées de ces 16 points dans le repère du robot, en guidant manuellement (à la souris) le robot au dessus de chacun d’eux.
  • Pour information voilà ces coordonnées (elles n’ont aucun intérêt pour vous, lecteur, c’est simplement pour « concrétiser ») :

            A4	140, -115
            A3	137,-56
            A2	138,2
            A1	138,71
            B4	68,-112
            B3	67,-53
            B2	65,6
            B1	59,77
            C4	-18, -121
            C3	-14, -57
            C2	-15, 9
            C1	-26, 80
            D4	-118, -127
            D3	-114,-59
            D2	-115, 12
            D1	-128, 87

A partir de ces données, je peux effectuer le changement de repère « local » (que j’appelle « linéaire ») de la manière suivante (à lire avec le schéma un peu plus bas) :

• je détermine dans quel rectangle se trouve le centre P de la rondelle (centre renvoyé par OpenCV)
  • Exemple: le rectangle B2, C2, B3, C3
• je calcule les coordonnées de P « dans » ce rectangle avec pour unité de mesure ses propres côtés. Exemple : (0,2 , 0,3)
• j’en déduis les coordonnées des 4 points « Phaut, Pbas, Pgauche, Pdroite » dans le repère du robot
• ce qui me permet de calculer les coordonnées de P dans le repère du robot : intersection du segment Phaut-Pbas avec le segment Pgauche-Pdroite.

Vous trouverez le code complet ci-dessous, chapître « Le code » – Projet « Delta_Camera ».

J’ai du créer une fonction permettant de trouver l’intersection de 2 droites. Je l’écris ici car elle peut être utile pour d’autres usages, et j’aurais aimé la trouver sur internet 🙂

public void intersectionDroites(double x1a, double y1a, double x2a, double y2a, double x1b, double y1b, double x2b, double y2b, out double xi, out double yi) {
    xi = ((y1b - y1a) * (x1b - x2b) * (x1a - x2a) - (y1b - y2b) * (x1a - x2a) * x1b + x1a * (y1a - y2a) * (x1b - x2b)) / ((y1a - y2a) * (x1b - x2b) - (y1b - y2b) * (x1a - x2a));
    yi = ((x1b - x1a) * (y1b - y2b) * (y1a - y2a) - (x1b - x2b) * (y1a - y2a) * y1b + y1a * (x1a - x2a) * (y1b - y2b)) / ((x1a - x2a) * (y1b - y2b) - (x1b - x2b) * (y1a - y2a));
}

Le code (téléchargeable)

J’ai choisi le langage C#, et j’ai créé 2 projets (donc 2 exécutables) sous Microsoft Visual Studio.
Si vous y tenez vraiment, vous trouverez ci-joint le code (ZIP des 2 projets).
Je ne l’ai pas mis sur GitHub car il n’est pas assez « finalisé », pas assez « pro » (les IHM ne sont pas très belles etc…)

• Projet « Delta_Dynamixel »
  • permet de piloter les moteurs (manuellement, automatiquement…) avec la librairie fournie par le constructeur des moteurs.
  • Téléchargeable ici

• Projet « Delta_Camera »
  • avec OpenCV, il récupère l’image de la caméra, calcule les positions des rondelles dans le repère de la caméra
  • calcule ces positions dans le repère du robot (calculs détaillés ici plus haut)
  • envoie ces informations à l’exécutable de Delta_Dynamixel, sur sa demande, en utilisant les « Named Pipe » (package NuGet IPC.NamedPipe) pour cette transmission.
  • Packages NuGet à installer pour OpenCV : OpenCvSharp4, OpenCvSharp4.Extensions, OpenCvSharp4.runtime.win, OpenCvSharp4.Windows, OpenCvSharp4.WpsExtensions
  • Téléchargeable ici

Pourquoi « Delta »

Précision

Les moteurs sont très précis… même si je n’ai pas de mesures précises à vous donner. Pour information, leur positionnement se fait avec un nombre compris en 0 et 4095.
Avec ma construction « faite maison » j’arrive à une précision d’environ 2mm ou 3mm sur le positionnement de la plateforme. Je pensais faire mieux, mais ce n’est pas grave, c’est déjà suffisant pour bien s’amuser !

Limitations

Les moteurs Dynamixel me permettent facilement de positionner la plateforme. Je maitrise donc cette position, mais pas la trajectoire. C’est parfois gênant : par exemple si je veux « raser le sol » à grande vitesse, la géométrie du robot delta fait que, dans certaines configurations, la plateforme va vouloir « rentrer » légèrement dans le « sol », elle va donc le « racler ».
Une solution est d’atteindre successivement les différents points de cette trajectoire. Mais dans ce cas le déplacement est plus lent.
Maitriser les trajectoires, les vitesses et les positions demanderait une commande très fine des moteurs, avec des notions de dynamiques. Je ne me suis pas lancé là-dedans, et je ne sais pas s »il est possible d’obtenir de bons résultats avec ces moteurs, et une commande en C#… Des moteurs pas à pas seraient peut-être plus adaptés.

Postface

J’ai essayé de rassembler ici (et dans la vidéo) des informations qui m’auraient aidé à réaliser ce projet. J’espère qu’elles seront utiles à d’autres personnes. Si c’est le cas, un commentaire me fera plaisir !