def __init__(self):
landmarks = {
cube1: Pose(55, 160, 0, angle_z=degrees(90)),
cube2: Pose(160, 55, 0, angle_z=degrees(0)),
cube3: Pose(160, -55, 0, angle_z=degrees(0))
}
pf = ParticleFilter(robot,
landmarks=landmarks,
sensor_model=CubeSensorModel(robot))
super().__init__(particle_filter=pf, particle_viewer=True)
def __init__(self):
landmarks = {
0 : Pose(-55, 160, 0, angle_z=degrees(90)),
1 : Pose( 55, 160, 0, angle_z=degrees(90)),
2 : Pose(160, 55, 0, angle_z=degrees( 0)),
3 : Pose(160, -55, 0, angle_z=degrees( 0))
}
pf = ParticleFilter(robot,
landmarks = landmarks,
sensor_model = ArucoCombinedSensorModel(robot))
super().__init__(particle_filter=pf, particle_viewer=True)
def main():
args = anki_vector.util.parse_command_args()
with anki_vector.Robot(args.serial) as robot:
#drive off charger
robot.behavior.drive_off_charger()
robot.world.connect_cube()
#pick up cube object
if robot.world.connected_light_cube:
robot.behavior.pickup_object(robot.world.connected_light_cube)
#get current position of robot
current_robot_pose = robot.pose.position
print(current_robot_pose)
#got to desired position with cube
pose = Pose(x=10, y=10, z=0, angle_z=Angle(degrees=0))
robot.behavior.go_to_pose(pose)
print(robot.pose.position)
#drop off the cube object
time.sleep(3.0)
pose_future.cancel()
robot.behavior.place_object_on_ground_here()
def start(self, event=None):
x = self.parent.obj.x
y = self.parent.obj.y
theta = self.parent.obj.theta
self.target_pose = Pose(x + 250*cos(theta), y+250*sin(theta), self.robot.pose.position.z,
angle_z=Angle(radians = wrap_angle(theta+pi)))
print('Traveling to',self.target_pose)
super().start(event)
def start(self, event=None):
wall = self.parent.object
print('Selected wall',self.parent.wobj)
(x, y, theta) = self.parent.pick_side(150)
self.target_pose = Pose(x, y, self.robot.pose.position.z,
angle_z=Angle(radians = wrap_angle(theta)))
print('Traveling to',self.target_pose)
super().start(event)
def main():
args = anki_vector.util.parse_command_args()
with anki_vector.Robot(args.serial,
show_3d_viewer=True,
enable_nav_map_feed=True) as robot:
robot.behavior.drive_off_charger()
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 custom object created successfully")
robot.behavior.go_to_pose(Pose(200, 0, 0, angle_z=degrees(0)),
relative_to_robot=True)
robot.world.delete_custom_objects()
def display(self, pose: util.Pose):
"""Displays the precomputed view at a specific pose in 3d space.
:param pose: Where to display the cube.
"""
glPushMatrix()
# TODO if cube_pose.is_accurate is False, render half-translucent?
# (This would require using a shader, or having duplicate objects)
cube_matrix = pose.to_matrix()
glMultMatrixf(cube_matrix.in_row_order)
# Cube is drawn slightly larger than the 10mm to 1 cm scale, as the model looks small otherwise
cube_scale_amt = 10.7
glScalef(cube_scale_amt, cube_scale_amt, cube_scale_amt)
self.display_all()
glPopMatrix()
def display(self, pose: util.Pose, head_angle: util.Angle,
lift_position: util.Distance):
"""Displays the precomputed view at a specific pose in 3d space.
:param pose: Where to display the robot.
"""
if not self._display_lists:
return
robot_matrix = pose.to_matrix()
head_angle_degrees = head_angle.degrees
# Get the angle of Vector's lift for rendering - we subtract the angle
# of the lift in the default pose in the object, and apply the inverse
# rotation
sin_angle = (lift_position.distance_mm -
LIFT_PIVOT_HEIGHT_MM) / LIFT_ARM_LENGTH_MM
angle_radians = math.asin(sin_angle)
lift_angle = -(angle_radians - LIFT_ANGLE_IN_DEFAULT_POSE)
lift_angle_degrees = math.degrees(lift_angle)
glPushMatrix()
glEnable(GL_LIGHTING)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glEnable(GL_BLEND)
glMultMatrixf(robot_matrix.in_row_order)
robot_scale_amt = 10.0 # cm to mm
glScalef(robot_scale_amt, robot_scale_amt, robot_scale_amt)
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
self._display_vector_body()
self._display_vector_lift(lift_angle_degrees)
self._display_vector_head(head_angle_degrees)
glDisable(GL_LIGHTING)
glPopMatrix()
import anki_vector
import time
from anki_vector.util import degrees, distance_mm, speed_mmps, Pose
from anki_vector.events import Events
import functools
import threading
import sys
cube_pose = Pose(x=0, y=0, z=0, angle_z=degrees(0))
observed_event = threading.Event()
moved_event = threading.Event()
def on_oo(event_type, event):
if (event.object_family == 3):
observed_event.set()
def on_om(event_type, event):
moved_event.set()
def main():
with anki_vector.Robot(requires_behavior_control=True) as robot:
try:
while True:
robot.behavior.set_eye_color(0.0, 1.0)
robot.anim.play_animation('anim_eyepose_furious')
def __init__(self, x, y, angle, abort_on_stop=True, **action_kwargs):
self.pose = Pose(x=x, y=y, z=0, angle_z=Angle(degrees=angle))
self.action_kwargs = action_kwargs
super().__init__(abort_on_stop)
