Blog

ContrĂ´ler un robot humanoĂŻde (Nao) avec une Kinect

Posté le
L’auteur: Maxence VisĂ©e
Technologies utilisées: Nao, C#
Niveau de difficulté: 200

Cet article explique les aspects théoriques et pratiques à appliquer lorsque l’on souhaite contrôler un robot Nao avec une caméra Kinect. Ces contrôles se limitent à ceux des bras et aux déplacements.

Interpolation des mouvements des bras perçus par Kinect sur le Nao

Aspects théoriques

Il est d’abord important de se rendre compte des capacités, offertes par Kinect et Nao, nous permettant d’atteindre ce but précis.

La récupération des coordonnées des articulations d’un squelette

Chaque « joint » ou « articulation » d’un squelette suivi par une Kinect est récupérable sous forme d’une coordonnée en trois dimensions. Il est ainsi possible de récupérer les composantes X, Y et Z de chacune d’entre elles.

Mise en mouvement des articulations du robot

Nao est équipé d’une série de moteurs et chacun de ceux-ci lui permet d’activer une articulation.

Pour pouvoir les enclencher, les moteurs ont besoin de connaitre l’angle d’inclinaison du membre qu’il va mettre en mouvement. La seule unité reconnue par Nao concernant ces angles est le Radian.

Détermination de l’angle

Pour pouvoir calculer l’angle qui sera envoyé vers le robot il va falloir user de formules de trigonométries. Étant donné que tous les angles envoyés vers Nao utilisent cette technique, un seul exemple sera expliqué dans cette partie. Imaginons que nous aimerions réaliser ceci :

  • Il va d’abord falloir crĂ©er deux vecteurs Ă  partir des 3 « joints » (HAND _RIGHT, ELBOW_RIGHT, SHOULDER_RIGHT) en utilisant la soustraction des vecteurs :
Vecteur1.X = ELBOW_RIGHT.X – HAND_RIGHT.X
Vecteur1.Y = ELBOW_RIGHT.Y – HAND_RIGHT.Y
Vecteur1.Z = ELBOW_RIGHT.Z – HAND_RIGHT.Z

Vecteur2.X = SHOULDER_RIGHT.X – ELBOW_RIGHT.X
Vecteur2.Y = SHOULDER_RIGHT.Y – ELBOW_RIGHT.Y
Vecteur2.Z = SHOULDER_RIGHT.Z – ELBOW_RIGHT.Z
  • DĂ©termination de la longueur des 2 vecteurs :
Longueur1 = SQRT (Vecteur1.X² + Vecteur1.Y² + Vecteur1.Z²)
Longueur2 = SQRT (Vecteur2.X² + Vecteur2.Y² + Vecteur2.Z²)
  • Normalisation des 2 vecteurs. Cette technique va faire en sorte que les 2vecteurs soient dans la mĂŞme Ă©chelle (entre 0 et 1) :
Vecteur1.X = Vecteur1.X / Longueur1
Vecteur1.Y = Vecteur1.Y / Longueur1
Vecteur1.Z = Vecteur1.Z / Longueur1

Vecteur2.X = Vecteur2.X / Longueur2
Vecteur2.Y = Vecteur2.Y / Longueur2
Vecteur2.Z = Vecteur2.Z / Longueur2
  • Calcul de l’angle en degrĂ©s et en radians :
ThĂŞta= aCos(Vecteur1.X * Vecteur2.X + Vecteur1.Y * Vecteur2.Y + Vecteur1.Z * Vecteur2.Z)
Angle_en_degres = Theta * 180 / PI
Angle_en_radians = Angle_en_degres * (PI/180)

En théorie il ne reste plus qu’à envoyer ce dernier angle vers Nao même si, en pratique, il faudra adapter celui-ci étant donné que Nao et Kinect ne travaillent pas dans le même plan cartésien.

Aspects pratiques

Nous allons crĂ©er une mĂ©thode qui va recevoir en arguments 3 coordonnĂ©es et qui va nous renvoyer un float contenant l’angle en degrĂ©s des 2 vecteurs formĂ©s avec ces points.

NB : La classe Coordonnee que j’utilise dans le code se compose simplement de 3 double (x,y,z). Mais vous pouvez toujours utiliser la classe Vector3 qui fait exactement la même chose.

float GetLimbAngle(Coordonnee coord0,Coordonnee coord1 ,Coordonnee coord2)
{
	Coordonnee[] vector = new Coordonnee[4];
	// Calcule le premier vecteur
	// V_{H,E} = V_{E} - V_{H}
	vector[0] = new Coordonnee(coord1.X - coord2.X, coord1.Y - coord2.Y, coord1.Z - coord2.Z);
	// Calcule le second vecteur
	// V_{E,S} = V_{S} - V_{E}
	vector[1] = new Coordonnee(coord0.X - coord1.X, coord0.Y - coord1.Y, coord0.Z - coord1.Z);
	// Calcule la longueur des vecteurs
	float v0_magnitude = (float)(Math.Sqrt(vector[0].X * vector[0].X + vector[0].Y * vector[0].Y + vector[0].Z * vector[0].Z));
	float v1_magnitude = (float)(Math.Sqrt(vector[1].X * vector[1].X + vector[1].Y * vector[1].Y + vector[1].Z * vector[1].Z));
	if (v0_magnitude != 0.0 && v1_magnitude != 0.0)
	{
		vector[0] = new Coordonnee((float)(vector[0].X * (1.0 / v0_magnitude)), (float)(vector[0].Y * (1.0 / v0_magnitude)), (float)(vector[0].Z * (1.0 / v0_magnitude)));
		vector[1] = new Coordonnee((float)(vector[1].X * (1.0 / v1_magnitude)), (float)(vector[1].Y * (1.0 / v1_magnitude)), (float)(vector[1].Z * (1.0 / v1_magnitude)));

		// Calcule l'angle entre les deux vecteurs exprimé en radian
		float theta = (float)(Math.Acos(vector[0].X * vector[1].X + vector[0].Y * vector[1].Y + vector[0].Z * vector[1].Z));
		float angle = (float)(theta * 180 / Math.PI);
		return -angle;
	}
	else
	{
		//histoire de retourner une erreur. mais ce n'est pas utilisé ici.
		return 500.0f;
	}
}

Simple réutilisation de la théorie vue plus haut.

Il ne reste plus qu’Ă  convertir l’angle en radian et Ă  envoyer l’information Ă  Nao.

Dans le code qui suit, on a la correspondance suivante:

 coord[4] -> HandRight
 coord[5] -> ElbowRight
 coord[6] -> ShoulderRight
//Right Elbow Roll
_rElbowRoll = GetLimbAngle(_coord[6], _coord[5], _coord[4]);
//degrees to radian
_rElbowRoll = (float)(((_rElbowRoll * (Math.PI / 180))) * -1);
if (_rElbowRoll <= 1.55 && _rElbowRoll >= 0.01)
_mp.setAngles("RElbowRoll", _rElbowRoll, _speed);

La condition permet de limiter les mouvements de Nao. Il vaut mieux lui fixer des limites pour chaque moteur sinon son bras reviendra Ă  sa position initiale Ă  chaque fois que le vĂ´tre ira trop loin (et ça s’applique pour tous les moteurs). Ce n’est pas forcĂ©ment beau Ă  voir.

Avec ce code vous devriez pouvoir faire pas mal de choses avec lui. Le tout est de bien choisir les points qui composeront les vecteurs et de comprendre que les angles doivent souvent être adaptés (entendez par-là qu’il faut souvent rajouter ou soustraire des radians étant donné que l’angle perçu par la Kinect ne donne pas forcément le même résultat quand il est envoyé au robot).

Gestion des déplacements du Nao avec Kinect

Dans cette deuxième partie, on va parcourir la stratégie mise en place pour contrôler les déplacements du Nao toujours grâce à la Kinect.

Aspects théoriques

L’image suivante reprĂ©sente les zones dans lesquelles l’utilisateur doit se dĂ©placer pour contrĂ´ler le robot:

Il faut imaginer que ces lignes se trouvent virtuellement au sol et que l’utilisateur de la Kinect se trouve au centre au moment de leur création. Cette dernière est effectuée lorsque l’utilisateur est prêt et le montre en mettant sa main droite devant son visage.

Tant que l’utilisateur se trouve derrière les Safe lines (dans la zone bleue que l’on appellera « safe zone »), celui-ci contrôle uniquement les bras du robot. Les contrôles activés derrière les Control Lines sont la marche du Nao. En pratique, un pas vers l’avant activerait la marche, un pas vers l’arrière activerait le recule, un pas vers la droite ou la gauche activerait les pas latéraux. De plus, quand l’utilisateur tourne ses épaules, le robot changerait de direction lorsque celui-ci avance.

Aspects pratiques

Contrôler si l’utilisateur est prêt
private void CheckUserIsReady()
{

	if (_coordinateJoints[4].X != 0 && _coordinateJoints[0].X != 0 && _coordinateJoints[8].X != 0)
	{
		if (_coordinateJoints[4].X <= _coordinateJoints[8].X + 0.1 &&
		    _coordinateJoints[4].X >= _coordinateJoints[8].X - 0.1 &&
		    _coordinateJoints[4].Y <= _coordinateJoints[8].Y + 0.1 &&
			_coordinateJoints[4].Y >= _coordinateJoints[8].Y - 0.1)
		{
			// TODO: ajouter la suite du code ici...
		}
	}
}

La première condition vérifie si les coordonnées X des 2 mains et de la tête ne sont pas égales à 0.

On regarde ensuite si les positions X et Y de la main (indice 4) se trouvent dans la « zone » de la tête (indice 8). Cette « zone » est un carré, de 0.2 de côté, formé autour de la position de la tête.

eGestureActived.Fill = new SolidColorBrush(Colors.Green);
eGestureActived.Stroke = new SolidColorBrush(Colors.Green);

L’interface graphique possède un indicateur signalant si le contrôle est activé. Celui-ci passe au vert quand c’est le cas.

//basis information
//Initial positions of the righfoot, the right & left shoulders.
_initPosition = _coordinateJoints[9];
_initRightShoulder = _coordinateJoints[6];
_initLeftShoulder = _coordinateJoints[2];

// Draw safe zone's lines and angle
_anteriorSafeLine = (float)_initPosition.Z - _safetyDist;
_posteriorSafeLine = (float)_initPosition.Z + _safetyDist;

_rightSafeLine = (float)_initPosition.X + _safetyDist;
_leftSafeLine = (float)_initPosition.X - _safetyDist;

// Draw control zone's lines and angle
_anteriorControlLine = _anteriorSafeLine - _controlDist;
_posteriorControlLine = _posteriorSafeLine + _controlDist;

_rightControlLine = _rightSafeLine + _controlDist;
_leftControlLine = _leftSafeLine - _controlDist;

_userIsReady = true;

Création des lignes virtuelles.

Activation des mouvements/déplacements du robot en fonction de la position de l’utilisateur
private void ThreadTick(object sender, EventArgs eventArgs)
{
    if (!_userIsReady)
    {
        CheckUserIsReady();
    }
    if (_connected)
    {
        //if there is a player tracked
        if (_nbSkeletonTracked > 0)
        {
            if (_userIsReady)
            {
                CheckBodyLocation();
            }
        }
    }
}

Une fois que l’utilisateur a montré qu’il était prêt et connecté à Nao, l’application vérifiera continuellement sa position par rapport aux différentes lignes virtuelles dans un thread. Ce thread fait appel à la méthode « CheckBodyLocation ».

 L'ensemble des 3 extraits de codes suivants constituent l'ensemble de la méthode CheckBodyLocation.
private void CheckBodyLocation()
{
    float currentAngleTorso = (float)( GetTorsoRotation(_coordinateJoints[2], _coordinateJoints[6])*Math.PI/180.0);

Calcul de l’angle actuel de rotation de l’utilisateur.

if ((_anteriorControlLine <= _coordinateJoints[9].Z) &&
    (_coordinateJoints[9].Z <= _anteriorSafeLine))
{
    //move forward
    if ((_controlRot >= currentAngleTorso) && (currentAngleTorso >= _safetyRot))
    {
        if ((_coordinateJoints[6].Z + 0.1f) < _coordinateJoints[2].Z)
        {
            //rotate left
            _motionProxy.walkTo(0, 0, -0.78f);
        }
        else if (_coordinateJoints[6].Z > (_coordinateJoints[2].Z + 0.1f))
        {
            //rotate right
            _motionProxy.walkTo(0, 0, 0.78f);
        }
    }
    else
    {
        _motionProxy.walkTo(0.1f, 0, 0);
    }
}

Si l’utilisateur (la vérification s’effectue sur son pied droit) se trouve au-delà de la ligne virtuelle _anteriorControlLine et avant la ligne virtuelle _anteriorSafeLine cela signifie qu’il veut faire avancer le robot. On vérifie d’abord l’angle de rotation de ses épaules. Si celui-ci est supérieur à un angle de contrôle, alors on active cette rotation. Sinon, on demande au robot de marcher.

	else if ((_posteriorSafeLine <= _coordinateJoints[9].Z) &&
			 (_coordinateJoints[9].Z <= _posteriorControlLine))
	{
		//move backward
		_motionProxy.walkTo(-0.1f,0,0);
	}
	else if ((_leftControlLine <= _coordinateJoints[9].X) && (_coordinateJoints[9].X <= _leftSafeLine))
	{
		// Step left
		_motionProxy.walkTo(0,-0.1f,0);
	}
	else if ((_rightControlLine >= _coordinateJoints[9].X) && (_coordinateJoints[9].X >= _rightSafeLine))
	{
		// Step right
		_motionProxy.walkTo(0, 0.1f, 0);
	}
	else
	{
		CheckArms();
	}
}

Dans cette partie du code, on vérifie encore la position de l’utilisateur mais celle-ci active le déplacement arrière, les pas de côté, et le contrôle des bras.


Ă€ propos de l’auteur

Je m’appelle Maxence VisĂ©e, Ă©tudiant en dernière annĂ©e d’informatique de gestion Ă  la HELHA Mons. Dans le cadre de mon stage de fin d’Ă©tudes, j’ai eu l’occasion de participer Ă  un projet utilisant des technologies de pointes qui sont NAO et Kinect. Pour en savoir plus Ă  ce sujet et sur moi, visitez mon blog : naokinect.wordpress.com !
/Viseem@vimaxence

Ajoutez cet article Ă  vos favoris.

réponses

  1. Wally a commenté :

    Dear Maxence Visée

    I’m a senior student who is now focus on this area, and I am very interested in your article. But unfortunately, I do not understand any French. So could you give me an English copy of this article or the whole source code in order to research.

    Best
    Wally

  2. Valentin a commenté :

    Bonjour, j’aurais voulu savoir oĂą est-ce que l’on doit mettre tout ces codes ? Dans Choregraphe, dans une Appli Kinect ?

    Cordialement.

    • Renaud a commentĂ© :

      Hello Valentin,

      Vous travaillez sur un projet Kinect/Nao ? :) C’est plutĂ´t cool ! HĂ©sitez pas Ă  partager si vous avez des choses Ă  montrer, j’relaierai l’info avec plaisir. ;)

      Tout ce code doit se trouver dans une application pouvant référence le SDK Kinect. Donc vous pourriez très bien le mettre dans une application WPF par exemple.

      L’idĂ©e est de faire une application Kinect classique, dans laquelle vous allez utiliser le SDK NAOqi pour communiquer avec le robot et lui envoyer des infos sur les mouvements qu’il doit faire suite aux calculs ci-dessus. La variable _motionProxy reprĂ©sente par exemple le proxy permettant de contrĂ´le les dĂ©placements du robot.

      J’imagine que le SDK a Ă©voluĂ© depuis l’annĂ©e dernière, mais le principe reste le mĂŞme.

      Notez aussi que dans les dernières versions du Kinect SDK, on a plein d’informations très pratiques comme des matrices d’orientation pour les diffĂ©rents membres du corps. Cela pourrait peut-ĂŞtre Ă©viter certains des calculs qui devaient ĂŞtre faits Ă  l’Ă©poque comme expliquĂ© dans cet article !

      Bonne continuation :)

      Renaud

  3. Pierre a commenté :

    Bonjour,

    Tout d’abord bravo pour votre travail et votre tutoriel très bien expliquĂ©.
    Je travaille actuellement sur NAO/Kinect et j’ai trouvĂ© très intĂ©ressant votre site.

    J’aurais une remarque Ă  vous faire concernant une autre mĂ©thode pour contrĂ´ler NAO avec la Kinect, sur laquelle je travaille actuellement, voici ma remarque:
    Pourquoi n’utilisez-vous pas le skeleton que l’on trouve dans les Samples (C# par exemple, langage que j’utilise)qui vous Ă©viterait de passer par tous les calculs?

    C’est sur quoi je travaille actuellement, Ă  savoir relier le skeleton (public class Skeleton) aux articulations de NAO, pour qu’il puisse imiter les gestes que l’on rĂ©alise devant la Kinect.

    Cordialement,
    Pierre

    • Renaud a commentĂ© :

      Bonjour Pierre,

      Vous avez tout Ă  fait raison pour l’utilisation du Skeleton! En fait, Ă  l’Ă©poque de la rĂ©alisation de ce projet, le Kinect SDK Ă©tait en version 1.0 et ne fournissait pas encore les informations de rotation des Joints.

      Aujourd’hui c’est sans doute plus prĂ©cis d’utiliser les matrices de rotation, mais cela ne marchera toute de mĂŞme pas tel quel. Je m’explique: les articulations de Nao ne correspondent pas exactement Ă  celles du Skeleton et n’ont pas les mĂŞmes rayons de libertĂ©. Il va donc falloir faire un petit mapping pour que tout tourne correctement.

      Avec la version actuelle du SDK, il est Ă©galement possible de contrĂ´ler l’ouverture et la fermeture des mains de Nao!

      Si vous Ă©crivez Ă  ce sujet n’hĂ©sitez pas Ă  partager un lien :)

      Cordialement,
      Renaud