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 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 _move_head(self, cmd):
"""
Move head to given angle.
:type cmd: Float64
:param cmd: The message containing angle in degrees. [-22.0 - 45.0]
"""
# action = self._vector.behavior.set_head_angle(radians(cmd.data * np.pi / 180.), duration=0.1,
# in_parallel=True)
action = self._vector.behavior.set_head_angle(radians(cmd.data * np.pi / 180.), duration=0.1)
def handle_mouse(self, mouse_x, mouse_y):
"""Called whenever mouse moves
mouse_x, mouse_y are in in 0..1 range (0,0 = top left, 1,1 = bottom right of window)
"""
if self.is_mouse_look_enabled:
mouse_sensitivity = 1.5 # higher = more twitchy
self.mouse_dir = remap_to_range(mouse_x, 0.0, 1.0, -mouse_sensitivity, mouse_sensitivity)
self.update_mouse_driving()
desired_head_angle = remap_to_range(mouse_y, 0.0, 1.0, 45, -25)
head_angle_delta = desired_head_angle - util.radians(self.vector.head_angle_rad).degrees
head_vel = head_angle_delta * 0.03
self.vector.motors.set_head_motor(head_vel)
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)
