def save_grasps(grasps, pfcs, obj, dest):
i = 0
for grasp, pfc in zip(grasps, pfcs):
grasp_json = grasp.to_json(quality=pfc, method='PFC')
grasp_filename = os.path.join(dest, obj.key + '_' + str(i) + '.json')
with open(grasp_filename, 'w') as grasp_file:
jsons.dump(grasp_json, grasp_file)
i += 1
def save_grasps(grasps, pfcs, obj, dest, num_successes=0, num_failures=0):
"""
Save grasps as json files with indices
"""
i = 0
for grasp, pfc in zip(grasps, pfcs):
grasp_json = grasp.to_json(quality=pfc, method='PFC', num_successes=num_successes, num_failures=num_failures)
grasp_filename = os.path.join(dest, obj.key + '_' + str(i) + '.json')
with open(grasp_filename, 'w') as grasp_file:
jsons.dump(grasp_json, grasp_file)
i += 1
def save_grasps(self, graspable, grasps):
"""Saves a list of grasps in the database.
Params:
graspable - the GraspableObject for the grasps
grasps - a list of Grasps or a single Grasp to be saved
"""
if not isinstance(grasps, list): # only one grasp
grasps = [grasps]
graspable_dict = {
'key': graspable.key,
'category': graspable.category,
'grasps': [g.to_json() for g in grasps]
}
file_root = os.path.join(self.dataset_root_dir_, graspable.key)
grasp_filename = Dataset.json_filename(file_root)
# TODO: what should happen if grasp_filename already exists?
with open(grasp_filename, 'w') as f:
jsons.dump(grasps, f)
def save_grasps(self, graspable, grasps):
"""Saves a list of grasps in the database.
Params:
graspable - the GraspableObject for the grasps
grasps - a list of Grasps or a single Grasp to be saved
"""
if not isinstance(grasps, list): # only one grasp
grasps = [grasps]
graspable_dict = {
'key': graspable.key,
'category': graspable.category,
'grasps': [g.to_json() for g in grasps]
}
file_root = os.path.join(self.dataset_root_dir_, graspable.key)
grasp_filename = Dataset.json_filename(file_root)
# TODO: what should happen if grasp_filename already exists?
with open(grasp_filename, 'w') as f:
jsons.dump(grasps, f)
def label_pfc(obj, dataset, output_dir, config):
""" Label an object with grasps according to probability of force closure """
# sample intial antipodal grasps
start = time.clock()
sampler = ags.AntipodalGraspSampler(config)
start_time = time.clock()
grasps, alpha_thresh, rho_thresh = sampler.generate_grasps(obj, vis=False)
end_time = time.clock()
duration = end_time - start_time
logging.info('Antipodal grasp candidate generation took %f sec' %(duration))
# partition grasps
grasp_partitions = pfc.space_partition_grasps(grasps, config)
# bandit params
max_iter = config['bandit_max_iter']
confidence = config['bandit_confidence']
snapshot_rate = config['bandit_snapshot_rate']
tc_list = [tc.MaxIterTerminationCondition(max_iter), tc.ConfidenceTerminationCondition(confidence)]
# run bandits on each partition
object_grasps = []
grasp_qualities = []
i = 0
for grasp_partition in grasp_partitions:
logging.info('Finding highest quality grasp in partition %d' %(i))
# create random variables
graspable_rv = pfc.GraspableObjectGaussianPose(obj, config)
f_rv = scipy.stats.norm(config['friction_coef'], config['sigma_mu']) # friction gaussian random variable
candidates = []
for grasp in grasp_partition:
grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config)
candidates.append(pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config))
# run bandits
objective = objectives.RandomBinaryObjective()
ts = das.ThompsonSampling(objective, candidates)
ts_result = ts.solve(termination_condition = tc.OrTerminationCondition(tc_list), snapshot_rate = snapshot_rate)
object_grasps.extend([c.grasp for c in ts_result.best_candidates])
grasp_qualities.extend(list(ts_result.best_pred_means))
i = i+1
stop = time.clock()
logging.info('Took %d sec' %(stop - start))
# get rotated, translated versions of grasps
delay = 0
pr2_grasps = []
pr2_grasp_qualities = []
theta_res = config['grasp_theta_res'] * np.pi
grasp_checker = pgc.OpenRaveGraspChecker(view=config['vis_grasps'])
i = 0
if config['vis_grasps']:
delay = config['vis_delay']
for grasp in object_grasps:
print 'Grasp', i
rotated_grasps = grasp.transform(obj.tf, theta_res)
rotated_grasps = grasp_checker.prune_grasps_in_collision(obj, rotated_grasps, auto_step=True, close_fingers=False, delay=delay)
pr2_grasps.extend(rotated_grasps)
pr2_grasp_qualities.extend([grasp_qualities[i]] * len(rotated_grasps))
i = i+1
logging.info('Num grasps: %d' %(len(pr2_grasps)))
# save grasps locally :( Due to problems with sudo
grasp_filename = os.path.join(output_dir, obj.key + '.json')
with open(grasp_filename, 'w') as f:
jsons.dump([pr2_grasps[i].to_json(quality=pr2_grasp_qualities[i]) for i in range(len(pr2_grasps))], f)
def label_correlated(obj, chunk, dest, config, plot=False):
"""Label an object with grasps according to probability of force closure,
using correlated bandits."""
bandit_start = time.clock()
np.random.seed(100)
# load grasps from database
sample_start = time.clock()
grasps = chunk.load_grasps(obj.key)
sample_end = time.clock()
sample_duration = sample_end - sample_start
logging.info('Loaded %d grasps' %(len(grasps)))
logging.info('Grasp candidate loading took %f sec' %(sample_duration))
if not grasps:
logging.info('Skipping %s' %(obj.key))
return None
# load features for all grasps
feature_start = time.clock()
feature_loader = ff.GraspableFeatureLoader(obj, chunk.name, config)
all_features = feature_loader.load_all_features(grasps) # in same order as grasps
feature_end = time.clock()
feature_duration = feature_end - feature_start
logging.info('Loaded %d features' %(len(all_features)))
logging.info('Grasp feature loading took %f sec' %(feature_duration))
# bandit params
brute_force_iter = config['bandit_brute_force_iter']
max_iter = config['bandit_max_iter']
confidence = config['bandit_confidence']
snapshot_rate = config['bandit_snapshot_rate']
tc_list = [
tc.MaxIterTerminationCondition(max_iter),
# tc.ConfidenceTerminationCondition(confidence)
]
# run bandits!
graspable_rv = pfc.GraspableObjectGaussianPose(obj, config)
f_rv = scipy.stats.norm(config['friction_coef'], config['sigma_mu']) # friction Gaussian RV
candidates = []
for grasp, features in zip(grasps, all_features):
logging.info('Adding grasp %d' %len(candidates))
grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config)
pfc_rv = pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config)
if features is None:
logging.info('Could not compute features for grasp.')
else:
pfc_rv.set_features(features)
candidates.append(pfc_rv)
# feature transform
def phi(rv):
return rv.features
nn = kernels.KDTree(phi=phi)
kernel = kernels.SquaredExponentialKernel(
sigma=config['kernel_sigma'], l=config['kernel_l'], phi=phi)
if config['grasp_symmetry']:
def swapped_phi(rv):
return rv.swapped_features
nn = kernels.SymmetricKDTree(phi=phi, alternate_phi=swapped_phi)
kernel = kernels.SymmetricSquaredExponentialKernel(
sigma=config['kernel_sigma'], l=config['kernel_l'],
phi=phi, alternate_phi=swapped_phi)
objective = objectives.RandomBinaryObjective()
# pre-computed pfc values
estimated_pfc = np.array([c.grasp.quality for c in candidates])
# uniform allocation baseline
ua = das.UniformAllocationMean(objective, candidates)
logging.info('Running uniform allocation.')
ua_result = ua.solve(termination_condition=tc.OrTerminationCondition(tc_list), snapshot_rate=snapshot_rate)
# Thompson sampling for faster convergence
ts = das.ThompsonSampling(objective, candidates)
logging.info('Running Thompson sampling.')
ts_result = ts.solve(termination_condition=tc.OrTerminationCondition(tc_list), snapshot_rate=snapshot_rate)
# correlated Thompson sampling for even faster convergence
ts_corr = das.CorrelatedThompsonSampling(
objective, candidates, nn, kernel, tolerance=config['kernel_tolerance'])
logging.info('Running correlated Thompson sampling.')
ts_corr_result = ts_corr.solve(termination_condition=tc.OrTerminationCondition(tc_list), snapshot_rate=snapshot_rate)
object_grasps = [candidates[i].grasp for i in ts_result.best_candidates]
grasp_qualities = list(ts_result.best_pred_means)
bandit_stop = time.clock()
logging.info('Bandits took %f sec' %(bandit_stop - bandit_start))
# get rotated, translated versions of grasps
delay = 0
pr2_grasps = []
pr2_grasp_qualities = []
theta_res = config['grasp_theta_res'] * np.pi
# grasp_checker = pgc.OpenRaveGraspChecker(view=config['vis_grasps'])
if config['vis_grasps']:
delay = config['vis_delay']
for grasp, grasp_quality in zip(object_grasps, grasp_qualities):
rotated_grasps = grasp.transform(obj.tf, theta_res)
# rotated_grasps = grasp_checker.prune_grasps_in_collision(obj, rotated_grasps, auto_step=True, close_fingers=False, delay=delay)
pr2_grasps.extend(rotated_grasps)
pr2_grasp_qualities.extend([grasp_quality] * len(rotated_grasps))
logging.info('Num grasps: %d' %(len(pr2_grasps)))
grasp_filename = os.path.join(dest, obj.key + '.json')
with open(grasp_filename, 'w') as f:
jsons.dump([g.to_json(quality=q) for g, q in
zip(pr2_grasps, pr2_grasp_qualities)], f)
ua_normalized_reward = reward_vs_iters(ua_result, estimated_pfc)
ts_normalized_reward = reward_vs_iters(ts_result, estimated_pfc)
ts_corr_normalized_reward = reward_vs_iters(ts_corr_result, estimated_pfc)
return BanditCorrelatedExperimentResult(ua_normalized_reward, ts_normalized_reward, ts_corr_normalized_reward,
estimated_pfc, ua_result.iters, kernel.matrix(candidates), obj_key=obj.key)
def label_correlated(obj, chunk, dest, config, plot=False):
"""Label an object with grasps according to probability of force closure,
using correlated bandits."""
bandit_start = time.clock()
np.random.seed(100)
# load grasps from database
sample_start = time.clock()
grasps = chunk.load_grasps(obj.key)
sample_end = time.clock()
sample_duration = sample_end - sample_start
logging.info('Loaded %d grasps' % (len(grasps)))
logging.info('Grasp candidate loading took %f sec' % (sample_duration))
if not grasps:
logging.info('Skipping %s' % (obj.key))
return None
# load features for all grasps
feature_start = time.clock()
feature_loader = ff.GraspableFeatureLoader(obj, chunk.name, config)
all_features = feature_loader.load_all_features(
grasps) # in same order as grasps
feature_end = time.clock()
feature_duration = feature_end - feature_start
logging.info('Loaded %d features' % (len(all_features)))
logging.info('Grasp feature loading took %f sec' % (feature_duration))
# bandit params
brute_force_iter = config['bandit_brute_force_iter']
max_iter = config['bandit_max_iter']
confidence = config['bandit_confidence']
snapshot_rate = config['bandit_snapshot_rate']
tc_list = [
tc.MaxIterTerminationCondition(max_iter),
# tc.ConfidenceTerminationCondition(confidence)
]
# run bandits!
graspable_rv = pfc.GraspableObjectGaussianPose(obj, config)
f_rv = scipy.stats.norm(config['friction_coef'],
config['sigma_mu']) # friction Gaussian RV
candidates = []
for grasp, features in zip(grasps, all_features):
logging.info('Adding grasp %d' % len(candidates))
grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config)
pfc_rv = pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config)
if features is None:
logging.info('Could not compute features for grasp.')
else:
pfc_rv.set_features(features)
candidates.append(pfc_rv)
# feature transform
def phi(rv):
return rv.features
nn = kernels.KDTree(phi=phi)
kernel = kernels.SquaredExponentialKernel(sigma=config['kernel_sigma'],
l=config['kernel_l'],
phi=phi)
if config['grasp_symmetry']:
def swapped_phi(rv):
return rv.swapped_features
nn = kernels.SymmetricKDTree(phi=phi, alternate_phi=swapped_phi)
kernel = kernels.SymmetricSquaredExponentialKernel(
sigma=config['kernel_sigma'],
l=config['kernel_l'],
phi=phi,
alternate_phi=swapped_phi)
objective = objectives.RandomBinaryObjective()
# pre-computed pfc values
estimated_pfc = np.array([c.grasp.quality for c in candidates])
# uniform allocation baseline
ua = das.UniformAllocationMean(objective, candidates)
logging.info('Running uniform allocation.')
ua_result = ua.solve(
termination_condition=tc.OrTerminationCondition(tc_list),
snapshot_rate=snapshot_rate)
# Thompson sampling for faster convergence
ts = das.ThompsonSampling(objective, candidates)
logging.info('Running Thompson sampling.')
ts_result = ts.solve(
termination_condition=tc.OrTerminationCondition(tc_list),
snapshot_rate=snapshot_rate)
# correlated Thompson sampling for even faster convergence
ts_corr = das.CorrelatedThompsonSampling(
objective,
candidates,
nn,
kernel,
tolerance=config['kernel_tolerance'])
logging.info('Running correlated Thompson sampling.')
ts_corr_result = ts_corr.solve(
termination_condition=tc.OrTerminationCondition(tc_list),
snapshot_rate=snapshot_rate)
object_grasps = [candidates[i].grasp for i in ts_result.best_candidates]
grasp_qualities = list(ts_result.best_pred_means)
bandit_stop = time.clock()
logging.info('Bandits took %f sec' % (bandit_stop - bandit_start))
# get rotated, translated versions of grasps
delay = 0
pr2_grasps = []
pr2_grasp_qualities = []
theta_res = config['grasp_theta_res'] * np.pi
# grasp_checker = pgc.OpenRaveGraspChecker(view=config['vis_grasps'])
if config['vis_grasps']:
delay = config['vis_delay']
for grasp, grasp_quality in zip(object_grasps, grasp_qualities):
rotated_grasps = grasp.transform(obj.tf, theta_res)
# rotated_grasps = grasp_checker.prune_grasps_in_collision(obj, rotated_grasps, auto_step=True, close_fingers=False, delay=delay)
pr2_grasps.extend(rotated_grasps)
pr2_grasp_qualities.extend([grasp_quality] * len(rotated_grasps))
logging.info('Num grasps: %d' % (len(pr2_grasps)))
grasp_filename = os.path.join(dest, obj.key + '.json')
with open(grasp_filename, 'w') as f:
jsons.dump([
g.to_json(quality=q)
for g, q in zip(pr2_grasps, pr2_grasp_qualities)
], f)
ua_normalized_reward = reward_vs_iters(ua_result, estimated_pfc)
ts_normalized_reward = reward_vs_iters(ts_result, estimated_pfc)
ts_corr_normalized_reward = reward_vs_iters(ts_corr_result, estimated_pfc)
return BanditCorrelatedExperimentResult(ua_normalized_reward,
ts_normalized_reward,
ts_corr_normalized_reward,
estimated_pfc,
ua_result.iters,
kernel.matrix(candidates),
obj_key=obj.key)
def extract_features(obj, dest, feature_dest, config):
# sample grasps
sample_start = time.clock()
if config['grasp_sampler'] == 'antipodal':
logging.info('Using antipodal grasp sampling')
sampler = ags.AntipodalGraspSampler(config)
grasps = sampler.generate_grasps(
obj, check_collisions=config['check_collisions'])
# pad with gaussian grasps
num_grasps = len(grasps)
min_num_grasps = config['min_num_grasps']
if num_grasps < min_num_grasps:
target_num_grasps = min_num_grasps - num_grasps
gaussian_sampler = gs.GaussianGraspSampler(config)
gaussian_grasps = gaussian_sampler.generate_grasps(
obj,
target_num_grasps=target_num_grasps,
check_collisions=config['check_collisions'])
grasps.extend(gaussian_grasps)
else:
logging.info('Using Gaussian grasp sampling')
sampler = gs.GaussianGraspSampler(config)
grasps = sampler.generate_grasps(
obj, check_collisions=config['check_collisions'])
sample_end = time.clock()
sample_duration = sample_end - sample_start
logging.info('Grasp candidate generation took %f sec' % (sample_duration))
if not grasps or len(grasps) == 0:
logging.info('Skipping %s' % (obj.key))
return
# compute all features
feature_start = time.clock()
feature_extractor = ff.GraspableFeatureExtractor(obj, config)
all_features = feature_extractor.compute_all_features(grasps)
feature_end = time.clock()
feature_duration = feature_end - feature_start
logging.info('Feature extraction took %f sec' % (feature_duration))
# generate pfc candidates
graspable_rv = pfc.GraspableObjectGaussianPose(obj, config)
f_rv = scipy.stats.norm(config['friction_coef'], config['sigma_mu'])
candidates = []
logging.info('%d grasps, %d valid features', len(grasps),
len(all_features) - all_features.count(None))
for grasp, features in zip(grasps, all_features):
logging.info('Adding grasp %d candidate' % (len(candidates)))
if features is None:
logging.info('No features computed.')
continue
grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config)
pfc_rv = pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config)
pfc_rv.set_features(features)
candidates.append(pfc_rv)
logging.info('%d candidates', len(candidates))
# brute force with uniform allocation
brute_force_iter = config['bandit_brute_force_iter']
snapshot_rate = config['bandit_snapshot_rate']
def phi(rv):
return rv.features
objective = objectives.RandomBinaryObjective()
ua = das.UniformAllocationMean(objective, candidates)
logging.info('Running uniform allocation for true pfc.')
bandit_start = time.clock()
ua_result = ua.solve(
termination_condition=tc.MaxIterTerminationCondition(brute_force_iter),
snapshot_rate=snapshot_rate)
bandit_end = time.clock()
bandit_duration = bandit_end - bandit_start
logging.info('Uniform allocation (%d iters) took %f sec' %
(brute_force_iter, bandit_duration))
cand_grasps = [c.grasp for c in candidates]
cand_features = [c.features_ for c in candidates]
final_model = ua_result.models[-1]
estimated_pfc = models.BetaBernoulliModel.beta_mean(
final_model.alphas, final_model.betas)
if len(cand_grasps) != len(estimated_pfc):
logging.warning(
'Number of grasps does not match estimated pfc results.')
IPython.embed()
# write to file
grasp_filename = os.path.join(dest, obj.key + '.json')
with open(grasp_filename, 'w') as grasp_file:
jsons.dump([
g.to_json(quality=q, num_successes=a, num_failures=b)
for g, q, a, b in zip(cand_grasps, estimated_pfc,
final_model.alphas, final_model.betas)
], grasp_file)
# HACK to make paths relative
features_as_json = [f.to_json(feature_dest) for f in cand_features]
output_dest = os.path.split(dest)[0]
for feature_as_json in features_as_json:
feature_as_json = list(feature_as_json.values())[0]
for wname in ('w1', 'w2'):
wdata = feature_as_json[wname]
for k, v in wdata.items():
wdata[k] = os.path.relpath(
v, output_dest) # relative to output_dest
feature_filename = os.path.join(feature_dest, obj.key + '.json')
with open(feature_filename, 'w') as feature_file:
jsons.dump(features_as_json, feature_file)
def label_pfc(obj, dataset, output_dir, config):
""" Label an object with grasps according to probability of force closure """
# sample intial antipodal grasps
start = time.clock()
sampler = ags.AntipodalGraspSampler(config)
start_time = time.clock()
grasps, alpha_thresh, rho_thresh = sampler.generate_grasps(obj, vis=False)
end_time = time.clock()
duration = end_time - start_time
logging.info('Antipodal grasp candidate generation took %f sec' %
(duration))
# partition grasps
grasp_partitions = pfc.space_partition_grasps(grasps, config)
# bandit params
max_iter = config['bandit_max_iter']
confidence = config['bandit_confidence']
snapshot_rate = config['bandit_snapshot_rate']
tc_list = [
tc.MaxIterTerminationCondition(max_iter),
tc.ConfidenceTerminationCondition(confidence)
]
# run bandits on each partition
object_grasps = []
grasp_qualities = []
i = 0
for grasp_partition in grasp_partitions:
logging.info('Finding highest quality grasp in partition %d' % (i))
# create random variables
graspable_rv = pfc.GraspableObjectGaussianPose(obj, config)
f_rv = scipy.stats.norm(
config['friction_coef'],
config['sigma_mu']) # friction gaussian random variable
candidates = []
for grasp in grasp_partition:
grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config)
candidates.append(
pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config))
# run bandits
objective = objectives.RandomBinaryObjective()
ts = das.ThompsonSampling(objective, candidates)
ts_result = ts.solve(
termination_condition=tc.OrTerminationCondition(tc_list),
snapshot_rate=snapshot_rate)
object_grasps.extend([c.grasp for c in ts_result.best_candidates])
grasp_qualities.extend(list(ts_result.best_pred_means))
i = i + 1
stop = time.clock()
logging.info('Took %d sec' % (stop - start))
# get rotated, translated versions of grasps
delay = 0
pr2_grasps = []
pr2_grasp_qualities = []
theta_res = config['grasp_theta_res'] * np.pi
grasp_checker = pgc.OpenRaveGraspChecker(view=config['vis_grasps'])
i = 0
if config['vis_grasps']:
delay = config['vis_delay']
for grasp in object_grasps:
print 'Grasp', i
rotated_grasps = grasp.transform(obj.tf, theta_res)
rotated_grasps = grasp_checker.prune_grasps_in_collision(
obj,
rotated_grasps,
auto_step=True,
close_fingers=False,
delay=delay)
pr2_grasps.extend(rotated_grasps)
pr2_grasp_qualities.extend([grasp_qualities[i]] * len(rotated_grasps))
i = i + 1
logging.info('Num grasps: %d' % (len(pr2_grasps)))
# save grasps locally :( Due to problems with sudo
grasp_filename = os.path.join(output_dir, obj.key + '.json')
with open(grasp_filename, 'w') as f:
jsons.dump([
pr2_grasps[i].to_json(quality=pr2_grasp_qualities[i])
for i in range(len(pr2_grasps))
], f)
def extract_features(obj, dest, feature_dest, config):
# sample grasps
sample_start = time.clock()
if config['grasp_sampler'] == 'antipodal':
logging.info('Using antipodal grasp sampling')
sampler = ags.AntipodalGraspSampler(config)
grasps = sampler.generate_grasps(
obj, check_collisions=config['check_collisions'])
# pad with gaussian grasps
num_grasps = len(grasps)
min_num_grasps = config['min_num_grasps']
if num_grasps < min_num_grasps:
target_num_grasps = min_num_grasps - num_grasps
gaussian_sampler = gs.GaussianGraspSampler(config)
gaussian_grasps = gaussian_sampler.generate_grasps(
obj, target_num_grasps=target_num_grasps, check_collisions=config['check_collisions'])
grasps.extend(gaussian_grasps)
else:
logging.info('Using Gaussian grasp sampling')
sampler = gs.GaussianGraspSampler(config)
grasps = sampler.generate_grasps(
obj, check_collisions=config['check_collisions'])
sample_end = time.clock()
sample_duration = sample_end - sample_start
logging.info('Grasp candidate generation took %f sec' %(sample_duration))
if not grasps or len(grasps) == 0:
logging.info('Skipping %s' %(obj.key))
return
# compute all features
feature_start = time.clock()
feature_extractor = ff.GraspableFeatureExtractor(obj, config)
all_features = feature_extractor.compute_all_features(grasps)
feature_end = time.clock()
feature_duration = feature_end - feature_start
logging.info('Feature extraction took %f sec' %(feature_duration))
# generate pfc candidates
graspable_rv = pfc.GraspableObjectGaussianPose(obj, config)
f_rv = scipy.stats.norm(config['friction_coef'], config['sigma_mu'])
candidates = []
logging.info('%d grasps, %d valid features', len(grasps), len(all_features) - all_features.count(None))
for grasp, features in zip(grasps, all_features):
logging.info('Adding grasp %d candidate' %(len(candidates)))
if features is None:
logging.info('No features computed.')
continue
grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config)
pfc_rv = pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config)
pfc_rv.set_features(features)
candidates.append(pfc_rv)
logging.info('%d candidates', len(candidates))
# brute force with uniform allocation
brute_force_iter = config['bandit_brute_force_iter']
snapshot_rate = config['bandit_snapshot_rate']
def phi(rv):
return rv.features
objective = objectives.RandomBinaryObjective()
ua = das.UniformAllocationMean(objective, candidates)
logging.info('Running uniform allocation for true pfc.')
bandit_start = time.clock()
ua_result = ua.solve(
termination_condition=tc.MaxIterTerminationCondition(brute_force_iter),
snapshot_rate=snapshot_rate)
bandit_end = time.clock()
bandit_duration = bandit_end - bandit_start
logging.info('Uniform allocation (%d iters) took %f sec' %(brute_force_iter, bandit_duration))
cand_grasps = [c.grasp for c in candidates]
cand_features = [c.features_ for c in candidates]
final_model = ua_result.models[-1]
estimated_pfc = models.BetaBernoulliModel.beta_mean(
final_model.alphas, final_model.betas)
if len(cand_grasps) != len(estimated_pfc):
logging.warning('Number of grasps does not match estimated pfc results.')
IPython.embed()
# write to file
grasp_filename = os.path.join(dest, obj.key + '.json')
with open(grasp_filename, 'w') as grasp_file:
jsons.dump([g.to_json(quality=q, num_successes=a, num_failures=b) for g, q, a, b
in zip(cand_grasps, estimated_pfc, final_model.alphas, final_model.betas)],
grasp_file)
# HACK to make paths relative
features_as_json = [f.to_json(feature_dest) for f in cand_features]
output_dest = os.path.split(dest)[0]
for feature_as_json in features_as_json:
feature_as_json = list(feature_as_json.values())[0]
for wname in ('w1', 'w2'):
wdata = feature_as_json[wname]
for k, v in wdata.items():
wdata[k] = os.path.relpath(v, output_dest) # relative to output_dest
feature_filename = os.path.join(feature_dest, obj.key + '.json')
with open(feature_filename, 'w') as feature_file:
jsons.dump(features_as_json, feature_file)
