COZMOのプログラミング

クリスマスなので、COZMOにプレゼントを届けさせるプログラムを作ることにしました。



キューブだけでなくペットも認識するので、簡単にプログラムを作れると思ったのですが






COZMOがカメラを通して、位置情報を取得できるのはキューブとチャージャーだけのようです。

どうやら、キューブに印刷してある模様の見かけの大きさから距離を計算しているようです。
Anki Cozmo Python SDKのサイトにある、3D viewer
https://github.com/anki/cozmo-python-sdk/blob/master/examples/apps/3d_viewer.py
のプログラムを動かしてみると、COZMOは、同じ位置から周囲を見渡すだけで、キューブの位置の地図を作れるのがわかります。

SDKのAPIにある　robot.world.wait_for_observed_pet　メソッドで、ペットを見つけて、go_to_object(pet, distance_mm(65.0))
とか、すればペットにプレゼントを届けられると思ったのに、ダメでした。残念...

ということで、今回は、ペットの方向にちょっと行くというところまで、

import time
import cozmo
from cozmo.util import degrees, distance_mm, speed_mmps

def cozmo_program(robot: cozmo.robot.Robot):
    # If the robot was on the charger, drive them forward and clear of the charger
    if robot.is_on_charger:
        # drive off the charger
        robot.drive_off_charger_contacts().wait_for_completed()
        robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
        # Start moving the lift down
        robot.move_lift(-3)
        # turn around to look at the charger
        robot.turn_in_place(degrees(180)).wait_for_completed()
        # drive backwards away from the charger
        robot.drive_straight(distance_mm(-60), speed_mmps(50)).wait_for_completed()
        # wait half a second to ensure Cozmo has seen the charger
        time.sleep(0.5)
    # Tilt the head to be level
    robot.set_head_angle(degrees(0)).wait_for_completed()
    robot.set_lift_height(0, in_parallel = True)
    robot.wait_for_all_actions_completed()
    # 
    robot.say_text(u"ぷれぜんと　を　さがす").wait_for_completed()
    lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
    cubes = robot.world.wait_until_observe_num_objects(num=1, object_type=cozmo.objects.LightCube, timeout=60)
    lookaround.stop()
    if len(cubes) == 0:
        print("Error: need 1 Cubes but only found", len(cubes), "Cube(s)")
    else:
        robot.pickup_object(cubes[0]).wait_for_completed()
        time.sleep(0.5)
    robot.say_text(u"ぷれぜんと　を　とどける").wait_for_completed()
    lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
    pet = robot.world.wait_for_observed_pet(timeout=60)
    lookaround.stop()
    print("pet")
    print(pet)
    robot.drive_straight(distance_mm(60), speed_mmps(50)).wait_for_completed()
    #action = robot.go_to_object(pet, distance_mm(65.0))
    #action.wait_for_completed()
    print("Done.")


cozmo.run_program(cozmo_program, use_viewer=True, force_viewer_on_top=True)