def test_grasp_quality_functions(self):
num_grasps = NUM_TEST_CASES
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = UniformGraspSampler(gripper, CONFIG)
grasps = ags.generate_grasps(obj, target_num_grasps=num_grasps)
# test with grasp quality function
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['force_closure'])
quality_fn = GraspQualityFunctionFactory.create_quality_function(
obj, quality_config)
for i, grasp in enumerate(grasps):
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
fn_fc = quality_fn(grasp).quality
true_fc = PointGraspMetrics3D.force_closure(
c1, c2, quality_config.friction_coef)
self.assertEqual(fn_fc, true_fc)
def test_force_closure(self):
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
for i in range(NUM_TEST_CASES):
contacts, normals, _, mu, _ = random_force_closure_test_case(
antipodal=True)
c1 = Contact3D(obj, contacts[:, 0])
c1.normal = normals[:, 0]
c2 = Contact3D(obj, contacts[:, 1])
c2.normal = normals[:, 1]
self.assertTrue(
PointGraspMetrics3D.force_closure(c1,
c2,
mu,
use_abs_value=False))
for i in range(NUM_TEST_CASES):
contacts, normals, _, mu, _ = random_force_closure_test_case(
antipodal=False)
c1 = Contact3D(obj, contacts[:, 0])
c1.normal = normals[:, 0]
c2 = Contact3D(obj, contacts[:, 1])
c2.normal = normals[:, 1]
self.assertFalse(
PointGraspMetrics3D.force_closure(c1,
c2,
mu,
use_abs_value=False))
def open_npy_and_obj(name_to_open_):
"""从路径指定的npy文件中读取并返回
npy_m_: 抓取序列（只是针对某单个物体的抓取序列）
obj_: 待抓取物体的模型mesh（单个模型）
ply_name_: 待抓取物体的路径
object_name_: 待抓取物体的名称
"""
npy_m_ = np.load(name_to_open_)
file_dir = home_dir + "/PointNetGPD/data/ycb_meshes_google/"
object_name_ = name_to_open_.split("/")[-1][:-4]
#object_name_=object_name_.split("_")[-1][:-3]
print(object_name_)
ply_name_ = file_dir + object_name_ + "/google_512k/nontextured.ply"
if not check_pcd_grasp_points:
of = ObjFile(file_dir + object_name_ + "/google_512k/nontextured.obj")
sf = SdfFile(file_dir + object_name_ + "/google_512k/nontextured.sdf")
mesh = of.read()
sdf = sf.read()
obj_ = GraspableObject3D(sdf, mesh)
else:
cloud_path = home_dir + "/dataset/ycb_rgbd/" + object_name_ + "/clouds/"
pcd_files = glob.glob(cloud_path + "*.pcd")
obj_ = pcd_files
obj_.sort()
return npy_m_, obj_, ply_name_, object_name_
def test_contacts(self):
num_samples = 128
mu = 0.5
num_faces = 8
num_grasps = NUM_TEST_CASES
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = UniformGraspSampler(gripper, CONFIG)
grasps = ags.generate_grasps(obj, target_num_grasps=num_grasps)
for grasp in grasps:
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
# test friction cones
fc1_exists, fc1, n1 = c1.friction_cone(num_cone_faces=num_faces,
friction_coef=mu)
if fc1_exists:
self.assertEqual(fc1.shape[1], num_faces)
alpha = np.tile(fc1.T.dot(c1.normal), [3,1]).T
w = np.tile(c1.normal.reshape(3,1), [1,8]).T
tan_vecs = fc1.T - alpha * w
self.assertTrue(np.allclose(np.linalg.norm(tan_vecs, axis=1), mu))
fc2_exists, fc2, n2 = c2.friction_cone(num_cone_faces=num_faces,
friction_coef=mu)
if fc2_exists:
self.assertEqual(fc2.shape[1], num_faces)
alpha = np.tile(fc2.T.dot(c2.normal), [3,1]).T
w = np.tile(c2.normal.reshape(3,1), [1,8]).T
tan_vecs = fc2.T - alpha * w
self.assertTrue(np.allclose(np.linalg.norm(tan_vecs, axis=1), mu))
# test reference frames
T_contact1_obj = c1.reference_frame(align_axes=True)
self.assertTrue(np.allclose(T_contact1_obj.z_axis, c1.in_direction))
self.assertTrue(np.allclose(T_contact1_obj.translation, c1.point))
for i in range(num_samples):
theta = 2 * i * np.pi / num_samples
v = np.cos(theta) * T_contact1_obj.x_axis + np.sin(theta) * T_contact1_obj.y_axis
self.assertLessEqual(v[0], T_contact1_obj.x_axis[0])
T_contact2_obj = c2.reference_frame(align_axes=True)
self.assertTrue(np.allclose(T_contact2_obj.z_axis, c2.in_direction))
self.assertTrue(np.allclose(T_contact2_obj.translation, c2.point))
for i in range(num_samples):
theta = 2 * i * np.pi / num_samples
v = np.cos(theta) * T_contact2_obj.x_axis + np.sin(theta) * T_contact2_obj.y_axis
self.assertLessEqual(v[0], T_contact2_obj.x_axis[0])
def grasp_quality_calculator(mesh, sdf, grasps, metric):
obj = GraspableObject3D(sdf, mesh)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics'][metric])
quality_function = GraspQualityFunctionFactory.create_quality_function(
obj, quality_config)
metrics = []
for grasp in grasps:
quality = quality_function.quality(grasp)
metrics.append(copy.deepcopy(quality.quality))
return metrics
def load_mesh(mesh_id, config, rescale_mesh = False):
# set up filepath from mesh id (this is where the service dumps the mesh
filepath = os.path.join(consts.MESH_CACHE_DIR, mesh_id) + '.obj'
# Initialize mesh processor.
mesh_processor = mp.MeshProcessor(filepath, consts.MESH_CACHE_DIR)
# Run through MP steps manually to make things easier
mesh_processor._load_mesh()
mesh_processor.mesh_.density = config['obj_density']
# _clean_mesh
mesh_processor._remove_bad_tris()
mesh_processor._remove_unreferenced_vertices()
# # standardize pose, recover transform
# verts_old = mesh_processor.mesh_.vertices.copy()
# mesh_processor._standardize_pose()
# verts_new = mesh_processor.mesh_.vertices
# # Transform recovery
# MAT_SIZE = min(verts_old.shape[0], 300)
# tmat_rec = np.dot(np.linalg.pinv(np.hstack((verts_old[:MAT_SIZE], np.ones((MAT_SIZE, 1)) ))),
# np.hstack((verts_new[:MAT_SIZE], np.ones((MAT_SIZE, 1)) ))).T
# rotation = tmat_rec[:3, :3]
# translation = tmat_rec[:3, 3]
# transform = RigidTransform(rotation=rotation, translation=translation)
# scale = 1.0
if rescale_mesh: # config['rescale_objects'] <- local config, current use case is pass in as arg
mesh_processor._standardize_pose()
mesh_processor._rescale_vertices(config['obj_target_scale'], config['obj_scaling_mode'], config['use_uniform_com'])
mesh_processor.sdf_ = None
if config['generate_sdf']:
mesh_processor._generate_sdf(config['path_to_sdfgen'], config['sdf_dim'], config['sdf_padding'])
mesh_processor._generate_stable_poses(config['stp_min_prob'])
mesh, sdf, stable_poses = (mesh_processor.mesh, mesh_processor.sdf, mesh_processor.stable_poses,)
# Make graspable
graspable = GraspableObject3D(sdf = sdf,
mesh = mesh,
key = mesh_id,
model_name = mesh_processor.obj_filename,
mass = config['default_mass'],
convex_pieces = None)
# resave mesh to the proc file because the new CoM thing translates the mesh
ObjFile(os.path.join(consts.MESH_CACHE_DIR, mesh_id) + '_proc.obj').write(graspable.mesh)
return graspable, stable_poses
def open_npy_and_obj(name_to_open_):
npy_m_ = np.load(name_to_open_)
file_dir = home_dir + "/PointNetGPD/data/ycb-tools/models/ycb/"
object_name_ = name_to_open_.split("/")[-1][:-4]
ply_name_ = file_dir + object_name_ + "/google_512k/nontextured.ply"
if not check_pcd_grasp_points:
of = ObjFile(file_dir + object_name_ + "/google_512k/nontextured.obj")
sf = SdfFile(file_dir + object_name_ + "/google_512k/nontextured.sdf")
mesh = of.read()
sdf = sf.read()
obj_ = GraspableObject3D(sdf, mesh)
else:
cloud_path = home_dir + "/dataset/ycb_rgbd/" + object_name_ + "/clouds/"
pcd_files = glob.glob(cloud_path + "*.pcd")
obj_ = pcd_files
obj_.sort()
return npy_m_, obj_, ply_name_, object_name_
def test_antipodal_grasp_sampler(self):
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = AntipodalGraspSampler(gripper, CONFIG)
grasps = ags.generate_grasps(obj, target_num_grasps=NUM_TEST_CASES)
# test with raw force closure function
for i, grasp in enumerate(grasps):
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
self.assertTrue(PointGraspMetrics3D.force_closure(c1, c2, CONFIG['sampling_friction_coef']))
def test_find_contacts(self):
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
surface_points, _ = obj.sdf.surface_points(grid_basis=False)
indices = np.arange(surface_points.shape[0])
for i in range(NUM_TEST_CASES):
np.random.shuffle(indices)
c1 = surface_points[0,:]
c2 = surface_points[1,:]
w = np.linalg.norm(c1 - c2) + 1e-2
g = ParallelJawPtGrasp3D.grasp_from_endpoints(c1, c2, width=w)
success, c = g.close_fingers(obj)
if success:
c1_est, c2_est = c
np.assertTrue(np.allclose(c1, c1_est, atol=1e-3, rtol=0.1))
np.assertTrue(np.allclose(c2, c2_est, atol=1e-3, rtol=0.1))
def open_pickle_and_obj(name_to_open_):
pickle_names_ = get_pickle_file_name(
home_dir + "/code/grasp-pointnet/dex-net/apps/generated_grasps")
suggestion_pickle = fuzzy_finder(name_to_open_, pickle_names_)
if len(suggestion_pickle) != 1:
print("Pickle file suggestions:", suggestion_pickle)
exit("Name error for pickle file!")
pickle_m_ = pickle.load(open(suggestion_pickle[0], 'rb'))
file_dir = home_dir + "/dataset/ycb_meshes_google/objects"
file_list_all = get_file_name(file_dir)
new_sug = re.findall(r'_\d+', suggestion_pickle[0], flags=0)
new_sug = new_sug[0].split('_')
new_sug = new_sug[1]
suggestion = fuzzy_finder(new_sug, file_list_all)
# very dirty way to support name with "-a, -b etc."
if len(suggestion) != 1:
new_sug = re.findall(r'_\d+\W\w', suggestion_pickle[0], flags=0)
new_sug = new_sug[0].split('_')
new_sug = new_sug[1]
suggestion = fuzzy_finder(new_sug, file_list_all)
if len(suggestion) != 1:
exit("Name error for obj file!")
object_name_ = suggestion[0][len(file_dir) + 1:]
ply_name_ = suggestion[0] + "/google_512k/nontextured.ply"
if not check_pcd_grasp_points:
of = ObjFile(suggestion[0] + "/google_512k/nontextured.obj")
sf = SdfFile(suggestion[0] + "/google_512k/nontextured.sdf")
mesh = of.read()
sdf = sf.read()
obj_ = GraspableObject3D(sdf, mesh)
else:
cloud_path = home_dir + "/code/grasp-pointnet/pointGPD/data/ycb_rgbd/" + object_name_ + "/clouds/"
pcd_files = glob.glob(cloud_path + "*.pcd")
obj_ = pcd_files
obj_.sort()
return pickle_m_, obj_, ply_name_, object_name_
def antipodal_grasp_sampler(self):
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = AntipodalGraspSampler(gripper, CONFIG)
grasps = ags.generate_grasps(obj, target_num_grasps=10)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['force_closure'])
quality_fn = GraspQualityFunctionFactory.create_quality_function(
obj, quality_config)
q_to_c = lambda quality: CONFIG['quality_scale']
i = 0
vis.figure()
vis.mesh(obj.mesh.trimesh, style='surface')
for grasp in grasps:
print(grasp.frame)
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
fn_fc = quality_fn(grasp).quality
true_fc = PointGraspMetrics3D.force_closure(
c1, c2, quality_config.friction_coef)
#print(grasp)
T_obj_world = RigidTransform(from_frame='obj', to_frame='world')
color = plt.get_cmap('hsv')(q_to_c(CONFIG['metrics']))[:-1]
T_obj_gripper = grasp.gripper_pose(gripper)
#vis.grasp(grasp,grasp_axis_color=color,endpoint_color=color)
i += 1
#T_obj_world = RigidTransform(from_frame='obj',to_frame='world')
vis.show(False)
def open_pickle_and_obj(object_to_display):
pickle_names_ = get_pickle_file_name(grasps_dir)
suggestion_pickle = fuzzy_finder(object_to_display['object_name'],
pickle_names_)
if len(suggestion_pickle) != 1:
print("Pickle file suggestions:", suggestion_pickle)
exit("Name error for pickle file!")
pickle_m_ = pickle.load(open(suggestion_pickle[0], 'rb'))
object_name_ = object_to_display['object_name']
ply_name_ = object_to_display['obj_file'].replace('.obj', '.stl')
if not check_pcd_grasp_points:
of = ObjFile(object_to_display['obj_file'])
sf = SdfFile(object_to_display['sdf_file'])
mesh = of.read()
sdf = sf.read()
obj_ = GraspableObject3D(sdf, mesh)
else:
cloud_path = home_dir + "/code/grasp-pointnet/pointGPD/data/ycb_rgbd/" + object_name_ + "/clouds/"
pcd_files = glob.glob(cloud_path + "*.pcd")
obj_ = pcd_files
obj_.sort()
return pickle_m_, obj_, ply_name_, object_name_
def antipodal_grasp_sampler_for_storing_stable_poses(mesh, sdf, stable_poses):
mass = 1.0
CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
ags = AntipodalGraspSampler(gripper, CONFIG)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['force_closure'])
quality_function = GraspQualityFunctionFactory.create_quality_function(
obj, quality_config)
max_poses = len(stable_poses)
grasps = [None] * max_poses
metrics = [None] * max_poses
all_grasps = ags.generate_grasps(obj, target_num_grasps=200, max_iter=4)
for id, stable_pose in enumerate(stable_poses):
print('sampling for stable pose: ', id)
if id == max_poses:
break
grasps_pose = filter(
lambda x: grasp_is_parallel_to_table(x.axis, stable_pose.r),
all_grasps)
#grasps_pose = ags.generate_grasps(obj,target_num_grasps=20, max_iter=5, stable_pose=stable_pose.r)
grasps[id] = []
metrics[id] = []
for grasp in grasps_pose:
quality = quality_function.quality(grasp)
#quality = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config)
grasps[id].append(copy.deepcopy(grasp))
metrics[id].append(copy.deepcopy(quality.quality))
return grasps, metrics
def antipodal_grasp_sampler(visual=False, debug=False):
mass = 1.0
CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE
mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir'])
mesh_processor.generate_graspable(CONFIG)
mesh = mesh_processor.mesh
sdf = mesh_processor.sdf
stable_poses = mesh_processor.stable_poses
obj = GraspableObject3D(sdf, mesh)
stable_pose = stable_poses[0]
if visual:
vis.figure()
T_table_world = RigidTransform(from_frame='table', to_frame='world')
T_obj_world = Visualizer3D.mesh_stable_pose(obj.mesh.trimesh,
stable_pose.T_obj_world,
T_table_world=T_table_world,
color=(0.5, 0.5, 0.5),
style='surface',
plot_table=True,
dim=0.15)
if debug:
print(len(stable_poses))
#for stable_pose in stable_poses:
# print(stable_pose.p)
# print(stable_pose.r)
# print(stable_pose.x0)
# print(stable_pose.face)
# print(stable_pose.id)
# glass = 22 is standing straight
if debug:
print(stable_pose.r)
print(stable_pose.T_obj_world)
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
ags = AntipodalGraspSampler(gripper, CONFIG)
stable_pose.id = 0
grasps = ags.generate_grasps(obj,
target_num_grasps=20,
max_iter=5,
stable_pose=stable_pose.r)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['robust_ferrari_canny'])
metrics = []
result = []
#grasps = map(lambda g : g.perpendicular_table(stable_pose), grasps)
for grasp in grasps:
c1, c2 = grasp.endpoints
true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config)
metrics.append(true_fc)
result.append((c1, c2))
if debug:
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
print("Grasp:")
print(c1.point)
print(c2.point)
print(true_fc)
low = np.min(metrics)
high = np.max(metrics)
if low == high:
q_to_c = lambda quality: CONFIG['quality_scale']
else:
q_to_c = lambda quality: CONFIG['quality_scale'] * (quality - low) / (
high - low)
if visual:
for grasp, metric in zip(grasps, metrics):
#grasp2 = grasp.perpendicular_table(stable_pose)
#c1, c2 = grasp.endpoints
#axis = ParallelJawPtGrasp3D.axis_from_endpoints(c1, c2)
#angle = np.dot(np.matmul(stable_pose.r, axis), [1,0,0])
#angle = math.tan(axis[1]/axis[0])
#angle = math.degrees(angle)%360
#print(angle)
#print(angle/360.0)
color = plt.get_cmap('hsv')(q_to_c(metric))[:-1]
vis.grasp(grasp,
T_obj_world=T_obj_world,
grasp_axis_color=color,
endpoint_color=color)
#axis = np.array([[0,0,0], point])
#points = [(x[0], x[1], x[2]) for x in axis]
#Visualizer3D.plot3d(points, color=(0,0,1), tube_radius=0.002)
vis.show(False)
pose_matrix = np.eye(4, 4)
pose_matrix[:3, :3] = T_obj_world.rotation
pose_matrix[:3, 3] = T_obj_world.translation
return pose_matrix, result
def antipodal_grasp_sampler(self):
#of = ObjFile(OBJ_FILENAME)
#sf = SdfFile(SDF_FILENAME)
#mesh = of.read()
#sdf = sf.read()
#obj = GraspableObject3D(sdf, mesh)
mass = 1.0
CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE
mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir'])
mesh_processor.generate_graspable(CONFIG)
mesh = mesh_processor.mesh
sdf = mesh_processor.sdf
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = AntipodalGraspSampler(gripper, CONFIG)
grasps = ags.sample_grasps(obj, num_grasps=100)
print(len(grasps))
quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['robust_ferrari_canny'])
#quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['force_closure'])
quality_fn = GraspQualityFunctionFactory.create_quality_function(obj, quality_config)
vis.figure()
#vis.points(PointCloud(nparraypoints), scale=0.001, color=np.array([0.5,0.5,0.5]))
#vis.plot3d(nparraypoints)
metrics = []
vis.mesh(obj.mesh.trimesh, style='surface')
for grasp in grasps:
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
#fn_fc = quality_fn(grasp).quality
true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config)
true_fc = true_fc
metrics.append(true_fc)
#color = quality_fn(grasp).quality
#if (true_fc > 1.0):
# true_fc = 1.0
#color = (1.0-true_fc, true_fc, 0.0)
low = np.min(metrics)
high = np.max(metrics)
print(low)
print(high)
if low == high:
q_to_c = lambda quality: CONFIG['quality_scale']
else:
q_to_c = lambda quality: CONFIG['quality_scale'] * (quality - low) / (high - low)
print(len(metrics))
for grasp, metric in zip(grasps, metrics):
color = plt.get_cmap('hsv')(q_to_c(metric))[:-1]
print(color)
vis.grasp(grasp, grasp_axis_color=color,endpoint_color=color)
vis.show(False)
def test_init_graspable(self):
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
def worker(i, sample_nums, grasp_amount, good_grasp):
object_name = file_list_all[i][len(home_dir) + 35:]
print('a worker of task {} start'.format(object_name))
yaml_config = YamlConfig(home_dir +
"/code/grasp-pointnet/dex-net/test/config.yaml")
gripper_name = 'robotiq_85'
gripper = RobotGripper.load(
gripper_name, home_dir + "/code/grasp-pointnet/dex-net/data/grippers")
grasp_sample_method = "antipodal"
if grasp_sample_method == "uniform":
ags = UniformGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "gaussian":
ags = GaussianGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "antipodal":
ags = AntipodalGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "gpg":
ags = GpgGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "point":
ags = PointGraspSampler(gripper, yaml_config)
else:
raise NameError("Can't support this sampler")
print("Log: do job", i)
if os.path.exists(str(file_list_all[i]) + "/google_512k/nontextured.obj"):
of = ObjFile(str(file_list_all[i]) + "/google_512k/nontextured.obj")
sf = SdfFile(str(file_list_all[i]) + "/google_512k/nontextured.sdf")
else:
print("can't find any obj or sdf file!")
raise NameError("can't find any obj or sdf file!")
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
print("Log: opened object", i + 1, object_name)
force_closure_quality_config = {}
canny_quality_config = {}
fc_list_sub1 = np.arange(2.0, 0.75, -0.4)
fc_list_sub2 = np.arange(0.5, 0.36, -0.05)
fc_list = np.concatenate([fc_list_sub1, fc_list_sub2])
for value_fc in fc_list:
value_fc = round(value_fc, 2)
yaml_config['metrics']['force_closure']['friction_coef'] = value_fc
yaml_config['metrics']['robust_ferrari_canny'][
'friction_coef'] = value_fc
force_closure_quality_config[
value_fc] = GraspQualityConfigFactory.create_config(
yaml_config['metrics']['force_closure'])
canny_quality_config[
value_fc] = GraspQualityConfigFactory.create_config(
yaml_config['metrics']['robust_ferrari_canny'])
good_count_perfect = np.zeros(len(fc_list))
count = 0
minimum_grasp_per_fc = grasp_amount
while np.sum(good_count_perfect < minimum_grasp_per_fc) != 0:
grasps = ags.generate_grasps(obj,
target_num_grasps=sample_nums,
grasp_gen_mult=10,
vis=False,
random_approach_angle=True)
count += len(grasps)
for j in grasps:
tmp, is_force_closure = False, False
for ind_, value_fc in enumerate(fc_list):
value_fc = round(value_fc, 2)
tmp = is_force_closure
is_force_closure = PointGraspMetrics3D.grasp_quality(
j, obj, force_closure_quality_config[value_fc], vis=False)
if tmp and not is_force_closure:
if good_count_perfect[ind_ - 1] < minimum_grasp_per_fc:
canny_quality = PointGraspMetrics3D.grasp_quality(
j,
obj,
canny_quality_config[round(fc_list[ind_ - 1], 2)],
vis=False)
good_grasp.append((j, round(fc_list[ind_ - 1],
2), canny_quality))
good_count_perfect[ind_ - 1] += 1
break
elif is_force_closure and value_fc == fc_list[-1]:
if good_count_perfect[ind_] < minimum_grasp_per_fc:
canny_quality = PointGraspMetrics3D.grasp_quality(
j, obj, canny_quality_config[value_fc], vis=False)
good_grasp.append((j, value_fc, canny_quality))
good_count_perfect[ind_] += 1
break
print('Object:{} GoodGrasp:{}'.format(object_name, good_count_perfect))
object_name_len = len(object_name)
object_name_ = str(object_name) + " " * (25 - object_name_len)
if count == 0:
good_grasp_rate = 0
else:
good_grasp_rate = len(good_grasp) / count
print('Gripper:{} Object:{} Rate:{:.4f} {}/{}'.format(
gripper_name, object_name_, good_grasp_rate, len(good_grasp), count))
def worker(i, sample_nums, grasp_amount, good_grasp): #主要是抓取采样器以及打分 100 20
"""
brief: 对制定的模型，利用随机采样算法，进行抓取姿态的检测和打分
param [in] i 处理第i个mesh模型
param [in] sample_nums 每个对象模型返回的目标抓取数量
param [in] grasp_amount
"""
#截取目标对象名称
object_name = file_list_all[i][len(home_dir) + 35:]
print('a worker of task {} start'.format(object_name))
#读取初始配置文件，读取并在内存中复制了一份
yaml_config = YamlConfig(home_dir + "/code/dex-net/test/config.yaml")
#设置夹名称
gripper_name = 'panda'
#根据设置的夹爪名称加载夹爪配置
gripper = RobotGripper.load(gripper_name,
home_dir + "/code/dex-net/data/grippers")
#设置抓取采样的方法
grasp_sample_method = "antipodal"
if grasp_sample_method == "uniform":
ags = UniformGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "gaussian":
ags = GaussianGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "antipodal":
#使用对映点抓取，输入夹爪与配置文件
ags = AntipodalGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "gpg":
ags = GpgGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "point":
ags = PointGraspSampler(gripper, yaml_config)
else:
raise NameError("Can't support this sampler")
print("Log: do job", i)
#设置obj模型文件与sdf文件路径
if os.path.exists(str(file_list_all[i]) +
"/google_512k/nontextured.obj") and os.path.exists(
str(file_list_all[i]) +
"/google_512k/nontextured.sdf"):
of = ObjFile(str(file_list_all[i]) + "/google_512k/nontextured.obj")
sf = SdfFile(str(file_list_all[i]) + "/google_512k/nontextured.sdf")
else:
print("can't find any obj or sdf file!")
raise NameError("can't find any obj or sdf file!")
#根据路径读取模型与sdf文件
mesh = of.read()
sdf = sf.read()
#构建抓取模型类
obj = GraspableObject3D(sdf, mesh)
print("Log: opened object", i + 1, object_name)
#########################################
#设置
force_closure_quality_config = {} #设置力闭合 字典
canny_quality_config = {}
#生成一个起点是2.0终点是0.75 步长为-0.4 （递减）的等距数列fc_list_sub1 (2.0, 0.75, -0.4)
fc_list_sub1 = np.arange(2.0, 0.75, -0.3)
#生成一个起点是0.5终点是0.36 步长为-0.05的等距数列fc_list_sub2 (0.5, 0.36, -0.05)
fc_list_sub2 = np.arange(0.5, 0.36, -0.1)
#将上面两个向量接起来，变成一个长条向量，使用不同的步长，目的是为了在更小摩擦力的时候，有更多的分辨率
fc_list = np.concatenate([fc_list_sub1, fc_list_sub2])
print("判断摩擦系数")
print(fc_list)
for value_fc in fc_list:
#对value_fc保留2位小数，四舍五入
value_fc = round(value_fc, 2)
#更改内存中配置中的摩擦系数，而没有修改硬盘中的yaml文件
yaml_config['metrics']['force_closure']['friction_coef'] = value_fc
yaml_config['metrics']['robust_ferrari_canny'][
'friction_coef'] = value_fc
#把每个摩擦力值当成键，
force_closure_quality_config[
value_fc] = GraspQualityConfigFactory.create_config(
yaml_config['metrics']['force_closure'])
canny_quality_config[
value_fc] = GraspQualityConfigFactory.create_config(
yaml_config['metrics']['robust_ferrari_canny'])
#####################准备开始采样############################
#填充一个与摩擦数量相同的数组，每个对应的元素都是0
good_count_perfect = np.zeros(len(fc_list))
count = 0
#设置每个摩擦值需要计算的最少抓取数量 （根据指定输入值20）
minimum_grasp_per_fc = grasp_amount
#如果每个摩擦系数下，有效的抓取(满足力闭合或者其他判断标准)小于要求值，就一直循环查找，直到所有摩擦系数条件下至少都存在20个有效抓取
while np.sum(good_count_perfect < minimum_grasp_per_fc) != 0:
#开始使用antipodes sample获得对映随机抓取，此时并不判断是否满足力闭合，只是先采集满足夹爪条件的抓取
#如果一轮多次随机采样之后，发现无法获得指定数量的随机抓取，就会重复迭代计算3次，之后放弃，并把已经找到的抓取返回来
grasps = ags.generate_grasps(obj,
target_num_grasps=sample_nums,
grasp_gen_mult=10,
max_iter=10,
vis=False,
random_approach_angle=True)
count += len(grasps)
#循环对每个采样抓取进行判断
for j in grasps:
tmp, is_force_closure = False, False
#循环对某个采样抓取应用不同的抓取摩擦系数，判断是否是力闭合
for ind_, value_fc in enumerate(fc_list):
value_fc = round(value_fc, 2)
tmp = is_force_closure
#判断在当前给定的摩擦系数下，抓取是否是力闭合的
is_force_closure = PointGraspMetrics3D.grasp_quality(
j, obj, force_closure_quality_config[value_fc], vis=False)
#假设当前,1号摩擦力为1.6 抓取不是力闭合的，但是上一个0号摩擦系数2.0 条件下抓取是力闭合的
if tmp and not is_force_closure:
#当0号2.0摩擦系数条件下采样的good抓取数量还不足指定的最低数量20
if good_count_perfect[ind_ - 1] < minimum_grasp_per_fc:
#以0号摩擦系数作为边界
canny_quality = PointGraspMetrics3D.grasp_quality(
j,
obj,
canny_quality_config[round(fc_list[ind_ - 1], 2)],
vis=False)
good_grasp.append((j, round(fc_list[ind_ - 1],
2), canny_quality))
#在0号系数的good抓取下计数加1
good_count_perfect[ind_ - 1] += 1
#当前抓取j的边界摩擦系数找到了，退出摩擦循环，判断下一个抓取
break
#如果当前1号摩擦系数1.6条件下，该抓取j本身就是力闭合的，且摩擦系数是列表中的最后一个（所有的摩擦系数都判断完了）
elif is_force_closure and value_fc == fc_list[-1]:
if good_count_perfect[ind_] < minimum_grasp_per_fc:
#以当前摩擦系数作为边界
canny_quality = PointGraspMetrics3D.grasp_quality(
j, obj, canny_quality_config[value_fc], vis=False)
good_grasp.append((j, value_fc, canny_quality))
good_count_perfect[ind_] += 1
#当前抓取j关于当前摩擦系数1.6判断完毕，而且满足所有的摩擦系数，就换到下一个摩擦系数
break
print('Object:{} GoodGrasp:{}'.format(object_name,
good_count_perfect)) #判断
object_name_len = len(object_name)
object_name_ = str(object_name) + " " * (25 - object_name_len)
if count == 0:
good_grasp_rate = 0
else:
good_grasp_rate = len(good_grasp) / count
print('Gripper:{} Object:{} Rate:{:.4f} {}/{}'.format(
gripper_name, object_name_, good_grasp_rate, len(good_grasp), count))
home_dir = os.environ['HOME']
file_dir = home_dir + "/dataset/ycb_meshes_google/objects"
mlab.figure(bgcolor=(1.0, 1.0, 1.0), fgcolor=(0.7, 0.7, 0.7))
file_list_all = get_file_name(file_dir)
object_numbers = file_list_all.__len__()
i = 0 # index of objects to define which object to show
if os.path.exists(str(file_list_all[i]) + "/google_512k/nontextured.obj"):
of = ObjFile(str(file_list_all[i]) + "/google_512k/nontextured.obj")
sf = SdfFile(str(file_list_all[i]) + "/google_512k/nontextured.sdf")
else:
print("can't find any obj or sdf file!")
raise NameError("can't find any obj or sdf file!")
mesh = of.read()
sdf = sf.read()
graspable = GraspableObject3D(sdf, mesh)
print("Log: opened object")
begin_time = time.time()
surface_points, _ = graspable.sdf.surface_points(grid_basis=False)
all_p = surface_points
method = "voxel"
if method == "random":
surface_points = surface_points[np.random.choice(
surface_points.shape[0], 1000, replace=False)]
surface_normal = []
elif method == "voxel":
surface_points = surface_points.astype(np.float32)
p = pcl.PointCloud(surface_points)
voxel = p.make_voxel_grid_filter()
voxel.set_leaf_size(*([graspable.sdf.resolution * 5] * 3))
surface_points = voxel.filter().to_array()
def worker(i, sample_nums, grasp_amount, good_grasp):
object_name = file_list_all[i][len(home_dir) + 48:-12]
print('a worker of task {} start'.format(object_name))
yaml_config = YamlConfig(home_dir +
"/Projects/PointNetGPD/dex-net/test/config.yaml")
gripper_name = 'robotiq_85'
gripper = RobotGripper.load(
gripper_name, home_dir + "/Projects/PointNetGPD/dex-net/data/grippers")
grasp_sample_method = "antipodal"
if grasp_sample_method == "uniform":
ags = UniformGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "gaussian":
ags = GaussianGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "antipodal":
ags = AntipodalGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "gpg":
ags = GpgGraspSampler(gripper, yaml_config)
elif grasp_sample_method == "point":
ags = PointGraspSampler(gripper, yaml_config)
else:
raise NameError("Can't support this sampler")
print("Log: do job", i)
if os.path.exists(str(file_list_all[i]) + "/nontextured.obj"):
of = ObjFile(str(file_list_all[i]) + "/nontextured.obj")
sf = SdfFile(str(file_list_all[i]) + "/nontextured.sdf")
else:
print("can't find any obj or sdf file!")
raise NameError("can't find any obj or sdf file!")
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
print("Log: opened object", i + 1, object_name)
force_closure_quality_config = {}
canny_quality_config = {}
fc_list_sub1 = np.arange(2.0, 0.75, -0.4)
fc_list_sub2 = np.arange(0.5, 0.36, -0.05)
fc_list = np.concatenate(
[fc_list_sub1,
fc_list_sub2]) # 摩擦系数列表 fc_list [2. 1.6 1.2 0.8 0.5 0.45 0.4 ]
print("fc_list", fc_list, "fc_list[-1]", fc_list[-1])
for value_fc in fc_list:
value_fc = round(value_fc, 2)
yaml_config['metrics']['force_closure']['friction_coef'] = value_fc
yaml_config['metrics']['robust_ferrari_canny'][
'friction_coef'] = value_fc
force_closure_quality_config[
value_fc] = GraspQualityConfigFactory.create_config(
yaml_config['metrics']['force_closure'])
canny_quality_config[
value_fc] = GraspQualityConfigFactory.create_config(
yaml_config['metrics']['robust_ferrari_canny'])
good_count_perfect = np.zeros(len(fc_list))
count = 0
minimum_grasp_per_fc = grasp_amount
# 各个摩擦系数生成一些抓取
while np.sum(good_count_perfect < minimum_grasp_per_fc
) != 0 and good_count_perfect[-1] < minimum_grasp_per_fc:
print("[INFO]:good | mini", good_count_perfect, minimum_grasp_per_fc)
print("[INFO]:good < mini", good_count_perfect < minimum_grasp_per_fc,
np.sum(good_count_perfect < minimum_grasp_per_fc))
grasps = ags.generate_grasps(
obj,
target_num_grasps=sample_nums,
grasp_gen_mult=10, # 生成抓取姿态
vis=False,
random_approach_angle=True) # 随机调整抓取方向
print("\033[0;32m%s\033[0m" %
"[INFO] Worker{} generate {} grasps.".format(i, len(grasps)))
count += len(grasps)
for j in grasps: # 遍历生成的抓取姿态, 判断是否为力闭合, 及其对应的摩擦系数
tmp, is_force_closure = False, False
for ind_, value_fc in enumerate(fc_list): # 为每个摩擦系数分配抓取姿态
value_fc = round(value_fc, 2)
tmp = is_force_closure
is_force_closure = PointGraspMetrics3D.grasp_quality(
j,
obj, # 依据摩擦系数 value_fc 评估抓取姿态
force_closure_quality_config[value_fc],
vis=False)
print("[INFO] is_force_closure:", is_force_closure,
"value_fc:", value_fc, "tmp:", tmp)
if tmp and not is_force_closure: # 前一个摩擦系数下为力闭合, 当前摩擦系数下非力闭合, 即找到此抓取对应的最小摩擦系数
print("[debug] tmp and not is_force_closure")
if good_count_perfect[ind_ - 1] < minimum_grasp_per_fc:
canny_quality = PointGraspMetrics3D.grasp_quality(
j,
obj,
canny_quality_config[round(fc_list[ind_ - 1], 2)],
vis=False)
good_grasp.append(
(j, round(fc_list[ind_ - 1],
2), canny_quality)) # 保存前一个抓取
good_count_perfect[ind_ - 1] += 1
break
elif is_force_closure and value_fc == fc_list[
-1]: # 力闭合并且摩擦系数最小
print(
"[debug] is_force_closure and value_fc == fc_list[-1]")
if good_count_perfect[ind_] < minimum_grasp_per_fc:
canny_quality = PointGraspMetrics3D.grasp_quality(
j, obj, canny_quality_config[value_fc], vis=False)
good_grasp.append((j, value_fc, canny_quality))
good_count_perfect[ind_] += 1
break
print('Worker:', i,
'Object:{} GoodGrasp:{}'.format(object_name, good_count_perfect))
object_name_len = len(object_name)
object_name_ = str(object_name) + " " * (25 - object_name_len)
if count == 0:
good_grasp_rate = 0
else:
good_grasp_rate = len(good_grasp) / count
print(
'Worker:', i, 'Gripper:{} Object:{} Rate:{:.4f} {}/{}'.format(
gripper_name, object_name_, good_grasp_rate, len(good_grasp),
count))
def figure_3():
#env.state.obj.T_obj_world.translation += np.array([-.01,-.05,.01])
action = policy.action(env.state)
mesh = env.state.obj.mesh.copy().apply_transform(
env.state.T_obj_world.matrix)
mesh = mesh.slice_plane([0, 0, .0005], -up)
from dexnet.grasping import GraspableObject3D
env.state.obj = GraspableObject3D(mesh)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.0005)
vis3d.points(Point(bottom_points[0]), color=[0, 0, 0], scale=.001)
vis3d.points(Point(bottom_points[1]), color=[0, 0, 0], scale=.001)
y_dir = normalize(bottom_points[1] - bottom_points[0])
origin = policy.toppling_model.com_projected_on_edges[0] - .0025 * y_dir
vis3d.points(Point(origin), color=[0, 0, 1], scale=.001)
# t = .002
# x = np.cross(y_dir, up)
# while t < np.linalg.norm(origin - bottom_points[0]):
# start_point = origin - t*y_dir
# end_point = start_point + .0075*x
# shaft_points = [start_point, end_point]
# h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(x)
# h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(x)
# head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1]
# vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
# vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
# t += .002
## try 2
x = np.cross(y_dir, up)
t = .004
arrow_dir = x
start_point = origin - t * y_dir
end_point = start_point + .0075 * arrow_dir
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0], [0, 0,
1]]).dot(x)
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0], [0, 0,
1]]).dot(x)
head_points = [end_point - 0.001 * h2, end_point, end_point - 0.001 * h1]
vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
# t = .000
# while t < np.linalg.norm(origin - bottom_points[1]):
# arrow_dir = x
# start_point = origin + t*y_dir
# end_point = start_point + .0075*arrow_dir
# shaft_points = [start_point, end_point]
# h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir)
# h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir)
# head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1]
# vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
# vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
# t += .002
## try 2
t = .01
arrow_dir = -x
start_point = origin + t * y_dir
end_point = start_point + .0075 * arrow_dir
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0],
[0, 0, 1]]).dot(arrow_dir)
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0],
[0, 0, 1]]).dot(arrow_dir)
head_points = [end_point - 0.001 * h2, end_point, end_point - 0.001 * h1]
vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
#arrow_dir = np.cross(y_dir, up)
#start_point = origin - .005*y_dir
#end_point = start_point + .0075*arrow_dir
#shaft_points = [start_point, end_point]
#h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir)
#h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir)
#head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1]
#vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002)
#vis3d.plot3d(head_points, color=purple, tube_radius=.0002)
#arrow_dir = -up
#end_point = start_point + .0075*arrow_dir
#shaft_points = [start_point, end_point]
#h1 = np.array([[1,0,0],[0,0.7071,-0.7071],[0,0.7071,0.7071]]).dot(arrow_dir)
#h2 = np.array([[1,0,0],[0,0.7071,0.7071],[0,-0.7071,0.7071]]).dot(arrow_dir)
#head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1]
#vis3d.plot3d(shaft_points, color=blue, tube_radius=.0002)
#vis3d.plot3d(head_points, color=blue, tube_radius=.0002)
#
#vis3d.points(Point(start_point), color=[0,1,0], scale=.001)
#vis_axes(origin, y_dir)
vis3d.show(starting_camera_pose=CAMERA_POSE)
sys.exit()