Cozmo人工智能机器人SDK使用笔记（4）-任务部分cubes_and_objects
【摘要】 先简单总结一下使用笔记1-3：
基础部分介绍了一些常用功能，比如运动控制、LED显示和扬声器交互等人机接口显示部分--输出，cozmo面部显示屏输出一些基本信息人机接口视觉部分--输入，cozmo摄像头获取环境信息并处理等
接着，就自然过渡到第四部分----立方体和物体任务部分，共有13个项目专题，非常有趣。
Cozmo环顾四周，找寻充电底座图标，然后行驶到...
先简单总结一下使用笔记1-3：
- 基础部分介绍了一些常用功能，比如运动控制、LED显示和扬声器交互等
- 人机接口显示部分--输出，cozmo面部显示屏输出一些基本信息
- 人机接口视觉部分--输入，cozmo摄像头获取环境信息并处理等
接着，就自然过渡到第四部分----立方体和物体任务部分，共有13个项目专题，非常有趣。
Cozmo环顾四周，找寻充电底座图标，然后行驶到附近，任务完成。
1. go to pose
给定Cozmo目标位置和角度，然后行驶到对应位置和角度。
先设定目标，然后如果将relative_to_robot设置为true，就是相对量，当年机器人为原点，给定目标相对原点的距离和角度。
核心代码如下：
-
def cozmo_program(robot: cozmo.robot.Robot):
-
robot.go_to_pose(Pose(100, 100, 0, angle_z=degrees(45)), relative_to_robot=True).wait_for_completed()
2. create wall
虚拟墙，并绕过行驶到目标。
核心代码：
-
import cozmo
-
from cozmo.util import degrees, Pose
-
-
-
def cozmo_program(robot: cozmo.robot.Robot):
-
fixed_object = robot.world.create_custom_fixed_object(Pose(100, 0, 0, angle_z=degrees(0)),
-
10, 100, 100, relative_to_robot=True)
-
if fixed_object:
-
print("fixed_object created successfully")
-
-
robot.go_to_pose(Pose(200, 0, 0, angle_z=degrees(0)), relative_to_robot=True).wait_for_completed()
-
-
-
cozmo.run_program(cozmo_program, use_3d_viewer=True)
3. go to object
Cozmo找到一个立方体（1-3皆可以），然后行驶到对应立方体附近。
-
def go_to_object_test(robot: cozmo.robot.Robot):
-
'''The core of the go to object test program'''
-
-
# Move lift down and tilt the head up
-
robot.move_lift(-3)
-
robot.set_head_angle(degrees(0)).wait_for_completed()
-
-
# look around and try to find a cube
-
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
-
-
cube = None
-
-
try:
-
cube = robot.world.wait_for_observed_light_cube(timeout=30)
-
print("Found cube: %s" % cube)
-
except asyncio.TimeoutError:
-
print("Didn't find a cube")
-
finally:
-
# whether we find it or not, we want to stop the behavior
-
look_around.stop()
-
-
if cube:
-
# Drive to 70mm away from the cube (much closer and Cozmo
-
# will likely hit the cube) and then stop.
-
action = robot.go_to_object(cube, distance_mm(70.0))
-
action.wait_for_completed()
-
print("Completed action: result = %s" % action)
-
print("Done.")
4. stack or roll
让Cozmo依据找到的立方体的数量执行不同的任务。
超时时间默认为10s，环顾四周，找寻立方体，根据数量进行操作：
0---恩，它很生气
1---翻滚立方体
2---堆叠立方体
-
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)
5. cube stack
Cozmo堆叠立方体。Cozmo将等到它看到两个立方体，然后拿起一个并将其放在另一个立方体上。它会拿起看到的第一个，然后把它放在第二个上。
-
def cozmo_program(robot: cozmo.robot.Robot):
-
# Attempt to stack 2 cubes
-
-
# Lookaround until Cozmo knows where at least 2 cubes are:
-
lookaround = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
-
cubes = robot.world.wait_until_observe_num_objects(num=2, object_type=cozmo.objects.LightCube, timeout=60)
-
lookaround.stop()
-
-
if len(cubes) < 2:
-
print("Error: need 2 Cubes but only found", len(cubes), "Cube(s)")
-
else:
-
# Try and pickup the 1st cube
-
current_action = robot.pickup_object(cubes[0], num_retries=3)
-
current_action.wait_for_completed()
-
if current_action.has_failed:
-
code, reason = current_action.failure_reason
-
result = current_action.result
-
print("Pickup Cube failed: code=%s reason='%s' result=%s" % (code, reason, result))
-
return
-
-
# Now try to place that cube on the 2nd one
-
current_action = robot.place_on_object(cubes[1], num_retries=3)
-
current_action.wait_for_completed()
-
if current_action.has_failed:
-
code, reason = current_action.failure_reason
-
result = current_action.result
-
print("Place On Cube failed: code=%s reason='%s' result=%s" % (code, reason, result))
-
return
-
-
print("Cozmo successfully stacked 2 blocks!")
6. pickup furthest
让Cozmo拿起最远的立方体。此实例演示对象位置姿态的简单数学运算。
(dst = translation.position.x ** 2 + translation.position.y ** 2)
机器人试图在视觉中找到三个立方体，可以在脚本打开的摄像头窗口中看到运行过程。每个立方体将显示轮廓和立方体编号。机器人等待直到找到3个立方体，然后尝试拿起最远的一个。
-
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=3, object_type=cozmo.objects.LightCube, timeout=60)
-
lookaround.stop()
-
-
max_dst, targ = 0, None
-
for cube in cubes:
-
translation = robot.pose - cube.pose
-
dst = translation.position.x ** 2 + translation.position.y ** 2
-
if dst > max_dst:
-
max_dst, targ = dst, cube
-
-
if len(cubes) < 3:
-
print("Error: need 3 Cubes but only found", len(cubes), "Cube(s)")
-
else:
-
robot.pickup_object(targ, num_retries=3).wait_for_completed()
-
-
-
cozmo.run_program(cozmo_program, use_viewer=True)
7. look around
Cozmo环顾四周，做出反应，然后拿起并放下一个立方体。
-
import asyncio
-
-
import cozmo
-
from cozmo.util import degrees
-
-
-
def cozmo_program(robot: cozmo.robot.Robot):
-
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
-
-
# try to find a block
-
cube = None
-
-
try:
-
cube = robot.world.wait_for_observed_light_cube(timeout=30)
-
print("Found cube", cube)
-
-
except asyncio.TimeoutError:
-
print("Didn't find a cube :-(")
-
-
finally:
-
# whether we find it or not, we want to stop the behavior
-
look_around.stop()
-
-
if cube is None:
-
robot.play_anim_trigger(cozmo.anim.Triggers.MajorFail)
-
return
-
-
print("Yay, found cube")
-
-
cube.set_lights(cozmo.lights.green_light.flash())
-
-
anim = robot.play_anim_trigger(cozmo.anim.Triggers.BlockReact)
-
anim.wait_for_completed()
-
-
-
action = robot.pickup_object(cube)
-
print("got action", action)
-
result = action.wait_for_completed(timeout=30)
-
print("got action result", result)
-
-
robot.turn_in_place(degrees(90)).wait_for_completed()
-
-
action = robot.place_object_on_ground_here(cube)
-
print("got action", action)
-
result = action.wait_for_completed(timeout=30)
-
print("got action result", result)
-
-
anim = robot.play_anim_trigger(cozmo.anim.Triggers.MajorWin)
-
cube.set_light_corners(None, None, None, None)
-
anim.wait_for_completed()
-
-
-
cozmo.run_program(cozmo_program)
8. drive to charger
让Cozmo行驶到充电底座附近。
-
import asyncio
-
import time
-
-
import cozmo
-
from cozmo.util import degrees, distance_mm, speed_mmps
-
-
-
def drive_to_charger(robot):
-
'''The core of the drive_to_charger program'''
-
-
# 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()
-
# Tilt the head to be level
-
robot.set_head_angle(degrees(0)).wait_for_completed()
-
# wait half a second to ensure Cozmo has seen the charger
-
time.sleep(0.5)
-
# drive backwards away from the charger
-
robot.drive_straight(distance_mm(-60), speed_mmps(50)).wait_for_completed()
-
-
# try to find the charger
-
charger = None
-
-
# see if Cozmo already knows where the charger is
-
if robot.world.charger:
-
if robot.world.charger.pose.is_comparable(robot.pose):
-
print("Cozmo already knows where the charger is!")
-
charger = robot.world.charger
-
else:
-
# Cozmo knows about the charger, but the pose is not based on the
-
# same origin as the robot (e.g. the robot was moved since seeing
-
# the charger) so try to look for the charger first
-
pass
-
-
if not charger:
-
# Tell Cozmo to look around for the charger
-
look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
-
try:
-
charger = robot.world.wait_for_observed_charger(timeout=30)
-
print("Found charger: %s" % charger)
-
except asyncio.TimeoutError:
-
print("Didn't see the charger")
-
finally:
-
# whether we find it or not, we want to stop the behavior
-
look_around.stop()
-
-
if charger:
-
# Attempt to drive near to the charger, and then stop.
-
action = robot.go_to_object(charger, distance_mm(65.0))
-
action.wait_for_completed()
-
print("Completed action: result = %s" % action)
-
print("Done.")
-
-
-
cozmo.robot.Robot.drive_off_charger_on_connect = False # Cozmo can stay on charger for now
-
cozmo.run_program(drive_to_charger, use_viewer=True, force_viewer_on_top=True)
9. custom objects
自定义对象，包括大小和形状等。
-
import time
-
-
import cozmo
-
from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes
-
-
-
def handle_object_appeared(evt, **kw):
-
# This will be called whenever an EvtObjectAppeared is dispatched -
-
# whenever an Object comes into view.
-
if isinstance(evt.obj, CustomObject):
-
print("Cozmo started seeing a %s" % str(evt.obj.object_type))
-
-
-
def handle_object_disappeared(evt, **kw):
-
# This will be called whenever an EvtObjectDisappeared is dispatched -
-
# whenever an Object goes out of view.
-
if isinstance(evt.obj, CustomObject):
-
print("Cozmo stopped seeing a %s" % str(evt.obj.object_type))
-
-
-
def custom_objects(robot: cozmo.robot.Robot):
-
# Add event handlers for whenever Cozmo sees a new object
-
robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared)
-
robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared)
-
-
# define a unique cube (44mm x 44mm x 44mm) (approximately the same size as a light cube)
-
# with a 30mm x 30mm Diamonds2 image on every face
-
cube_obj = robot.world.define_custom_cube(CustomObjectTypes.CustomType00,
-
CustomObjectMarkers.Diamonds2,
-
44,
-
30, 30, True)
-
-
# define a unique cube (88mm x 88mm x 88mm) (approximately 2x the size of a light cube)
-
# with a 50mm x 50mm Diamonds3 image on every face
-
big_cube_obj = robot.world.define_custom_cube(CustomObjectTypes.CustomType01,
-
CustomObjectMarkers.Diamonds3,
-
88,
-
50, 50, True)
-
-
# define a unique wall (150mm x 120mm (x10mm thick for all walls)
-
# with a 50mm x 30mm Circles2 image on front and back
-
wall_obj = robot.world.define_custom_wall(CustomObjectTypes.CustomType02,
-
CustomObjectMarkers.Circles2,
-
150, 120,
-
50, 30, True)
-
-
# define a unique box (60mm deep x 140mm width x100mm tall)
-
# with a different 30mm x 50mm image on each of the 6 faces
-
box_obj = robot.world.define_custom_box(CustomObjectTypes.CustomType03,
-
CustomObjectMarkers.Hexagons2, # front
-
CustomObjectMarkers.Circles3, # back
-
CustomObjectMarkers.Circles4, # top
-
CustomObjectMarkers.Circles5, # bottom
-
CustomObjectMarkers.Triangles2, # left
-
CustomObjectMarkers.Triangles3, # right
-
60, 140, 100,
-
30, 50, True)
-
-
if ((cube_obj is not None) and (big_cube_obj is not None) and
-
(wall_obj is not None) and (box_obj is not None)):
-
print("All objects defined successfully!")
-
else:
-
print("One or more object definitions failed!")
-
return
-
-
print("Show the above markers to Cozmo and you will see the related objects "
-
"annotated in Cozmo's view window, you will also see print messages "
-
"everytime a custom object enters or exits Cozmo's view.")
-
-
print("Press CTRL-C to quit")
-
while True:
-
time.sleep(0.1)
-
-
-
cozmo.run_program(custom_objects, use_3d_viewer=True, use_viewer=True)
10. object moving
检测移动立方体并实时跟踪其速度等。
-
import time
-
-
import cozmo
-
-
-
def handle_object_moving_started(evt, **kw):
-
# This will be called whenever an EvtObjectMovingStarted event is dispatched -
-
# whenever we detect a cube starts moving (via an accelerometer in the cube)
-
print("Object %s started moving: acceleration=%s" %
-
(evt.obj.object_id, evt.acceleration))
-
-
-
def handle_object_moving(evt, **kw):
-
# This will be called whenever an EvtObjectMoving event is dispatched -
-
# whenever we detect a cube is still moving a (via an accelerometer in the cube)
-
print("Object %s is moving: acceleration=%s, duration=%.1f seconds" %
-
(evt.obj.object_id, evt.acceleration, evt.move_duration))
-
-
-
def handle_object_moving_stopped(evt, **kw):
-
# This will be called whenever an EvtObjectMovingStopped event is dispatched -
-
# whenever we detect a cube stopped moving (via an accelerometer in the cube)
-
print("Object %s stopped moving: duration=%.1f seconds" %
-
(evt.obj.object_id, evt.move_duration))
-
-
-
def cozmo_program(robot: cozmo.robot.Robot):
-
# Add event handlers that will be called for the corresponding event
-
robot.add_event_handler(cozmo.objects.EvtObjectMovingStarted, handle_object_moving_started)
-
robot.add_event_handler(cozmo.objects.EvtObjectMoving, handle_object_moving)
-
robot.add_event_handler(cozmo.objects.EvtObjectMovingStopped, handle_object_moving_stopped)
-
-
# keep the program running until user closes / quits it
-
print("Press CTRL-C to quit")
-
while True:
-
time.sleep(1.0)
-
-
-
cozmo.robot.Robot.drive_off_charger_on_connect = False # Cozmo can stay on his charger for this example
-
cozmo.run_program(cozmo_program)
11. dock with cube
行驶到立方体附近，并对接立方体，但是并不拿起立方体。
-
import cozmo
-
from cozmo.util import degrees
-
-
async def dock_with_cube(robot: cozmo.robot.Robot):
-
await robot.set_head_angle(degrees(-5.0)).wait_for_completed()
-
-
print("Cozmo is waiting until he sees a cube.")
-
cube = await robot.world.wait_for_observed_light_cube()
-
-
print("Cozmo found a cube, and will now attempt to dock with it:")
-
# Cozmo will approach the cube he has seen
-
# using a 180 approach angle will cause him to drive past the cube and approach from the opposite side
-
# num_retries allows us to specify how many times Cozmo will retry the action in the event of it failing
-
action = robot.dock_with_cube(cube, approach_angle=cozmo.util.degrees(180), num_retries=2)
-
await action.wait_for_completed()
-
print("result:", action.result)
-
-
cozmo.run_program(dock_with_cube)
12. roll cube
翻滚立方体。
-
import cozmo
-
from cozmo.util import degrees
-
-
async def roll_a_cube(robot: cozmo.robot.Robot):
-
await robot.set_head_angle(degrees(-5.0)).wait_for_completed()
-
-
print("Cozmo is waiting until he sees a cube")
-
cube = await robot.world.wait_for_observed_light_cube()
-
-
print("Cozmo found a cube, and will now attempt to roll with it:")
-
# Cozmo will approach the cube he has seen and roll it
-
# check_for_object_on_top=True enforces that Cozmo will not roll cubes with anything on top
-
action = robot.roll_cube(cube, check_for_object_on_top=True, num_retries=2)
-
await action.wait_for_completed()
-
print("result:", action.result)
-
-
cozmo.run_program(roll_a_cube)
13. pop a wheelie
借助立方体，将Cozmo身体直立起来。
-
import cozmo
-
-
async def pop_a_wheelie(robot: cozmo.robot.Robot):
-
print("Cozmo is waiting until he sees a cube")
-
cube = await robot.world.wait_for_observed_light_cube()
-
-
print("Cozmo found a cube, and will now attempt to pop a wheelie on it")
-
-
action = robot.pop_a_wheelie(cube, num_retries=2)
-
await action.wait_for_completed()
-
-
-
cozmo.run_program(pop_a_wheelie)
Fin
文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。
原文链接：zhangrelay.blog.csdn.net/article/details/86626580
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）