def show(configs, paths, x_low, x_high, y_low, y_high, point_interval=0.2):
for path in paths:
points = path.generate_points(point_interval)
x = [p[0] for p in points]
y = [p[1] for p in points]
plt.plot(x, y)
uplt.limit(x_low, x_high, y_low, y_high)
uplt.plotRobotPoses(configs)
def show(dubincurve, x_low, x_high, y_low, y_high, no_plot=False):
dubincurve.compute()
points = dubincurve.generate_points()
x = [p[0] for p in points]
y = [p[1] for p in points]
plt.plot(x, y)
uplt.limit(x_low, x_high, y_low, y_high)
uplt.plotRobotPoses([dubincurve.start, dubincurve.end])
if not no_plot:
plt.show()
def load_cost_to_go_and_display(location, configs):
cost_to_go = pickle.load(open(location + 'cost_to_go', 'rb'))
cost_to_go_table = cost_to_go.get_state_space_cost_table()
policy = pickle.load(open(location + 'policy', 'rb'))
policy_table = policy.get_state_space_cost_table()
start_state = np.array([-30, -30, 3.14])
path, _ = apply_control_policy(policy, get_dynamics(), start_state,
simulation_delta_t, 100)
uplt.plotRobotPoses(path)
plt.ylim((configs[1]['min'], configs[1]['max']))
plt.xlim((configs[0]['min'], configs[0]['max']))
plt.show()
draw_cost_to_go_and_policy(cost_to_go_table, policy_table, configs)
def load_cost_to_go_and_display(location, configs):
cost_to_go = pickle.load(open(location + 'spline_cost_to_go', 'rb'))
cost_to_go_table = cost_to_go.get_state_space_cost_table()
policy = pickle.load(open(location + 'spline_policy', 'rb'))
policy_table = policy.get_state_space_cost_table()
# value_iteration = vi.ValueIteration(get_dynamics(), cost_to_go, get_control_set(), get_cost_function(), computation_delta_t)
# value_iteration.compute_control_policy()
# policy = value_iteration.get_policy()
# policy_table = policy.get_state_space_cost_table()
draw_spline(get_spline())
start_state = np.array([-50, 30, 0])
path, controls = apply_control_policy(policy, get_dynamics(), start_state,
simulation_delta_t, 30)
print(controls)
uplt.plotRobotPoses(path)
plt.ylim((configs[1]['min'], configs[1]['max']))
plt.xlim((configs[0]['min'], configs[0]['max']))
plt.show()
draw_cost_to_go_and_policy(cost_to_go_table, policy_table, configs)
for i in range(len(command)):
robot.motionUpdate(command[i], deltaT)
poses.append(robot.getPose())
for i in range(len(poses)):
robot.setPose(poses[i])
measurements.append(robot.measurementUpdate())
for i in range(len(measurements)):
fastSlam.measurementUpdate(measurements[i])
particle = fastSlam.getBestParticle()
fPositions = map(lambda f: f.getPosition(), particle.getFeatures())
plot.plotRobotPose(poses[i])
# plot.plotRobotPoses(map(lambda p: p.getPose(), fastSlam.getParticles()), style="ro")
plot.plotRobotPose(particle.getPose(), style="ro")
plot.plotFeatures(fPositions, 'bo')
plot.plotFeatures(landmarks)
plot.show()
fastSlam.resample()
if i < len(command):
fastSlam.motionUpdate(command[i], deltaT)
plot.plotRobotPoses(poses)
plot.plotRobotPoses(fastSlam.getBestParticle().getPath(), style="ro")
plot.plotFeatures(landmarks)
plot.show()
def show_path_points(configs, points, x_low, x_high, y_low, y_high):
x = [p[0] for p in points]
y = [p[1] for p in points]
plt.plot(x, y)
uplt.limit(x_low, x_high, y_low, y_high)
uplt.plotRobotPoses(configs)
z_max=limit)
robot = bot.Robot(grid, initialPose, motionModel, numRays, noiseSigma,
resolution, limit)
poses, measurements = generateData(robot, command1, initialPose)
shape = (int(grid.shape[0] / 10), int(grid.shape[1] / 10))
origin = [0, 0]
mapResolution = 0.1
locc = 1
lfree = -1
alpha = 0.5
beta = 2 * math.pi / numRays
map = occmap.OccupancyMap(shape[:], origin, mapResolution, locc, lfree,
limit, alpha, beta)
for i in range(len(poses)):
pose = poses[i]
measurement = measurements[i]
map.update(pose, measurement)
plt.figure()
probOccMap = map.generateProbabilityOccupancyGrid(probFree=True)
plt.imshow(probOccMap, cmap='gray', origin='lower')
plt.figure()
plot.plotOccupancyGrid(grid, resolution)
plot.plotRobotPoses(poses)
plot.show()
measurements.extend(newMeasurements)
command1.extend(command2)
# We use different random seed to generate localization particles
random.seed(1)
N = 500
poseGuess = [8, 22, 3, 23]
mcl = MCL.MonteCarloLocalization(grid,
resolution,
poseGuess,
N,
mclMotionModel,
measurementModel,
enableParticleInjection=True,
alphaFast=1.2,
alphaSlow=0.8)
estimatedPoses, particles = generateLocalizationData(
mcl, command1, measurements, deltaT)
# for i in range(0, len(estimatedPoses)):
# plot.plotOccupancyGrid(grid, resolution, plotLim)
# plot.plotRobotPoses(particles[i], 'r+')
# plot.plotRobotPose(poses[i], 'bo')
# plot.show()
plot.plotOccupancyGrid(grid, resolution)
plot.plotRobotPoses(poses)
plot.plotRobotPoses(estimatedPoses, 'r+')
plot.show()