def get_straight_traj(env, init_stds, n_points=50):
"""Straight line demonstration"""
start = env.start_state
goal = env.goal_state
dx = (goal[:3] - start[:3]) / float(n_points)
traj_pts = [start]
traj_stds = [init_stds]
dists = []
for i in range(n_points):
point = copy.deepcopy(traj_pts[0])
point[:3] += dx * float(i + 1)
q = qt.slerp(start[3:], goal[3:], float(i + 1) / float(n_points))
point[3] = q[0]
point[4] = q[1]
point[5] = q[2]
point[6] = q[3]
traj_pts.append(point)
## traj_stds.append( list(np.array(init_stds)*(1. + float(n_points-i)/float(n_points))/2.0 ))
traj_stds.append(
list(
np.array(init_stds) *
np.exp(-1.5 * float(i) / float(n_points))))
dists.append(pose_distance(traj_pts[i - 1], traj_pts[i]))
assert np.amax(
dists
) < MAX_EDGE_LEN, "reference trajectory's resolution {} is too low".format(
np.amax(dists))
return traj_pts, traj_stds
def sample_points_rrm(env,
traj_pts,
traj_stds,
samples,
node_groups,
roadmap,
max_dist=0.005,
min_dist=0.001,
leafsize=30):
# random samples
nearest_samples = []
if len(node_groups) > 1:
sub_samples = []
for group in node_groups:
sub_samples.append(np.array(samples)[group].tolist())
else:
sub_samples = [samples]
n_points = len(traj_pts)
p = np.linspace(1., 0.1, n_points)
p /= np.sum(p)
for i, (point, std) in enumerate(zip(traj_pts, traj_stds)):
# random
i = np.random.choice(range(n_points), p=p)
point = traj_pts[i]
std = traj_stds[i]
## from IPython import embed; embed(); sys.exit()
rnd = np.array([
truncnorm((env.observation_space.low[0] - point[0]) / std[0],
(env.observation_space.high[0] - point[0]) / std[0],
loc=point[0],
scale=std[0]).rvs(1)[0],
truncnorm((env.observation_space.low[1] - point[1]) / std[1],
(env.observation_space.high[1] - point[1]) / std[1],
loc=point[1],
scale=std[1]).rvs(1)[0],
truncnorm((env.observation_space.low[2] - point[2]) / std[2],
(env.observation_space.high[2] - point[2]) / std[2],
loc=point[2],
scale=std[2]).rvs(1)[0], 0, 0, 0, 1
])
rnd[3:7] = qt.quat_QuTem(point[3:7], 1, std[3:7])[0]
for sub_sample in sub_samples:
# Find nearest node
nind = np.argmin(pose_distance(sub_sample, rnd))
#block
d_max = 0.01 # 0.005
w_max = 1.
max_dist = MAX_EDGE_LEN
min_dist = 0.008 #0.009 # 0.017 = 1deg, scale=0.3
nearest_sample = copy.deepcopy(sub_sample[nind])
# ang
org_dist = qt.quat_angle(nearest_sample[3:7],
rnd[3:7]) / np.pi / 2.
nearest_sample[3:7] = qt.slerp(
nearest_sample[3:7], rnd[3:7],
min(w_max / 180. * np.pi / org_dist, 1.))
# pos
unit_vec = rnd[:3] - nearest_sample[:3]
unit_vec /= np.linalg.norm(unit_vec)
d_collision = env.get_distance(nearest_sample)
if d_collision <= 0: continue
else: expandPosDis = d_max
nearest_sample[:3] += expandPosDis * unit_vec
if expandPosDis == 0: continue
if env.isValid(nearest_sample, check_collision=True) is False:
continue
dists = pose_distance(samples + nearest_samples,
np.array(nearest_sample))
j = np.argmin(dists)
pose_dist = dists[j]
grip_dist = abs((samples + nearest_samples)[j][7] -
nearest_sample[7])
if pose_dist > max_dist or (pose_dist < min_dist
and grip_dist < 0.5):
continue
nearest_samples.append(nearest_sample)
if len(nearest_samples) == 0: return samples, roadmap
#expand roadmap
pre_max_indx = len(samples) - 1
roadmap += [[i + len(samples)] for i in range(len(nearest_samples))]
samples += nearest_samples
state_inds = np.array(range(len(samples)))
max_edge = 0
for s in nearest_samples:
dists = pose_distance(samples, s)
try:
inds = state_inds[dists <= max_dist]
dists = dists[inds]
tmp = np.argsort(dists)
inds = np.array(inds)[tmp]
roadmap[inds[0]] = inds.tolist()
except:
print "Maybe env's observation limit does not match with reference traj"
from IPython import embed
embed()
sys.exit()
for ii, ind in enumerate(inds):
if ii == 0: continue
if pre_max_indx < ind: continue
roadmap[ind].append(inds[0])
if len(roadmap[ind]) > max_edge:
max_edge = len(roadmap[ind])
print len(samples), max_edge, len(nearest_samples)
return samples, roadmap
def sample_points_rrm2(env,
traj_pts,
traj_stds,
samples,
node_groups,
roadmap,
default_y_angle=0,
max_dist=0.005,
min_dist=0.001,
leafsize=30):
# random samples
nearest_samples = []
if len(node_groups) > 1:
sub_samples = []
for group in node_groups:
sub_samples.append(np.array(samples)[group].tolist())
else:
sub_samples = [samples]
n_points = len(traj_pts)
p = np.linspace(1., 0.1, n_points)
p /= np.sum(p)
for i, (point, std) in enumerate(zip(traj_pts, traj_stds)):
# random
i = np.random.choice(range(n_points), p=p)
#i = random.randint(0, len(traj_pts)-1)
point = traj_pts[i]
std = traj_stds[i]
## from IPython import embed; embed(); sys.exit()
rnd = np.array([
truncnorm((env.observation_space.low[0] - point[0]) / std[0],
(env.observation_space.high[0] - point[0]) / std[0],
loc=point[0],
scale=std[0]).rvs(1)[0],
truncnorm((env.observation_space.low[1] - point[1]) / std[1],
(env.observation_space.high[1] - point[1]) / std[1],
loc=point[1],
scale=std[1]).rvs(1)[0],
truncnorm((env.observation_space.low[2] - point[2]) / std[2],
(env.observation_space.high[2] - point[2]) / std[2],
loc=point[2],
scale=std[2]).rvs(1)[0], 0, 0, 0, 1
])
M = PyKDL.Rotation.RPY(random.gauss(point[3], std[3]),
random.gauss(point[4], std[4]), default_y_angle)
q = M.GetQuaternion()
rnd[3] = q[0]
rnd[4] = q[1]
rnd[5] = q[2]
rnd[6] = q[3]
for sub_sample in sub_samples:
# Find nearest node
nind = np.argmin(pose_distance(sub_sample, rnd))
#peginhole
d_max = 0.01 # 0.005
w_max = 1.
max_dist = MAX_EDGE_LEN
min_dist = 0.003 #0.002 # 0.005 0.017 = 1deg, scale=0.3
#clamp
d_max = 0.005 #0.01 # 0.005
w_max = 1.
max_dist = MAX_EDGE_LEN
min_dist = 0.003 #0.005 # 0.005 0.017 = 1deg, scale=0.3
## dist = env.get_distance(sub_sample[nind])
## if dist > collision_dist:
## d_max = d_max*dist/collision_dist if dist < collision_dist*collision_mult else d_max*collision_mult
## w_max = w_max*dist/collision_dist if dist < collision_dist*collision_mult else w_max*collision_mult
## max_dist = min(dist, max_dist*collision_mult)
## min_dist = min_dist #*mult
nearest_sample = copy.deepcopy(sub_sample[nind])
# ang
org_dist = orient_distance(nearest_sample[3:], rnd[3:])
q = qt.slerp(nearest_sample[3:], rnd[3:],
min(w_max / 180. * np.pi / org_dist, 1.))
nearest_sample[3] = q[0]
nearest_sample[4] = q[1]
nearest_sample[5] = q[2]
nearest_sample[6] = q[3]
# pos
unit_vec = rnd[:3] - nearest_sample[:3]
unit_vec /= np.linalg.norm(unit_vec)
d_collision = env.get_distance(nearest_sample)
if d_collision <= 0:
continue
#elif d_collision < d_max: expandPosDis = d_collision*0.9
else:
expandPosDis = d_max
nearest_sample[:3] += expandPosDis * unit_vec
if expandPosDis == 0: continue
if env.isValid(nearest_sample, check_collision=True) is False:
continue
dist = np.amin(
pose_distance(samples + nearest_samples,
np.array(nearest_sample)))
## print dist, d_max, w_max, max_dist, min_dist
if dist > max_dist or dist < min_dist: continue
nearest_samples.append(nearest_sample)
if len(nearest_samples) == 0: return samples, roadmap
#start = time.time()
#expand roadmap
pre_max_indx = len(samples) - 1
roadmap += [[i + len(samples)] for i in range(len(nearest_samples))]
samples += nearest_samples
state_inds = np.array(range(len(samples)))
max_edge = 0
for s in nearest_samples:
dists = pose_distance(samples, s)
## dist = env.get_distance(s)
## if dist < collision_dist:
## max_dist = MAX_EDGE_LEN
## else:
## max_dist = min(dist, MAX_EDGE_LEN*collision_mult)
try:
inds = state_inds[dists <= max_dist]
dists = dists[inds]
tmp = np.argsort(dists)
inds = np.array(inds)[tmp]
roadmap[inds[0]] = inds.tolist()
except:
print "Maybe env's observation limit does not match with reference traj"
from IPython import embed
embed()
sys.exit()
for ii, ind in enumerate(inds):
if ii == 0: continue
if pre_max_indx < ind: continue
roadmap[ind].append(inds[0])
if len(roadmap[ind]) > max_edge:
max_edge = len(roadmap[ind])
## if max_edge>100:
## dd = pose_distance(samples, samples[ind])
## print dd[dd<=MAX_EDGE_LEN]
## sys.exit()
print len(samples), max_edge, len(nearest_samples)
#print(time.time() - start, len(samples)); start = time.time()
return samples, roadmap