Cozmo SDK : gestion des cubes

Cozmo SDK : gestion et détection des light cubes

This entry was posted in Education & robotique on by Louis-Gabriel Pouillot.

Le module Python cozmo.objects et la gestion des cubes de Cozmo

Dans ce TP pour Cozmo SDK, nous abordons la gestion des Power Cubes de Cozmo. Ces cubes ajoutent une couche d’interaction supplémentaire entre le robot et son environnement, mais aussi avec les humains. Cozmo se sert également de ces cubes pour cartographier son environnement.

cozmo-power-cubes-mapping-slam
Cozmo SDK – Utilisation des Power Cubes pour le SLAM

Détection d’objets

Les Power Cubes(LightCube dans le SDK) sont des objets ( cozmo.objects ) pouvant être reconnus et détectés par Cozmo (détection via la caméra de Cozmo ou par détection d’un changement de valeur dans les cubes, quand on les tapote).

Marqueur personnalisé pour Cozmo SDK

Cozmo peut reconnaître d’autres choses que ses cubes grâce à sa caméra, il suffit d’attacher des marqueurs personnalisés sur ces nouveaux objets pour les distinguer. Voici un exemple de marqueur ci-dessous :

Il est aussi possible de créer des éléments virtuels dans le monde de Cozmo. Ce sera l’objet d’un autre tutoriel.

Fonctionnement des cubes

Les Power Cubes de Cozmo embarquent un accéléromètre, un module BlueTooth et 4 LEDs. Cozmo peut reconnaître ses cubes (vision) mais aussi détecter quand ils sont déplacés ou tapotés.

Dans ce TP, vous allez apprendre à utiliser les 4 fonctionnalités suivantes :

  • Détecter les cubes
  • Allumer les LEDs
  • Déterminer la position des cubes
  • Détecter le tapotement sur un cube

Objectif : détecter des objets et interagir avec

  • Durée : 45 minutes
  • Niveau de difficulté : intermédiaire
  • Compétence acquises : détection des cubes et communication entre Cozmo et ses cubes

Les prérequis

Vous devez avoir acquis les notions abordées dans les tutoriels ci-dessous

Par ailleurs vous devrez importer certains éléments de la librairie Cozmo tel que ci-dessous : 

from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id

Les nouvelles notions que vous allez aborder dans ce TP

  • La fonction cozmo.object

Détection des cubes

Les cubes seront détectés par la caméra embarquée de Cozmo. Cette détection visuelle permet de positionner le cube dans le référentiel du robot.

Parcours de l’environnement

Le robot Cozmo embarque des behaviors qui se chargent de scanner l’environnement autour de lui pour détecter les objets présents, comme ses cubes. Le robot se déplace et scanne l’environnement grâce à sa camera embarquée.

LookAroundInPlace est l’un de ces behaviors . 

look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
...
look_around.stop()

Fonctions de détection des cubes

Lorsque Cozmo se déplace dans son environnement, il cherche à détecter de nouveaux objets (grâce aux marqueurs visuels se trouvant sur les différentes faces de l’objet).

Le cube 1 dispose d’une image qui ressemble à un trombone, le cube 2 ressemble à une lampe ou à un cœur et le cube 3 ressemble à un bébé dans son siège.

Reconnaître les 3 cubes de Cozmo

Deux fonctions peuvent lancer la recherche de ces marqueurs :

  • robot.world.wait_until_observe_num_objects(…)
  • robot.world.wait_for_observed_light_cube(…)

wait_until_observe_num_objects prend en argument les nombre d’objets à détecter et le type d’objets ainsi qu’un timeout si la recherche n’aboutit pas. Il retourne une liste d’objets. 

cubes = robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube, timeout=60)

Dans l’exemple ci-dessus, 3 objets de type cozmo.objects.LightCube sont recherchés.

wait_for_observed_light_cube permet de scanner l’environnement à la recherche d’un cube. La recherche s’interrompt au premier cube trouvé. La fonction prend un paramètre timeout si la recherche n’aboutit pas. Elle retourne une instance de l’objet s’il est vu par Cozmo. 

cube = robot.world.wait_for_observed_light_cube(timeout=30)

Cette étape est indispensable si vous souhaitez déplacer des cubes, les pousser, les empiler afin qu’ils soient présents dans le monde World du robot.

Allumage et extinction des LEDs des cubes

L’objet cozmo.objects.LightCube

Avant d’allumer un cube, il faut d’abord le sélectionner. Un cube est accessible par son id grâce à la fonction get_light_cube ( cube_id )

Le cube_id est une valeur parmi LightCube1Id , LightCube2Id et LightCube3Id

La fonction retourne un objet cozmo.objects.LightCube 

def lights():
    cube1 = robot.world.get_light_cube(LightCube1Id)  # il ressemble à un trombonne
    cube2 = robot.world.get_light_cube(LightCube2Id)  # il ressemble à une lampe ou un coeur
    cube3 = robot.world.get_light_cube(LightCube3Id)  # il ressemble aux lettres ab sur un T

    # on allume le cube
    if cube1 is not None:
        cube1.set_lights(cozmo.lights.red_light)
    else:
        cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")

    time.sleep(3)

    # on éteint le cube
    if cube1 is not None:
        cube1.set_lights(cozmo.lights.off_light)
    else:
        cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")

L’objet cozmo.lights.Light object

Il existe une infinité de couleurs mais 4 sont prédéfinies et facilement utilisables grâce à la fonction set_lights de l’objet LighCube .

Ces couleurs sont rouge, bleu, vert et blanc, vous pouvez y accéder via l’objet cozmo.lights.red_light, cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.white_light.

Il est aussi possible de passer des valeurs numériques.

Voici un exemple complet permettant de manipuler les LEDs des cubes lumineux de Cozmo.

cube_lights_demos.py affichage brut 
'''Control Cozmo's Cube lights

Ce script montre un exemple on allume les leds de cubes
'''

import time

import cozmo
from cozmo.objects import LightCube1Id, LightCube2Id, LightCube3Id

def shift(l, n=0):
    """
        Fonction qui décale la position des éléments d'une liste de n positions
    """
    return l[n:] + l[:n]


def cozmo_program(robot: cozmo.robot.Robot):
    cube1 = robot.world.get_light_cube(LightCube1Id)  # looks like a paperclip
    cube2 = robot.world.get_light_cube(LightCube2Id)  # looks like a lamp / heart
    cube3 = robot.world.get_light_cube(LightCube3Id)  # looks like the letters 'ab' over 'T'


    red, green, blue, white=cozmo.lights.red_light, cozmo.lights.green_light, cozmo.lights.blue_light, cozmo.lights.white_light
    colors=[red, white, white, white]

    time.sleep(5)

    if cube1 is not None:
        for l in [red, green, blue, white]: 
            cube1.set_lights(l)
            time.sleep(1)
    else:
        cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")

    if cube2 is not None:
        for i in range(20):
            colors = shift(colors, n=1)
            cube2.set_light_corners(colors[0], colors[1], colors[2], colors[3])
            time.sleep(0.4)
    else:
        cozmo.logger.warning("Cozmo is not connected to a LightCube2Id cube - check the battery.")


    if cube1 is not None:
        cube1.set_lights(cozmo.lights.off_light)
    else:
        cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")

    if cube2 is not None:
        cube2.set_lights(cozmo.lights.off_light)
    else:
        cozmo.logger.warning("Cozmo is not connected to a LightCube2Id cube - check the battery.")

Récupération des coordonnées des cubes

Un cube détecté par Cozmo renvoie les coordonnées de sa position. La position d’un cube qui n’est pas positionné dans le monde de Cozmo aura pour coordonnées (0, 0, 0).

Un objet dispose d’une variable de type Pose . L’objet Pose contient lui même un objet de type Position qui contient les coordonnées x, y et z de l’objet.

Pour afficher les coordonnées du cube 1 on peut utiliser le code suivant : 

print(cube1.pose.position.x, cube.pose.position.y, cube.pose.position.z)

On peut également récupérer les coordonnées dans un tuple (x, y, z). 

print(cube1.pose.position.x_y_z)

Détection d’un tapotement sur le cube

Les cubes disposent d’un accéléromètre, qui renvoie ses valeurs au robots Cozmo via Bluetooth. Cozmo peut donc « sentir » quand quelqu’un tapote sur un de ces cubes.

Lorsque cela se produit, un événement cozmo.objects.EvtObject Tapped est émis.

Voici comment le récupérer un événement et le traitement par une fonction callback : 

def on_cube_tapped(event, *, obj, tap_count, tap_duration, **kw): 
    print("Received a tap event", event, obj)

handler = robot.add_event_handler(cozmo.objects.EvtObjectTapped,on_cube_tapped)