def grow_tree(self,
curr_manifold: Manifold,
next_manifold: Manifold):
curr_projector = Projection(f=curr_manifold.y, J=curr_manifold.J, step_size=self.proj_step_size)
joint_manifold = ManifoldStack([curr_manifold, next_manifold])
joint_projector = Projection(f=joint_manifold.y, J=joint_manifold.J, step_size=self.proj_step_size)
pbar = tqdm.tqdm(total=self.n_samples)
for i in range(self.n_samples):
pbar.update()
q_target = self.task.sample()
q_near, q_near_id = self.G.get_nearest_neighbor(node_value=q_target)
q_new = self.steer(q_near, q_near_id, q_target, curr_manifold, next_manifold)
if q_new is None:
continue
# project q_new on current or next manifold
on_next_manifold = False
if np.linalg.norm(next_manifold.y(q_new)) < np.random.rand() * self.r_max:
res, q_new_proj = joint_projector.project(q_new)
else:
res, q_new_proj = curr_projector.project(q_new)
if not res:
continue # continue if projection was not successful
# check if q_new_proj is on the next manifold
if is_on_manifold(next_manifold, q_new_proj, self.eps):
if len(self.V_goal) == 0:
on_next_manifold = True
else:
q_proj_near = min(self.V_goal, key=lambda idx: np.linalg.norm(self.G.V[idx].value - q_new_proj))
if np.linalg.norm(self.G.V[q_proj_near].value - q_new_proj) > self.rho:
on_next_manifold = True
else:
continue # continue if a node close to q_new_proj is already in the tree
q_new_idx = self.G.node_count
extended = self.extend(q_from=q_near, q_from_id=q_near_id, q_to=q_new_proj, q_to_id=q_new_idx)
if extended:
self.rewire(q_from=q_new_proj, q_from_id=q_new_idx)
# add to V_goal if q_new is on the next manifold
if on_next_manifold:
self.V_goal.append(q_new_idx)
pbar.close()
print('')
def run(self) -> (list, list):
# the start node is only node in tree with id=0, cost=0, parent=None
self.G.add_node(node_id=0, node_value=self.start_value, node_cost=0.0, inc_cost=0.0)
self.result = False
proj = Projection(f=self.manifold.y, J=self.manifold.J, step_size=self.proj_step_size)
pbar = tqdm.tqdm(total=self.n_samples)
for i in range(self.n_samples):
pbar.update()
if np.random.rand() < self.beta:
q_target = self.goal_value
else:
q_target = self.task.sample()
q_near, q_near_id = self.G.get_nearest_neighbor(node_value=q_target)
q_new = self.steer(q_near, q_target)
q_new_idx = self.G.node_count
res, q_new = proj.project(q_new)
if not res:
continue
extended = self.extend(q_from=q_near, q_from_id=q_near_id, q_to=q_new, q_to_id=q_new_idx)
if extended:
self.rewire(q_from=q_new, q_from_id=q_new_idx)
pbar.close()
print('')
self.G.comp_opt_path(self.goal_value, self.conv_tol)
opt_path = [self.G.V[idx].value for idx in self.G.path]
return self.G.path, opt_path
def constrained_extend(self, G: Tree, q_from: np.ndarray, q_from_id: int,
q_to: np.ndarray) -> np.ndarray:
q_s = q_from.copy()
q_s_id = q_from_id
q_s_old = q_s.copy()
while True:
if np.isclose(q_s, q_to).all():
return q_s
if np.linalg.norm(q_s - q_to) > np.linalg.norm(q_s_old - q_to):
return q_s_old
q_s_old = q_s.copy()
q_s_old_id = q_s_id
# step towards the target configuration
q_s = self.steer(q_from=q_s, q_to=q_to)
# project q_s onto nearest manifold
h = [np.linalg.norm(m.y(q_s)) for m in self.task.manifolds]
idx = np.argmin(h)
if idx != G.V[q_s_id].aux:
curr_manifold = self.task.manifolds[G.V[q_s_id].aux]
next_manifold = self.task.manifolds[idx]
joint_manifold = ManifoldStack([curr_manifold, next_manifold])
joint_projector = Projection(joint_manifold.y,
joint_manifold.J)
res, q_s = joint_projector.project(q_s)
else:
res, q_s = self.manifold_projectors[idx].project(q_s)
if res and not self.is_collision(q_s_old, q_s):
inc_cost = np.linalg.norm(q_s - q_s_old)
node_cost = G.V[q_s_old_id].cost + inc_cost
q_s_id = G.node_count
G.add_node(node_id=q_s_id,
node_value=q_s,
node_cost=node_cost,
inc_cost=inc_cost)
G.add_edge(edge_id=G.edge_count,
node_a=q_s_old_id,
node_b=q_s_id)
G.V[q_s_id].aux = idx
if np.isclose(q_s, q_s_old).all():
return q_s
else:
return q_s_old
def run(self) -> bool:
# iterate over sequence of manifolds
q_start = self.start.copy()
cost = 0.0
path = []
for n in range(self.n_manifolds - 1):
print('######################################################')
print('n', n)
print('Active Manifold: ', self.task.manifolds[n].name)
print('Target Manifold: ', self.task.manifolds[n + 1].name)
# sample a goal configuration with IK
curr_manifold = self.task.manifolds[n]
next_manifold = ManifoldStack(
manifolds=[self.task.manifolds[n], self.task.manifolds[n + 1]])
ik_proj = Projection(f=next_manifold.y, J=next_manifold.J)
res_plan = False
max_goals = 10
iter_goals = 0
while not res_plan:
res_proj = False
while not res_proj:
q_rand = self.task.sample()
res_proj, q_goal = ik_proj.project(q_rand)
if not self.task.is_valid_conf(q_goal):
res_proj = False
if self.task.is_collision_conf(q_goal):
res_proj = False
# plan path to goal configuration with RRT*
rrt_task = Task('empty')
rrt_task.start = q_start
rrt_task.goal = q_goal
rrt_task.obstacles = self.task.obstacles
planner = RRTStarManifold(rrt_task, curr_manifold, self.cfg)
path_idx, opt_path = planner.run()
if path_idx:
q_reached = planner.G.V[path_idx[-1]].value
res_plan = np.linalg.norm(q_reached - q_goal) < self.eps
else:
res_plan = False
if not res_plan:
iter_goals += 1
if iter_goals == max_goals:
return False
cost += planner.G.comp_opt_path(q_goal)
path += [[planner.G.V[idx].value for idx in planner.G.path]]
q_start = q_goal.copy()
# store results for later evaluation
self.G_list.append(planner.G)
self.V_goal_list.append([0])
self.path = path
return True
def run(self) -> bool:
# iterate over sequence of manifolds
self.G = Tree(self.d, exact_nn=False)
self.G.add_node(node_id=0,
node_value=self.start,
node_cost=0.0,
inc_cost=0.0)
self.G.V[0].aux = 0 # store manifold id for every node
self.G.V[0].path = []
for n in range(100):
node_id = self.G.sample_node()
curr_manifold_id = self.G.V[node_id].aux
next_manifold_id = curr_manifold_id + 1
q_start = self.G.V[node_id].value
if is_on_manifold(self.task.manifolds[next_manifold_id], q_start):
# move node directly to next manifold
self.G.V[node_id].aux += 1
q_reached = q_start
else:
# sample a goal configuration with IK
curr_manifold = self.task.manifolds[curr_manifold_id]
next_manifold = ManifoldStack(manifolds=[
self.task.manifolds[curr_manifold_id],
self.task.manifolds[next_manifold_id]
])
ik_proj = Projection(f=next_manifold.y, J=next_manifold.J)
res_proj = False
while not res_proj:
q_rand = self.task.sample()
res_proj, q_goal = ik_proj.project(q_rand)
if not self.task.is_valid_conf(q_goal):
res_proj = False
if self.task.is_collision_conf(q_goal):
res_proj = False
# plan path to goal configuration with RRT*
rrt_task = Task('empty')
rrt_task.start = q_start
rrt_task.goal = q_goal
rrt_task.obstacles = self.task.obstacles
planner = RRTStarManifold(task=rrt_task,
manifold=curr_manifold,
cfg=self.cfg)
path_idx, opt_path = planner.run()
result = False
if path_idx:
q_reached = planner.G.V[path_idx[-1]].value
if np.linalg.norm(q_reached - q_goal) < self.eps:
result = True
if not result:
continue
cost = path_cost(opt_path)
node_id_new = self.G.node_count
self.G.add_node(node_id=node_id_new,
node_value=q_reached,
node_cost=self.G.V[node_id].cost + cost,
inc_cost=cost)
self.G.V[node_id_new].aux = next_manifold_id
self.G.V[node_id_new].path = opt_path
self.G.add_edge(edge_id=self.G.edge_count,
node_a=node_id,
node_b=node_id_new)
if next_manifold_id == self.n_manifolds - 1:
self.G.comp_opt_path(q_reached, self.eps)
opt_path = [self.G.V[idx].path for idx in self.G.path[1:]]
self.path = opt_path
return True
return False