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)