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 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_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 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_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 grasps(data):
""" Return a list of grasp objects from the data provided in the HDF5 dictionary """
# need to read in a bunch of grasps but also need to know what kind of grasp it is
grasps = []
num_grasps = data.attrs[NUM_GRASPS_KEY]
for i in range(num_grasps):
# get the grasp data y'all
grasp_key = GRASP_KEY + '_' + str(i)
if grasp_key in data.keys():
grasp_id = data[grasp_key].attrs[GRASP_ID_KEY]
grasp_type = data[grasp_key].attrs[GRASP_TYPE_KEY]
configuration = data[grasp_key].attrs[GRASP_CONFIGURATION_KEY]
frame = data[grasp_key].attrs[GRASP_RF_KEY]
timestamp = data[grasp_key].attrs[GRASP_TIMESTAMP_KEY]
# create object based on type
g = None
if grasp_type == 'ParallelJawPtGrasp3D':
g = grasp.ParallelJawPtGrasp3D(configuration=configuration,
frame=frame,
timestamp=timestamp,
grasp_id=grasp_id)
grasps.append(g)
else:
logging.debug('Grasp %s is corrupt. Skipping' % (grasp_key))
return grasps
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 sample(self, size=1):
samples = []
for i in range(size):
# sample random pose
xi = self.r_xi_rv_.rvs(size=1)
S_xi = skew(xi)
v = scipy.linalg.expm(S_xi).dot(self.grasp_.axis)
t = self.t_rv_.rvs(size=1)
# transform object by pose
#grasp_sample = copy.copy(self.grasp_)
grasp_sample = gr.ParallelJawPtGrasp3D(t, v,
self.grasp_.grasp_width)
samples.append(grasp_sample)
if size == 1:
return samples[0]
return samples
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_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 sample(self, size=1):
samples = []
for i in range(size):
# sample random pose
xi = self.r_xi_rv_.rvs(size=1)
S_xi = skew(xi)
axis_sigma = self.R_sample_sigma_.T.dot(self.grasp_.axis)
v = self.R_sample_sigma_.dot(
scipy.linalg.expm(S_xi).dot(axis_sigma))
t = self.R_sample_sigma_.dot(self.t_rv_.rvs(size=1).T).T
# transform object by pose
grasp_sample = gr.ParallelJawPtGrasp3D(
gr.ParallelJawPtGrasp3D.configuration_from_params(
t, v, self.grasp_.grasp_width))
samples.append(grasp_sample)
if size == 1:
return samples[0]
return samples
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 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()
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()
