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_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 _generate_grasps(self, graspable, num_grasps,
check_collisions=False, vis=False,
sigma_scale=2.5):
"""
Returns a list of candidate grasps for graspable object by Gaussian with
variance specified by principal dimensions
Params:
graspable - (GraspableObject3D) the object to grasp
num_grasps - (int) the number of grasps to generate
sigma_scale - (float) the number of sigmas on the tails of the
Gaussian for each dimension
Returns:
list of ParallelJawPtGrasp3D objects
"""
# get object principal axes
center_of_mass = graspable.mesh.center_of_mass
principal_dims = graspable.mesh.principal_dims()
sigma_dims = principal_dims / (2 * sigma_scale)
# sample centers
grasp_centers = stats.multivariate_normal.rvs(
mean=center_of_mass, cov=sigma_dims**2, size=num_grasps)
# samples angles uniformly from sphere
u = stats.uniform.rvs(size=num_grasps)
v = stats.uniform.rvs(size=num_grasps)
thetas = 2 * np.pi * u
phis = np.arccos(2 * v - 1.0)
grasp_dirs = np.array([np.sin(phis) * np.cos(thetas), np.sin(phis) * np.sin(thetas), np.cos(phis)])
grasp_dirs = grasp_dirs.T
# convert to grasp objects
grasps = []
for i in range(num_grasps):
grasp = ParallelJawPtGrasp3D(grasp_centers[i,:], grasp_dirs[i,:], self.grasp_width)
contacts_found, contacts = grasp.close_fingers(graspable)
# add grasp if it has valid contacts
if contacts_found and np.linalg.norm(contacts[0].point - contacts[1].point) > self.min_contact_dist:
grasps.append(grasp)
# visualize
if vis:
for grasp in grasps:
plt.clf()
h = plt.gcf()
plt.ion()
grasp.close_fingers(graspable, vis=vis)
plt.show(block=False)
time.sleep(0.5)
grasp_centers_grid = graspable.sdf.transform_pt_obj_to_grid(grasp_centers.T)
grasp_centers_grid = grasp_centers_grid.T
com_grid = graspable.sdf.transform_pt_obj_to_grid(center_of_mass)
plt.clf()
ax = plt.gca(projection = '3d')
graspable.sdf.scatter()
ax.scatter(grasp_centers_grid[:,0], grasp_centers_grid[:,1], grasp_centers_grid[:,2], s=60, c=u'm')
ax.scatter(com_grid[0], com_grid[1], com_grid[2], s=120, c=u'y')
ax.set_xlim3d(0, graspable.sdf.dims_[0])
ax.set_ylim3d(0, graspable.sdf.dims_[1])
ax.set_zlim3d(0, graspable.sdf.dims_[2])
plt.show()
# optionally use openrave to check collisionsx
if check_collisions:
rave.raveSetDebugLevel(rave.DebugLevel.Error)
grasp_checker = pgc.OpenRaveGraspChecker(view=vis)
# loop through grasps
collision_free_grasps = []
for grasp in grasps:
rotated_grasps = grasp.transform(graspable.tf, self.theta_res)
rotated_grasps = grasp_checker.prune_grasps_in_collision(graspable, rotated_grasps, auto_step=True, delay=0.0)
if len(rotated_grasps) > 0:
collision_free_grasps.append(grasp)
grasps = collision_free_grasps
return grasps
def _generate_grasps(self,
graspable,
num_grasps,
check_collisions=False,
vis=False,
sigma_scale=2.5):
"""
Returns a list of candidate grasps for graspable object by Gaussian with
variance specified by principal dimensions
Params:
graspable - (GraspableObject3D) the object to grasp
num_grasps - (int) the number of grasps to generate
sigma_scale - (float) the number of sigmas on the tails of the
Gaussian for each dimension
Returns:
list of ParallelJawPtGrasp3D objects
"""
# get object principal axes
center_of_mass = graspable.mesh.center_of_mass
principal_dims = graspable.mesh.principal_dims()
sigma_dims = principal_dims / (2 * sigma_scale)
# sample centers
grasp_centers = stats.multivariate_normal.rvs(mean=center_of_mass,
cov=sigma_dims**2,
size=num_grasps)
# samples angles uniformly from sphere
u = stats.uniform.rvs(size=num_grasps)
v = stats.uniform.rvs(size=num_grasps)
thetas = 2 * np.pi * u
phis = np.arccos(2 * v - 1.0)
grasp_dirs = np.array([
np.sin(phis) * np.cos(thetas),
np.sin(phis) * np.sin(thetas),
np.cos(phis)
])
grasp_dirs = grasp_dirs.T
# convert to grasp objects
grasps = []
for i in range(num_grasps):
grasp = ParallelJawPtGrasp3D(grasp_centers[i, :], grasp_dirs[i, :],
self.grasp_width)
contacts_found, contacts = grasp.close_fingers(graspable)
# add grasp if it has valid contacts
if contacts_found and np.linalg.norm(
contacts[0].point -
contacts[1].point) > self.min_contact_dist:
grasps.append(grasp)
# visualize
if vis:
for grasp in grasps:
plt.clf()
h = plt.gcf()
plt.ion()
grasp.close_fingers(graspable, vis=vis)
plt.show(block=False)
time.sleep(0.5)
grasp_centers_grid = graspable.sdf.transform_pt_obj_to_grid(
grasp_centers.T)
grasp_centers_grid = grasp_centers_grid.T
com_grid = graspable.sdf.transform_pt_obj_to_grid(center_of_mass)
plt.clf()
ax = plt.gca(projection='3d')
graspable.sdf.scatter()
ax.scatter(grasp_centers_grid[:, 0],
grasp_centers_grid[:, 1],
grasp_centers_grid[:, 2],
s=60,
c=u'm')
ax.scatter(com_grid[0], com_grid[1], com_grid[2], s=120, c=u'y')
ax.set_xlim3d(0, graspable.sdf.dims_[0])
ax.set_ylim3d(0, graspable.sdf.dims_[1])
ax.set_zlim3d(0, graspable.sdf.dims_[2])
plt.show()
# optionally use openrave to check collisionsx
if check_collisions:
rave.raveSetDebugLevel(rave.DebugLevel.Error)
grasp_checker = pgc.OpenRaveGraspChecker(view=vis)
# loop through grasps
collision_free_grasps = []
for grasp in grasps:
rotated_grasps = grasp.transform(graspable.tf, self.theta_res)
rotated_grasps = grasp_checker.prune_grasps_in_collision(
graspable, rotated_grasps, auto_step=True, delay=0.0)
if len(rotated_grasps) > 0:
collision_free_grasps.append(grasp)
grasps = collision_free_grasps
return grasps
