Cozmo SDK : déplacement du robot et des cubes

Dans un précédent TP sur Cozmo SDK, vous avez appris comment Cozmo peut détecter ses cubes et interagir avec ces objets . N’hésitez pas à le consulter si vous souhaitez en savoir plus sur la notion d’objet pour Cozmo ( cozmo.objects ).

Dans ce nouveau TP, vous allez apprendre à les attraper, les déplacer, les faire rouler ou les empiler en utilisant le SDK Python de Cozmo. Vous allez apprendre ici à :

1. Déplacer Cozmo (utilisation de la fonction go_to_pose )
2. Attraper un cube ( pickup )
3. Faire rouler un cube ou empiler deux cubes
4. Détecter les mouvements d’un cube avec l’accéléromètre intégré

Objectif : faire bouger les cubes, notion de référentiel

Durée : 40 minutes

• Niveau de difficulté : intermédiaire
• Compétence acquises : mouvement de cubes, déplacement de Cozmo

Les prérequis

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

• Cozmo SDK : comment activer le mode SDK de Cozmo
• Cozmo SDK : gestion et détection des light cubes

Déplacer le robot : la fonction go_to_pose

Nous allons voir comment prendre le contrôle des déplacements de Cozmo dans l’espace avec la fonction go_to_pose() .

robot.go_to_pose(Pose(100, 50, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()

La fonction go_to_pose applique une transformation au robot. La fonction prend 2 paramètres : la position destination cozmo.util.Pose et un paramètre qui indiquera si la position à atteindre est relative à la position actuelle du robot ou absolue dans le référentiel utilisé par le robot dans lequel il se trouve.

La Pose passée en paramètre indique les coordonnées à rejoindre relativement à la position actuelle du robot ou à son référentiel initial

• pose – ( cozmo.util.Pose ): la pose cible.
• relative_to_robot ( bool ) – information si la pose est relative ou absolue.

Dans l’exemple ci-après nous déplaçons le robot une première fois relativement à son référentiel puis une seconde fois dans le référentiel global. Pour que la position du robot soit à (0, 0, 0) il faut  soulever le robot, le retourner et le remettre en place.

Vous pouvez exécuter ce programme une première fois en réinitialisant la position avant l’exécution et en observant les coordonnées affichées. Exécuter le programme une seconde fois, cette fois en réinitialisant la position avant et au cours de la pause de 5 secondes qui a lieu entre les 2 appels à la fonction go_to_pose .

import cozmo
from cozmo.util import degrees, Pose
import time

def cozmo_program(robot: cozmo.robot.Robot):
	print("Robot position before (relative) movement"   str(robot.pose.position.x_y_z))
	print("Robot moving (relative)...")
	robot.go_to_pose(Pose(100, 50, 0, angle_z=degrees(0)), relative_to_robot=False).wait_for_completed()
	print("Robot position after (relative) movement"   str(robot.pose.position.x_y_z))

	time.sleep(5)

	print("Robot position before (absolute) movement"   str(robot.pose.position.x_y_z))
	print("Robot moving (absolute)...")
	robot.go_to_pose(Pose(100, 50, 0, angle_z=degrees(0)), relative_to_robot=False).wait_for_completed()
	print("Robot position after (absolute) movement"   str(robot.pose.position.x_y_z))

cozmo.run_program(cozmo_program)

L’output ressemble à ceci :

On remarquera que le deuxième mouvement est très limité puisque le robot a effectué un déplacement de la position (0,0,0) vers la position (100, 50, 0) et il est censé être à la position (100, 50) lorsqu’on lui demande d’aller à la même position.

La position du robot n’étant pas parfaitement exacte il y a un léger décalage entre la position demandée et la position réellement atteinte .

Soulever le robot Cozmo a pour effet de réinitialiser ses coordonnées..

Déplacer les cubes

Pour pouvoir déplacer un cube avec un programme en Python, il faut :

1. Connaître son emplacement
2. Aller le chercher et le soulever
3. Le porter jusqu’à son nouvel emplacement
4. Le poser

Note : il est possible de poser un cube sur un autre cube ou un autre objet.

Pour en savoir plus sur la détection de cubes (1) , se référer au TP : « Cozmo SDK : gestion et détection des light cubes.

Pour en savoir plus sur le déplacement de Cozmo (2) , se référer au TP : « Cozmo SDK : coder les déplacements de Cozmo » (en cours de rédaction).

Ici, vous allez apprendre à soulever le cube (2) , le porter (3) et le poser (4).

• pickup _object ( obj , use_pre_dock_pose=True , in_parallel=False , num_retries=0 )
• place_object_on_ground_here ( obj , in_parallel=False , num_retries=0 )
• place_on_object ( obj , use_pre_dock_pose=True , in_parallel=False , num_retries=0 )

Ces fonctions permettront de saisir un objet, de le poser au sol ou de la poser sur un autre objet.

Dans le code ci-dessous, le robot Cozmo soulève le cube 1, s déplace jusqu’à la position location1, puis pose le cube sur le sol.

        robot.pickup_object(cube1, num_retries=3).wait_for_completed()
        robot.go_to_pose(location1).wait_for_completed()
        robot.place_object_on_ground_here(cube1, num_retries=3).wait_for_completed()

Le code ci-dessous permet d’empiler 2 cubes :

        robot.pickup_object(cube3, num_retries=3).wait_for_completed()
        robot.place_on_object(cube2, num_retries=3).wait_for_completed()

Vous pouvez télécharger un exemple complet de ces codes sur le GitHub de Génération Robots : cube_pickup_transport.py .

''' Transporter des cubes

Ce script permet de détecter les 3 cubes, défini 1 position, déplace 1 cube à cette position puis empiler 2 cubes

Utilisation : 
- Placer les 3 cubes alignés devant Cozmo
- Executer le script
'''

import time
from cozmo.util import degrees, Pose

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)  # le cube trombone
    cube2 = robot.world.get_light_cube(LightCube2Id)  # le cube lampe 
    cube3 = robot.world.get_light_cube(LightCube3Id)  # le cube ab sur un T

    # scan de l'environnement pour trouver les cubes
    look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
    try:
        _cubes = robot.world.wait_until_observe_num_objects(num=3, object_type=cozmo.objects.LightCube, timeout=60)
        print("Found %s cubes: %s" % (len(_cubes), _cubes))
    except asyncio.TimeoutError:
        print("Didn't find a cube")
    finally:
        look_around.stop()
    
    # position arbitraire
    location1 =  Pose(cube1.pose.position.x-200, cube1.pose.position.y-50 , 0, angle_z=degrees(0))

    # déplacer le cube 1 à la position location1
    if cube1 is not None:
        robot.pickup_object(cube1, num_retries=3).wait_for_completed()
        robot.go_to_pose(location1).wait_for_completed()
        robot.place_object_on_ground_here(cube1, num_retries=3).wait_for_completed()
    else:
        cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")

    # placer le cube 3 sur le cube 2
    if cube2 is not None and cube3 is not None:
        robot.pickup_object(cube3, num_retries=3).wait_for_completed()
        robot.place_on_object(cube2, num_retries=3).wait_for_completed()
    else:
        cozmo.logger.warning("Cozmo is not connected to a LightCube1Id cube - check the battery.")

cozmo.run_program(cozmo_program,  use_viewer=True)

Faire rouler un cube ou empiler des cubes

Pour faire rouler un cube ou empiler deux cubes, le robot Cozmo doit connaître leur position. Si Cozmo connait la position d’au moins un cube, il peut le faire rouler. S’il connait la position de deux cubes, alors il peux les empiler.

Nous avons déjà vu ci-dessus une fonction d’empilement de cubes . Nous allons voir ci-dessous une autre méthode. Cette dernière offre moins de contrôle sur le cube sélectionné.

Cette fonctionnalité est disponible grâce aux deux behaviors suivants :

• cozmo.behavior.BehaviorTypes.RollBlock
• cozmo.behavior.BehaviorTypes.StackBlocks

Ci-dessous,  un exemple d’utilisation. Placez un cube devant Cozmo et ce dernier le fera rouler. Si vous positionnez deux cubes, il les empilera.

import cozmo


def cozmo_program(robot: cozmo.robot.Robot):
    lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)

    cubes = robot.world.wait_until_observe_num_objects(num=2, object_type=cozmo.objects.LightCube, timeout=10)

    print("Found %s cubes" % len(cubes))

    lookaround.stop()

    if len(cubes) == 0:
        robot.play_anim_trigger(cozmo.anim.Triggers.MajorFail).wait_for_completed()
    elif len(cubes) == 1:
        robot.run_timed_behavior(cozmo.behavior.BehaviorTypes.RollBlock, active_time=60)
    else:
        robot.run_timed_behavior(cozmo.behavior.BehaviorTypes.StackBlocks, active_time=60)


cozmo.run_program(cozmo_program, use_viewer=1)

Cette méthode est la plus simple, mais n’offre pas de véritable contrôle au niveau de l’empilement des cubes. Dans autre TP, nous verrons comment créer un script Python qui permet de construire une pyramide avec les trois cubes.

Prochainement de nouveaux contenus

Détecter le mouvement d’un cube

Un autre fonctionnalité intéressante des cubes est l’accéléromètre qui est embarqué dedans. Il permet de détecter les mouvements des cubes même lorsqu’il n’ont pas été vus par Cozmo !

Cozmo peut ainsi « sentir » si l’un de ses cubes a été secoué ou simplement qu’il a été soulevé ou reposé.

Pour réaliser ce « tour de magie » il faut écouter l’événement cozmo.objects.EvtObjectMoving . Lorsque cet événement est émis, il faut alors exécuter une fonction callback et traiter l’événement. L’exemple ci-dessous affiche un message en ligne de commande, qui contient l’accélération enregistrée et la durée du déplacement.

3 événements peuvent être utilisés pour la détection des déplacements des cubes de Cozmo.

• cozmo.objects.EvtObjectMovingStarted
• cozmo.objects.EvtObjectMoving
• cozmo.objects.EvtObjectMovingStopped

'''Démonstration des événements de détection de mouvement des cubes

Les cubes n'ont pas besoin d'être vu par la caméra du robot

'''

import time

import cozmo


def cb_object_moving_started(evt, **kw):
    """ fonction callback appelée à chaque fois que l'événement EvtObjectMovingStarted est émis
        Si un cube commence à être déplacé (détection via l'accéléromètre du cube) l'événement est émis 
            
        On peut récupérer l'accélération du cube
    """
    
    print("Le cube %s a commencé à bouger: accéleration=%s" %
          (evt.obj.object_id, evt.acceleration))


def cb_object_moving(evt, **kw):
    """ fonction callback appelée à chaque fois que l'événement EvtObjectMoving est émis
        Si un cube commence à être déplacé (détection via l'accéléromètre du cube) l'événement est émis 

        On peut récupérer l'accélération et la durée du déplacement
    """
    print("Le cube %s est en mouvement: accéleration=%s, durée=%.1f secondes" %
          (evt.obj.object_id, evt.acceleration, evt.move_duration))


def cb_object_moving_stopped(evt, **kw):
    """ fonction callback appelée à chaque fois que l'événement EvtObjectMovingStopped est émis
        Si un cube arrête d'être déplacé (détection via l'accéléromètre du cube) l'événement est émis 

        On peut récupérer la durée du déplacement de l'objet
    """
    print("Le cube %s a arrêté de bouger: durée=%.1f secondes" %
          (evt.obj.object_id, evt.move_duration))


def cozmo_program(robot: cozmo.robot.Robot):
    # on ajoute des handlers pour chaque événement
    robot.add_event_handler(cozmo.objects.EvtObjectMovingStarted, cb_object_moving_started)
    robot.add_event_handler(cozmo.objects.EvtObjectMoving, cb_object_moving)
    robot.add_event_handler(cozmo.objects.EvtObjectMovingStopped, cb_object_moving_stopped)

    # boucle infinie jusqu'à la combinaison de touches CTRL+C
    print("Presser CTRL-C pour arrêter")
    while True:
        time.sleep(1.0)

Voici l’affichage sur la console :

Observation : les événements cozmo.objects.EvtObjectMovingStarted et cozmo.objects.EvtObjectMovingStopped sont émis dès qu’un cube est soulevé ou que le mouvement est interrompu.

Pour déclencher l’événement cozmo.objects.EvtObjectMoving plusieurs fois,  il faut secouer le cube énergiquement, ou bien le faire tourner sur lui même. Un déplacement où une accélération peu marqués ne sont pas détectés.