Esempio n. 1
0
    def __init__(self):

        config = ConfigSingleton.get_instance()
        self.cheating = config.getboolean("Odometer", "cheating")

        # Parameters.  These are the four alpha parameter's from Thrun et. al's
        # odometry motion model.  We simplify this model so that angular error
        # is represented with ALPHA1 and translational error with ALPHA2.  We
        # further simplify by relating these two parameters linearly, reducing
        # the error model down to one parameter.
        self.alpha = config.getfloat("Odometer", "alpha") / CM_TO_PIXELS
        self.alpha1 = self.alpha * 0.01
        self.alpha2 = self.alpha * 0.1
        self.alpha3 = self.alpha2
        self.alpha4 = 0

        self.x = 0
        self.y = 0
        self.theta = 0
        self.lastX = 0
        self.lastY = 0
        self.lastTheta = 0

        # Needed by noisyEstimate
        self.lastTrueX = None
        self.lastTrueY = None
        self.lastTrueTheta = None
Esempio n. 2
0
    def __init__(self, this_robot, puck_mask):
        """
        puck_mask -- The mask for pucks this controller recognizes
        """
        self.puck_mask = puck_mask

        config = ConfigSingleton.get_instance()

        self.linear_speed = config.getfloat("FlowController", "linear_speed")
        self.angular_speed = config.getfloat("FlowController", "angular_speed")
        self.slow_factor = config.getfloat("FlowController", "slow_factor")
        self.inside_radius = config.getfloat("FlowController", "inside_radius")
        self.outside_radius = config.getfloat("FlowController",
                                             "outside_radius")
        self.wander_prob = config.getfloat("FlowController", "wander_prob")
        self.wander_mean_duration = config.getfloat("FlowController",
                                           "wander_mean_duration")
        self.wander_sigma_duration = config.getfloat("FlowController",
                                           "wander_sigma_duration")
        self.poke_prob = config.getfloat("FlowController", "poke_prob")
        self.poke_mean_duration = config.getfloat("FlowController",
                                           "poke_mean_duration")
        self.poke_sigma_duration = config.getfloat("FlowController",
                                           "poke_sigma_duration")

        self.transition_to_FLOW(this_robot)
Esempio n. 3
0
    def __init__(self, landmark_mask, acceptable_puck_mask):
        self.landmark_mask = landmark_mask
        self.acceptable_puck_mask = acceptable_puck_mask

        config = ConfigSingleton.get_instance()

        self.threshold = config.getfloat("BounceController", "threshold")
Esempio n. 4
0
    def __init__(self, this_robot, puck_mask):
        """
        puck_mask -- The mask for pucks this controller recognizes
        """
        self.puck_mask = puck_mask

        config = ConfigSingleton.get_instance()

        self.linear_speed = config.getfloat("FlowController", "linear_speed")
        self.angular_speed = config.getfloat("FlowController", "angular_speed")
        self.slow_factor = config.getfloat("FlowController", "slow_factor")
        self.inside_radius = config.getfloat("FlowController", "inside_radius")
        self.outside_radius = config.getfloat("FlowController",
                                              "outside_radius")
        self.wander_prob = config.getfloat("FlowController", "wander_prob")
        self.wander_mean_duration = config.getfloat("FlowController",
                                                    "wander_mean_duration")
        self.wander_sigma_duration = config.getfloat("FlowController",
                                                     "wander_sigma_duration")
        self.poke_prob = config.getfloat("FlowController", "poke_prob")
        self.poke_mean_duration = config.getfloat("FlowController",
                                                  "poke_mean_duration")
        self.poke_sigma_duration = config.getfloat("FlowController",
                                                   "poke_sigma_duration")

        self.transition_to_FLOW(this_robot)
Esempio n. 5
0
    def __init__(self, mask, radius):
        self.body = Body(0, 0, Body.STATIC)
        self.body.position = 0, 0
        self.body.angle = 0
        self.body.velocity = 0, 0
        self.body.angular_velocity = 0

        self.shape = Circle(self.body, radius)

        self.mask = mask
        self.shape.filter = ShapeFilter(categories=mask)
        if mask == ARC_LANDMARK_MASK:
            self.shape.color = 0, 255, 0
        elif mask == POLE_LANDMARK_MASK:
            self.shape.color = 0, 0, 255
        elif mask == BLAST_LANDMARK_MASK:
            self.shape.color = 255, 0, 0
        else:
            sys.exit("Unknown landmark mask: " + str(mask))

        # The following is just to set the appropriate params to visualize below
        config = ConfigSingleton.get_instance()
        self.vis_range_max =  \
            config.getfloat("RangeScan:landmarks", "range_max") \
            + radius
        self.vis_inside_radius = \
            config.getfloat("LandmarkCircleController", "inside_radius") \
            + radius
        self.vis_outside_radius = \
            config.getfloat("LandmarkCircleController", "outside_radius") \
            + radius
Esempio n. 6
0
    def __init__(self, landmark_mask, acceptable_puck_mask):
        self.landmark_mask = landmark_mask
        self.acceptable_puck_mask = acceptable_puck_mask

        config = ConfigSingleton.get_instance()

        self.threshold = config.getfloat("BounceController", "threshold")
Esempio n. 7
0
    def __init__(self, config_section):

        config = ConfigSingleton.get_instance()
        self.config_section = config_section
        self.NUMBER_POINTS = config.getint(config_section, "number_points")
        self.ANGLE_MIN = config.getfloat(config_section, "angle_min")
        self.ANGLE_MAX = config.getfloat(config_section, "angle_max")
        #self.RANGE_MIN = config.getfloat(config_section, "range_min")
        self.RANGE_MAX = config.getfloat(config_section, "range_max")

        self.angles = []
        if self.NUMBER_POINTS == 1:
            self.angles.append(self.ANGLE_MIN)
        else:
            angle_delta = (self.ANGLE_MAX - self.ANGLE_MIN) / \
                          (self.NUMBER_POINTS - 1)
            for i in range(self.NUMBER_POINTS):
                self.angles.append(self.ANGLE_MIN + i * angle_delta)

        self.ranges = []
        self.masks = []

        #self.INNER_RADIUS = robot.radius + 5
        #self.INNER_RADIUS = robot.radius + self.RANGE_MIN
        #self.OUTER_RADIUS = self.INNER_RADIUS + self.RANGE_MAX
        self.OUTER_RADIUS = self.RANGE_MAX
Esempio n. 8
0
    def __init__(self, mask, radius):
        self.body = Body(0, 0, Body.STATIC)
        self.body.position = 0, 0
        self.body.angle = 0
        self.body.velocity = 0, 0
        self.body.angular_velocity = 0

        self.shape = Circle(self.body, radius)

        self.mask = mask
        self.shape.filter = ShapeFilter(categories = mask)
        if mask == ARC_LANDMARK_MASK:
            self.shape.color = 0, 255, 0
        elif mask == POLE_LANDMARK_MASK:
            self.shape.color = 0, 0, 255 
        elif mask == BLAST_LANDMARK_MASK:
            self.shape.color = 255, 0, 0
        else:
            sys.exit("Unknown landmark mask: " + str(mask))

        # The following is just to set the appropriate params to visualize below
        config = ConfigSingleton.get_instance()
        self.vis_range_max =  \
            config.getfloat("RangeScan:landmarks", "range_max") \
            + radius
        self.vis_inside_radius = \
            config.getfloat("LandmarkCircleController", "inside_radius") \
            + radius
        self.vis_outside_radius = \
            config.getfloat("LandmarkCircleController", "outside_radius") \
            + radius
Esempio n. 9
0
    def __init__(self):

        config = ConfigSingleton.get_instance()
        self.MIN_ANGLE = config.getfloat("LandmarkScan", "min_angle")
        self.MAX_ANGLE = config.getfloat("LandmarkScan", "max_angle")
        self.MIN_RANGE = config.getfloat("LandmarkScan", "min_range")
        self.MAX_RANGE = config.getfloat("LandmarkScan", "max_range")

        self.landmarks = []
Esempio n. 10
0
    def __init__(self, acceptable_puck_mask):
        self.acceptable_puck_mask = acceptable_puck_mask

        config = ConfigSingleton.get_instance()

        # These properties are shared with GauciController
        self.linear_speed = config.getfloat("GauciController", "linear_speed")
        self.angular_speed = config.getfloat("GauciController", "angular_speed")
        self.slow_factor = config.getfloat("GauciController", "slow_factor")
Esempio n. 11
0
    def __init__(self, puck_mask):
        self.puck_mask = puck_mask

        config = ConfigSingleton.get_instance()

        self.front_angle_threshold = config.getfloat("GauciController",
                                            "front_angle_threshold")
        self.linear_speed = config.getfloat("GauciController", "linear_speed")
        self.angular_speed = config.getfloat("GauciController", "angular_speed")
        self.slow_factor = config.getfloat("GauciController", "slow_factor")
Esempio n. 12
0
    def __init__(self, acceptable_puck_mask):
        self.acceptable_puck_mask = acceptable_puck_mask

        config = ConfigSingleton.get_instance()

        # These properties are shared with GauciController
        self.linear_speed = config.getfloat("GauciController", "linear_speed")
        self.angular_speed = config.getfloat("GauciController",
                                             "angular_speed")
        self.slow_factor = config.getfloat("GauciController", "slow_factor")
Esempio n. 13
0
    def __init__(self, robot, puck_mask):
        """
        puck_mask -- The mask for pucks this controller recognizes
        """
        self.puck_mask = puck_mask

        config = ConfigSingleton.get_instance()

        self.linear_speed = config.getfloat("BumpController", 
                                            "linear_speed")
        self.angular_speed = config.getfloat("BumpController",
                                             "angular_speed")
        self.slow_factor = config.getfloat("BumpController",
                                           "slow_factor")
        self.puck_dist_threshold = config.getfloat("BumpController",
                                           "puck_dist_threshold")
        self.robot_react_range = config.getfloat("BumpController",
                                           "robot_react_range")
        self.wall_react_range = config.getfloat("BumpController",
                                           "wall_react_range")
        #self.target_angle_bias = config.getfloat("BumpController",
        #                                   "target_angle_bias")
        self.inner_exclude_angle = config.getfloat("BumpController",
                                           "inner_exclude_angle")
        self.outer_exclude_angle = config.getfloat("BumpController",
                                           "outer_exclude_angle")
        self.use_tracking = config.getboolean("BumpController", "use_tracking")
        self.tracking_threshold = config.getfloat("BumpController",
                                           "tracking_threshold")
        self.lmark_ideal_range = config.getfloat("BumpController",
                                           "lmark_ideal_range")

        # One of: "SIMPLE" "BACK_RIGHT" "FRONT_POSITIVE" "BOTH"
        self.pair_condition = config.get("BumpController", "pair_condition")

        # One of: "SIMPLE", "ON RIGHT", "ONLY_IF_NO_PAIR"
        self.single_condition = config.get("BumpController", "single_condition")

        # One of: "SIMPLE", "ALPHA", "BETA", "BOTH"
        self.puck_condition = config.get("BumpController", "puck_condition")

        self.wall_turn_on_spot = config.getboolean("BumpController",
                                           "wall_turn_on_spot")
        self.no_target_turn_on_spot = config.getboolean("BumpController",
                                           "no_target_turn_on_spot")
        self.curve_gamma = config.getfloat("BumpController",
                                           "curve_gamma")

        self.lmark_pair_dist = config.getint("AlvinSim", "lmark_pair_dist") + 10

        self.robot = robot

        # Position of the target
        self.target_pos = None
Esempio n. 14
0
    def __init__(self, puck_mask):
        self.puck_mask = puck_mask

        config = ConfigSingleton.get_instance()

        self.front_angle_threshold = config.getfloat("GauciController",
                                                     "front_angle_threshold")
        self.linear_speed = config.getfloat("GauciController", "linear_speed")
        self.angular_speed = config.getfloat("GauciController",
                                             "angular_speed")
        self.slow_factor = config.getfloat("GauciController", "slow_factor")
Esempio n. 15
0
    def __init__(self, acceptable_puck_mask):
        self.acceptable_puck_mask = acceptable_puck_mask

        config = ConfigSingleton.get_instance()

        # These properties are shared with GauciController
        self.linear_speed = config.getfloat("GauciController", "linear_speed")

        # This property is shared with LeftmostController
        self.circle_radius = config.getfloat("PushoutController", "circle_radius")

        # This property is for this controller
        self.homing_timeout = config.getfloat("PushoutController", "homing_timeout")
        self.angular_speed = config.getfloat("PushoutController", "angular_speed")

        # Possible states are PUSHING and HOMING
        self.state = "PUSHING"
Esempio n. 16
0
    def __init__(self, acceptable_puck_mask):
        self.acceptable_puck_mask = acceptable_puck_mask

        config = ConfigSingleton.get_instance()

        # These properties are shared with GauciController
        self.linear_speed = config.getfloat("GauciController", "linear_speed")
        self.angular_speed = config.getfloat("GauciController", "angular_speed")
        self.slow_factor = config.getfloat("GauciController", "slow_factor")

        # These properties are particular to this controller
        self.modulate = config.getboolean("LeftmostController", "modulate")
        self.ingress_angle = config.getfloat("LeftmostController", "ingress_angle")
        self.egress_angle = config.getfloat("LeftmostController", "egress_angle")
        self.circle_radius = config.getfloat("LeftmostController", "circle_radius")


        self.summed_angular_speed = 0
Esempio n. 17
0
    def __init__(self, output_dir, landmarks):

        config = ConfigSingleton.get_instance()
        self.lmark_pair_dist = config.get("AlvinSim", "lmark_pair_dist")

        # Open data files and place a comment in the first line of each
        self.avg_puck_segment_dist_file = open(
            '{}/avg_puck_segment_dist.dat'.format(output_dir), 'a')
        self.avg_puck_segment_dist_file.write(
            '# STEP, AVG_PUCK_SEGMENT_DIST\n')

        self.pucks_file = open('{}/pucks.dat'.format(output_dir), 'a')
        self.pucks_file.write('# STEP, (X, Y) COORDINATES FOR ALL PUCKS\n')

        self.robots_file = open('{}/robots.dat'.format(output_dir), 'a')
        self.robots_file.write(
            '# STEP, (X, Y, THETA) COORDINATES FOR ALL ROBOTS\n')

        # For the landmarks file, we will go ahead and enter it here.
        if len(landmarks) > 0:
            landmarks_file = open('{}/landmarks.dat'.format(output_dir), 'a')
            landmarks_file.write('# (X, Y) COORDINATES FOR ALL LANDMARKS\n')
            for i in range(len(landmarks) - 1):
                pos = landmarks[i].body.position
                landmarks_file.write('{:.5g}, {:.5g}, '.format(pos.x, pos.y))
            pos = landmarks[-1].body.position
            landmarks_file.write('{:.5g}, {:.5g}\n'.format(pos.x, pos.y))

        # Get a list of all landmark pairs for the inter-landmark distance is
        # less than the threshold (meaning that the line segment between them
        # should be filled in).  The result will be list of tuples representing
        # the coordinates of the landmark pairs in the robot reference frame.
        self.lmark_pairs = []
        n = len(landmarks)
        for i in range(n):
            lmark_i = landmarks[i]
            for j in range(i + 1, n):
                lmark_j = landmarks[j]
                dx = lmark_i.body.position.x - lmark_j.body.position.x
                dy = lmark_i.body.position.y - lmark_j.body.position.y
                inter_lmark_dist = sqrt(dx * dx + dy * dy)
                if inter_lmark_dist < self.lmark_pair_dist:
                    self.lmark_pairs.append((lmark_i, lmark_j))
Esempio n. 18
0
    def __init__(self, acceptable_puck_mask):
        self.acceptable_puck_mask = acceptable_puck_mask

        config = ConfigSingleton.get_instance()

        # These properties are shared with GauciController
        self.linear_speed = config.getfloat("GauciController", "linear_speed")

        # This property is shared with LeftmostController
        self.circle_radius = config.getfloat("PushoutController",
                                             "circle_radius")

        # This property is for this controller
        self.homing_timeout = config.getfloat("PushoutController",
                                              "homing_timeout")
        self.angular_speed = config.getfloat("PushoutController",
                                             "angular_speed")

        # Possible states are PUSHING and HOMING
        self.state = "PUSHING"
Esempio n. 19
0
    def __init__(self, this_robot, puck_mask):
        """
        puck_mask -- The mask for pucks this controller recognizes
        """
        self.puck_mask = puck_mask

        config = ConfigSingleton.get_instance()

        # These properties are shared with GauciController
        self.front_angle_threshold = config.getfloat("GauciController",
                                            "front_angle_threshold")
        self.linear_speed = config.getfloat("GauciController", "linear_speed")
        self.angular_speed = config.getfloat("GauciController", "angular_speed")
        self.slow_factor = config.getfloat("GauciController", "slow_factor")

        # These properties are for this controller
        self.inside_radius = config.getfloat("LandmarkCircleController",
                                             "inside_radius")
        self.outside_radius = config.getfloat("LandmarkCircleController",
                                             "outside_radius")
        self.homing_timeout = config.getfloat("LandmarkCircleController",
                                              "homing_timeout")
        self.homing_angular_speed = config.getfloat("LandmarkCircleController",
                                             "homing_angular_speed")
        self.homing_angular_speed = config.getfloat("LandmarkCircleController",
                                             "homing_angular_speed")
        self.outie_trans_prob = config.getfloat("LandmarkCircleController",
                                             "outie_trans_prob")

        """
        We will randomly assign 'outie'.  An "outie" moves out from arc
        landmarks and then acts according to a variant of Gauci et al's
        controller.  An "innie" (outie == False) pushes pucks outwards when
        within the desired radius and tries to maintain itself within that
        radius.
        """
        self.outie = (random() < 0.5)
        self.outie_changed(this_robot)

        # Initial state for innies.  Possible states are PUSHING and HOMING.
        self.innie_state = "PUSHING"
    def __init__(self, this_robot, puck_mask):
        """
        puck_mask -- The mask for pucks this controller recognizes
        """
        self.puck_mask = puck_mask

        config = ConfigSingleton.get_instance()

        # These properties are shared with GauciController
        self.front_angle_threshold = config.getfloat("GauciController",
                                                     "front_angle_threshold")
        self.linear_speed = config.getfloat("GauciController", "linear_speed")
        self.angular_speed = config.getfloat("GauciController",
                                             "angular_speed")
        self.slow_factor = config.getfloat("GauciController", "slow_factor")

        # These properties are for this controller
        self.inside_radius = config.getfloat("LandmarkCircleController",
                                             "inside_radius")
        self.outside_radius = config.getfloat("LandmarkCircleController",
                                              "outside_radius")
        self.homing_timeout = config.getfloat("LandmarkCircleController",
                                              "homing_timeout")
        self.homing_angular_speed = config.getfloat("LandmarkCircleController",
                                                    "homing_angular_speed")
        self.homing_angular_speed = config.getfloat("LandmarkCircleController",
                                                    "homing_angular_speed")
        self.outie_trans_prob = config.getfloat("LandmarkCircleController",
                                                "outie_trans_prob")
        """
        We will randomly assign 'outie'.  An "outie" moves out from arc
        landmarks and then acts according to a variant of Gauci et al's
        controller.  An "innie" (outie == False) pushes pucks outwards when
        within the desired radius and tries to maintain itself within that
        radius.
        """
        self.outie = (random() < 0.5)
        self.outie_changed(this_robot)

        # Initial state for innies.  Possible states are PUSHING and HOMING.
        self.innie_state = "PUSHING"
Esempio n. 21
0
    def __init__(self, acceptable_puck_mask):
        self.acceptable_puck_mask = acceptable_puck_mask

        config = ConfigSingleton.get_instance()

        # These properties are shared with GauciController
        self.linear_speed = config.getfloat("GauciController", "linear_speed")
        self.angular_speed = config.getfloat("GauciController",
                                             "angular_speed")
        self.slow_factor = config.getfloat("GauciController", "slow_factor")

        # These properties are particular to this controller
        self.modulate = config.getboolean("LeftmostController", "modulate")
        self.ingress_angle = config.getfloat("LeftmostController",
                                             "ingress_angle")
        self.egress_angle = config.getfloat("LeftmostController",
                                            "egress_angle")
        self.circle_radius = config.getfloat("LeftmostController",
                                             "circle_radius")

        self.summed_angular_speed = 0
Esempio n. 22
0
    def __init__(self, config_file, trial_number):
        # Use the base of the config file's name as the name of the output dir
        output_dir_base = str.split(config_file, '.')[0]
        self.output_dir = output_dir_base + '/' + str(trial_number)
        print "OUTPUT DIR: "
        print self.output_dir

        config = ConfigSingleton.get_instance(config_file)

        # Load parameters from config file.  The values 'width' and 'height'
        # will actually be set in the call to 'super' below.
        w = config.getint("AlvinSim", "width")
        h = config.getint("AlvinSim", "height")
        self.number_robots = config.getint("AlvinSim", "number_robots")
        self.number_pucks = config.getint("AlvinSim", "number_pucks")
        self.number_puck_kinds = config.getint("AlvinSim", "number_puck_kinds")
        self.number_landmarks = config.getint("AlvinSim", "number_landmarks")
        self.number_steps = config.getint("AlvinSim", "number_steps")
        self.puck_ring = config.getboolean("AlvinSim", "puck_ring")
        self.puck_ring_radius = config.getint("AlvinSim", "puck_ring_radius")
        self.landmark_ring = config.getboolean("AlvinSim", "landmark_ring")
        self.landmark_ring_radius = config.getint("AlvinSim",
                                                  "landmark_ring_radius")
        self.puck_kinds = range(self.number_puck_kinds)
        self.wall_thickness = config.getint("AlvinSim", "wall_thickness")
        self.analyze = config.getboolean("AlvinSim", "analyze")
        self.capture_screenshots = config.getboolean("AlvinSim",
                                                     "capture_screenshots")
        self.visualize_probes = config.getboolean("AlvinSim",
                                                  "visualize_probes")
        self.controller_name = config.get("AlvinSim", "controller_name")

        super(AlvinSim, self).__init__(w, h, visible=False)

        self.set_caption(config_file)

        self.keyboard = key.KeyStateHandler()
        self.push_handlers(self.keyboard)
        self.draw_options = pymunk.pyglet_util.DrawOptions()
        self.draw_options.flags = self.draw_options.DRAW_SHAPES

        # Help text
        self.helpLabel = pyglet.text.Label(
            'ESC: quit, arrow-keys: move, p: puck sens, l: lmark sens, c: vis ctrls, t: trans, r: rot',
            font_size=12,
            x=self.width // 2,
            y=self.height // 20,
            anchor_x='center',
            anchor_y='center')

        # Label to show various stats
        self.statsLabel = pyglet.text.Label(font_size=18,
                                            x=self.width // 2,
                                            y=self.height - 30,
                                            anchor_x='center',
                                            anchor_y='center')

        self.set_stats_label_text()

        # Objects for mouse interaction
        self.spring_body = None
        self.selected_static_body = None
        self.mouse_body = pymunk.Body(body_type=pymunk.Body.KINEMATIC)

        # build simulation environment
        self.env = pymunk.Space()
        self.env.damping = 0.01  # 99% of velocity is lost per second

        # Seed random number generator.
        seed(trial_number)

        # Create the walls, robots, pucks, and landmarks
        self.create_border_walls()
        self.create_random_walls()
        #self.create_one_wall()
        self.robots = []
        self.pucks = []
        self.landmarks = []
        self.probes = []
        self.create_robots()
        if self.puck_ring:
            self.create_pucks_ring()
        else:
            self.create_pucks_random()
        #self.create_immobile_pucks()
        #if self.landmark_ring:
        #    self.create_landmarks_ring()
        #else:
        #    self.create_landmarks_random()
        #self.create_canned_landmarks()
        if self.visualize_probes:
            self.create_probe_grid()

        # Prep for capturing screenshots
        if self.capture_screenshots:
            shutil.rmtree(self.output_dir, ignore_errors=True)
            os.makedirs(self.output_dir)

        if self.analyze:
            init(self.output_dir)

        # Schedule the key callbacks
        for robot in self.robots:
            pyglet.clock.schedule_interval(robot.control_step, 1.0 / 120)
        pyglet.clock.schedule_interval(self.update, 1.0 / 60)
        pyglet.clock.schedule_interval(self.env.step, 1.0 / 60)

        # Setup to handle collisions
        #self.env.add_default_collision_handler().begin = self.collision_handler

        # start simulation
        self.set_visible(True)
Esempio n. 23
0
    def __init__(self, robot, puck_mask):
        """
        puck_mask -- The mask for pucks this controller recognizes
        """
        self.puck_mask = puck_mask

        config = ConfigSingleton.get_instance()

        self.linear_speed = config.getfloat("OdoBumpController",
                                            "linear_speed")
        self.angular_speed = config.getfloat("OdoBumpController",
                                             "angular_speed")
        self.slow_factor = config.getfloat("OdoBumpController", "slow_factor")
        self.puck_dist_threshold = config.getfloat("OdoBumpController",
                                                   "puck_dist_threshold")
        self.robot_react_range = config.getfloat("OdoBumpController",
                                                 "robot_react_range")
        self.target_angle_bias = config.getfloat("OdoBumpController",
                                                 "target_angle_bias")
        self.inner_exclude_angle = config.getfloat("OdoBumpController",
                                                   "inner_exclude_angle")
        self.outer_exclude_angle = config.getfloat("OdoBumpController",
                                                   "outer_exclude_angle")
        self.odo_home_threshold = config.getfloat("OdoBumpController",
                                                  "odo_home_threshold")
        self.odo_home_factor = config.getfloat("OdoBumpController",
                                               "odo_home_factor")
        self.odo_home_timeout = config.getint("OdoBumpController",
                                              "odo_home_timeout")

        # One of: "NO_ODO", "DISTANCE_TRAVELLED", "COUNT":
        self.odo_condition = config.get("OdoBumpController", "odo_condition")

        # One of: "SIMPLE" "BACK_RIGHT" "FRONT_POSITIVE" "BOTH"
        self.pair_condition = config.get("OdoBumpController", "pair_condition")

        # One of: "SIMPLE", "ON RIGHT", "ONLY_IF_NO_PAIR"
        self.single_condition = config.get("OdoBumpController",
                                           "single_condition")

        # One of: "SIMPLE", "ALPHA", "BETA", "BOTH"
        self.puck_condition = config.get("OdoBumpController", "puck_condition")

        # lmark_dist_threshold is a bit different.  We set it depending upon
        # the configuration of canned landmarks.
        canned_landmarks_name = config.get("AlvinSim", "canned_landmarks_name")
        if canned_landmarks_name == "ONE_CENTRAL":
            self.lmark_dist_threshold = 0

        elif canned_landmarks_name == "L_SHAPE":
            self.lmark_dist_threshold = 480

        elif canned_landmarks_name == "M_SHAPE":
            self.lmark_dist_threshold = 230

        elif canned_landmarks_name == "C_SHAPE":
            self.lmark_dist_threshold = 215

        self.robot = robot

        # Possible states:
        #   "SENSE"    -- Process sensor data and select target point.
        #   "ODO_HOME" -- Move to target point via odometry-based homing.
        self.state = "SENSE"

        # Range and bearing of the target in the odometry reference frame.
        self.target_range_bearing = None

        # ODO_HOME-related
        self.odo_home_start_pos = None
        self.odo_home_start_distance = None
        self.odo_home_count = 0
Esempio n. 24
0
    def __init__(self, config_file, trial_number):
        # Use the base of the config file's name as the name of the output dir
        output_dir_base = str.split(config_file, '.')[0]
        self.output_dir = output_dir_base + '/' + str(trial_number)
        print(self.output_dir)

        config = ConfigSingleton.get_instance(config_file)

        # Load parameters from config file.
        self.width_cm = config.getint("AlvinSim", "width_cm")
        self.height_cm = config.getint("AlvinSim", "height_cm")
        self.width = int(self.width_cm * CM_TO_PIXELS)
        self.height = int(self.height_cm * CM_TO_PIXELS)
        #print "width, height: {}, {}".format(self.width, self.height);
        self.circ_border = config.getboolean("AlvinSim", "circ_border")
        self.number_robots = config.getint("AlvinSim", "number_robots")
        self.number_pucks = config.getint("AlvinSim", "number_pucks")
        self.number_puck_kinds = config.getint("AlvinSim", "number_puck_kinds")
        self.number_steps = config.getint("AlvinSim", "number_steps")
        self.puck_ring = config.getboolean("AlvinSim", "puck_ring")
        self.puck_ring_radius = config.getint("AlvinSim", "puck_ring_radius")
        self.canned_landmarks_name = config.get("AlvinSim",
                                                "canned_landmarks_name")
        self.puck_shape = config.get("AlvinSim", "puck_shape")
        self.lmark_pair_dist = config.getint("AlvinSim", "lmark_pair_dist")
        self.puck_kinds = list(range(self.number_puck_kinds))
        self.wall_thickness = config.getint("AlvinSim", "wall_thickness")
        self.capture_interval = config.getint("AlvinSim", "capture_interval")
        self.analyze = config.getboolean("AlvinSim", "analyze")
        self.capture_screenshots = config.getboolean("AlvinSim",
                                                     "capture_screenshots")
        self.visualize_probes = config.getboolean("AlvinSim",
                                                  "visualize_probes")
        self.controller_name = config.get("AlvinSim", "controller_name")
        self.render_skip = config.getint("AlvinSim", "render_skip")

        # build simulation environment
        self.env = pymunk.Space()
        self.env.damping = 0.0001  # 9999% of velocity is lost per second

        # Seed random number generator.
        seed(trial_number)

        # Create the walls, robots, pucks, and landmarks
        if self.circ_border:
            self.create_circ_border()
        else:
            self.create_rect_border()
        #self.create_random_walls()
        #self.create_one_wall()
        self.robots = []
        self.pucks = []
        self.landmarks = []
        self.probes = []
        self.create_robots()

        if self.puck_ring:
            self.create_pucks_ring()
        else:
            self.create_pucks_random()
        #self.create_immobile_pucks()
        self.create_canned_landmarks()
        if self.visualize_probes:
            self.create_probe_grid()

        # Prepare output directory.
        shutil.rmtree(self.output_dir, ignore_errors=True)
        os.makedirs(self.output_dir)

        if self.analyze:
            #init(self.output_dir)
            self.analyzer = Analyzer(self.output_dir, self.landmarks)

        # Setup to handle collisions
        #self.env.add_default_collision_handler().begin = self.collision_handler

        self.avg_dt = None
Esempio n. 25
0
#!/usr/bin/env python

import sys, os, shutil, math
from math import pi
from colorama import init, Fore, Style
from configsingleton import ConfigSingleton

def execute(cmd):
    # Print the command to execute in green
    print(Fore.GREEN + cmd)
    print(Style.RESET_ALL)
    os.system(cmd)

config = ConfigSingleton.get_instance('default.cfg')

#linear_speeds = [x / 2.0 for x in range(2, 10 + 1)]
#print linear_speeds

#angular_speeds = [x / 2.0 for x in range(1, 10 + 1)]
#print angular_speeds

#slow_factors = [x / 4.0 for x in range(1, 4 + 1)]
#print slow_factors

#front_angle_thresholds = [(pi/4)*(i/10.0) for i in range(0, 11)]
#print front_angle_thresholds

#number_robots = [i for i in range(2, 6)]
number_robots = [5]
print number_robots
Esempio n. 26
0
    def __init__(self, config_file, trial_number):
        # Use the base of the config file's name as the name of the output dir
        output_dir_base = str.split(config_file, '.')[0]
        self.output_dir = output_dir_base + '/' + str(trial_number)
        print "OUTPUT DIR: "
        print self.output_dir

        config = ConfigSingleton.get_instance(config_file)

        # Load parameters from config file.  The values 'width' and 'height'
        # will actually be set in the call to 'super' below.
        w = config.getint("AlvinSim", "width")
        h = config.getint("AlvinSim", "height")
        self.number_robots = config.getint("AlvinSim", "number_robots")
        self.number_pucks = config.getint("AlvinSim", "number_pucks")
        self.number_puck_kinds = config.getint("AlvinSim", "number_puck_kinds")
        self.number_landmarks = config.getint("AlvinSim", "number_landmarks")
        self.number_steps = config.getint("AlvinSim", "number_steps")
        self.puck_ring = config.getboolean("AlvinSim", "puck_ring")
        self.puck_ring_radius = config.getint("AlvinSim", "puck_ring_radius")
        self.landmark_ring = config.getboolean("AlvinSim", "landmark_ring")
        self.landmark_ring_radius = config.getint("AlvinSim", "landmark_ring_radius")
        self.puck_kinds = range(self.number_puck_kinds)
        self.wall_thickness = config.getint("AlvinSim", "wall_thickness")
        self.analyze = config.getboolean("AlvinSim", "analyze")
        self.capture_screenshots = config.getboolean("AlvinSim", "capture_screenshots")
        self.visualize_probes = config.getboolean("AlvinSim", "visualize_probes")
        self.controller_name = config.get("AlvinSim", "controller_name")

        super(AlvinSim, self).__init__(w, h, visible=False)

        self.set_caption(config_file)

        self.keyboard = key.KeyStateHandler()
        self.push_handlers(self.keyboard)
        self.draw_options = pymunk.pyglet_util.DrawOptions()
        self.draw_options.flags = self.draw_options.DRAW_SHAPES

        # Help text
        self.helpLabel = pyglet.text.Label(
            'ESC: quit, arrow-keys: move, p: puck sens, l: lmark sens, c: vis ctrls, t: trans, r: rot',
            font_size=12,
            x=self.width//2, y=self.height//20,
            anchor_x='center', anchor_y='center')

        # Label to show various stats
        self.statsLabel = pyglet.text.Label(
            font_size=18,
            x=self.width//2, y=self.height - 30,
            anchor_x='center', anchor_y='center')

        self.set_stats_label_text()

        # Objects for mouse interaction
        self.spring_body = None
        self.selected_static_body = None
        self.mouse_body = pymunk.Body(body_type = pymunk.Body.KINEMATIC)

        # build simulation environment
        self.env = pymunk.Space()
        self.env.damping = 0.01 # 99% of velocity is lost per second

        # Seed random number generator.
        seed(trial_number)

        # Create the walls, robots, pucks, and landmarks
        self.create_border_walls()
        #self.create_random_walls()
        #self.create_one_wall()
        self.robots = []
        self.pucks = []
        self.landmarks = []
        self.probes = []
        self.create_robots()
        if self.puck_ring:
            self.create_pucks_ring()
        else:
            self.create_pucks_random()
        #self.create_immobile_pucks()
        #if self.landmark_ring:
        #    self.create_landmarks_ring()
        #else:
        #    self.create_landmarks_random()
        #self.create_canned_landmarks()
        if self.visualize_probes:
            self.create_probe_grid()

        # Prep for capturing screenshots
        if self.capture_screenshots:
            shutil.rmtree(self.output_dir, ignore_errors=True)
            os.makedirs(self.output_dir)

        if self.analyze:
            init(self.output_dir)

        # Schedule the key callbacks
        for robot in self.robots:
            pyglet.clock.schedule_interval(robot.control_step, 1.0/120)
        pyglet.clock.schedule_interval(self.update, 1.0/60)
        pyglet.clock.schedule_interval(self.env.step, 1.0/60)

        # Setup to handle collisions
        #self.env.add_default_collision_handler().begin = self.collision_handler

        # start simulation
        self.set_visible(True)
Esempio n. 27
0
    def __init__(self, calib_filename, detection_mask, acceptance_mask,
                 frontal_only):
        config = ConfigSingleton.get_instance()
        self.min_distance = config.getfloat("PointSampleCam", "min_distance")
        self.max_distance = config.getfloat("PointSampleCam", "max_distance")
        self.max_abs_angle = config.getfloat("PointSampleCam", "max_abs_angle")

        # The detection mask is used to indicate all types of objects that
        # the sensor should be sensitive to.  However, if a detected object
        # doesn't also match the acceptance mask then it will be treated as
        # a wall.
        self.detection_mask = detection_mask
        self.acceptance_mask = acceptance_mask

        self.calib_array = np.loadtxt(calib_filename, delimiter=',')
        self.calib_array[:, 2] *= CM_TO_PIXELS
        self.calib_array[:, 3] *= CM_TO_PIXELS

        # We will also store within calib_array the following additional
        # quantities derived from (xr, yr) so that we don't need to compute
        # these again.
        #   angle of (xr, yr) w.r.t. to X_R axis --- atan2(yr, xr)
        #   length of (xr, yr)                   --- sqrt(xr*xr + yr*yr)
        n_rows = self.calib_array.shape[0]
        # Add the two extra columns
        self.calib_array = np.append(self.calib_array,
                                     np.zeros((n_rows, 2)),
                                     axis=1)
        for i in range(n_rows):
            (xr, yr) = self.calib_array[i, 2], self.calib_array[i, 3]
            self.calib_array[i, 4] = atan2(yr, xr)
            self.calib_array[i, 5] = sqrt(xr * xr + yr * yr)

        if frontal_only:
            # Delete all rows with distance outside of [min_distance,
            # max_distance] and angle outside of [-max_abs_angle, max_abs_angle]
            delete_indices = []
            for i in range(n_rows):
                angle = self.calib_array[i, 4]
                dist = self.calib_array[i, 5]
                if fabs(dist) < self.min_distance:
                    delete_indices.append(i)

                if fabs(dist) > self.max_distance:
                    delete_indices.append(i)

                if fabs(angle) > self.max_abs_angle:
                    delete_indices.append(i)

            self.calib_array = np.delete(self.calib_array,
                                         delete_indices,
                                         axis=0)

        # We also pre-compute the indices of the neighbours for each pixel.
        self.neighbour_array = []
        n_rows = self.calib_array.shape[0]
        for i in range(n_rows):
            #(ixi, iyi) = self.calib_array[i,0], self.calib_array[i,1]
            (ixr, iyr) = self.calib_array[i, 2], self.calib_array[i, 3]
            nghbrs = []
            for j in range(i + 1, n_rows):
                #(jxi, jyi) = self.calib_array[j,0], self.calib_array[j,1]
                (jxr, jyr) = self.calib_array[j, 2], self.calib_array[j, 3]
                """ Determining neighbourhood based on 8-adjacency
                dx = ixi - jxi
                dy = iyi - jyi
                ij_dist = sqrt(dx*dx + dy*dy)
                if ij_dist <= sqrt(2) + 0.01:
                    nghbrs.append(j)
                """

                # Determining neighbourhood based on a threshold distance
                dx = ixr - jxr
                dy = iyr - jyr
                ij_dist = sqrt(dx * dx + dy * dy)
                if ij_dist <= 50:
                    nghbrs.append(j)

            self.neighbour_array.append(nghbrs)

        self.shape_filter = ShapeFilter(mask=self.detection_mask)
Esempio n. 28
0
from math import pi
from time import time
from colorama import init, Fore, Style
from configsingleton import ConfigSingleton


def execute(cmd):
    # Print the command to execute in green
    print((Fore.GREEN + cmd))
    print((Style.RESET_ALL))

    os.system(cmd)


config_filename = 'one_robot.cfg'
config = ConfigSingleton.get_instance(config_filename)

start_trial = 0
stop_trial = 0

#shape_list = ["C_SHAPE", "T_SHAPE"]
shape_list = ["C_SHAPE"]

for shape_name in shape_list:
    config.set("AlvinSim", "canned_landmarks_name", shape_name)
    #output_dir = '/users/cs/faculty/av/DATA/AAMAS18/' + shape_name + "/"
    output_dir = '/Users/av/DATA/AAMAS18/' + shape_name + "/"
    #shutil.rmtree(output_dir, ignore_errors=True)
    try:
        os.makedirs(output_dir)
    except OSError:
Esempio n. 29
0
from math import pi
from time import time
from colorama import init, Fore, Style
from configsingleton import ConfigSingleton


def execute(cmd):
    # Print the command to execute in green
    print((Fore.GREEN + cmd))
    print((Style.RESET_ALL))

    os.system(cmd)


#config = ConfigSingleton.get_instance('default.cfg')
config = ConfigSingleton.get_instance('pair.cfg')

# CUSTOMIZE ONLY THIS BLOCK

config_section = "PointSampleCam"
var_name = "max_distance"
var_list = [0, 50, 100, 1000]
start_trial = 0
stop_trial = 0
"""
config_section = "BumpController"
var_name = "puck_dist_threshold"
var_list = [0, 10, 20, 30, 40, 50, 60]
start_trial = 0
stop_trial = 11
"""