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 __init__(self, task: Task, cfg: dict):
self.name = "IK_RRT"
self.n_manifolds = len(task.manifolds)
self.task = task
self.start = task.start
self.cfg = cfg
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.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))
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)))
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 __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:
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 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 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 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
