def PlanAhead(self):
"""
Generate the reward vector for the LP formulation of planning and solve.
"""
problem = Problem(self.num_rows_, self.num_cols_, self.num_sources_,
self.num_steps_, self.angular_step_, self.sensor_params_,
self.num_samples_, self.map_)
# Generate LP parameters at the current pose.
(pzx, hzm, trajectory_ids) = problem.GenerateConditionals(self.pose_)
pzx = np.asmatrix(pzx)
hzm = np.asmatrix(hzm).T
num_trajectories = pzx.shape[1]
# Set up LP.
result = linprog(np.asarray((pzx.T * hzm)).ravel(),
A_eq=np.ones((1, num_trajectories)),
b_eq=np.ones((1, 1)),
method='simplex')
if (not result.success):
print "Could not find a feasible solution to the LP."
return []
# Decode solution into trajectory.
print "Found information-optimal trajectory."
trajectory_id = trajectory_ids[np.argmax(result.x)]
delta_xs = [-1, 0, 1]
delta_ys = [-1, 0, 1]
delta_as = [-self.angular_step_, 0.0, self.angular_step_]
trajectory = DecodeTrajectory(delta_xs, delta_ys, delta_as,
trajectory_id, self.pose_, self.num_steps_)
return trajectory
def test_distribution_convergence():
# Max deviation at any point in estimated matrices/vectors.
kPrecision = 0.5
# Define hyperparameters.
kNumSamples = 1000
kNumRows = 5
kNumCols = 5
kNumSources = 1
kNumSteps = 1
kAngularStep = 0.25 * math.pi
kSensorParams = {
"x": kNumRows / 2,
"y": kNumCols / 2,
"angle": 0.0,
"fov": 0.5 * math.pi
}
# Generate conditionals from the specified pose.
pose = GridPose2D(kNumRows, kNumCols,
int(np.random.uniform(0.0, kNumRows)) + 0.5,
int(np.random.uniform(0.0, kNumCols)) + 0.5,
np.random.uniform(0.0, 2.0 * math.pi))
# Create a problem.
problem = Problem(kNumRows, kNumCols, kNumSources, kNumSteps, kAngularStep,
kSensorParams, kNumSamples)
(pzx1, hzm1, traj_ids1) = problem.GenerateConditionals(pose)
(pzx2, hzm2, traj_ids2) = problem.GenerateConditionals(pose)
# Check that distributions are close.
assert_equal(traj_ids1, traj_ids2)
assert_array_less(abs(pzx1 - pzx2).max(), kPrecision)
assert_array_less(abs(hzm1 - hzm2).max(), kPrecision)
# Check that cost vectors are not too different.
c1 = pzx1.T * hzm1
c2 = pzx2.T * hzm2
assert_array_less(abs(c1 - c2).max(), kPrecision)
# Files to save to.
pzx_file = "pzx_5x5_1000.csv"
hzm_file = "hmz_5x5_1000.csv"
# Define hyperparameters.
kNumSamples = 1000
kNumRows = 5
kNumCols = 5
kNumSources = 1
kNumSteps = 1
kAngularStep = 0.25 * math.pi
kSensorParams = {"x" : kNumRows/2,
"y" : kNumCols/2,
"angle" : 0.0,
"fov" : 0.5 * math.pi}
# Create a problem.
problem = Problem(kNumRows, kNumCols, kNumSources, kNumSteps,
kAngularStep, kSensorParams, kNumSamples)
# Generate conditionals from the specified pose.
pose = GridPose2D(kNumRows, kNumCols, kNumRows/2, kNumCols/2, 0.0)
(pzx, hzm, trajectory_ids) = problem.GenerateConditionals(pose)
print "P_{Z|X} shape: " + str(pzx.shape)
print "h_{M|Z} shape: " + str(hzm.shape)
np.savetxt(pzx_file, pzx, delimiter=",")
np.savetxt(hzm_file, hzm, delimiter=",")
print "Successfully saved to disk."
