Contrôler un robot humanoïde (Nao) avec une Kinect

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