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)