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 __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 __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.')