def part_2a():
"""Main Routine"""
#create Robot Object
bot = robot_library.Robot()
bot.set_initial_pos([0, 0, 0])
#Part A2 Data Set
movement_data = [[0.5, 0, 1], [0, -1 / (2 * np.pi), 1], [.5, 0, 1],
[0, 1 / (2 * np.pi), 1], [.5, 0, 1]]
# loop through data set calulate robot movement using
# motion model
for item in enumerate(movement_data):
# calculate new state
bot.make_move(item[1], 0)
# parse data to plot
x_arr, y_arr, _t_arr = map(list, zip(*bot.motion_path))
plt.plot(x_arr, y_arr, 'b')
plt.title('Part A.2 Results')
plt.xlabel('Robot X Position (m)')
plt.ylabel('Robot Y Position (m)')
plt.legend(['Robot Trajectory'])
plt.show()
def main():
"""Main Routine"""
bot = robot_library.Robot()
bot.part_a6()
bot.part_a2()
bot.part_a3()
bot.part_b7()
bot.create_plots()
def part_a6():
"""Main Routine"""
#create Robot Object
bot = robot_library.Robot()
bot.set_initial_pos([0, 0, 0])
#Part A6 Data Set
sub_num = [6, 13, 17]
marker_set = [[1.88032539, -5.57229508], [3.07964257, 0.24942861],
[-1.04151642, 2.80020985]]
robot_locs = [[2, 3, 0], [0, 3, 0], [1, -2, 0]]
global_results = [0, 0, 0]
error_calc = [0, 0, 0]
# Calculate Measurement
for i, item in enumerate(marker_set):
# set current poisition
bot.new_pos = robot_locs[i]
# calculate measurement
results = bot.read_sensor(item, 1)
# convert measurement to global domain
global_results[i] = [
bot.new_pos[0] + np.cos(results[1]) * results[0],
bot.new_pos[1] + np.sin(results[1]) * results[0]
]
# calculate error
error_calc[i] = [
marker_set[i][0] - global_results[i][0],
marker_set[i][1] - global_results[i][1]
]
# print error calculations to console
print "For Subject #" + str(sub_num[i]) + ":"
print "The error in the x direction is " + str(
round(error_calc[i][0], 5))
print "The error in the y direction is " + str(
round(error_calc[i][1], 5))
ow = obslib.ChannelWorld(ranges, (2.5,7), 3, 0.2)
# Create the point robot
robot = roblib.Robot(sample_world = world.sample_value, #function handle for collecting observations
start_loc = (5.0, 5.0, 0.0), #where robot is instantiated
extent = ranges, #extent of the explorable environment
kernel_file = None,
kernel_dataset = None,
prior_dataset = None, #(data, observations),
init_lengthscale = 1.0,
init_variance = 100.0,
noise = 0.0001,
path_generator = 'fully_reachable_goal', #options: default, dubins, equal_dubins, fully_reachable_goal, fully_reachable_step
goal_only = True, #select only if using fully reachable step and you want the reward of the step to only be the goal
frontier_size = 15,
horizon_length = 1.5,
turning_radius = 0.05,
sample_step = 0.5,
evaluation = evaluation,
f_rew = reward_function,
create_animation = True, #logs images to the file folder
learn_params=False, #if kernel params should be trained online
nonmyopic=False, #select if you want to use MCTS
discretization=(20,20), #parameterizes the fully reachable sets
use_cost=False, #select if you want to use a cost heuristic
MIN_COLOR=MIN_COLOR,
MAX_COLOR=MAX_COLOR,
obstacle_world=ow)
robot.planner(T = 150)
#robot.visualize_world_model(screen = True)
def part_a3():
"""Main Routine"""
# load in data files
odom_data = load_data('ds1_Odometry.dat', 3, 0, [0, 4, 5])
meas_data = load_data('ds1_Measurement.dat', 3, 0, [0, 4, 6, 7])
marker_data = load_data('ds1_Landmark_Groundtruth.dat', 3, 0,
[0, 2, 4, 6, 8])
bar_data = load_data('ds1_Barcodes.dat', 3, 0, [0, 3])
groundtruth_data = load_data('ds1_Groundtruth.dat', 3, 0, [0, 3, 5, 7])
# create Robot Object
bot = robot_library.Robot([
groundtruth_data[0][1], groundtruth_data[0][2], groundtruth_data[0][3]
])
# Transform Measurement Subject from Barcode # to Subject #
for _counter, item in enumerate(meas_data):
for _match, match in enumerate(bar_data):
if match[1] == item[1]:
item[1] = match[0]
break
# plot Landmarks and Walls
plot_map(marker_data)
# Plot Groundtruth Data
_ignore, ground_x, ground_y, _ground_t = map(list, zip(*groundtruth_data))
#ground_x = ground_x[0:20000]
#ground_y = ground_y[0:20000]
#plt.plot(ground_x[0], ground_y[0], 'kd', markersize=3, label='_nolegend_')
plt.plot(ground_x, ground_y, 'g')
#plt.plot(ground_x[-1], ground_y[-1], 'ko', markersize=3, label='_nolegend_')
# Plot Odometry Dataset
for i, cur_action in enumerate(odom_data):
if i + 1 >= len(odom_data):
break
movement_data = [
odom_data[i][1], odom_data[i][2],
odom_data[i + 1][0] - cur_action[0]
]
bot.make_move(movement_data, 1)
x_arr, y_arr, _t_arr = map(list, zip(*bot.motion_path))
plt.plot(x_arr, y_arr, 'b')
plt.legend(['Landmark', 'Wall', 'Groundtruth', 'Robot Trajectory'])
plt.xlabel('World X Position (m)')
plt.ylabel('World Y Position (m)')
plt.title('Odometry Data Vs. Groundtruth Data')
plt.autoscale(True)
plt.show()
robot = roblib.Robot(sample_world = world.sample_value, #function handle for collecting observations
start_loc = (5.0, 5.0, 0.0), #where robot is instantiated
extent = ranges, #extent of the explorable environment
MAX_COLOR = MAX_COLOR,
MIN_COLOR = MIN_COLOR,
kernel_file = None,
kernel_dataset = None,
# prior_dataset = (data, observations),
prior_dataset = (xobs, zobs),
# prior_dataset = None,
init_lengthscale = LEN,
init_variance = VAR,
noise = NOISE,
# init_lengthscale = 2.0122,
# init_variance = 5.3373,
# noise = 0.19836,
# noise = float(sys.argv[1]),
path_generator = PATHSET, #options: default, dubins, equal_dubins, fully_reachable_goal, fully_reachable_step
goal_only = GOAL_ONLY, #select only if using fully reachable step and you want the reward of the step to only be the goal
frontier_size = 10,
horizon_length = 1.50,
turning_radius = 0.11,
sample_step = 0.1,
evaluation = evaluation,
f_rew = REWARD_FUNCTION,
create_animation = True, #logs images to the file folder
learn_params = False, #if kernel params should be trained online
nonmyopic = NONMYOPIC,
discretization = (20, 20), #parameterizes the fully reachable sets
use_cost = USE_COST, #select if you want to use a cost heuristic
computation_budget = 250,
rollout_length = 1,
obstacle_world = ow,
tree_type = TREE_TYPE)
PATHSET, #options: default, dubins, equal_dubins, fully_reachable_goal, fully_reachable_step
'goal_only':
GOAL_ONLY, #select only if using fully reachable step and you want the reward of the step to only be the goal
'frontier_size': 15,
'horizon_length': 1.5,
'turning_radius': 0.05,
'sample_step': 0.5,
'evaluation': evaluation,
'f_rew': REWARD_FUNCTION,
'create_animation': True, #logs images to the file folder
'learn_params': False, #if kernel params should be trained online
'nonmyopic': NONMYOPIC,
'discretization': (20, 20), #parameterizes the fully reachable sets
'use_cost': USE_COST, #select if you want to use a cost heuristic
'MIN_COLOR': MIN_COLOR,
'MAX_COLOR': MAX_COLOR,
'computation_budget': 250,
'rollout_length': 5,
'obstacle_world': ow,
'tree_type': TREE_TYPE,
'dimension': DIM
}
# Create the point robot
robot = roblib.Robot(**kwargs)
robot.planner(T=150)
#robot.visualize_world_model(screen = True)
robot.visualize_trajectory(screen=False) #creates a summary trajectory image
robot.plot_information() #plots all of the metrics of interest