def propose_grasps(pc, radius, num_grasps=1, vis=False):
output_grasps = []
for _ in range(num_grasps):
center_index = np.random.randint(pc.shape[0])
center_point = pc[center_index, :].copy()
d = np.sqrt(np.sum(np.square(pc - np.expand_dims(center_point, 0)),
-1))
index = np.where(d < radius)[0]
neighbors = pc[index, :]
eigen_values, eigen_vectors = cov_matrix(center_point, neighbors)
direction = eigen_vectors[:, 2]
direction = choose_direction(direction, center_point)
surface_orientation = trimesh.geometry.align_vectors([0, 0, 1],
direction)
roll_orientation = tra.quaternion_matrix(
tra.quaternion_about_axis(np.random.uniform(0, 2 * np.pi),
[0, 0, 1]))
gripper_transform = surface_orientation.dot(roll_orientation)
gripper_transform[:3, 3] = center_point
translation_transform = np.eye(4)
translation_transform[2, 3] = -np.random.uniform(0.0669, 0.1122)
gripper_transform = gripper_transform.dot(translation_transform)
output_grasps.append(gripper_transform.copy())
if vis:
draw_scene(pc, grasps=output_grasps)
mlab.show()
return np.asarray(output_grasps)
def spherical_rotate(max_rot_degrees):
"""Calculate a random rotation matrix using angle magnitudes in max_rot."""
assert isinstance(max_rot_degrees, tuple) and len(max_rot_degrees) == 3
xrot, yrot, zrot = max_rot_degrees
# Build the global rotation matrix using quaternions
q_xr = tf.quaternion_about_axis(rand_step(xrot), [1, 0, 0])
q_yr = tf.quaternion_about_axis(rand_step(yrot), [0, 1, 0])
q_zr = tf.quaternion_about_axis(rand_step(zrot), [0, 0, 1])
# Multiply global and local rotations
rotation = tf.quaternion_multiply(q_xr, q_yr)
rotation = tf.quaternion_multiply(rotation, q_zr)
return tf.quaternion_matrix(rotation)
def cable(a, b, thick=0.20):
"""
Rigid cable (segment) between two points.
TODO: this extrusion approach and rotation can be improved using extrude along like cable_catenary does.
"""
a = np.array(a)
b = np.array(b)
#path = ddd.line([a, b])
#path_section = ddd.point(name="Cable").buffer(thick * 0.5, resolution=1, cap_style=ddd.CAP_ROUND)
#cable = path_section.extrude_path(path)
length = np.linalg.norm(b - a)
cable = ddd.point(name="Cable").buffer(thick * 0.5, resolution=1).extrude(length + thick).translate([0, 0, -thick * 0.5])
cable = cable.merge_vertices().smooth(math.pi)
cable = ddd.uv.map_cylindrical(cable, split=False)
vector_up = [0, 0, 1]
vector_dir = (b - a) / length
rot_axis = np.cross(vector_up, vector_dir)
rot_angle = math.asin(np.linalg.norm(rot_axis))
if rot_angle > 0.00001:
rotation = transformations.quaternion_about_axis(rot_angle, rot_axis / np.linalg.norm(rot_axis))
cable = cable.rotate_quaternion(rotation)
cable = cable.translate(a)
return cable
def load_dice_nodes(self, scene_setup: SceneSetup):
"""Creates internal list of dice nodes, including calculating pose rotation and translation, from a given SceneSetup."""
for dice_setup in [
dice_setup for dice_setup in scene_setup.dice_setups
if dice_setup.exists
]:
dice_node = pyrender.Node(mesh=self.dice_mesh, matrix=np.eye(4))
quaternion_y_rotation = quaternion_about_axis(
dice_setup.rotation_y_degrees * np.pi / 180, [0, 1, 0])
side_axis = [0, 0, 1
] if dice_setup.side_rotation_around_z else [1, 0, 0]
quaternion_side_rotation = quaternion_about_axis(
dice_setup.side_rotation_times * np.pi / 2, side_axis)
dice_node.rotation = np.roll(
quaternion_multiply(quaternion_y_rotation,
quaternion_side_rotation), -1)
dice_node.translation = (dice_setup.x, 0, dice_setup.z)
self.dice_nodes.append(dice_node)
def get_empty_scene(self):
"""Creates an empty scene with a camera and spot light."""
scene = pyrender.Scene(ambient_light=[0.2, 0.2, 0.2],
bg_color=[0.1, 0.1, 0.1])
camera = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1)
camera_pose = translation_matrix([0, 3, 4]) @ quaternion_matrix(
quaternion_about_axis(np.deg2rad(-45.0), [1, 0, 0]))
scene.add(camera, pose=camera_pose)
light = pyrender.SpotLight(color=np.ones(3),
intensity=3.0,
innerConeAngle=np.pi / 16.0)
scene.add(light, pose=camera_pose)
return scene
def sample_multiple_grasps(number_of_candidates,
mesh,
gripper_name,
systematic_sampling,
surface_density=0.005 * 0.005,
standoff_density=0.01,
roll_density=15,
type_of_quality='antipodal',
min_quality=-1.0,
silent=False):
"""Sample a set of grasps for an object.
Arguments:
number_of_candidates {int} -- Number of grasps to sample
mesh {trimesh} -- Object mesh
gripper_name {str} -- Name of gripper model
systematic_sampling {bool} -- Whether to use grid sampling for roll
Keyword Arguments:
surface_density {float} -- surface density, in m^2 (default: {0.005*0.005})
standoff_density {float} -- density for standoff, in m (default: {0.01})
roll_density {float} -- roll density, in deg (default: {15})
type_of_quality {str} -- quality metric (default: {'antipodal'})
min_quality {float} -- minimum grasp quality (default: {-1})
silent {bool} -- verbosity (default: {False})
Raises:
Exception: Unknown quality metric
Returns:
[type] -- points, normals, transforms, roll_angles, standoffs, collisions, quality
"""
origins = []
orientations = []
transforms = []
standoffs = []
roll_angles = []
gripper = create_gripper(gripper_name)
if systematic_sampling:
# systematic sampling. input:
# - Surface density:
# - Standoff density:
# - Rotation density:
# Resulting number of samples:
# (Area/Surface Density) * (Finger length/Standoff density) * (360/Rotation Density)
surface_samples = int(np.ceil(mesh.area / surface_density))
standoff_samples = np.linspace(
gripper.standoff_range[0], gripper.standoff_range[1],
max(1, (gripper.standoff_range[1] - gripper.standoff_range[0]) /
standoff_density))
rotation_samples = np.arange(0, 1 * np.pi, np.deg2rad(roll_density))
number_of_candidates = surface_samples * \
len(standoff_samples) * len(rotation_samples)
tmp_points, face_indices = mesh.sample(surface_samples,
return_index=True)
tmp_normals = mesh.face_normals[face_indices]
number_of_candidates = len(tmp_points) * \
len(standoff_samples) * len(rotation_samples)
print("Number of samples ", number_of_candidates, "(", len(tmp_points),
" x ", len(standoff_samples), " x ", len(rotation_samples), ")")
points = []
normals = []
position_idx = []
pos_cnt = 0
cnt = 0
batch_position_idx = []
batch_points = []
batch_normals = []
batch_roll_angles = []
batch_standoffs = []
batch_transforms = []
for point, normal in tqdm(zip(tmp_points, tmp_normals),
total=len(tmp_points),
disable=silent):
for roll in rotation_samples:
for standoff in standoff_samples:
batch_position_idx.append(pos_cnt)
batch_points.append(point)
batch_normals.append(normal)
batch_roll_angles.append(roll)
batch_standoffs.append(standoff)
orientation = tra.quaternion_matrix(
tra.quaternion_about_axis(roll, [0, 0, 1]))
origin = point + normal * standoff
batch_transforms.append(
np.dot(
np.dot(
tra.translation_matrix(origin),
trimesh.geometry.align_vectors([0, 0, -1],
normal)),
orientation))
cnt += 1
pos_cnt += 1
if cnt % 1000 == 0 or cnt == len(tmp_points):
valid = raycast_collisioncheck(np.asarray(batch_transforms),
np.asarray(batch_points), mesh)
transforms.extend(np.array(batch_transforms)[valid])
position_idx.extend(np.array(batch_position_idx)[valid])
points.extend(np.array(batch_points)[valid])
normals.extend(np.array(batch_normals)[valid])
roll_angles.extend(np.array(batch_roll_angles)[valid])
standoffs.extend(np.array(batch_standoffs)[valid])
batch_position_idx = []
batch_points = []
batch_normals = []
batch_roll_angles = []
batch_standoffs = []
batch_transforms = []
points = np.array(points)
normals = np.array(normals)
position_idx = np.array(position_idx)
else:
points, face_indices = mesh.sample(number_of_candidates,
return_index=True)
normals = mesh.face_normals[face_indices]
# generate transformations
for point, normal in tqdm(zip(points, normals),
total=len(points),
disable=silent):
# roll along approach vector
angle = np.random.rand() * 2 * np.pi
roll_angles.append(angle)
orientations.append(
tra.quaternion_matrix(
tra.quaternion_about_axis(angle, [0, 0, 1])))
# standoff from surface
standoff = (gripper.standoff_range[1] - gripper.standoff_range[0]) * np.random.rand() \
+ gripper.standoff_range[0]
standoffs.append(standoff)
origins.append(point + normal * standoff)
transforms.append(
np.dot(
np.dot(tra.translation_matrix(origins[-1]),
trimesh.geometry.align_vectors([0, 0, -1], normal)),
orientations[-1]))
verboseprint("Checking collisions...")
collisions = in_collision_with_gripper(mesh,
transforms,
gripper_name=gripper_name,
silent=silent)
verboseprint("Labelling grasps...")
quality = {}
quality_key = 'quality_' + type_of_quality
if type_of_quality == 'antipodal':
quality[quality_key] = grasp_quality_antipodal(
transforms,
collisions,
object_mesh=mesh,
gripper_name=gripper_name,
silent=silent)
elif type_of_quality == 'number_of_contacts':
quality[quality_key] = grasp_quality_point_contacts(
transforms,
collisions,
object_mesh=mesh,
gripper_name=gripper_name,
silent=silent)
else:
raise Exception("Quality metric unknown: ", quality)
# Filter out by quality
quality_np = np.array(quality[quality_key])
collisions = np.array(collisions)
f_points = []
f_normals = []
f_transforms = []
f_roll_angles = []
f_standoffs = []
f_collisions = []
f_quality = []
for i, _ in enumerate(transforms):
if quality_np[i] >= min_quality:
f_points.append(points[i])
f_normals.append(normals[i])
f_transforms.append(transforms[i])
f_roll_angles.append(roll_angles[i])
f_standoffs.append(standoffs[i])
f_collisions.append(int(collisions[i]))
f_quality.append(quality_np[i])
points = np.array(f_points)
normals = np.array(f_normals)
transforms = np.array(f_transforms)
roll_angles = np.array(f_roll_angles)
standoffs = np.array(f_standoffs)
collisions = f_collisions
quality[quality_key] = f_quality
return points, normals, transforms, roll_angles, standoffs, collisions, quality
def generate_candidates(mesh_properties):
"""Generates grasp candidates via predicted 6DoF pose of gripper."""
bbox = mesh_properties['bbox']
work2obj = mesh_properties['work2obj']
obj2grip = mesh_properties['obj2grip']
# Extract the bounding box corners and planes of the object
_, planes = get_corners_and_plances(bbox)
xyz_grid = [to_rad(x, y, z)
for x in range(0, 360, GLOBAL_X) \
for y in range(0, 360, GLOBAL_Y) \
for z in range(0, 360, GLOBAL_Z)]
total_trials = len(xyz_grid)**2
# Angles holds the information about global and local rotations
# Matrices holds information about the final transformation.
angles = np.empty((total_trials, 6))
matrices = np.empty((total_trials, 12))
# Going to extend a vector along z-direction of palm. The extra 1 o the end
# is for multiplying with a 4x4 HT matrix
palm_axis = np.atleast_2d([0, 0, 10, 1]).T
curr_trial = 0
success = 0
# Exhaustively evaluate local and global rotations
for gx, gy, gz in xyz_grid:
for lx, ly, lz in xyz_grid:
# Monitor our status, print something every 10%
if curr_trial % int(0.1 * total_trials) == 0:
print 'Trial %d/%d, successful: %d'%\
(curr_trial, total_trials, success)
curr_trial += 1
# Build the global rotation matrix using quaternions
q_gx = tf.quaternion_about_axis(gx + rand_step(GLOBAL_X),
[1, 0, 0])
q_gy = tf.quaternion_about_axis(gy + rand_step(GLOBAL_Y),
[0, 1, 0])
q_gz = tf.quaternion_about_axis(gz + rand_step(GLOBAL_Z),
[0, 0, 1])
# Build the local rotation matrix using quaternions
q_lx = tf.quaternion_about_axis(lx + rand_step(GLOBAL_X),
[1, 0, 0])
q_ly = tf.quaternion_about_axis(ly + rand_step(GLOBAL_Y),
[0, 1, 0])
q_lz = tf.quaternion_about_axis(lz + rand_step(GLOBAL_Z),
[0, 0, 1])
# Multiply global and local rotations
global_rotation = tf.quaternion_multiply(q_gx, q_gy)
global_rotation = tf.quaternion_multiply(global_rotation, q_gz)
global_rotation = tf.quaternion_matrix(global_rotation)
local_rotation = tf.quaternion_multiply(q_lx, q_ly)
local_rotation = tf.quaternion_multiply(local_rotation, q_lz)
local_rotation = tf.quaternion_matrix(local_rotation)
rotation = np.dot(np.dot(global_rotation, obj2grip),
local_rotation)
# Don't want to test any candidates that are below the workspace
workspace2grip = np.dot(work2obj, rotation)
if workspace2grip[2, 3] < 0:
continue
# Check if a line from the center of the grippers palm intersects
# with the planes of the object
line_start = rotation[:3, 3]
line_end = np.dot(rotation, palm_axis).flatten()[:3]
for plane in planes:
itx_plane = intersect_plane(line_start, line_end, *plane)
if intersect_box(itx_plane, bbox) is False:
continue
angles[success] = np.asarray([gx, gy, gz, lx, ly, lz])
matrices[success] = rotation[:3].flatten()
success = success + 1
break
# Only keep the successful transformations
angles = np.asarray(angles)[:success]
matrices = np.asarray(matrices)[:success]
return angles, matrices
def test_nodes():
x = Node()
assert x.name is None
assert x.camera is None
assert x.children == []
assert x.skin is None
assert np.allclose(x.matrix, np.eye(4))
assert x.mesh is None
assert np.allclose(x.rotation, [0, 0, 0, 1])
assert np.allclose(x.scale, np.ones(3))
assert np.allclose(x.translation, np.zeros(3))
assert x.weights is None
assert x.light is None
x.name = 'node'
# Test node light/camera/mesh tests
c = PerspectiveCamera(yfov=2.0)
m = Mesh([])
d = DirectionalLight()
x.camera = c
assert x.camera == c
with pytest.raises(TypeError):
x.camera = m
x.camera = d
x.camera = None
x.mesh = m
assert x.mesh == m
with pytest.raises(TypeError):
x.mesh = c
x.mesh = d
x.light = d
assert x.light == d
with pytest.raises(TypeError):
x.light = m
x.light = c
# Test transformations getters/setters/etc...
# Set up test values
x = np.array([1.0, 0.0, 0.0])
y = np.array([0.0, 1.0, 0.0])
t = np.array([1.0, 2.0, 3.0])
s = np.array([0.5, 2.0, 1.0])
Mx = transformations.rotation_matrix(np.pi / 2.0, x)
qx = np.roll(transformations.quaternion_about_axis(np.pi / 2.0, x), -1)
Mxt = Mx.copy()
Mxt[:3, 3] = t
S = np.eye(4)
S[:3, :3] = np.diag(s)
Mxts = Mxt.dot(S)
My = transformations.rotation_matrix(np.pi / 2.0, y)
qy = np.roll(transformations.quaternion_about_axis(np.pi / 2.0, y), -1)
Myt = My.copy()
Myt[:3, 3] = t
x = Node(matrix=Mx)
assert np.allclose(x.matrix, Mx)
assert np.allclose(x.rotation, qx)
assert np.allclose(x.translation, np.zeros(3))
assert np.allclose(x.scale, np.ones(3))
x.matrix = My
assert np.allclose(x.matrix, My)
assert np.allclose(x.rotation, qy)
assert np.allclose(x.translation, np.zeros(3))
assert np.allclose(x.scale, np.ones(3))
x.translation = t
assert np.allclose(x.matrix, Myt)
assert np.allclose(x.rotation, qy)
x.rotation = qx
assert np.allclose(x.matrix, Mxt)
x.scale = s
assert np.allclose(x.matrix, Mxts)
x = Node(matrix=Mxt)
assert np.allclose(x.matrix, Mxt)
assert np.allclose(x.rotation, qx)
assert np.allclose(x.translation, t)
assert np.allclose(x.scale, np.ones(3))
x = Node(matrix=Mxts)
assert np.allclose(x.matrix, Mxts)
assert np.allclose(x.rotation, qx)
assert np.allclose(x.translation, t)
assert np.allclose(x.scale, s)
# Individual element getters
x.scale[0] = 0
assert np.allclose(x.scale[0], 0)
x.translation[0] = 0
assert np.allclose(x.translation[0], 0)
x.matrix = np.eye(4)
x.matrix[0, 0] = 500
assert x.matrix[0, 0] == 1.0
# Failures
with pytest.raises(ValueError):
x.matrix = 5 * np.eye(4)
with pytest.raises(ValueError):
x.matrix = np.eye(5)
with pytest.raises(ValueError):
x.matrix = np.eye(4).dot([5, 1, 1, 1])
with pytest.raises(ValueError):
x.rotation = np.array([1, 2])
with pytest.raises(ValueError):
x.rotation = np.array([1, 2, 3])
with pytest.raises(ValueError):
x.rotation = np.array([1, 2, 3, 4])
with pytest.raises(ValueError):
x.translation = np.array([1, 2, 3, 4])
with pytest.raises(ValueError):
x.scale = np.array([1, 2, 3, 4])