def test_init_grasp(self):
# random grasp
g1 = np.random.rand(3)
g2 = np.random.rand(3)
x = (g1 + g2) / 2
v = g2 - g1
width = np.linalg.norm(v)
v = v / width
configuration = ParallelJawPtGrasp3D.configuration_from_params(x, v, width)
# test init
random_grasp = ParallelJawPtGrasp3D(configuration)
read_configuration = random_grasp.configuration
self.assertTrue(np.allclose(configuration, read_configuration))
self.assertTrue(np.allclose(x, random_grasp.center))
self.assertTrue(np.allclose(v, random_grasp.axis))
self.assertTrue(np.allclose(width, random_grasp.open_width))
read_g1, read_g2 = random_grasp.endpoints
self.assertTrue(np.allclose(g1, read_g1))
self.assertTrue(np.allclose(g2, read_g2))
# test bad init
configuration[4] = 1000
caught_bad_init = False
try:
random_grasp = ParallelJawPtGrasp3D(configuration)
except:
caught_bad_init = True
self.assertTrue(caught_bad_init)
def sample(self, size=1):
""" Sample random variables from the model.
Parameters
----------
size : int
number of sample to take
Returns
-------
:obj:`list` of :obj:`ParallelJawPtGrasp3D`
sampled grasps in various poses
"""
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
open_width = max(self.open_width_rv_.rvs(size=1), 0)
close_width = max(self.close_width_rv_.rvs(size=1), 0)
approach = self.approach_rv_.rvs(size=1)
# transform grasp by pose
grasp_sample = ParallelJawPtGrasp3D(ParallelJawPtGrasp3D.configuration_from_params(t, v, open_width, approach, self.grasp_.jaw_width, close_width))
samples.append(grasp_sample)
if size == 1:
return samples[0]
return samples
def sample_grasps(self,
graspable,
num_grasps,
vis=False,
max_num_samples=1000):
"""
Returns a list of candidate grasps for graspable object using uniform point pairs from the SDF
Parameters
----------
graspable : :obj:`GraspableObject3D`
the object to grasp
num_grasps : int
the number of grasps to generate
Returns
-------
:obj:`list` of :obj:`ParallelJawPtGrasp3D`
list of generated grasps
"""
# get all surface points
surface_points, _ = graspable.sdf.surface_points(grid_basis=False)
num_surface = surface_points.shape[0]
i = 0
grasps = []
# get all grasps
while len(grasps) < num_grasps and i < max_num_samples:
# get candidate contacts
indices = np.random.choice(num_surface, size=2, replace=False)
c0 = surface_points[indices[0], :]
c1 = surface_points[indices[1], :]
if np.linalg.norm(c1 -
c0) > self.gripper.min_width and np.linalg.norm(
c1 - c0) < self.gripper.max_width:
# compute centers and axes
grasp_center = ParallelJawPtGrasp3D.center_from_endpoints(
c0, c1)
grasp_axis = ParallelJawPtGrasp3D.axis_from_endpoints(c0, c1)
g = ParallelJawPtGrasp3D(
ParallelJawPtGrasp3D.configuration_from_params(
grasp_center, grasp_axis, self.gripper.max_width))
# keep grasps if the fingers close
success, contacts = g.close_fingers(graspable)
if success:
grasps.append(g)
i += 1
return grasps
def test_init_grasp(self):
# random grasp
g1 = np.random.rand(3)
g2 = np.random.rand(3)
x = (g1 + g2) / 2
v = g2 - g1
width = np.linalg.norm(v)
v = v / width
configuration = ParallelJawPtGrasp3D.configuration_from_params(
x, v, width)
# test init
random_grasp = ParallelJawPtGrasp3D(configuration)
read_configuration = random_grasp.configuration
read_g1, read_g2 = random_grasp.endpoints
# test bad init
configuration[4] = 1000
caught_bad_init = False
try:
random_grasp = ParallelJawPtGrasp3D(configuration)
except:
caught_bad_init = True
def sample_grasps(self, graspable, num_grasps,
vis=False,
sigma_scale=2.5):
"""
Returns a list of candidate grasps for graspable object by Gaussian with
variance specified by principal dimensions.
Parameters
----------
graspable : :obj:`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
-------
:obj:`list` of obj:`ParallelJawPtGrasp3D`
list of generated grasps
"""
# 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(ParallelJawPtGrasp3D.configuration_from_params(grasp_centers[i,:], grasp_dirs[i,:], self.gripper.max_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()
return grasps