dt_range = [0.1, 0.5]
bounds = numpy.array([dx_range, dy_range, dt_range])
num_actions = args.num_actions
num_sampled_paths = args.num_paths
node_color = (0., 0., 0.)
edge_color = (0., 0., 0.)
node_size = 3.
start_pose = numpy.array([0., 0.])
start_node = RenderNode(0, 0., 0., color = node_color)
# Generate the random paths
G = PathGraph()
G.set_goal_region((.5, 0.), radius=0.1)
node_id = 1
for pidx in range(num_sampled_paths):
path = sample_random_trajectory(start_node, num_actions, bounds)
for idx in range(1, len(path)):
path[idx].id = node_id
node_id += 1
path[idx].parent_id = path[idx-1].id
G.add_path(path)
# Now plot
G.render(edge_color=edge_color,
node_size=node_size,
savefile=args.savefile,
savefile_size=(2., 2.))
def run_cma(path, goal_center, goal_radius, num_iterations=10, debug=False):
num_points_in_path = len(path)
num_samples = 10
sigma = 0.05
C = numpy.eye(num_points_in_path * 2.)
ccov = 0.5 #(2/n^2)
zero_mean = numpy.zeros(num_points_in_path * 2.)
mean_path = path
# Now compute weights
mu = num_samples/2.
w = numpy.log(mu + 0.5) - numpy.log(range(1, int(mu)+1))
w = w / sum(w)
mu_w = 1./ sum([w[idx]*w[idx] for idx in range(int(mu))])
node_id = len(mean_path)
for idx in range(num_iterations):
# Sample y_i
noise_vals = numpy.random.multivariate_normal(zero_mean, C, num_samples)
# Sample a set of x candidates
path_samples = sample_noisy_paths(mean_path, noise_vals, sigma)
# Debug
if args.debug:
G = PathGraph()
G.set_goal_region(goal_center, radius=goal_radius)
G.add_path(mean_path, bold=True)
for p in path_samples:
for idx in range(1, len(p)):
p[idx].id = node_id
node_id += 1
p[idx].parent_id = p[idx-1].id
G.add_path(p)
# Evaluate each
dists = [distance_goal(p, goal_center, goal_radius) for p in path_samples]
dists = zip(range(len(dists)), dists)
# Now sort in order of increasing f value
dists = sorted(dists, key=lambda k: k[1])
# Now move the mean
kept_idx = [ d[0] for d in dists[:len(w)] ]
yw = numpy.array([w[idx]*noise_vals[kept_idx[idx],:] for idx in range(len(w))])
yw = numpy.sum(yw, axis=0)
# Now update the mean
path_vals = []
for n in mean_path:
path_vals = path_vals + [n.x, n.y]
path_vals = numpy.array(path_vals) + sigma*yw
new_mean_path = [ mean_path[0] ] # same start node
for idx in range(1, len(mean_path)):
new_node = RenderNode(node_id,
path_vals[idx*2],
path_vals[idx*2+1],
color = (0., 0., 0.5),
parent_id = new_mean_path[-1].id)
new_mean_path.append(new_node)
node_id += 1
if args.debug:
G.add_path(new_mean_path, weight=2)
G.render(edge_color=edge_color,
node_size=node_size,
savefile=args.savefile,
savefile_size=(2., 2.))
mean_path = new_mean_path
if distance_goal(mean_path, goal_center, goal_radius) == 0.0:
break
yw_tmp = numpy.array([yw])
yyt = numpy.dot(yw_tmp.T, yw_tmp)
#C = (1. - ccov) * C + ccov * mu_w * yyt
return mean_path
path_orig_pts = [(0.0, 0.0), (0.1, 0.1), (0.2, -0.1), (0.25, 0.0), (0.32, 0.08), (0.45, 0.02)]
path_orig = []
node_id = 0
for pt in path_orig_pts:
parent_id = None
if len(path_orig) > 0:
parent_id = path_orig[-1].id
node = RenderNode(node_id, pt[0], pt[1], parent_id=parent_id)
path_orig.append(node)
node_id += 1
# Generate the random paths
G = PathGraph()
G.set_goal_region((0.5, 0.0), radius=0.1)
# G.add_path(path_orig, bold=True)
for sidx in range(args.num_paths):
# Sample a random number of nodes to remove
# Must remove something, can't remove either endpt
num_kept = random.randint(2, len(path_orig))
print num_kept
# Now sample which to remove
inds = range(1, len(path_orig) - 1)
random.shuffle(inds)
kept = sorted([0, len(path_orig) - 1] + inds[: num_kept - 2])
print kept
new_path = [path_orig[0]]
num_actions = args.num_actions
num_sampled_paths = args.num_paths
node_color = (0., 0., 0.)
edge_color = (0., 0., 0.)
node_size = 3.
start_pose = numpy.array([0., 0.])
start_node = RenderNode(0, 0., 0., color = node_color)
goal_center = [0.5, 0.]
goal_radius = 0.1
# Generate the random paths
G = PathGraph()
G.set_goal_region(goal_center, radius=goal_radius)
node_id = 1
path_samples = []
for pidx in range(num_sampled_paths):
path = sample_random_trajectory(start_node, num_actions, bounds, node_color = node_color)
# Now run CMA
path = run_cma(path, goal_center, goal_radius, num_iterations=args.iterations, debug=args.debug)
for idx in range(1, len(path)):
path[idx].id = node_id
node_id += 1
path[idx].parent_id = path[idx-1].id
G.add_path(path)