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_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_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 write(self):
"""
Writes stable pose data for meshes in the input directory to an stp file.
path -- path to directory containing meshes to be converted to .stp
"""
min_prob_str = sys.argv[1]
min_prob = float(min_prob_str)
mesh_index = 1
mesh_files = [filename for filename in os.listdir(sys.argv[2]) if filename[-4:] == ".obj"]
for filename in mesh_files:
print "Writing file: " + sys.argv[2] + "/" + filename
ob = obj_file.ObjFile(sys.argv[2] + "/" + filename)
mesh = ob.read()
mesh.remove_unreferenced_vertices()
prob_mapping, cv_hull = st.compute_stable_poses(mesh), mesh.convex_hull()
R_list = []
for face, p in prob_mapping.items():
if p >= min_prob:
vertices = [cv_hull.vertices()[i] for i in face]
basis = st.compute_basis(vertices, cv_hull)
R_list.append([p, basis])
self.write_mesh_stable_poses(mesh, filename, min_prob)
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 make_image(self, filename, rot):
proj_img = self.project_binary(cp, T)
file_root, file_ext = os.path.splitext(filename)
proj_img.save(file_root + ".jpg")
oof = obj_file.ObjFile(filename)
oof.write(self)
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 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 obj_mesh_filename(self, key, scale=1.0, output_dir=None):
""" Writes an obj file in the database "cache" directory and returns the path to the file """
mesh = hfact.Hdf5ObjectFactory.mesh_3d(self.mesh_data(key))
mesh.rescale(scale)
if output_dir is None:
output_dir = self.cache_dir_
obj_filename = os.path.join(output_dir, key + OBJ_EXT)
of = obj_file.ObjFile(obj_filename)
of.write(mesh)
return obj_filename
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 assign_components(path):
"""
Sets values for component attribute of all meshes in an input directory.
Utilizes breadth first search to determine connected components and assign values accordingly.
Returns a dictionary mapping component numbers to indexes of all the vertices contained in that component.
path -- path to directory containing mesh objects as .obj
"""
mesh_files = [filename for filename in os.listdir(path) if filename[-4:] == ".obj"]
mesh_components_list = []
for filename in mesh_files:
print "Assigning components: " + path + "/" + filename
ob = obj_file.ObjFile(path + "/" + filename)
mesh = ob.read()
mesh.remove_unreferenced_vertices()
# generate graph and connect nodes appropriately
nodes = []
for triangle in mesh.triangles():
node_0 = Node(triangle[0])
node_1 = Node(triangle[1])
node_2 = Node(triangle[2])
node_0.connect_nodes(node_1)
node_1.connect_nodes(node_2)
node_2.connect_nodes(node_0)
nodes.append(node_0)
nodes.append(node_1)
nodes.append(node_2)
# breadth-first-search to mark elements of each component
component_counter = 0
component_to_index = {}
while nodes != []:
q = Queue.Queue()
q.put(nodes[0])
while (q.qsize() != 0):
curr = q.get()
curr.visited = True;
if curr in nodes:
nodes.remove(curr)
else:
continue
for child in curr.children:
if not child.visited:
q.put(child)
if component_counter in component_to_index:
if curr.index not in component_to_index[component_counter]:
component_to_index[component_counter].append(curr.index)
else:
component_to_index[component_counter] = [curr.index]
component_counter += 1
mesh_components_list.append(component_to_index)
return mesh_components_list
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 extract_mesh(filename, obj_filename, script_to_apply):
meshlabserver_cmd = 'meshlabserver -i \"%s\" -o \"%s\"' % (
filename, obj_filename)
# meshlabserver_cmd = 'meshlabserver -i \"%s\" -o \"%s\" -s \"%s\"' %(filename, obj_filename, script_to_apply)
os.system(meshlabserver_cmd)
print 'MeshlabServer Command:', meshlabserver_cmd
if not os.path.exists(obj_filename):
print 'Meshlab conversion failed for', obj_filename
return
of = obj_file.ObjFile(obj_filename)
return of.read()
def test_fanuc_gripper():
mesh_filename = '/home/jmahler/jeff_working/GPIS/data/grippers/fanuc_lehf/gripper.obj'
of = objf.ObjFile(mesh_filename)
gripper_mesh = of.read()
gripper_mesh.center_vertices_bb()
oof = objf.ObjFile(mesh_filename)
oof.write(gripper_mesh)
T_mesh_world = stf.SimilarityTransform3D(pose=tfx.pose(np.eye(4)), from_frame='world', to_frame='mesh')
R_grasp_gripper = np.array([[0, 0, 1],
[0, -1, 0],
[1, 0, 0]])
R_mesh_gripper = np.array([[0, 1, 0],
[-1, 0, 0],
[0, 0, 1]])
t_mesh_gripper = np.array([0.0, 0.0, 0.065])
T_mesh_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_mesh_gripper, t_mesh_gripper),
from_frame='gripper', to_frame='mesh')
T_gripper_world = T_mesh_gripper.inverse().dot(T_mesh_world)
T_grasp_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_grasp_gripper), from_frame='gripper', to_frame='grasp')
T_grasp_world = T_grasp_gripper.dot(T_gripper_world)
#T_mesh_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/fanuc_lehf/T_mesh_gripper.stf')
#T_grasp_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/fanuc_lehf/T_grasp_gripper.stf')
gripper_params = {}
gripper_params['min_width'] = 0.015
gripper_params['max_width'] = 0.048
#f = open('/home/jmahler/jeff_working/GPIS/data/grippers/fanuc_lehf/params.json', 'w')
#json.dump(gripper_params, f)
MayaviVisualizer.plot_pose(T_mesh_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005)
#MayaviVisualizer.plot_pose(T_gripper_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005)
MayaviVisualizer.plot_pose(T_grasp_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005)
MayaviVisualizer.plot_mesh(gripper_mesh, T_mesh_world, style='surface', color=(1,1,1))
mv.axes()
def test_new_feature_matching():
a_features_filename = "data/test/features/pepper_orig_features.txt"
b_features_filename = "data/test/features/pepper_tf_features.txt"
a_mesh_filename = "data/test/features/pepper_orig.obj"
b_mesh_filename = "data/test/features/pepper_tf.obj"
# read data
a_feat_file = ff.LocalFeatureFile(a_features_filename)
a_features = a_feat_file.read()
b_feat_file = ff.LocalFeatureFile(b_features_filename)
b_features = b_feat_file.read()
a_obj_file = of.ObjFile(a_mesh_filename)
a_mesh = a_obj_file.read()
b_obj_file = of.ObjFile(b_mesh_filename)
b_mesh = b_obj_file.read()
# match features
feat_matcher = fm.RawDistanceFeatureMatcher()
ab_corrs = feat_matcher.match(a_features, b_features)
reg_solver = reg.RigidRegistrationSolver()
tf = reg_solver.register(ab_corrs)
# compare with true transform
tf_true = np.loadtxt("data/test/features/tf_true.txt", delimiter=" ")
delta_tf = tf.dot(np.linalg.inv(tf_true))
print 'Estimated TF'
print tf
print 'True TF'
print tf_true
print 'Delta TF'
print delta_tf
def main():
#hand_param = {'max_open': .0445, 'center_distance': .0381}
hand_param = {
'max_open': 0.08,
'min_open': 0.0425,
'center_distance': 0.0381
}
# read from mesh
of = obj_file.ObjFile(MESH_FILENAME)
mesh = of.read()
# file I/O stuff
# call fc_to_hand_pose for every FC point
# save list of all grasps
in_f = open(FC_DATA_FILENAME, 'r')
data = in_f.read()
data = data.split('\n')
data.pop(0)
data.pop()
# fc_points structure: list of tuples, with each tuple being one fc pair
# (i, j, [xi, yi, zi], [xj, yj, zj])
# i and j are 0 indexed
fc_points = list()
for row in data:
fc_points.append(tuple([eval(e) for e in row.split(';')]))
in_f.close()
### WESLEY DOES MAGIC HERE ###
# grasp_list structure: list of tuples, with each tuple being one grasp
# (RBT, c1, c2, dist(c1, c2))
grasp_list = []
for i in fc_points:
valid_grasps = fc_to_hand_pose(i[2], i[3], mesh, hand_param)
if valid_grasps is not None:
grasp_list.extend(valid_grasps)
out_f = open(GRASP_DATA_FILENAME, 'w')
out_f.write('rbt; c1; c2; dist\n')
for g in grasp_list:
rbt, c1, c2, d = g
out_f.write(str(list(rbt.flatten())) + '; ')
out_f.write(str(list(c1.flatten())) + '; ')
out_f.write(str(list(c2.flatten())) + '; ')
out_f.write(str(d) + '\n')
out_f.close()
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 load_mesh(self, script_to_apply=None):
""" Loads the mesh from the file by first converting to an obj and then loading """
# convert to an obj file using meshlab
if script_to_apply is None:
meshlabserver_cmd = 'meshlabserver -i \"%s\" -o \"%s\"' %(self.filename, self.obj_filename)
else:
meshlabserver_cmd = 'meshlabserver -i \"%s\" -o \"%s\" -s \"%s\"' %(self.filename, self.obj_filename, script_to_apply)
os.system(meshlabserver_cmd)
logging.info('MeshlabServer Command: %s' %(meshlabserver_cmd))
if not os.path.exists(self.obj_filename):
raise ValueError('Meshlab conversion failed for %s' %(self.obj_filename))
# read mesh from obj file
of = obj_file.ObjFile(self.obj_filename)
self.mesh_ = of.read()
return self.mesh_
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 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 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_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 transform_meshes(path):
"""
Transforms meshes in the directory given by path based on their highest probability stable poses.
path -- path leading to directory containing meshes
num_poses -- variable denoting number of desired poses per object
"""
f = open()
# write header
f.write('###########################################################\n')
f.write('# POSE TRANSFORM DATA file generated by UC Berkeley Automation Sciences Lab\n')
f.write('#\n')
min_prob = sys.argv[1]
for filename in os.listdir(path):
if filename[-4:] == ".obj":
ob = obj_file.ObjFile(path + "/" + filename)
mesh = ob.read()
mesh.remove_unreferenced_vertices()
prob_mapping, cv_hull = compute_stable_poses(mesh), mesh.convex_hull()
R_list = []
for face, p in prob_mapping.items():
if p >= min_prob:
R_list.append([p, compute_basis([cv_hull.vertices()[i] for i in face])])
# write necessary data to compute each pose (R and pose probability)
for i in range(len(R_list)):
f.write('# Pose ' + str(i + 1) + ':\n')
f.write('# Probability: ' + str(R_list[i][0]) + '\n')
for p in range(3):
f.write(str(R_list[i][1][p][0]) + ' ' + str(R_list[i][1][p][1]) + ' ' + str(R_list[i][1][p][2]) + '\n')
f.write('#\n')
f.write('###########################################################\n')
f.write('\n')
import os
import numpy as np
import sys
import obj_file
import mesh
import tfx
import IPython
argc = len(sys.argv)
filename = sys.argv[1]
file_root, file_ext = os.path.splitext(filename)
of = obj_file.ObjFile(filename)
m = of.read()
tf = tfx.random_tf()
v_array = np.array(m.get_vertices())
w_array = np.array(m.get_vertices())
w_list = []
for i in range(v_array.shape[0]):
w = np.array(tf.apply(v_array[i, :]))
w = w.T[0]
w_array[i, :] = w
w_list.append([w[0], w[1], w[2]])
m_t = mesh.Mesh(w_list, m.get_triangles())
new_filename = file_root + '_tf' + file_ext
def startpipeline(self):
found_objects = getobjects(self.objectlist)
tps_x = []
tps_y = []
rigid_x = []
rigid_y = []
line_x = [0, 1]
line_y = [0, 1]
for object in found_objects:
a_features = get_feature(object)
grasplist = getgrasps(object)
try:
grasp_probability = getprobability(object, grasplist)
#IPython.embed()
for other_obj in found_objects:
if other_obj == object:
continue
print other_obj[0], " vs. ", object[0]
b_features = get_feature(other_obj)
feat_matcher = fm.RawDistanceFeatureMatcher()
ab_corrs = feat_matcher.match(a_features, b_features)
reg_solver2 = reg.SimilaritytfSolver()
reg_solver2.register(ab_corrs)
reg_solver2.add_source_mesh(of.ObjFile(object[1]).read())
reg_solver2.scale(of.ObjFile(other_obj[1]).read())
reg_solver = reg.tpsRegistrationSolver(1, 1e-4)
reg_solver.register(ab_corrs)
grasplist_tf = []
grasplist_tps = []
for grasp in grasplist:
targetgrasp = transformgrasp(grasp, reg_solver2)
grasplist_tf.append(targetgrasp)
grasplist_tps.append(transformgrasp(grasp, reg_solver))
try:
tps_probability = getprobability(
other_obj, grasplist_tps)
if len(tps_probability) <= len(grasp_probability):
temp_tps = np.zeros(len(grasp_probability))
for i, probability in enumerate(tps_probability):
temp_tps[i] = probability
tps_x.extend(grasp_probability.tolist())
tps_y.extend(temp_tps.tolist())
else:
temp_og = np.zeros(len(tps_probability))
for i, probability in enumerate(grasp_probability):
temp_og[i] = probability
tps_x.extend(temp_og.tolist())
tps_y.extend(tps_probability.tolist())
except ValueError:
pass
try:
rigid_probability = getprobability(
other_obj, grasplist_tf)
if len(rigid_probability) <= len(grasp_probability):
temp_tf = np.zeros(len(grasp_probability))
for i, probability in enumerate(rigid_probability):
temp_tf[i] = probability
rigid_x.extend(grasp_probability.tolist())
rigid_y.extend(temp_tf.tolist())
else:
temp_og = np.zeros(len(rigid_probability))
for i, probability in enumerate(grasp_probability):
temp_og[i] = probability
rigid_x.extend(temp_og.tolist())
rigid_y.extend(rigid_probability.tolist())
except ValueError:
pass
#mv.show()
#tempobj=of.ObjFile(other_obj[1]).read()
#tempobj.add_sdf(sf.SdfFile(other_obj[2]).read())
#tempobj.visualize()
#grasplist_tf[0].visualize(tempobj)
#time.sleep(200)
#return None
except ValueError:
continue
plt.title('PFC of Untransformed Grasp vs. PFC of Transformed Grasp')
plt.subplot(121)
plt.ylabel("Transformed Grasp PFC")
plt.xlabel("Untransformed Grasp PFC")
plt.plot(line_x, line_y)
plt.scatter(tps_x, tps_y)
plt.subplot(122)
plt.xlabel("Untransformed Grasp PFC")
plt.plot(line_x, line_y)
plt.scatter(rigid_x, rigid_y)
plt.savefig("grasp_transfer_results.png", dpi=100)
plt.show()
z_axis = np.array([y_axis[1], -y_axis[0], 0]) # the z axis will always be in the table plane for now
z_axis = z_axis / np.linalg.norm(z_axis)
x_axis = np.cross(y_axis, z_axis)
# convert to hand pose
R_obj_gripper = np.array([x_axis, y_axis, z_axis]).T
t_obj_gripper = center
T_obj_gripper = np.eye(4)
T_obj_gripper[:3,:3] = R_obj_gripper
T_obj_gripper[:3,3] = t_obj_gripper
q_obj_gripper = transformations.quaternion_from_matrix(T_obj_gripper)
return t_obj_gripper, q_obj_gripper
if __name__ == '__main__':
of = obj_file.ObjFile(MESH_FILENAME)
mesh = of.read()
mesh.tri_normals()
vertices = mesh.tri_centers()
triangles = mesh.triangles
# Manually flip normals based on what gives results
normals = mesh.normals
# normals = [[-n[0], -n[1], -n[2]] for n in mesh.normals]
print 'Num vertices:', len(vertices)
print 'Num triangles:', len(triangles)
print 'Num normals:', len(normals)
# 1. Generate candidate pairs of contact points
pairs = []
def test_yumi_gripper():
# build new mesh
finger_filename = '/home/jmahler/jeff_working/GPIS/data/grippers/yumi/finger.obj'
base_filename = '/home/jmahler/jeff_working/GPIS/data/grippers/yumi/base.obj'
of = objf.ObjFile(finger_filename)
finger1_mesh = of.read()
finger2_mesh = copy.copy(finger1_mesh)
of = objf.ObjFile(base_filename)
base_mesh = of.read()
R_finger1_world = np.eye(3)
t_finger1_world = np.array([-0.025, -0.005, 0.082])
T_finger1_world = stf.SimilarityTransform3D(pose=tfx.pose(R_finger1_world,
t_finger1_world),
from_frame='mesh', to_frame='world')
R_finger2_world = np.array([[-1,0,0],
[0,-1,0],
[0,0,1]])
t_finger2_world = np.array([0.025, 0.005, 0.082])
T_finger2_world = stf.SimilarityTransform3D(pose=tfx.pose(R_finger2_world,
t_finger2_world),
from_frame='mesh', to_frame='world')
T_mesh_world = stf.SimilarityTransform3D(pose=tfx.pose(np.eye(4)), from_frame='world', to_frame='mesh')
finger1_mesh = finger1_mesh.transform(T_finger1_world)
finger2_mesh = finger2_mesh.transform(T_finger2_world)
offset = 0
vertices = []
triangles = []
meshes = [base_mesh, finger1_mesh, finger2_mesh]
for i, mesh in enumerate(meshes):
vertices.extend(mesh.vertices())
offset_tris = [[t[0]+offset,t[1]+offset,t[2]+offset] for t in mesh.triangles()]
triangles.extend(offset_tris)
offset += len(mesh.vertices())
gripper_mesh = m.Mesh3D(vertices, triangles)
gripper_mesh.center_vertices_bb()
of = objf.ObjFile('/home/jmahler/jeff_working/GPIS/data/grippers/yumi/gripper.obj')
#of.write(gripper_mesh)
# frames of reference
R_grasp_gripper = np.array([[0, 0, -1],
[0, 1, 0],
[1, 0, 0]])
R_mesh_gripper = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
t_mesh_gripper = np.array([0.0, -0.002, 0.058])
T_mesh_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_mesh_gripper, t_mesh_gripper),
from_frame='gripper', to_frame='mesh')
T_gripper_world = T_mesh_gripper.inverse().dot(T_mesh_world)
T_grasp_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_grasp_gripper), from_frame='gripper', to_frame='grasp')
#T_mesh_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/yumi/T_mesh_gripper.stf')
#T_grasp_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/yumi/T_grasp_gripper.stf')
gripper_params = {}
gripper_params['min_width'] = 0.0
gripper_params['max_width'] = 0.05
#f = open('/home/jmahler/jeff_working/GPIS/data/grippers/yumi/params.json', 'w')
#json.dump(gripper_params, f)
MayaviVisualizer.plot_mesh(gripper_mesh, T_mesh_world, style='surface', color=(0,0,1))
MayaviVisualizer.plot_pose(T_gripper_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005)
mv.axes()