PYTHON   14

Lab3 State Machine Program

Guest on 2nd August 2022 12:40:09 AM

  1. """
  2. In the particle viewer window:
  3.   the WASD keys move the robot
  4.  'e' forces an evaluation step
  5.  'r' forces a resampling
  6.  'v' displays the weight statistics
  7.  'z' re-randomizes the particles.
  8. """
  9.  
  10. from cozmo_fsm import *
  11. from cozmo.util import degrees, Pose
  12.  
  13. ARUCO_MARKER_SIZE = 50   # millimeters
  14.  
  15. class Lab3(StateMachineProgram):
  16.     def __init__(self):
  17.         landmarks = {
  18.             'Aruco-0' : Pose(-55, 160, 0, angle_z=degrees(-90)),
  19.             'Aruco-1' : Pose( 55, 160, 0, angle_z=degrees(-90)),
  20.             'Aruco-2' : Pose(160,  55, 0, angle_z=degrees(180)),
  21.             'Aruco-3' : Pose(160, -55, 0, angle_z=degrees(180))
  22.         }
  23.  
  24.         pf = ParticleFilter(robot,
  25.                             landmarks = landmarks,
  26.                             sensor_model = ArucoDistanceSensorModel(robot)
  27.                             #sensor_model = ArucoBearingSensorModel(robot)
  28.                             #sensor_model = ArucoCombinedSensorModel(robot)
  29.         )
  30.  
  31.         super().__init__(aruco_marker_size=ARUCO_MARKER_SIZE,
  32.                          particle_filter=pf)

Raw Paste

Login or Register to edit or fork this paste. It's free.