def read_datum(self, key):
"""Read in the GraspableObject3D corresponding to given key."""
if key not in self.data_keys_:
raise ValueError('Key %s not found in dataset %s' %
(key, self.name))
file_root = os.path.join(self.dataset_root_dir_, key)
sdf_filename = Dataset.sdf_filename(file_root)
obj_filename = Dataset.obj_filename(file_root)
features_filename = Dataset.features_filename(file_root)
# read in data
sf = sdf_file.SdfFile(sdf_filename)
sdf = sf.read()
of = obj_file.ObjFile(obj_filename)
mesh = of.read()
if os.path.exists(features_filename):
ff = feature_file.LocalFeatureFile(features_filename)
features = ff.read()
else:
features = None
return go.GraspableObject3D(sdf,
mesh=mesh,
features=features,
key=key,
model_name=obj_filename,
category=self.data_categories_[key])
def test_window_distance(width, num_steps, plot=None):
import sdf_file, obj_file
np.random.seed(100)
mesh_file_name = 'data/test/meshes/Co_clean.obj'
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sdf = sdf_file.SdfFile(sdf_3d_file_name).read()
mesh = obj_file.ObjFile(mesh_file_name).read()
graspable = GraspableObject3D(sdf, mesh)
grasp_axis = np.array([0, 1, 0])
grasp_width = 0.1
grasp1_center = np.array([0, 0, -0.025])
grasp1 = g.ParallelJawPtGrasp3D(grasp1_center, grasp_axis, grasp_width)
grasp2_center = np.array([0, 0, -0.030])
grasp2 = g.ParallelJawPtGrasp3D(grasp2_center, grasp_axis, grasp_width)
w1, w2 = graspable.surface_information(grasp1, width, num_steps)
v1, v2 = graspable.surface_information(grasp2, width, num_steps)
# IPython.embed()
if plot:
plot(w1.proj_win, num_steps)
plot(w2.proj_win, num_steps)
plot(v1.proj_win, num_steps)
plot(v2.proj_win, num_steps)
plt.show()
IPython.embed()
return
def test_gaussian_grasp_sampling(vis=False):
np.random.seed(100)
h = plt.figure()
ax = h.add_subplot(111, projection='3d')
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
mesh_name = 'data/test/meshes/Co_clean.obj'
of = obj_file.ObjFile(mesh_name)
m = of.read()
graspable = graspable_object.GraspableObject3D(sdf_3d,
mesh=m,
model_name=mesh_name)
config_file = 'cfg/correlated.yaml'
config = ec.ExperimentConfig(config_file)
sampler = GaussianGraspSampler(config)
start_time = time.clock()
grasps = sampler.generate_grasps(graspable,
target_num_grasps=200,
vis=False)
end_time = time.clock()
duration = end_time - start_time
logging.info('Gaussian grasp candidate generation took %f sec' %
(duration))
def test_grasp_collisions():
np.random.seed(100)
h = plt.figure()
ax = h.add_subplot(111, projection='3d')
sdf_3d_file_name = 'data/test/sdf/Co.sdf'
# sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v0/amazon_picking_challenge/dove_beauty_bar.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
mesh_name = 'data/test/meshes/Co.obj'
# mesh_name = '/mnt/terastation/shape_data/MASTER_DB_v0/amazon_picking_challenge/dove_beauty_bar.obj'
of = obj_file.ObjFile(mesh_name)
m = of.read()
graspable = graspable_object.GraspableObject3D(sdf_3d,
mesh=m,
model_name=mesh_name)
rave.raveSetDebugLevel(rave.DebugLevel.Error)
grasp_checker = OpenRaveGraspChecker()
center = np.array([0, 0, 0.02])
axis = np.array([1, 0, 0])
axis = axis / np.linalg.norm(axis)
width = 0.1
grasp = g.ParallelJawPtGrasp3D(center, axis, width)
grasp.close_fingers(graspable, vis=True)
grasp_checker.prune_grasps_in_collision(graspable, [grasp],
auto_step=True,
close_fingers=True,
delay=30)
def test_2d_transform():
sdf_2d_file_name = 'data/test/sdf/medium_black_spring_clamp_optimized_poisson_texture_mapped_mesh_clean_0.csv'
sf2 = sf.SdfFile(sdf_2d_file_name)
sdf_2d = sf2.read()
# transform
pose_mat = np.matrix([[0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
tf = tfx.transform(pose_mat, from_frame='world')
start_t = time.clock()
sdf_tf = sdf_2d.transform(tf, scale=1.5)
end_t = time.clock()
duration = end_t - start_t
logging.info('2D Transform took %f sec' % (duration))
# display
plt.figure()
plt.subplot(1, 2, 1)
sdf_2d.imshow()
plt.title('Original')
plt.subplot(1, 2, 2)
sdf_tf.imshow()
plt.title('Transformed')
plt.show()
def test_grasp_from_contacts():
""" Should visually check for reasonable contacts (large green circles) """
np.random.seed(100)
sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf'
sf3 = sf.SdfFile(sdf_3d_file_name)
sdf_3d = sf3.read()
# create point on sdf surface
obj_3d = go.GraspableObject3D(sdf_3d)
surf_pts, surf_sdf = obj_3d.sdf.surface_points()
rand_pt_ind = np.random.choice(surf_pts.shape[0])
rand_surf_pt_grid = surf_pts[rand_pt_ind, :]
rand_surf_pt = obj_3d.sdf.transform_pt_grid_to_obj(rand_surf_pt_grid)
# get grasp direction
axis = -obj_3d.sdf.gradient(rand_surf_pt)
axis = axis / np.linalg.norm(axis)
axis = obj_3d.sdf.transform_pt_grid_to_obj(axis, direction=True)
plt.figure()
test_grasp_width = 0.8
ax = plt.gca(projection='3d')
ax.scatter(rand_surf_pt_grid[0],
rand_surf_pt_grid[1],
rand_surf_pt_grid[2],
s=80,
c=u'g')
g, _ = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(
obj_3d, rand_surf_pt, axis, test_grasp_width, vis=True)
plt.show()
def test_window_curvature(width, num_steps, plot=None):
import sdf_file, obj_file
np.random.seed(100)
mesh_file_name = 'data/test/meshes/Co_clean.obj'
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sdf = sdf_file.SdfFile(sdf_3d_file_name).read()
mesh = obj_file.ObjFile(mesh_file_name).read()
graspable = GraspableObject3D(sdf, mesh)
grasp_axis = np.array([0, 1, 0])
grasp_width = 0.1
for i, z in enumerate([-0.030, -0.035, -0.040, -0.045], 1):
print 'w%d' % (i)
grasp_center = np.array([0, 0, z])
grasp = g.ParallelJawPtGrasp3D(
ParallelJawPtGrasp3D.configuration_from_params(
grasp_center, grasp_axis, grasp_width))
w, _ = graspable.surface_information(grasp, width, num_steps)
for info in (w.proj_win_, w.gauss_curvature_):
print 'min:', np.min(info), np.argmin(info)
print 'max:', np.max(info), np.argmax(info)
if plot:
plot(w.proj_win_, num_steps, 'w%d proj_win' % (i), save=True)
def test_plot_friction_cone():
import sdf_file, obj_file, grasp as g, graspable_object
np.random.seed(100)
mesh_file_name = 'data/test/meshes/Co_clean.obj'
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sdf = sdf_file.SdfFile(sdf_3d_file_name).read()
mesh = obj_file.ObjFile(mesh_file_name).read()
graspable = graspable_object.GraspableObject3D(sdf, mesh)
grasp_axis = np.array([0, 1, 0])
grasp_width = 0.1
grasp_center = np.array([0, 0, -0.025])
grasp = g.ParallelJawPtGrasp3D(
ParallelJawPtGrasp3D.configuration_from_params(grasp_center,
grasp_axis, 0,
grasp_width, 0))
_, (c1, c2) = grasp.close_fingers(graspable)
plt.figure()
c1_proxy = c1.plot_friction_cone(color='m')
c2_proxy = c2.plot_friction_cone(color='y')
plt.legend([c1_proxy, c2_proxy], ['Cone 1', 'Cone 2'])
plt.show()
IPython.embed()
def getprobability(object, grasps):
obj_name = object[1]
sdf_name = object[2]
obj_mesh = of.ObjFile(obj_name).read()
sdf_ = sf.SdfFile(sdf_name).read()
obj = go.GraspableObject3D(sdf_,
mesh=obj_mesh,
key=object[0].replace("_features.txt", ""),
model_name=obj_name)
config_name = "cfg/correlated.yaml"
config = ec.ExperimentConfig(config_name)
np.random.seed(100)
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
# compute feature vectors for all grasps
feature_extractor = ff.GraspableFeatureExtractor(obj, config)
all_features = feature_extractor.compute_all_features(grasps)
candidates = []
for grasp, features in zip(grasps, all_features):
grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config)
pfc_rv = pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config)
if features is None:
pass
else:
pfc_rv.set_features(features)
candidates.append(pfc_rv)
def phi(rv):
return rv.features
nn = kernels.KDTree(phi=phi)
kernel = kernels.SquaredExponentialKernel(sigma=config['kernel_sigma'],
l=config['kernel_l'],
phi=phi)
objective = objectives.RandomBinaryObjective()
# uniform allocation for true values
ua = das.UniformAllocationMean(objective, candidates)
ua_result = ua.solve(
termination_condition=tc.MaxIterTerminationCondition(brute_force_iter),
snapshot_rate=snapshot_rate)
estimated_pfc = models.BetaBernoulliModel.beta_mean(
ua_result.models[-1].alphas, ua_result.models[-1].betas)
return estimated_pfc
def generate_window_for_figure(width, num_steps, plot=None):
import sdf_file, obj_file
np.random.seed(100)
mesh_file_name = '/mnt/terastation/shape_data/MASTER_DB_v2/SHREC14LSGTB/M000385.obj'
sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v2/SHREC14LSGTB/M000385.sdf'
sdf = sdf_file.SdfFile(sdf_3d_file_name).read()
mesh = obj_file.ObjFile(mesh_file_name).read()
graspable = GraspableObject3D(sdf, mesh)
grasp_axis = np.array([1, 0, 0])
grasp_width = 0.15
grasp1_center = np.array([0, 0, 0.075])
grasp1 = g.ParallelJawPtGrasp3D(grasp1_center, grasp_axis, grasp_width)
_, contacts1 = grasp1.close_fingers(graspable, vis=True)
contact1 = contacts1[0]
grasp2_center = np.array([0, 0, 0.025])
grasp2 = g.ParallelJawPtGrasp3D(grasp2_center, grasp_axis, grasp_width)
_, contacts2 = grasp2.close_fingers(graspable, vis=True)
contact2 = contacts2[0]
grasp3_center = np.array([0, 0.012, -0.089])
grasp3 = g.ParallelJawPtGrasp3D(grasp3_center, grasp_axis, grasp_width)
_, contacts3 = grasp3.close_fingers(graspable, vis=True)
contact3 = contacts3[0]
if plot:
plt.figure()
contact1.plot_friction_cone()
contact2.plot_friction_cone()
print 'aligned projection window'
aligned_window1 = contact1.surface_window_projection(width,
num_steps,
vis=True)
aligned_window2 = contact2.surface_window_projection(width,
num_steps,
vis=True)
aligned_window3 = contact3.surface_window_projection(width,
num_steps,
vis=True)
plt.show()
IPython.embed()
if plot:
plot(sdf_window1, num_steps, 'SDF Window 1')
plot(unaligned_window1, num_steps, 'Unaligned Projection Window 1')
plot(aligned_window1, num_steps, 'Aligned Projection Window 1')
plot(sdf_window2, num_steps, 'SDF Window 2')
plot(unaligned_window2, num_steps, 'Unaligned Projection Window 2')
plot(aligned_window2, num_steps, 'Aligned Projection Window 2')
plt.show()
def test_antipodal_grasp_sampling(vis=False):
np.random.seed(100)
h = plt.figure()
ax = h.add_subplot(111, projection='3d')
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
mesh_name = 'data/test/meshes/Co_clean.obj'
of = obj_file.ObjFile(mesh_name)
m = of.read()
graspable = graspable_object.GraspableObject3D(sdf_3d,
mesh=m,
model_name=mesh_name)
config_file = 'cfg/correlated.yaml'
config = ec.ExperimentConfig(config_file)
sampler = AntipodalGraspSampler(config)
start_time = time.clock()
grasps = sampler.generate_grasps(graspable, vis=False)
end_time = time.clock()
duration = end_time - start_time
logging.info('Antipodal grasp candidate generation took %f sec' %
(duration))
if vis:
plt.close() # lol
for i, grasp in enumerate(grasps, 1):
plt.figure()
ax = plt.gca(projection='3d')
found, (c1, c2) = grasp.close_fingers(graspable)
c1_proxy = c1.plot_friction_cone(color='m')
c2_proxy = c2.plot_friction_cone(color='y')
ax.set_xlim([5, 20])
ax.set_ylim([5, 20])
ax.set_zlim([5, 20])
plt.title('Grasp %d' % (i))
plt.axis('off')
plt.show(block=False)
for angle in range(0, 360, 10):
ax.view_init(elev=5.0, azim=angle)
plt.draw()
plt.close()
def test_find_contacts():
""" Should visually check for reasonable contacts (large green circles) """
sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf'
sf3 = sf.SdfFile(sdf_3d_file_name)
sdf_3d = sf3.read()
# create grasp
test_grasp_center = np.zeros(3)
test_grasp_axis = np.array([1, 0, 0])
test_grasp_width = 1.0
obj_3d = go.GraspableObject3D(sdf_3d)
grasp = ParallelJawPtGrasp3D(test_grasp_center, test_grasp_axis, test_grasp_width)
contact_found, _ = grasp.close_fingers(obj_3d, vis=True)
plt.ioff()
plt.show()
assert(contact_found)
def test_feature_extraction():
# load object
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
mesh_name = 'data/test/meshes/Co_clean.obj'
of = obj_file.ObjFile(mesh_name)
m = of.read()
graspable = go.GraspableObject3D(sdf_3d, mesh=m, model_name=mesh_name)
shot_features_name = 'data/test/features/Co_clean_features.txt'
feature_extractor = SHOTFeatureExtractor()
feature_extractor.extract(graspable, shot_features_name)
IPython.embed()
def test_3d():
sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
# get params from read in sdf
all_points = sdf_3d.pts_
all_sdf = sdf_3d.data_.flatten()
num_points = all_points.shape[0]
num_rand_samples = 5000
# choose a random subset of points for construction
random_ind = np.random.choice(num_points, num_rand_samples)
rand_points = all_points[random_ind, :]
rand_sdf = all_sdf[random_ind].reshape([num_rand_samples, 1])
# create fp
gp = Gpis3D(rand_points, rand_sdf, sdf_3d.dimensions())
gp.scatter()
def grasp_model_figure():
np.random.seed(100)
h = plt.figure()
ax = h.add_subplot(111, projection='3d')
sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v2/Cat50_ModelDatabase/5c7bf45b0f847489181be2d6e974dccd.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
mesh_name = '/mnt/terastation/shape_data/MASTER_DB_v2/Cat50_ModelDatabase/5c7bf45b0f847489181be2d6e974dccd.obj'
of = obj_file.ObjFile(mesh_name)
m = of.read()
clean_mesh_name = 'data/test/meshes/flashlight.obj'
mc = mesh_cleaner.MeshCleaner(m)
mc.rescale_vertices(0.07)
oof = obj_file.ObjFile(clean_mesh_name)
oof.write(mc.mesh())
graspable = graspable_object.GraspableObject3D(
sdf_3d,
mesh=m,
model_name=clean_mesh_name,
tf=stf.SimilarityTransform3D(tfx.identity_tf(), 0.75))
rave.raveSetDebugLevel(rave.DebugLevel.Error)
grasp_checker = OpenRaveGraspChecker()
center = np.array([0, 0.01, 0])
axis = np.array([1, 0, 0])
axis = axis / np.linalg.norm(axis)
width = 0.1
grasp = g.ParallelJawPtGrasp3D(center, axis, width)
rotated_grasps = grasp.transform(graspable.tf, 2.0 * np.pi / 20.0)
print len(rotated_grasps)
grasp_checker.prune_grasps_in_collision(graspable,
rotated_grasps,
auto_step=False,
close_fingers=True,
delay=1)
def generate_sdf(self, path_to_sdfgen, dim, padding):
""" Converts mesh to an sdf object """
# write the mesh to file
of = obj_file.ObjFile(self.obj_filename)
of.write(self.mesh_)
# create the SDF using binary tools
sdfgen_cmd = '%s \"%s\" %d %d' %(path_to_sdfgen, self.obj_filename, dim, padding)
os.system(sdfgen_cmd)
logging.info('SDF Command: %s' %(sdfgen_cmd))
if not os.path.exists(self.sdf_filename):
logging.warning('SDF computation failed for %s' %(self.sdf_filename))
return None
os.system('chmod a+rwx \"%s\"' %(self.sdf_filename) )
# read the generated sdf
sf = sdf_file.SdfFile(self.sdf_filename)
self.sdf_ = sf.read()
return self.sdf_
def test_quality_metrics(vis=True):
np.random.seed(100)
mesh_file_name = 'data/test/meshes/Co_clean.obj'
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
of = obj_file.ObjFile(mesh_file_name)
mesh_3d = of.read()
graspable = go.GraspableObject3D(sdf_3d, mesh=mesh_3d)
z_vals = np.linspace(-0.025, 0.025, 3)
for i in range(z_vals.shape[0]):
print 'Evaluating grasp with z val %f' % (z_vals[i])
grasp_center = np.array([0, 0, z_vals[i]])
grasp_axis = np.array([0, 1, 0])
grasp_width = 0.1
grasp_params = g.ParallelJawPtGrasp3D.configuration_from_params(
grasp_center, grasp_axis, grasp_width)
grasp = g.ParallelJawPtGrasp3D(grasp_params)
qualities = []
metrics = [
'force_closure', 'force_closure_qp', 'min_singular',
'wrench_volume', 'grasp_isotropy', 'ferrari_canny_L1'
]
for metric in metrics:
q = PointGraspMetrics3D.grasp_quality(grasp,
graspable,
metric,
soft_fingers=True)
qualities.append(q)
print 'Grasp quality according to %s: %f' % (metric, q)
if vis:
cf, contacts = grasp.close_fingers(graspable, vis=True)
contacts[0].plot_friction_cone(color='y', scale=-2.0)
contacts[1].plot_friction_cone(color='y', scale=-2.0)
plt.show()
IPython.embed()
def test_3d_transform():
np.random.seed(100)
sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf'
s = sf.SdfFile(sdf_3d_file_name)
sdf_3d = s.read()
# transform
pose_mat = np.matrix([[0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
tf = tfx.transform(pose_mat, from_frame='world')
tf = tfx.random_tf()
tf.position = 0.01 * np.random.rand(3)
start_t = time.clock()
s_tf = stf.SimilarityTransform3D(tf, scale=1.2)
sdf_tf = sdf_3d.transform(s_tf)
end_t = time.clock()
duration = end_t - start_t
logging.info('3D Transform took %f sec' % (duration))
logging.info('Transformed resolution %f' % (sdf_tf.resolution))
start_t = time.clock()
sdf_tf_d = sdf_3d.transform(s_tf, detailed=True)
end_t = time.clock()
duration = end_t - start_t
logging.info('Detailed 3D Transform took %f sec' % (duration))
logging.info('Transformed detailed resolution %f' % (sdf_tf_d.resolution))
# display
plt.figure()
sdf_3d.scatter()
plt.title('Original')
plt.figure()
sdf_tf.scatter()
plt.title('Transformed')
plt.figure()
sdf_tf_d.scatter()
plt.title('Detailed Transformed')
plt.show()
def getgrasps(object):
obj_name = object[1]
sdf_name = object[2]
obj_mesh = of.ObjFile(obj_name).read()
sdf_ = sf.SdfFile(sdf_name).read()
obj = go.GraspableObject3D(sdf_,
mesh=obj_mesh,
key=object[0].replace("_features.txt", ""),
model_name=obj_name)
config_name = "cfg/correlated.yaml"
config = ec.ExperimentConfig(config_name)
np.random.seed(100)
if config['grasp_sampler'] == 'antipodal':
sampler = ags.AntipodalGraspSampler(config)
grasps = sampler.generate_grasps(
obj, check_collisions=config['check_collisions'], vis=False)
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'],
vis=False)
grasps.extend(gaussian_grasps)
else:
sampler = gs.GaussianGraspSampler(config)
grasps = sampler.generate_grasps(
obj,
check_collisions=config['check_collisions'],
vis=False,
grasp_gen_mult=6)
max_num_grasps = config['max_num_grasps']
if len(grasps) > max_num_grasps:
np.random.shuffle(grasps)
grasps = grasps[:max_num_grasps]
return grasps
def test_grasp_poses():
sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v0/amazon_picking_challenge/munchkin_white_hot_duck_bath_toy.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
mesh_name = '/mnt/terastation/shape_data/MASTER_DB_v0/amazon_picking_challenge/munchkin_white_hot_duck_bath_toy.obj'
of = obj_file.ObjFile(mesh_name)
m = of.read()
graspable = graspable_object.GraspableObject3D(sdf_3d,
mesh=m,
model_name=mesh_name)
center = np.array([0, 0, 0.02])
axis = np.array([1, 0, 0])
axis = axis / np.linalg.norm(axis)
grasp = g.ParallelJawPtGrasp3D(center, axis, 0.1)
rotated_grasps = grasp.transform(graspable.tf, 2 * np.pi / 10)
grasp_checker = OpenRaveGraspChecker()
rotated_grasps = grasp_checker.prune_grasps_in_collision(graspable,
rotated_grasps,
auto_step=True)
def test_2d():
sdf_2d_file_name = 'data/test/sdf/medium_black_spring_clamp_optimized_poisson_texture_mapped_mesh_clean_0.csv'
sf2 = sdf_file.SdfFile(sdf_2d_file_name)
sdf_2d = sf2.read()
# get params from read in sdf
all_points = sdf_2d.pts_
all_sdf = sdf_2d.data_.flatten()
num_points = all_points.shape[0]
num_rand_samples = 100
# choose a random subset of points for construction
random_ind = np.random.choice(num_points, num_rand_samples)
rand_points = all_points[random_ind, :]
rand_sdf = all_sdf[random_ind].reshape([num_rand_samples, 1])
# create fp
gp = Gpis2D(rand_points, rand_sdf, sdf_2d.dimensions())
gp.imshow()
gp.gpis_blur()
exit(0)
mean_surf_image = gp.mean_sdf().surface_image_thresh()
plt.figure()
plt.imshow(mean_surf_image, cmap=plt.get_cmap('Greys'))
plt.show()
num_shape_samp = 10
sdf_samples = gp.sample_sdfs(num_shape_samp)
plt.figure()
for i in range(num_shape_samp):
plt.subplot(1,num_shape_samp, i+1)
sdf_samples[i].imshow()
plt.show()
def test_quality_metrics(vis=True):
np.random.seed(100)
mesh_file_name = 'data/test/meshes/Co_clean.obj'
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
of = obj_file.ObjFile(mesh_file_name)
mesh_3d = of.read()
graspable = go.GraspableObject3D(sdf_3d, mesh=mesh_3d)
z_vals = np.linspace(-0.025, 0.025, 3)
for i in range(z_vals.shape[0]):
print 'Evaluating grasp with z val %f' % (z_vals[i])
grasp_center = np.array([0, 0, z_vals[i]])
grasp_axis = np.array([0, 1, 0])
grasp_width = 0.1
grasp = g.ParallelJawPtGrasp3D(grasp_center, grasp_axis, grasp_width)
qualities = []
metrics = [
'force_closure', 'min_singular', 'wrench_volume', 'grasp_isotropy',
'ferrari_canny_L1'
]
for metric in metrics:
q = PointGraspMetrics3D.grasp_quality(grasp,
graspable,
metric,
soft_fingers=True)
qualities.append(q)
print 'Grasp quality according to %s: %f' % (metric, q)
if vis:
grasp.visualize(graspable)
graspable.visualize()
mv.show()
def sdfFromFile(self, file_name):
sf = sdf_file.SdfFile(file_name)
return sf.read()
def test_antipodal_grasp_thompson():
np.random.seed(100)
# h = plt.figure()
# ax = h.add_subplot(111, projection = '3d')
# load object
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sf = sdf_file.SdfFile(sdf_3d_file_name)
sdf_3d = sf.read()
mesh_name = 'data/test/meshes/Co_clean.obj'
of = obj_file.ObjFile(mesh_name)
m = of.read()
graspable = go.GraspableObject3D(sdf_3d, mesh=m, model_name=mesh_name)
config = {
'grasp_width': 0.1,
'friction_coef': 0.5,
'num_cone_faces': 8,
'grasp_samples_per_surface_point': 4,
'dir_prior': 1.0,
'alpha_thresh_div': 32,
'rho_thresh': 0.75, # as pct of object max moment
'vis_antipodal': False,
'min_num_grasps': 20,
'alpha_inc': 1.1,
'rho_inc': 1.1,
'sigma_mu': 0.1,
'sigma_trans_grasp': 0.001,
'sigma_rot_grasp': 0.1,
'sigma_trans_obj': 0.001,
'sigma_rot_obj': 0.1,
'sigma_scale_obj': 0.1,
'num_prealloc_obj_samples': 100,
'num_prealloc_grasp_samples': 0,
'min_num_collision_free_grasps': 10,
'grasp_theta_res': 1
}
sampler = ags.AntipodalGraspSampler(config)
start_time = time.clock()
grasps, alpha_thresh, rho_thresh = sampler.generate_grasps(graspable,
vis=False)
end_time = time.clock()
duration = end_time - start_time
logging.info('Antipodal grasp candidate generation took %f sec' %
(duration))
# convert grasps to RVs for optimization
graspable_rv = GraspableObjectGaussianPose(graspable, config)
f_rv = scipy.stats.norm(config['friction_coef'], config['sigma_mu'])
candidates = []
for grasp in grasps:
grasp_rv = ParallelJawGraspGaussian(grasp, config)
candidates.append(ForceClosureRV(grasp_rv, graspable_rv, f_rv, config))
objective = objectives.RandomBinaryObjective()
# run bandits
eps = 5e-4
ua_tc_list = [tc.MaxIterTerminationCondition(1000)
] #, tc.ConfidenceTerminationCondition(eps)]
ua = das.UniformAllocationMean(objective, candidates)
ua_result = ua.solve(
termination_condition=tc.OrTerminationCondition(ua_tc_list),
snapshot_rate=100)
logging.info('Uniform allocation took %f sec' % (ua_result.total_time))
ts_tc_list = [
tc.MaxIterTerminationCondition(1000),
tc.ConfidenceTerminationCondition(eps)
]
ts = das.ThompsonSampling(objective, candidates)
ts_result = ts.solve(
termination_condition=tc.OrTerminationCondition(ts_tc_list),
snapshot_rate=100)
logging.info('Thompson sampling took %f sec' % (ts_result.total_time))
true_means = models.BetaBernoulliModel.beta_mean(
ua_result.models[-1].alphas, ua_result.models[-1].betas)
# plot results
plt.figure()
plot_value_vs_time_beta_bernoulli(ua_result, true_means, color='red')
plot_value_vs_time_beta_bernoulli(ts_result, true_means, color='blue')
plt.show()
das.plot_num_pulls_beta_bernoulli(ua_result)
plt.title('Observations Per Variable for Uniform allocation')
das.plot_num_pulls_beta_bernoulli(ts_result)
plt.title('Observations Per Variable for Thompson sampling')
plt.show()
def test_windows(width, num_steps, plot=None):
import sdf_file, obj_file
np.random.seed(100)
mesh_file_name = 'data/test/meshes/Co_clean.obj'
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
sdf = sdf_file.SdfFile(sdf_3d_file_name).read()
mesh = obj_file.ObjFile(mesh_file_name).read()
graspable = GraspableObject3D(sdf, mesh)
grasp_axis = np.array([0, 1, 0])
grasp_width = 0.1
grasp1_center = np.array([0, 0, -0.025])
grasp1 = g.ParallelJawPtGrasp3D(
ParallelJawPtGrasp3D.configuration_from_params(grasp1_center,
grasp_axis,
grasp_width))
_, contacts1 = grasp1.close_fingers(graspable)
contact1 = contacts1[0]
grasp2_center = np.array([0, 0, -0.030])
grasp2 = g.ParallelJawPtGrasp3D(
ParallelJawPtGrasp3D.configuration_from_params(grasp2_center,
grasp_axis,
grasp_width))
_, contacts2 = grasp2.close_fingers(graspable)
contact2 = contacts2[0]
if plot:
plt.figure()
contact1.plot_friction_cone()
contact2.plot_friction_cone()
print 'sdf window'
sdf_window1 = contact1.surface_window_sdf(width, num_steps)
sdf_window2 = contact2.surface_window_sdf(width, num_steps)
print 'unaligned projection window'
unaligned_window1 = contact1.surface_window_projection_unaligned(
width, num_steps)
unaligned_window2 = contact2.surface_window_projection_unaligned(
width, num_steps)
print 'aligned projection window'
aligned_window1 = contact1.surface_window_projection(width, num_steps)
aligned_window2 = contact2.surface_window_projection(width, num_steps)
print 'proj, sdf, proj - sdf at contact'
contact_index = num_steps // 2
if num_steps % 2 == 0:
contact_index += 1
contact_index = (contact_index, contact_index)
print aligned_window1[contact_index], sdf_window1[
contact_index], aligned_window1[contact_index] - sdf_window1[
contact_index]
print aligned_window2[contact_index], sdf_window2[
contact_index], aligned_window2[contact_index] - sdf_window2[
contact_index]
if plot:
plot(sdf_window1, num_steps, 'SDF Window 1')
plot(unaligned_window1, num_steps, 'Unaligned Projection Window 1')
plot(aligned_window1, num_steps, 'Aligned Projection Window 1')
plot(sdf_window2, num_steps, 'SDF Window 2')
plot(unaligned_window2, num_steps, 'Unaligned Projection Window 2')
plot(aligned_window2, num_steps, 'Aligned Projection Window 2')
plt.show()
def test_window_correlation(width, num_steps, vis=True):
import scipy
import sdf_file, obj_file
import discrete_adaptive_samplers as das
import experiment_config as ec
import feature_functions as ff
import graspable_object as go # weird Python issues
import kernels
import models
import objectives
import pfc
import termination_conditions as tc
np.random.seed(100)
mesh_file_name = 'data/test/meshes/Co_clean.obj'
sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf'
config = ec.ExperimentConfig('cfg/correlated.yaml')
config['window_width'] = width
config['window_steps'] = num_steps
brute_force_iter = 100
snapshot_rate = config['bandit_snapshot_rate']
sdf = sdf_file.SdfFile(sdf_3d_file_name).read()
mesh = obj_file.ObjFile(mesh_file_name).read()
graspable = go.GraspableObject3D(sdf, mesh)
grasp_axis = np.array([0, 1, 0])
grasp_width = 0.1
grasps = []
for z in [-0.030, -0.035, -0.040, -0.045]:
grasp_center = np.array([0, 0, z])
grasp = g.ParallelJawPtGrasp3D(
ParallelJawPtGrasp3D.configuration_from_params(
grasp_center, grasp_axis, grasp_width))
grasps.append(grasp)
graspable_rv = pfc.GraspableObjectGaussianPose(graspable, config)
f_rv = scipy.stats.norm(config['friction_coef'],
config['sigma_mu']) # friction Gaussian RV
# compute feature vectors for all grasps
feature_extractor = ff.GraspableFeatureExtractor(graspable, config)
all_features = feature_extractor.compute_all_features(grasps)
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)
pfc_rv.set_features(features)
candidates.append(pfc_rv)
if vis:
_, (c1, c2) = grasp.close_fingers(graspable)
plt.figure()
c1_proxy = c1.plot_friction_cone(color='m')
c2_proxy = c2.plot_friction_cone(color='y')
plt.legend([c1_proxy, c2_proxy], ['Cone 1', 'Cone 2'])
plt.title('Grasp %d' % (len(candidates)))
objective = objectives.RandomBinaryObjective()
ua = das.UniformAllocationMean(objective, candidates)
logging.info('Running uniform allocation for true pfc.')
ua_result = ua.solve(
termination_condition=tc.MaxIterTerminationCondition(brute_force_iter),
snapshot_rate=snapshot_rate)
estimated_pfc = models.BetaBernoulliModel.beta_mean(
ua_result.models[-1].alphas, ua_result.models[-1].betas)
print 'true pfc'
print estimated_pfc
def phi(rv):
return rv.features
kernel = kernels.SquaredExponentialKernel(sigma=config['kernel_sigma'],
l=config['kernel_l'],
phi=phi)
print 'kernel matrix'
print kernel.matrix(candidates)
if vis:
plt.show()
