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
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)
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")
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)
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
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")
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
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
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 = []
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")
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")
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")
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
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")
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"
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
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))
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"
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"
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
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)
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
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
#!/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
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)
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)
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:
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
"""