class PSMSingleTree:
def __init__(self, task: Task, cfg: dict):
self.name = "PSM_Single_Tree"
self.n_manifolds = len(task.manifolds)
self.task = task
self.start = task.start
self.n_samples = cfg['N'] * (self.n_manifolds - 1)
self.alpha = cfg['ALPHA']
self.beta = cfg['BETA']
self.eps = cfg['EPS']
self.rho = cfg['RHO']
self.r_max = cfg['R_MAX']
self.collision_res = cfg['COLLISION_RES']
self.d = task.d
self.greedy = cfg['GREEDY']
self.proj_step_size = cfg['PROJ_STEP_SIZE']
self.lim_lo = task.lim_lo
self.lim_up = task.lim_up
self.gamma = np.power(2 * (1 + 1.0 / float(self.d)), 1.0 / float(self.d)) * \
np.power(get_volume(self.lim_lo, self.lim_up) / unit_ball_measure(self.d), 1. / float(self.d))
self.Q_near_ids = []
self.G = None
self.V_goals = []
self.path = None
self.path_id = None
# check if start point is on first manifold
if not is_on_manifold(task.manifolds[0], task.start, self.eps):
raise Exception(
'The start point is not on the manifold h(start)= ' +
str(task.manifolds[0].y(task.start)))
# check if start point is in configuration space limits
if not self.task.is_valid_conf(task.start):
raise Exception('The start point is not in the system limits.')
# check if start point is in collision
if self.task.is_collision_conf(task.start):
raise Exception('The start point is in collision.')
def run(self) -> bool:
curr_projectors = []
next_projectors = []
curr_manifolds = []
next_manifolds = []
# iterate over sequence of manifolds
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)
curr_manifold = self.task.manifolds[n]
next_manifold = self.task.manifolds[n + 1]
joint_manifold = ManifoldStack([curr_manifold, next_manifold])
# initiate manifold and projector sequence
curr_manifolds += [curr_manifold]
next_manifolds += [next_manifold]
curr_projectors += [
Projection(f=curr_manifold.y,
J=curr_manifold.J,
step_size=self.proj_step_size)
]
next_projectors += [
Projection(f=joint_manifold.y,
J=joint_manifold.J,
step_size=self.proj_step_size)
]
if n < self.n_manifolds - 1:
self.V_goals += [[]]
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
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)
manifold_id = self.G.V[q_near_id].aux
if manifold_id >= self.n_manifolds - 1:
continue # do not extend goal nodes
if is_on_manifold(self.task.manifolds[manifold_id + 1], q_near):
# move node directly to next manifold
self.G.V[q_near_id].aux += 1
continue
curr_manifold = curr_manifolds[manifold_id]
next_manifold = next_manifolds[manifold_id]
curr_projector = curr_projectors[manifold_id]
joint_projector = next_projectors[manifold_id]
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 unsuccessful
# check if q_new_proj is on the next manifold
if is_on_manifold(next_manifold, q_new_proj, self.eps):
on_next_manifold = True
if len(self.V_goals[manifold_id]) > 0:
q_proj_near = min(self.V_goals[manifold_id],
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:
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,
manifold_id=manifold_id + on_next_manifold)
if extended:
self.rewire(q_from=q_new_proj, q_from_id=q_new_idx)
if on_next_manifold:
# add to V_goal if q_new is on the next manifold
self.V_goals[manifold_id].append(q_new_idx)
pbar.close()
print('')
if len(self.V_goals[-1]) == 0:
return False
opt_idx = min(self.V_goals[-1],
key=lambda idx: np.linalg.norm(self.G.V[idx].cost))
opt_path_idx = self.G.comp_path(opt_idx)
# split into individual path segments per manifold
opt_path = []
opt_path_m = []
m = 0
for idx in opt_path_idx:
opt_path_m += [self.G.V[idx].value]
if self.G.V[idx].aux != m:
opt_path += [opt_path_m.copy()]
opt_path_m = [self.G.V[idx].value]
m += 1
if len(opt_path) != self.n_manifolds - 1:
return False
self.path = opt_path
self.path_idx = opt_path_idx
return True
def steer(self, q_from: np.ndarray, q_from_id: int, q_to: np.ndarray,
curr_manifold: Manifold, next_manifold: Manifold) -> np.ndarray:
if np.random.rand() < self.beta and not self.G.V[q_from_id].con_extend:
# steer towards next_manifolds
self.G.V[q_from_id].con_extend = True
yn = next_manifold.y(q_from)
Jn = next_manifold.J(q_from)
d = -Jn.T @ yn
# project on current manifold
J = null_space(curr_manifold.J(q_from))
if J.shape[1] != 0:
d = J @ J.T @ d
else:
# steer towards q_to
d = (q_to - q_from)
# project on current manifold
J = null_space(curr_manifold.J(q_from))
if J.shape[1] != 0:
d = J @ J.T @ d
if np.linalg.norm(d) > 0.0:
q_new = q_from + self.alpha * d * (1.0 / np.linalg.norm(d))
return q_new
else:
return None
def is_collision(self, q_a: np.ndarray, q_b: np.ndarray) -> bool:
N = int(ceil(np.linalg.norm(q_b - q_a) / self.collision_res))
for i in range(N + 1):
q = q_a if N == 0 else q_a + i / float(N) * (q_b - q_a)
res = self.task.is_collision_conf(q)
if res:
return True
return False
def extend(self, q_from: np.ndarray, q_from_id: int, q_to: np.ndarray,
q_to_id: int, manifold_id: int) -> bool:
if self.is_collision(q_from, q_to):
return False
n = float(len(self.G.V))
r = min(
[self.gamma * np.power(np.log(n) / n, 1.0 / self.d), self.alpha])
self.Q_near_ids = self.G.get_nearest_neighbors(node_value=q_to,
radius=r)
c_min = self.G.V[q_from_id].cost + np.linalg.norm(q_from - q_to)
c_min_inc = np.linalg.norm(q_from - q_to)
q_min_idx = q_from_id
for idx in self.Q_near_ids:
q_idx = self.G.V[idx].value
c_idx = self.G.V[idx].cost + np.linalg.norm(q_idx - q_to)
if not self.is_collision(q_idx, q_to) and c_idx < c_min:
c_min = c_idx
c_min_inc = np.linalg.norm(q_idx - q_to)
q_min_idx = idx
self.G.add_node(node_id=q_to_id,
node_value=q_to,
node_cost=c_min,
inc_cost=c_min_inc)
self.G.V[q_to_id].aux = manifold_id
self.G.add_edge(edge_id=self.G.edge_count,
node_a=q_min_idx,
node_b=q_to_id)
return True
def rewire(self, q_from: np.ndarray, q_from_id: int):
for idx in self.Q_near_ids: # Q_near_ids was previously computed in extend function
q_idx = self.G.V[idx].value
c_idx = self.G.V[idx].cost
c_new = self.G.V[q_from_id].cost + np.linalg.norm(q_from - q_idx)
if not self.is_collision(q_from, q_idx) and c_new < c_idx:
idx_parent = self.G.V[idx].parent
self.G.remove_edge(idx_parent, idx)
self.G.add_edge(edge_id=self.G.edge_count,
node_a=q_from_id,
node_b=idx)
self.G.V[idx].cost = c_new
self.G.V[idx].parent = q_from_id
self.G.V[idx].inc_cost = np.linalg.norm(q_from - q_idx)
self.G.update_child_costs(node_id=idx)
class CBIRRT:
def __init__(self, task: Task, cfg: dict):
self.name = "CBIRRT"
self.n_manifolds = len(task.manifolds)
self.task = task
self.start = task.start
self.n_samples = cfg['N']
self.alpha = cfg['ALPHA']
self.eps = cfg['EPS']
self.collision_res = cfg['COLLISION_RES']
self.d = task.d
self.lim_lo = task.lim_lo
self.lim_up = task.lim_up
self.result = False
self.goal = task.goal
self.path = None
self.path_id = None
self.G_a = Tree(self.d, exact_nn=False)
self.G_b = Tree(self.d, exact_nn=False)
self.manifold_projectors = []
for m in self.task.manifolds:
self.manifold_projectors.append(Projection(f=m.y, J=m.J))
# check if start point is on first manifold
if not is_on_manifold(task.manifolds[0], task.start, self.eps):
raise Exception(
'The start point is not on the manifold h(start)= ' +
str(task.manifolds[0].y(task.start)))
# check if start point is in collision
if self.task.is_collision_conf(task.start):
raise Exception('The start point is in collision.')
def run(self) -> bool:
self.G_a.add_node(node_id=0,
node_value=self.start,
node_cost=0.0,
inc_cost=0.0)
self.G_b.add_node(node_id=0,
node_value=self.goal,
node_cost=0.0,
inc_cost=0.0)
self.G_a.V[0].aux = 0
self.G_b.V[0].aux = self.n_manifolds - 1
# grow tree with bidirectional strategy
for i in range(self.n_samples):
q_rand = self.task.sample()
q_near_a, q_near_a_id = self.G_a.get_nearest_neighbor(
node_value=q_rand)
q_reached_a = self.constrained_extend(self.G_a, q_near_a,
q_near_a_id, q_rand)
q_near_b, q_near_b_id = self.G_b.get_nearest_neighbor(
node_value=q_reached_a)
q_reached_b = self.constrained_extend(self.G_b, q_near_b,
q_near_b_id, q_reached_a)
if np.isclose(q_reached_a, q_reached_b).all():
cost_a = self.G_a.comp_opt_path(q_reached_a)
cost_b = self.G_b.comp_opt_path(q_reached_b)
path_idx = [self.G_a.path, list(reversed(self.G_b.path))]
path = [self.G_a.V[idx].value for idx in self.G_a.path] + \
[self.G_b.V[idx].value for idx in list(reversed(self.G_b.path))]
path_m = [self.G_a.V[idx].aux for idx in self.G_a.path] + \
[self.G_b.V[idx].aux for idx in list(reversed(self.G_b.path))]
if not np.isclose(path[0], self.start).all():
path_idx.reverse()
path.reverse()
path_m.reverse()
path_sc = self.shortcut(path, N=2000)
# split the found path into subpath corresponding to the individual manifolds
path_sc_out = []
i_m = 1
manifold_idx = [0]
for idx, q in enumerate(path_sc):
if is_on_manifold(self.task.manifolds[i_m], q):
manifold_idx += [idx]
i_m += 1
if i_m == len(self.task.manifolds):
break
for i in range(len(manifold_idx) - 1):
path_sc_out += [
path_sc[manifold_idx[i]:manifold_idx[i + 1] + 1]
]
i_m = 1
manifold_idx = [0]
path_out = []
path_idx_out = []
for idx, q in enumerate(path):
if is_on_manifold(self.task.manifolds[i_m], q):
manifold_idx += [idx]
i_m += 1
if i_m == len(self.task.manifolds):
break
for i in range(len(manifold_idx) - 1):
path_out += [path[manifold_idx[i]:manifold_idx[i + 1] + 1]]
path_idx_out += [
path_idx[manifold_idx[i]:manifold_idx[i + 1] + 1]
]
self.path_id = path_idx_out
self.path = path_sc_out
return True
else:
self.G_a, self.G_b = self.G_b, self.G_a
return False
def shortcut(self, path: list, N: int = 100) -> list:
path_sc = path.copy()
for n in range(N):
m = len(path_sc)
i = np.random.randint(0, m - 1)
j = np.random.randint(i, m)
q_i = path_sc[i]
q_j = path_sc[j]
G = Tree(self.d, exact_nn=False)
G.add_node(0, q_i, 0, 0)
h = [np.linalg.norm(m.y(q_i)) for m in self.task.manifolds]
G.V[0].aux = np.argmin(h)
q_reach = self.constrained_extend(G, q_i, 0, q_j)
if np.linalg.norm(q_reach - q_j) < self.eps:
if G.comp_opt_path(q_reach) < path_cost(path_sc[i:j + 1]):
path_i_j = [G.V[idx].value for idx in G.path]
path_sc = path_sc[:i] + path_i_j + path_sc[j + 1:]
return path_sc
def steer(self, q_from: np.ndarray, q_to: np.ndarray) -> np.ndarray:
if np.linalg.norm(q_to - q_from) < self.alpha:
q_new = np.copy(q_to)
else:
diff = q_to - q_from
q_new = q_from + self.alpha * diff * (1.0 / np.linalg.norm(diff))
return q_new
def is_collision(self, q_a: np.ndarray, q_b: np.ndarray) -> bool:
N = int(ceil(np.linalg.norm(q_b - q_a) / self.collision_res))
for i in range(N + 1):
q = q_a if N == 0 else q_a + i / float(N) * (q_b - q_a)
res = self.task.is_collision_conf(q)
if res:
return True
return False
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
class RRTStarManifold:
def __init__(self,
task: Task,
manifold: Manifold,
cfg: dict):
self.name = "RRT_Manifold"
self.task = task
self.start_value = task.start
self.goal_value = task.goal
self.manifold = manifold
self.n_samples = cfg['N']
self.alpha = cfg['ALPHA']
self.beta = cfg['BETA']
self.conv_tol = cfg['CONV_TOL']
self.collision_res = cfg['COLLISION_RES']
self.proj_step_size = cfg['PROJ_STEP_SIZE']
self.d = task.d
self.G = Tree(task.d, exact_nn=False)
self.result = False
self.Q_near_ids = []
self.lim_lo = task.lim_lo
self.lim_up = task.lim_up
self.gamma = np.power(2 * (1 + 1.0 / float(self.d)), 1.0 / float(self.d)) * \
np.power(task.get_joint_space_volume() / unit_ball_measure(self.d), 1. / float(self.d))
def steer(self,
q_from: np.ndarray,
q_to: np.ndarray) -> np.ndarray:
if np.linalg.norm(q_to - q_from) < self.alpha:
q_new = np.copy(q_to)
else:
diff = q_to - q_from
q_new = q_from + self.alpha * diff * (1.0 / np.linalg.norm(diff))
return q_new
def is_collision(self,
q_a: np.ndarray,
q_b: np.ndarray) -> bool:
N = int(ceil(np.linalg.norm(q_b - q_a) / self.collision_res))
for i in range(N + 1):
q = q_a if N == 0 else q_a + i / float(N) * (q_b - q_a)
res = self.task.is_collision_conf(q)
if res:
return True
return False
def extend(self,
q_from: np.ndarray,
q_from_id: int,
q_to: np.ndarray,
q_to_id: int) -> bool:
if not self.is_collision(q_from, q_to):
n = float(len(self.G.V))
r = min([self.gamma * np.power(np.log(n) / n, 1.0 / self.d), self.alpha])
self.Q_near_ids = self.G.get_nearest_neighbors(node_value=q_to, radius=r)
c_min = self.G.V[q_from_id].cost + np.linalg.norm(q_from - q_to)
c_min_inc = np.linalg.norm(q_from - q_to)
q_min_idx = q_from_id
for idx in self.Q_near_ids:
q_idx = self.G.V[idx].value
c_idx = self.G.V[idx].cost + np.linalg.norm(q_idx - q_to)
if not self.is_collision(q_idx, q_to) and c_idx < c_min:
c_min = c_idx
c_min_inc = np.linalg.norm(q_idx - q_to)
q_min_idx = idx
self.G.add_node(node_id=q_to_id, node_value=q_to, node_cost=c_min, inc_cost=c_min_inc)
self.G.add_edge(edge_id=self.G.edge_count, node_a=q_min_idx, node_b=q_to_id)
if np.linalg.norm(q_to - self.goal_value) < self.conv_tol:
self.result = True
return True
return False
def rewire(self,
q_from: np.ndarray,
q_from_id: int):
for idx in self.Q_near_ids: # Q_near_ids was previously computed in extend function
q_idx = self.G.V[idx].value
c_idx = self.G.V[idx].cost
c_new = self.G.V[q_from_id].cost + np.linalg.norm(q_from - q_idx)
if not self.is_collision(q_from, q_idx) and c_new < c_idx:
idx_parent = self.G.V[idx].parent
self.G.remove_edge(idx_parent, idx)
self.G.add_edge(edge_id=self.G.edge_count, node_a=q_from_id, node_b=idx)
self.G.V[idx].cost = c_new
self.G.V[idx].parent = q_from_id
self.G.V[idx].inc_cost = np.linalg.norm(q_from - q_idx)
self.G.update_child_costs(node_id=idx)
# check for convergence
if np.linalg.norm(q_idx - self.goal_value) < self.conv_tol:
self.result = True
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
class PSM:
def __init__(self, task: Task, cfg: dict):
self.name = 'PSM'
self.n_manifolds = len(task.manifolds)
self.task = task
self.start = task.start
self.n_samples = cfg['N']
self.alpha = cfg['ALPHA']
self.beta = cfg['BETA']
self.eps = cfg['EPS']
self.rho = cfg['RHO']
self.r_max = cfg['R_MAX']
self.collision_res = cfg['COLLISION_RES']
self.d = task.d
self.proj_step_size = cfg['PROJ_STEP_SIZE']
self.lim_lo = task.lim_lo
self.lim_up = task.lim_up
self.gamma = np.power(2 * (1 + 1.0 / float(self.d)), 1.0 / float(self.d)) * \
np.power(get_volume(self.lim_lo, self.lim_up) / unit_ball_measure(self.d), 1. / float(self.d))
self.Q_near_ids = []
self.G = None
self.G_list = []
self.V_goal = []
self.V_goal_list = []
self.path = None
self.path_id = None
# check if start point is on first manifold
if not is_on_manifold(task.manifolds[0], task.start, self.eps):
raise Exception(
'The start point is not on the manifold h(start)= ' +
str(task.manifolds[0].y(task.start)))
# check if start point is in configuration space limits
if not self.task.is_valid_conf(task.start):
raise Exception('The start point is not in the system limits.')
# check if start point is in collision
if self.task.is_collision_conf(task.start):
raise Exception('The start point is in collision.')
def run(self) -> bool:
# iterate over sequence of manifolds
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)
self.G = Tree(self.d, exact_nn=False)
if n == 0:
# init tree with start node
self.G.add_node(node_id=0,
node_value=self.start,
node_cost=0.0,
inc_cost=0.0)
else:
# init tree with transition nodes
self.G.abstract_root = True
self.G.add_node(node_id=0,
node_value=np.ones(self.d) * np.inf,
node_cost=0.0,
inc_cost=0.0) # virtual root node
for idx, v_id in enumerate(self.V_goal):
node_id = self.G_list[-1].V[v_id]
self.G.add_node(node_id=idx + 1,
node_value=node_id.value,
node_cost=node_id.cost,
inc_cost=node_id.cost)
self.G.add_edge(edge_id=self.G.edge_count,
node_a=0,
node_b=idx + 1)
self.V_goal.clear()
# check if an initial configuration fulfills the next constraint
for v in self.G.V.values():
if not np.isinf(v.value).any() and is_on_manifold(
self.task.manifolds[n + 1], v.value):
self.V_goal.append(v.id)
if len(self.V_goal) == 0:
self.grow_tree(curr_manifold=self.task.manifolds[n],
next_manifold=self.task.manifolds[n + 1])
print('number of nodes in tree_' + str(n) + ' = ' +
str(len(self.G.V)))
if len(self.V_goal) == 0:
print('RRT extension did not reach any intersection nodes')
return False
else:
print('number of goal nodes in tree_' + str(n) + ' = ' +
str(len(self.V_goal)))
# store results for later evaluation
self.G_list.append(self.G)
self.V_goal_list.append(self.V_goal.copy())
# compute optimal path
opt_idx = min(self.V_goal,
key=lambda idx: np.linalg.norm(self.G.V[idx].cost))
path_idx = self.G.comp_path(opt_idx)
opt_path = [[self.G.V[idx].value for idx in path_idx]]
opt_path_idx = [path_idx.copy()]
opt_path_cost = self.G.V[opt_idx].cost
for G, V_goal in zip(reversed(self.G_list[:-1]),
reversed(self.V_goal_list[:-1])):
opt_idx = V_goal[path_idx[0] -
1] # -1 offset due to virtual root node
path_idx = G.comp_path(opt_idx)
opt_path_idx.append(path_idx.copy())
opt_path.append([G.V[idx].value for idx in path_idx])
opt_path = list(reversed(opt_path))
opt_path_idx = list(reversed(opt_path_idx))
self.path = opt_path
self.path_idx = opt_path_idx
return True
def steer(self, q_from: np.ndarray, q_from_id: int, q_to: np.ndarray,
curr_manifold: Manifold, next_manifold: Manifold) -> np.ndarray:
if np.random.rand() < self.beta and not self.G.V[q_from_id].con_extend:
# steer towards next_manifolds
self.G.V[q_from_id].con_extend = True
yn = next_manifold.y(q_from)
Jn = next_manifold.J(q_from)
d = -Jn.T @ yn
# project on current manifold
J = null_space(curr_manifold.J(q_from))
if J.shape[1] != 0:
d = J @ J.T @ d
else:
# steer towards q_to
d = (q_to - q_from)
# project on current manifold
J = null_space(curr_manifold.J(q_from))
if J.shape[1] != 0:
d = J @ J.T @ d
if np.linalg.norm(d) > 0.0:
q_new = q_from + self.alpha * d * (1.0 / np.linalg.norm(d))
return q_new
else:
return None
def is_collision(self, q_a: np.ndarray, q_b: np.ndarray) -> bool:
N = int(ceil(np.linalg.norm(q_b - q_a) / self.collision_res))
for i in range(N + 1):
q = q_a if N == 0 else q_a + i / float(N) * (q_b - q_a)
res = self.task.is_collision_conf(q)
if res:
return True
return False
def extend(self, q_from: np.ndarray, q_from_id: int, q_to: np.ndarray,
q_to_id: int) -> bool:
if self.is_collision(q_from, q_to):
return False
n = float(len(self.G.V))
r = min(
[self.gamma * np.power(np.log(n) / n, 1.0 / self.d), self.alpha])
self.Q_near_ids = self.G.get_nearest_neighbors(node_value=q_to,
radius=r)
c_min = self.G.V[q_from_id].cost + np.linalg.norm(q_from - q_to)
c_min_inc = np.linalg.norm(q_from - q_to)
q_min_idx = q_from_id
for idx in self.Q_near_ids:
q_idx = self.G.V[idx].value
c_idx = self.G.V[idx].cost + np.linalg.norm(q_idx - q_to)
if not self.is_collision(q_idx, q_to) and c_idx < c_min:
c_min = c_idx
c_min_inc = np.linalg.norm(q_idx - q_to)
q_min_idx = idx
self.G.add_node(node_id=q_to_id,
node_value=q_to,
node_cost=c_min,
inc_cost=c_min_inc)
self.G.add_edge(edge_id=self.G.edge_count,
node_a=q_min_idx,
node_b=q_to_id)
return True
def rewire(self, q_from: np.ndarray, q_from_id: int):
for idx in self.Q_near_ids: # Q_near_ids was previously computed in extend function
q_idx = self.G.V[idx].value
c_idx = self.G.V[idx].cost
c_new = self.G.V[q_from_id].cost + np.linalg.norm(q_from - q_idx)
if not self.is_collision(q_from, q_idx) and c_new < c_idx:
idx_parent = self.G.V[idx].parent
self.G.remove_edge(idx_parent, idx)
self.G.add_edge(edge_id=self.G.edge_count,
node_a=q_from_id,
node_b=idx)
self.G.V[idx].cost = c_new
self.G.V[idx].parent = q_from_id
self.G.V[idx].inc_cost = np.linalg.norm(q_from - q_idx)
self.G.update_child_costs(node_id=idx)
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('')