def cozmo_program(robot: cozmo.robot.Robot):
robot.enable_device_imu(True, True, True)
while True:
yaw = robot.pose.rotation.angle_z.radians
position = [
robot.pose.position.x / 1000, robot.pose.position.y / 1000,
robot.pose.position.z / 1000, 0, 0, yaw
]
print("Cozmo yaw", yaw)
print("Cozmo position", position)
cube = robot.world.wait_until_observe_num_objects(
num=1, object_type=cozmo.objects.LightCube, timeout=1)
if cube:
cube_yaw = cube[0].pose.rotation.angle_z.radians
cube_position = [
cube[0].pose.position.x / 1000, cube[0].pose.position.y / 1000,
cube[0].pose.position.z / 1000, 0, 0, cube_yaw
]
print("Cube yaw", cube_yaw)
print("Cube position", cube_position)
time.sleep(2)
def cozmo_program(robot: cozmo.robot.Robot):
print("Cozmo SDK has behavior control...")
robot.world.image_annotator.add_annotator('robotState', RobotStateDisplay)
robot.enable_device_imu(True, True, True)
robot.camera.image_stream_enabled = True
robot.camera.color_image_enabled = False
robot.camera.enable_auto_exposure()
robot.set_head_angle(cozmo.util.degrees(0)).wait_for_completed()
robot.move_lift(-1)
robot.say_text("Hello World!").wait_for_completed()
time.sleep(1)
robot.say_text("Let's take pictures!").wait_for_completed()
time.sleep(1)
# take pictures
# myargs = ["1", "order", "drone", "inspection"]
myargs = sys.argv[1:]
if len(myargs) <= 1:
sys.exit("Incorrect arguments")
num_images_per_type = int(
myargs[0]) # number of images to take of each type of object
print("Taking ", num_images_per_type, "images each of ", myargs[1:])
for type in myargs[1:]:
for i in range(num_images_per_type):
time.sleep(.5)
robot.say_text(type).wait_for_completed()
time.sleep(3)
latest_image = robot.world.latest_image
new_image = latest_image.raw_image
timestamp = datetime.datetime.now().strftime("%dT%H%M%S%f")
new_image.save("./outputs/" + str(type) + "_" + timestamp + ".bmp")
time.sleep(3)
time.sleep(0.5)
robot.say_text("I will drive now!").wait_for_completed()
time.sleep(0.5)
robot.drive_straight(distance_mm(1000),
speed_mmps(1000)).wait_for_completed()
robot.say_text("Check out my arms!").wait_for_completed()
robot.move_lift(1)
time.sleep(1)
robot.move_lift(-1)
time.sleep(1)
robot.say_text("Pick me up and shake me around!").wait_for_completed()
time.sleep(2)
robot.say_text("You can put me down now!").wait_for_completed()
f = open("./outputs/test_output.txt", 'r+')
f.seek(0)
f.write("Min Gyro: " + str(min(gyro)) + "\n")
f.write("Max Gyro: " + str(max(gyro)) + "\n")
f.write("Min Acc: " + str(min(acc)) + "\n")
f.write("Max Acc: " + str(max(acc)))
f.truncate()
f.close()
if (all(i > -10
for i in min(gyro)) and all(i < 10 for i in max(gyro))) and (all(
i > -25000 for i in min(acc)) and all(i < 25000
for i in max(acc))):
robot.say_text("Sensors look good!").wait_for_completed()
else:
robot.say_text("Sensors look bad!").wait_for_completed()
