def give_shot(robot):
robot.world.define_custom_cube(
custom_object_type=CustomObjectTypes.CustomType00,
marker=CustomObjectMarkers.Circles2,
size_mm=20.0,
marker_width_mm=50.0,
marker_height_mm=50.0,
is_unique=True
)
robot.motors.set_head_motor(0)
robot.anim.play_animation('anim_referencing_squint_01')
robot.anim.play_animation('anim_eyecontact_squint_01')
robot.motors.set_lift_motor(0)
for obj in robot.world.visible_custom_objects:
new_pose = robot.pose.define_pose_relative_this(obj.pose)
print(new_pose)
x = new_pose.position.x
y = new_pose.position.y
z = new_pose.position.z
ang = new_pose.rotation.angle_z
a = x - (y / math.tan(ang.radians))
speed = 80
robot.behavior.drive_straight(distance_mm(x - 30),
speed_mmps(speed))
robot.behavior.turn_in_place(radians(math.pi / 3))
robot.behavior.drive_straight(distance_mm(y - 30),
speed_mmps(speed))
robot.motors.set_lift_motor(2)
robot.behavior.drive_straight(distance_mm(-y), speed_mmps(speed))
def robot_moves(self):
with anki_vector.Robot() as robot:
robot.behavior.turn_in_place(degrees(-360))
robot.behavior.turn_in_place(degrees(360))
robot.behavior.drive_straight(distance_mm(80), speed_mmps(80))
robot.behavior.turn_in_place(degrees(180))
robot.behavior.drive_straight(distance_mm(80), speed_mmps(80))
robot.behavior.turn_in_place(degrees(180))
def main():
args = anki_vector.util.parse_command_args()
#anki_vector.nav_map.NavMapComponent.init_nav_map_feed(frequency=0.5)
with anki_vector.Robot(args.serial, show_3d_viewer=True) as robot:
robot.show_viewer = True
robot.enable_camera_feed = True
robot.viewer.show_video()
robot.enable_custom_object_detection = True
robot.enable_nav_map_feed = True
current_robot_pose = robot.pose.position
print(current_robot_pose)
print("Drive Vector off of charger...")
robot.behavior.drive_off_charger()
current_robot_pose = robot.pose.position
print(current_robot_pose)
robot.behavior.drive_straight(distance_mm(100), speed_mmps(100))
current_robot_pose = robot.pose.position
print(current_robot_pose)
robot.behavior.turn_in_place(degrees(90))
current_robot_pose = robot.pose.position
print(current_robot_pose)
robot.behavior.drive_straight(distance_mm(100), speed_mmps(100))
current_robot_pose = robot.pose.position
print(current_robot_pose)
robot.behavior.turn_in_place(degrees(90))
current_robot_pose = robot.pose.position
print(current_robot_pose)
robot.behavior.drive_straight(distance_mm(100), speed_mmps(100))
current_robot_pose = robot.pose.position
print(current_robot_pose)
robot.behavior.turn_in_place(degrees(90))
current_robot_pose = robot.pose.position
print(current_robot_pose)
robot.behavior.drive_straight(distance_mm(100), speed_mmps(100))
current_robot_pose = robot.pose.position
print(current_robot_pose)
robot.behavior.turn_in_place(degrees(90))
current_robot_pose = robot.pose.position
print(current_robot_pose)
#latest_nav_map = robot.nav_map.latest_nav_map
#throws exception not to be initialized
robot.behavior.drive_on_charger()
current_robot_pose = robot.pose.position
print(current_robot_pose)
def backward(self, distance, speed):
"""
Makes vector go backward
"""
if speed.upper() == "FAST":
self.conn.behavior.drive_straight(distance_mm(-distance),
speed_mmps(500))
elif speed.upper() == "SLOW":
self.conn.behavior.drive_straight(distance_mm(-distance),
speed_mmps(150))
def __init__(self, robot):
self.pose = robot.pose
if robot.head_angle_rad is None:
self.head_angle = util.radians(0.0)
else:
self.head_angle = util.radians(robot.head_angle_rad)
if robot.lift_height_mm is None:
self.lift_position = util.distance_mm(0.0)
else:
self.lift_position = util.distance_mm(robot.lift_height_mm)
def testing_cube_pick_up_robust_old(robot):
connect_response = robot.world.connect_cube()
light_cube = robot.world.connected_light_cube
while light_cube is None:
time.sleep(0.1)
print('Still connecting')
light_cube = robot.world.connected_light_cube
print('Connected')
print(connect_response.result())
#print(connect_response.result().status)
time.sleep(1)
print(light_cube.is_visible, 'Vis')
moving = robot.behavior.pickup_object(light_cube, use_pre_dock_pose = True)
#print(type(moving.result()))
print(moving.result().result.code)
# 50331652 - Bad object
# 50331683 - can't see
flag = True
curr = moving.result()
while flag:
if curr.result.code == 50331652:
print('BAD OBJECT')
turn = robot.behavior.turn_in_place(degrees(45))
turn.result()
elif curr.result.code == 50331683:
print('CAN"T SEE')
turn = robot.behavior.turn_in_place(degrees(45))
turn.result()
elif curr.result.code == 3 or curr.result.code == 0:
print('Got there')
print(curr.result.code)
flag = False
elif curr.result.code == 67108867:
back = robot.behavior.drive_straight(distance_mm(-100), speed_mmps(30))
back.result()
#print(moving.result)
#print(light_cube.pose.x)
if flag:
moving = robot.behavior.pickup_object(light_cube, use_pre_dock_pose = True)
curr = moving.result()
time.sleep(2)
print('im out', curr.result)
pick_up = robot.behavior.pickup_object(light_cube, use_pre_dock_pose = True)
pick_up.result()
back = robot.behavior.drive_straight(distance_mm(-50), speed_mmps(30))
back.result()
print('placing down now')
drop = robot.behavior.place_object_on_ground_here()
drop.result()
'''
def main():
args = anki_vector.util.parse_command_args()
with anki_vector.Robot(args.serial) as robot:
if robot.status.is_charging:
print("Vector is currently charging.")
robot.behavior.drive_off_charger()
robot.behavior.say_text('Charge.')
object_dist = robot.proximity.last_sensor_reading.distance
print(f'Distance from object: {object_dist.distance_mm}')
if object_dist.distance_mm == 30:
robot.behavior.say_text('I go straights.')
robot.behavior.drive_straight(distance_mm(800), speed_mmps(100))
robot.motors.stop_all_motors()
robot.behavior.say_text('I escaped the Minotaur.')
elif object_dist.distance_mm == 400:
robot.behavior.say_text('I go straights.')
robot.behavior.drive_straight(distance_mm(400), speed_mmps(100))
object_dist = robot.proximity.last_sensor_reading.distance
print(f'Distance from object: {object_dist.distance_mm}')
if object_dist.distance_mm == 400:
robot.behavior.say_text('I am not there yet.')
robot.behavior.drive_straight(distance_mm(400),
speed_mmps(100))
robot.motors.stop_all_motors()
robot.behavior.say_text('I escaped the Minotaur.')
elif object_dist.distance_mm < 400:
robot.behavior.say_text('This is my last run.')
robot.behavior.drive_straight(
distance_mm(object_dist.distance_mm - 50), speed_mmps(100))
print("Turn right 90 degrees.")
robot.behavior.say_text("I go rights.")
robot.behavior.turn_in_place(degrees(-90))
elif object_dist.distance_mm < 400:
stopping_distance = object_dist.distance_mm - 50.0
print(f'Distance from object: {object_dist.distance_mm}')
print(f'Stopping distance: {stopping_distance}')
print("Go straight.")
robot.behavior.say_text('I go strights.')
robot.behavior.drive_straight(distance_mm(stopping_distance),
speed_mmps(50))
robot.motors.stop_all_motors()
print("Turn right 90 degrees.")
robot.behavior.say_text("I go rights.")
robot.behavior.turn_in_place(degrees(-90))
def rrt_move_to(robot, start_x, start_y, start_h, end_x, end_y):
start_point = Node((start_x, start_y))
end_goal = Node((end_x, end_y))
cmap.clear_goals()
cmap.reset_paths()
cmap.add_goal(end_goal)
cmap.set_start(start_point)
print('Starting RRT - x: {0:.2f}, y: {1:.2f}, h: {2:.2f}'.format(start_point.x, start_point.y, start_h))
RRT(cmap, cmap.get_start())
path = cmap.get_smooth_path()
marked = {}
next_node = 1
robot.behavior.set_head_angle(MIN_HEAD_ANGLE)
vector_angle = start_h
vector_pos = start_point
while vector_pos not in cmap.get_goals():
next_pos = path.pop(0)
#print('x: {0:.2f}, y: {1:.2f}'.format(next_pos.x, next_pos.y))
angle = np.arctan2(next_pos.y - vector_pos.y, next_pos.x - vector_pos.x)
if abs(angle - vector_angle) > 0.01:
turn_node = robot.behavior.turn_in_place(anki_vector.util.Angle(radians=angle - vector_angle))
turn_node.result()
#print('Had to turn')
#print('Driving to next node in path')
straight_move = robot.behavior.drive_straight(distance_mm(get_dist(vector_pos, next_pos)),speed_mmps(200))
straight_move.result()
vector_pos = next_pos
vector_angle = angle
cmap.set_start(vector_pos)
print('Reached goal - x: {0:.2f}, y: {1:.2f}, h: {2:.2f}'.format(vector_pos.x, vector_pos.y, vector_angle)) #TODO : Confirm this works
return vector_pos, vector_angle
def thinkandmove(self, y, thres, depth=0):
maxval = max(y[280:360])
if maxval > thres:
self.robot.drive_wheels(0, 0)
leftval = sum((g-279) * y[g] for g in range(280, 321)) / sum(list(range(1, 41)))
rightval = sum((361-g) * y[g] for g in range(320, 361)) / sum(list(range(1, 41)))
direction = (-1 * 90 * (leftval - 300)/180) if leftval > rightval else (1 * 90 * (rightval - 300)/180)
# # print(sum(y[280:320]), sum(y[320:360]))
self.robot.turn_in_place(degrees(direction)).wait_for_completed()
print(direction)
self.robot.set_head_angle(vector.util.Angle(-0.3)).wait_for_completed()
self.image, y = self.groundplane()
if max(y[280:360]) > thres:
self.robot.drive_straight(distance_mm(-25), speed_mmps(50)).wait_for_completed()
leftval = sum((g-279) * y[g] for g in range(280, 321)) / sum(list(range(1, 41)))
rightval = sum((361-g) * y[g] for g in range(320, 361)) / sum(list(range(1, 41)))
direction = (-1 * 60 * (leftval - 300)/180) if leftval > rightval else (1 * 60 * (rightval - 300)/180)
self.robot.turn_in_place(degrees(direction)).wait_for_completed()
# self.image, y = self.groundplane()
# print("Hello")
# print(maxval)
# self.thinkandmove(y, thres+30, depth=depth+1)
# print("Finish")
self.robot.set_head_angle(vector.util.Angle(0)).wait_for_completed()
else:
self.robot.drive_wheels(50, 50)
def approach_box(self, closeness):
if abs(closeness - self.closeness_threshold) > self.closeness_range:
# error positive if too far, negative if too close
error = (self.closeness_threshold -
closeness) * 4 / self.closeness_range
print(error)
self.robot.behavior.drive_straight(distance_mm(error),
speed_mmps(self.def_speed_mmps))
def main():
args = anki_vector.util.parse_command_args()
with anki_vector.Robot(args.serial) as robot:
robot.behavior.drive_off_charger()
print("Drive Backwards")
robot.behavior.drive_straight(distance_mm(-200), speed_mmps(50))
def look_at_wall():
args = anki_vector.util.parse_command_args()
# Drive straight forward then stop and turn around.
with anki_vector.Robot(args.serial) as robot:
if robot.status.is_charging:
print("Vector is currently charging.")
robot.behavior.drive_off_charger()
robot.behavior.say_text('Charge.')
#Check for distance of next wall. Vector sees max of 400mm away. If his next object comes back as 400mm he's told to go move forward 400mm and then check again for objects.
object_dist = robot.proximity.last_sensor_reading.distance
print(f'Distance from object: {object_dist.distance_mm}')
if object_dist.distance_mm == 400:
robot.behavior.say_text('I am free!! You remind me of the babe! The babe with the power.')
robot.behavior.drive_straight(distance_mm(400), speed_mmps(100))
#Once vector approaches wall, he willl scan for a face. If no face is found he will turn right.
look_for_face()
object_dist = robot.proximity.last_sensor_reading.distance
print(f'Distance from object: {object_dist.distance_mm}')
if object_dist.distance_mm == 400:
robot.behavior.say_text('I am not there yet.')
robot.behavior.drive_straight(distance_mm(400), speed_mmps(100))
robot.motors.stop_all_motors()
look_for_face()
robot.behavior.say_text('I escaped the maze!!.')
elif object_dist.distance_mm < 400:
stopping_distance = object_dist.distance_mm - 35.0
print("Go stright.")
robot.behavior.say_text('I go strights!!')
robot.behavior.drive_straight(distance_mm(stopping_distance), speed_mmps(100))
robot.motors.stop_all_motors()
look_for_face()
print("Turn right 90 degrees.")
robot.behavior.say_text("I go rights." )
turn_right()
if object_dist.distance_mm < 400:
stopping_distance = object_dist.distance_mm - 35.0
robot.behavior.say_text('I go strights!!')
print("Go straight.")
robot.behavior.drive_straight(distance_mm(stopping_distance), speed_mmps(100))
robot.motors.stop_all_motors()
look_for_face()
print("Turn right 90 degrees.")
robot.behavior.say_text("I go rights." )
turn_right()
def main():
args = anki_vector.util.parse_command_args()
with anki_vector.Robot(args.serial) as robot:
print("Vector SDK has behavior control...")
robot.behavior.drive_off_charger()
print("Drive Vector straight until he reaches cliff...")
robot.behavior.drive_straight(distance_mm(500), speed_mmps(100))
robot.conn.control_lost_event.wait()
print(
"Lost SDK behavior control. Request SDK behavior control again...")
robot.conn.request_control()
print("Drive Vector backward away from the cliff...")
robot.behavior.drive_straight(distance_mm(-200), speed_mmps(100))
async def driveOffCharger(robot):
"""Drive off the charger if placed on it. MOves by a random rotation to ensure that there are no collissions
"""
status = robot.status
if status.is_on_charger:
robot.behavior.drive_off_charger()
time.sleep(10)
robot.behavior.drive_straight(distance_mm(150), speed_mmps(150))
time.sleep(10)
#iGenerate random number between 0 and 5 and multiply by 60 degrees to have random rotations
#The purpose of this is to ensure that the Vector robots do not collide
rotate = random.randint(0, 2)
robot.behavior.turn_in_place(degrees(rotate * 60))
time.sleep(10)
robot.behavior.drive_straight(distance_mm(150), speed_mmps(100))
return
else:
return
async def forward(self, distance):
handle = self.robot.drive_straight(distance_mm(distance),
speed_mmps(50),
in_parallel=True,
should_play_anim=False)
await handle.wait_for_completed()
pf = self.robot.world.particle_filter
self.robot.conn.loop.call_later(0.1, pf.look_for_new_landmarks)
self.report_pose()
def drive_straight():
args = anki_vector.util.parse_command_args()
dynamic_speed = dist() * 0.18333333
dynamic_distance = dist() * 0.75
print("average brightness:", img_brightness(), "dynamic speed:",
dynamic_speed, "dynamic distance", dynamic_distance, "/", dist())
with anki_vector.Robot(args.serial,
requires_behavior_control=True) as robot:
robot.behavior.drive_straight(distance_mm(dynamic_distance),
speed_mmps(dynamic_speed))
def wheels_drive(self, distance: float, speed_percentage: int):
#Parameter Speed range = 0-100
min_speed = 1
max_speed = 220
self.robot.behavior.drive_straight(
distance_mm(distance),
speed_mmps(
self.percentage_value(percentage=speed_percentage,
min_value=min_speed,
max_value=max_speed)))
def do_workout(robot):
# Aquire control
robot.conn.request_control()
robot.conn.control_granted_event.wait()
# If necessary, Move Vector's Head and Lift down
robot.behavior.set_head_angle(degrees(-5.0))
robot.behavior.set_lift_height(0.0)
# connect the cube
robot.world.connect_cube()
# If connected to cube do the workout
if robot.world.connected_light_cube:
print("Begin cube docking...")
# TODO Try with num_retries of 3
dock_response = robot.behavior.dock_with_cube(
robot.world.connected_light_cube, num_retries=3)
docking_result = dock_response.result
if docking_result.code != anki_vector.messaging.protocol.ActionResult.ACTION_RESULT_SUCCESS:
print("Cube docking failed with code {0} ({1})".format(
str(docking_result).rstrip('\n\r'), docking_result.code))
else:
# Have Vector Lift his Cube 5 times
robot.behavior.set_lift_height(100.0)
robot.say_text("One")
robot.behavior.set_lift_height(20.0, duration=0.5)
robot.behavior.set_lift_height(100.0)
robot.say_text("Two")
robot.behavior.set_lift_height(20.0, duration=0.5)
robot.behavior.set_lift_height(100.0, duration=0.75)
robot.say_text("Three")
robot.behavior.set_lift_height(20.0, duration=0.5)
robot.behavior.set_lift_height(100.0, duration=1.0)
robot.say_text("Four")
robot.behavior.set_lift_height(20.0, duration=0.5)
robot.behavior.set_lift_height(100.0, duration=1.5)
robot.say_text("Five")
robot.behavior.set_lift_height(0.0, duration=0.5)
robot.behavior.drive_straight(distance_mm(-25), speed_mmps(50))
robot.anim.play_animation("anim_fistbump_success_01")
# Vector completed his workout
vector_state.workout_done = True
robot.world.disconnect_cube()
robot.release_control()
def main():
'''Play fetch with Vector
Touch his back to signal to him that he can go get his cube
Once he comes back, touch his back again to tell him to put down the cube
'''
args = anki_vector.util.parse_command_args()
with anki_vector.Robot(args.serial) as robot:
robot.behavior.drive_off_charger()
robot.behavior.set_head_angle(degrees(-5.0))
robot.behavior.set_lift_height(0.0)
print("Connecting to a cube...")
robot.world.connect_cube()
robot.behavior.say_text(
"I am looking for my cube and trying to fetch it")
for i in range(2):
dock_response = False
docking_result = None
while not robot.touch.last_sensor_reading.is_being_touched:
robot.motors.set_wheel_motors(-50, 50)
time.sleep(0.1)
robot.motors.set_wheel_motors(50, -50)
time.sleep(0.1)
robot.motors.set_wheel_motors(0, 0)
time.sleep(0.25)
dock_response = robot.behavior.dock_with_cube(
robot.world.connected_light_cube, num_retries=3)
if dock_response:
docking_result = dock_response.result
robot.motors.set_lift_motor(50)
time.sleep(1)
robot.behavior.turn_in_place(degrees(90))
robot.behavior.drive_straight(distance_mm(100),
speed_mmps(100))
while not robot.touch.last_sensor_reading.is_being_touched:
robot.motors.set_wheel_motors(-50, 50)
time.sleep(0.1)
robot.motors.set_wheel_motors(50, -50)
time.sleep(0.1)
robot.motors.set_wheel_motors(0, 0)
time.sleep(0.25)
robot.motors.set_lift_motor(-50)
time.sleep(1)
robot.behavior.say_text("Here is your cube!")
robot.behavior.turn_in_place(degrees(180))
robot.world.disconnect_cube()
def __init__(self, robot, arguments=None):
robot.behavior.drive_off_charger()
robot.world.connect_cube()
robot.behavior.say_text("Lulu is playing").result(10)
robot.anim.play_animation_trigger('GreetAfterLongTime').result(10)
robot.behavior.drive_straight(distance_mm(100),
speed_mmps(100)).result(10)
robot.behavior.say_text("he is playing").result(10)
robot.behavior.turn_in_place(degrees(70)).result(10)
robot.behavior.say_text("Lulu is playing").result(10)
robot.behavior.turn_in_place(degrees(220)).result(10)
robot.anim.play_animation_trigger('GreetAfterLongTime').result(10)
robot.behavior.drive_on_charger()
def __init__(self,
distance=distance_mm(50),
speed=speed_mmps(50),
abort_on_stop=True,
**action_kwargs):
if isinstance(distance, (int, float)):
distance = distance_mm(distance)
elif not isinstance(distance, anki_vector.util.Distance):
raise ValueError(
'%s distance must be a number or a anki_vector.util.Distance' %
self)
if isinstance(speed, (int, float)):
speed = speed_mmps(speed)
elif not isinstance(speed, anki_vector.util.Speed):
raise ValueError(
'%s speed must be a number or a anki_vector.util.Speed' % self)
self.distance = distance
self.speed = speed
if 'should_play_anim' not in action_kwargs:
action_kwargs['should_play_anim'] = False
self.action_kwargs = action_kwargs
# super's init must come last because it checks self.action_kwargs
super().__init__(abort_on_stop)
def main():
args = anki_vector.util.parse_command_args()
# The robot drives straight, stops and then turns around
with anki_vector.Robot(args.serial) as robot:
robot.behavior.drive_off_charger()
# Use a "for loop" to repeat the indented code 4 times
# Note: the _ variable name can be used when you don't need the value
for _ in range(4):
print("Drive Vector straight...")
robot.behavior.drive_straight(distance_mm(200), speed_mmps(50))
print("Turn Vector in place...")
robot.behavior.turn_in_place(degrees(90))
def look_for_faces(robot):
found_face = False
while not found_face:
dist = random.randint(0, 200)
rot = random.randint(-90, 90)
robot.behavior.turn_in_place(degrees(rot))
time.sleep(1)
robot.behavior.drive_straight(distance_mm(dist), speed_mmps(70))
robot.behavior.set_head_angle(degrees(30))
time.sleep(1)
for trial in range(500):
for _ in robot.world.visible_faces:
found_face = True
# Continue searching
robot.behavior.set_head_angle(degrees(10))
def main():
with Robot() as vector:
vector.behavior.drive_on_charger()
vector.behavior.drive_off_charger()
vector.behavior.drive_straight(distance_mm(150), speed_mmps(25))
vector.behavior.turn_in_place(degrees(180), degrees(45),
degrees(5), degrees(5))
# ^ If vector's base is facing away from the camera
vector.anim.play_animation_trigger(random.choice(["GreetAfterLongTime", "PRDemoGreeting", "ReactToGoodMorning", "ReactToGreeting"]))
vector.behavior.say_text(get_speakable_text(random.choice(get_introductory_phrases())))
vector.behavior.say_text(get_speakable_text(random.choice(get_world_news_segment_lead_in_phrases())))
vector.anim.play_animation_trigger("ExploringQuickScan")
for story in get_world_news():
announcement = get_announcement_from_story(story)
vector.behavior.say_text(get_speakable_text(announcement))
vector.anim.play_animation_trigger(random.choice(["NeutralFace",
"NothingToDoBoredIdle",
"ObservingIdleEyesOnly",
"ObservingIdleWithHeadLookingStraight",
"ObservingIdleWithHeadLookingUp",
"ObservingLookStraight",
"ObservingLookUp",
"LookAround"]))
vector.behavior.say_text(get_speakable_text(random.choice(get_brief_description_lead_in_phrases())))
for sentence in story.get_summary():
vector.behavior.say_text(get_speakable_text(sentence))
vector.behavior.say_text(get_speakable_text(random.choice(get_sports_news_segment_lead_in_phrases())))
for announcement in [_announcement for _announcement in get_announcements_from_stories(get_sports_news())][:3]:
vector.behavior.say_text(get_speakable_text(announcement))
vector.behavior.say_text(random.choice(get_outer_space_news_segment_lead_in_phrases()))
for announcement in [_announcement for _announcement in get_announcements_from_stories(get_outer_space_news())][:3]:
vector.behavior.say_text(get_speakable_text(announcement))
vector.behavior.say_text(get_speakable_text(random.choice(get_vector_news_segment_lead_in_phrases())))
for post in [_post for _post in get_reddit_vector_posts()][:3]:
vector.behavior.say_text(get_speakable_text(f"Here is one by {post.author} (thank you, {post.author}). It's titled: {post.post}."))
vector.behavior.say_text(random.choice(get_brief_description_lead_in_phrases()))
for sentence in post.get_summary():
vector.behavior.say_text(get_speakable_text(sentence))
vector.behavior.say_text(get_speakable_text(random.choice(get_outro_phrases())))
vector.anim.play_animation_trigger(random.choice(["Feedback_ILoveYou",
"Feedback_GoodRobot",
"LookAtUserEndearingly",
"OnboardingReactToFaceHappy",
"OnboardingLookAtUser",
"ReactToGoodBye",
"ReactToGoodNight"]))
vector.behavior.drive_on_charger()
def obstacle_detection(self):
with anki_vector.Robot() as robot:
robot.motors.set_head_motor(-3.0)
i = 1
deq = collections.deque([15, 15, 15], 3)
while True:
distance = robot.proximity.last_sensor_reading.distance
deq.append(distance.distance_inches)
print('Distance reading DEQUE', sum(deq) / 3)
robot.motors.set_wheel_motors(50, 50)
if sum(deq) / 3 <= 3.5:
robot.motors.stop_all_motors()
robot.behavior.turn_in_place(degrees(-60))
else:
robot.behavior.drive_straight(distance_mm(100),
speed_mmps(80))
robot.behavior.turn_in_place(degrees(60))
def main():
args = anki_vector.util.parse_command_args()
# Drive straight forward then stop and turn around.
with anki_vector.Robot(args.serial) as robot:
robot.behavior.drive_on_charger()
robot.behavior.drive_off_charger()
# loop 4 times
for _ in range(4):
print("Go straight.")
robot.behavior.say_text('I go strights.')
robot.behavior.drive_straight(distance_mm(100), speed_mmps(50))
print("Turn right 90 degrees.")
robot.behavior.say_text("I go rights.")
robot.behavior.turn_in_place(degrees(-90))
robot.behavior.drive_on_charger()
def testing_cube_pick_up_robust(robot):
robot.behavior.set_head_angle(degrees(0))
connect_response = robot.world.connect_cube()
light_cube = robot.world.connected_light_cube
prev_pose = robot.pose
while light_cube is None:
time.sleep(0.1)
print('Still connecting')
light_cube = robot.world.connected_light_cube
print('Connected')
print(connect_response.result())
#print(connect_response.result().status)
time.sleep(1)
print(light_cube.is_visible, 'Vis')
#moving = robot.behavior.pickup_object(light_cube, use_pre_dock_pose = True)
#print(type(moving.result()))
#print(moving.result().result.code)
# 50331652 - Bad object
# 50331683 - can't see
flag = True
curr = light_cube.is_visible
while curr is False:
turn = robot.behavior.turn_in_place(degrees(45))
turn.result()
time.sleep(2)
curr = light_cube.is_visible
print('im out', curr)
pick_up = robot.behavior.pickup_object(light_cube, use_pre_dock_pose = True)
pick_up.result()
print("Done picking up")
back = robot.behavior.drive_straight(distance_mm(-50), speed_mmps(30))
back.result()
print('Starting to go back')
backing = robot.behavior.go_to_pose(prev_pose)
backing.result()
print('Done going back now')
print('placing down now')
drop = robot.behavior.place_object_on_ground_here()
drop.result()
'''
def actions():
# Use a "for loop" to repeat the indented code 4 times
# Note: the _ variable name can be used when you don't need the value
args = anki_vector.util.parse_command_args()
with anki_vector.Robot(args.serial) as robot:
for _ in range(randint(0, 10)):
print("Raise Vector's lift...")
robot.motors.set_lift_motor(5)
print("Drive Vector straight...")
robot.behavior.drive_straight(distance_mm(200),
speed_mmps(200)) # original 50
print("Move Vector's lift...")
robot.motors.set_lift_motor(-5.0)
time.sleep(.25)
robot.motors.set_lift_motor(5.0)
time.sleep(.25)
robot.motors.set_lift_motor(-5.0)
time.sleep(.25)
print("Turn Vector in place...")
# robot.behavior.turn_in_place(degrees(455))
robot.behavior.turn_in_place(degrees(-90))
time.sleep(.25)
robot.behavior.turn_in_place(degrees(90))
time.sleep(.25)
robot.behavior.turn_in_place(degrees(450))
time.sleep(.25)
print("Move Vector's lift...")
robot.motors.set_lift_motor(5.0)
time.sleep(.25)
robot.motors.set_lift_motor(-5.0)
time.sleep(.25)
robot.motors.set_lift_motor(5.0)
time.sleep(.25)
def main():
args = anki_vector.util.parse_command_args()
with anki_vector.Robot(args.serial) as robot:
robot.say_text("Welcome to Vector's Way. Programmed by Alex and Dad. Let's go!")
b = robot.behavior
b.drive_off_charger()
right = degrees(-90)
left = degrees(90)
b.turn_in_place(right)
b.drive_straight(distance_mm(150), speed_mmps(100))
b.turn_in_place(left)
b.drive_straight(distance_mm(180), speed_mmps(100))
robot.say_text("Hmm... Which way should I go?")
b.turn_in_place(left)
b.drive_straight(distance_mm(180), speed_mmps(100))
robot.say_text("I think I'll take a right!")
b.turn_in_place(right)
b.drive_straight(distance_mm(180), speed_mmps(100))
robot.say_text("Should I go off the table? Or should I go this way? Ok! I'll go this way.")
b.turn_in_place(right)
b.drive_straight(distance_mm(250), speed_mmps(300))
robot.say_text("Whoa! Almost there.")
b.turn_in_place(left)
b.drive_straight(distance_mm(120), speed_mmps(100))
end_anim = 'anim_eyepose_happy'
robot.anim.play_animation(end_anim)
robot.say_text("I made it! Woo hoo!")
lift_up(b)
lift_down(b)
lift_up(b)
for i in range(0,1):
b.turn_in_place(degrees(1080),degrees(360))
i +=1
robot.anim.play_animation('anim_dizzy_reaction_hard_01')
robot.say_text("Ooo. I'm dizzy!")
robot.say_text("All that driving made me tired. I'm going home now.")
b.drive_on_charger()
async def map_explorer(robot: anki_vector.robot.Robot):
# Drop the lift, so that it does not block the proximity sensor
robot.behavior.set_lift_height(0.0)
# Create the map state, and add it's rendering function to the viewer's render pipeline
state = MapState()
robot.viewer_3d.add_render_call(state.render)
# Comparison function used for sorting which open nodes are the furthest from all existing
# walls and loose contacts.
# (Using 1/r^2 to respond strongly to small numbers of close contact and weaking to many distant contacts)
def open_point_sort_func(position: Vector3):
proximity_sum = 0
for p in state.contact_nodes:
proximity_sum = proximity_sum + 1 / (p - position).magnitude_squared
for c in state.walls:
for p in c.vertices:
proximity_sum = proximity_sum + 1 / (p - position).magnitude_squared
return proximity_sum
# Loop until running out of open samples to navigate to,
# or if the process has yet to start (indicated by a lack of cleared_territories).
while (state.open_nodes and ACTIVELY_EXPLORE_SPACE) or not state.cleared_territories:
if robot.pose:
# Delete any open samples range of the robot.
state.open_nodes = [position for position in state.open_nodes if (position - robot.pose.position).magnitude > PROXIMITY_SCAN_DISTANCE_THRESHOLD_MM]
# Collect map data for the robot's current location.
await scan_area(robot, state)
# Add where the robot is to the map's cleared territories.
state.cleared_territories.append(ClearedTerritory(robot.pose.position, PROXIMITY_SCAN_DISTANCE_THRESHOLD_MM))
# @TODO: This whole block should ideally be replaced with the go_to_pose actions when that is ready to go.
# Alternatively, the turn&drive commands can be modified to respond to collisions by cancelling. After
# either change, ACTIVELY_EXPLORE_SPACE should be defaulted True
if ACTIVELY_EXPLORE_SPACE and state.open_nodes:
# Sort the open nodes and find our next navigation point.
state.open_nodes.sort(key=open_point_sort_func)
nav_point = state.open_nodes[0]
# Calculate the distance and direction of this next navigation point.
nav_point_delta = Vector3(
nav_point.x - robot.pose.position.x,
nav_point.y - robot.pose.position.y,
0)
nav_distance = nav_point_delta.magnitude
nav_direction = nav_point_delta.normalized
# Convert the nav_direction into a turn angle relative to the robot's current facing.
robot_forward = Vector3(*robot.pose.rotation.to_matrix().forward_xyz).normalized
turn_angle = acos(nav_direction.dot(robot_forward))
if nav_direction.cross(robot_forward).z > 0:
turn_angle = -turn_angle
# Turn toward the nav point, and drive to it.
robot.behavior.turn_in_place(angle=radians(turn_angle), speed=degrees(EXPLORE_TURN_SPEED_DPS))
robot.behavior.drive_straight(distance=distance_mm(nav_distance), speed=speed_mmps(EXPLORE_DRIVE_SPEED_MMPS))
if PROXIMITY_EXPLORATION_SHUTDOWN_DELAY_S == 0.0:
while True:
await asyncio.sleep(1.0)
else:
print('finished exploring - waiting an additional {0} seconds, then shutting down'.format(PROXIMITY_EXPLORATION_SHUTDOWN_DELAY_S))
await asyncio.sleep(PROXIMITY_EXPLORATION_SHUTDOWN_DELAY_S)
