def test_mixture_proposal1():
def prop0(mrf, nAdd, x):
return {'v': [0]}
def prop1(mrf, nAdd, x):
return {'v': [1]}
prop = mixture_proposal([prop0, prop1])
points = [prop(None, None, None)['v'][0] for _ in range(1000)]
assert np.abs(np.sum(points) / 1000.0 - 0.5) < 0.1
def test_mixture_proposal3():
def prop0(mrf, nAdd, x):
return {'v': [0]}
def prop1(mrf, nAdd, x):
return {'v': [1]}
prop = mixture_proposal([prop0, prop1], weights=[1, 0])
points = [prop(None, None, None)['v'][0] for _ in range(1000)]
assert all([p == 0 for p in points])
def test_dpmp_infer_rw_prop_1d_mixed():
"""Test DPMP when the true MAP is in x0. Final MAP should the true MAP."""
mrf = MRF([0, 1], [(0, 1)],
lambda _1, x: -(x ** 2),
lambda _1, _2, x, y: -((x - y) ** 2))
x0 = {0: [0.0], 1: [0.0]}
nParticles = 5
prop1 = random_walk_proposal_1d(10)
prop2 = random_walk_proposal_1d(5)
prop = mixture_proposal([prop1, prop2])
xMAP, _, stats = DPMP_infer(mrf, x0, nParticles, prop, SelectDiverse(),
MaxSumMP(mrf), max_iters=50)
assert xMAP == {0: 0.0, 1: 0.0}
assert stats['converged'] == True
def solve(self, ptc):
pdef = self.getProblemDefinition()
si = self.getSpaceInformation()
space = si.getStateSpace()
svc = si.getStateValidityChecker()
goal = pdef.getGoal()
self._goal_state = sample_goal(space, goal)
assert pdef.getStartStateCount() == 1
self._start_state = pdef.getStartState(0)
print 'Start/goal distance:', si.distance(self._start_state, self._goal_state)
def huber(clearance, epsilon):
if clearance < 0:
return clearance - epsilon / 2.0
elif (0 <= clearance) and (clearance <= epsilon):
return -1.0 / (2.0 * epsilon) * ((clearance - epsilon) ** 2)
else:
return 0.0
T = self.params()['n_waypoints'].getValue()
n_particles = self.params()['n_particles'].getValue()
max_iters = self.params()['max_iters'].getValue()
clearance = self.params()['clearance'].getValue()
avoidance_weight = self.params()['avoidance_weight'].getValue()
sig_rw = self.params()['sig_rw'].getValue()
uniform_proposal_weight = self.params()['uniform_proposal_weight'].getValue()
nodes = range(T)
obstacle_pots = {
'obs_{}'.format(t):
Factor([t], lambda x_t: avoidance_weight * huber(svc.clearance(x_t), clearance))
for t in nodes
}
def pw_pot(x_u, x_v):
return -1 * (si.distance(x_u, x_v) ** 2)
def pw_pot_check_motion(x_u, x_v):
if si.checkMotion(x_u, x_v):
return -1 * (si.distance(x_u, x_v) ** 2)
else:
return -1e32
start_pot = {
'motion_start':
Factor([0], lambda x_0: pw_pot(self._start_state, x_0))
}
motion_pots = {
'motion_t{}'.format(t): Factor([t, t + 1], pw_pot)
for t in range(T - 1)
}
goal_pot = {
'motion_goal':
Factor([T - 1], lambda x_T: -1 * goal.distanceGoal(x_T) ** 2)
}
# Create the MRF
mrf = MRF(nodes, merge_dicts(
obstacle_pots,
start_pot,
motion_pots,
goal_pot))
# Set up message passing
maxsum = MaxSumMP(mrf, sched=tree_sched(mrf, 'motion_start'))
x0 = {t: [interpolate(space,
self._start_state,
self._goal_state,
float(t) / T)]
for t in mrf.nodes}
def uniform_proposal(mrf, nAdd, x):
return {v: [sample_uniform(space, self.sampler_)
for _ in range(nAdd[v])]
for v in mrf.nodes}
def rw_proposal(mrf, nAdd, x):
return {v: [sample_gaussian(space,
self.sampler_,
random.choice(x[v]),
sig_rw)
for _ in range(nAdd[v])]
for v in mrf.nodes}
# def callback(info):
# print info['iter']
self._xMAP, self._xParticles, self._stats = DPMP_infer(
mrf,
x0,
n_particles,
mixture_proposal([uniform_proposal, rw_proposal],
weights=[uniform_proposal_weight,
1.0 - uniform_proposal_weight]),
SelectLazyGreedy(),
maxsum,
conv_tol=None,
max_iters=max_iters,
# callback=callback,
verbose=True)
path_states = ([self._start_state]
+ [self._xMAP[t] for t in range(T)]
+ [self._goal_state])
path = construct_path(si, path_states)
path_valid = path.check()
pdef.addSolutionPath(path)
print 'DPMP path valid:', path_valid
return ob.PlannerStatus(path_valid, False)
def solve(self, ptc):
pdef = self.getProblemDefinition()
si = self.getSpaceInformation()
space = si.getStateSpace()
svc = si.getStateValidityChecker()
goal = pdef.getGoal()
goal_state = sample_goal(space, goal)
assert pdef.getStartStateCount() == 1
start_state = pdef.getStartState(0)
def huber(clearance, epsilon):
if clearance < 0:
return clearance - epsilon / 2.0
elif (0 <= clearance) and (clearance <= epsilon):
return -1.0 / (2.0 * epsilon) * ((clearance - epsilon) ** 2)
else:
return 0.0
T = self.n_waypoints
nodes = range(T)
obstacle_pots = {
'obs_{}'.format(t):
Factor([t], lambda x_t: huber(svc.clearance(x_t), 0.1))
for t in nodes
}
start_pot = {
'motion_start':
Factor([0], lambda x_0: -1 * si.distance(start_state, x_0) ** 2)
}
motion_pots = {
'motion_t{}'.format(t):
Factor([t, t + 1], lambda x_u, x_v: -1 * si.distance(x_u, x_v) ** 2)
for t in range(T - 1)
}
goal_pot = {
'motion_goal':
Factor([T - 1], lambda x_T: -1 * goal.distanceGoal(x_T) ** 2)
}
# Create the MRF
mrf = MRF(nodes, merge_dicts(
obstacle_pots,
start_pot,
motion_pots,
goal_pot))
# Set up message passing
maxsum = MaxSumMP(mrf, sched=tree_sched(mrf, 'motion_start'))
x0 = {t: [interpolate(space, start_state, goal_state, float(t) / T)]
for t in mrf.nodes}
def uniform_proposal(mrf, nAdd, x):
return {v: [sample_uniform(space, self.sampler_)
for _ in range(nAdd[v])]
for v in mrf.nodes}
def rw_proposal(mrf, nAdd, x):
return {v: [sample_gaussian(space, self.sampler_, random.choice(x[v]), 0.25)
for _ in range(nAdd[v])]
for v in mrf.nodes}
# Turn on matplotlib's interactive mode
plt.ion()
plt.figure(figsize=(8, 8))
def callback(info):
print info['iter']
x = info['x']
xMAP = info['xMAP']
plt.clf()
plt.axis([-2.5, 2.5, -2.5, 2.5])
an = np.linspace(0, 2 * np.pi, 100)
plt.plot(np.cos(an), np.sin(an))
particles = [(x_t[0], x_t[1]) for t in range(T) for x_t in x[t]]
c = [float(t) / float(T) for t in range(T) for x_t in x[t]]
plt.scatter(*zip(*particles), s=10, c=c, marker='x')
MAPparticles = [start_state] + [xMAP[t] for t in range(T)] + [goal_state]
plt.plot(*zip(*[(s[0], s[1]) for s in MAPparticles]), marker='o')
plt.xlabel('x_1')
plt.ylabel('x_2')
plt.title('Iter. %d' % info['iter'])
plt.draw()
time.sleep(0.25)
xMAP, xParticles, stats = DPMP_infer(
mrf,
x0,
self.n_particles,
mixture_proposal([uniform_proposal, rw_proposal], weights=[0.0, 1.0]),
SelectLazyGreedy(),
maxsum,
conv_tol=None,
max_iters=25,
callback=callback,
verbose=False)
# Turn off matplotlib's interactive mode
plt.ioff()
path_states = [start_state] + [xMAP[t] for t in range(T)] + [goal_state]
path = construct_path(si, path_states)
path_valid = path.check()
pdef.addSolutionPath(path)
return ob.PlannerStatus(path_valid, False)
