def predict(self):
rot, trans = RigidTransform.rotation_and_translation_from_matrix(
self.datapoint['obj_pose'])
dataset_pose = RigidTransform(rot, trans, 'obj', 'world')
dataset_final_poses = self.datapoint['final_poses']
num_vertices = self.datapoint['num_vertices']
final_poses = []
for i in range(20):
final_pose_mat = dataset_final_poses[i * 4:(i + 1) * 4]
if np.array_equal(final_pose_mat, np.zeros((4, 4))):
break
rot, trans = RigidTransform.rotation_and_translation_from_matrix(
final_pose_mat)
final_poses.append(RigidTransform(rot, trans, 'obj', 'world'))
vertices = self.datapoint['vertices'][:num_vertices]
normals = self.datapoint['normals'][:num_vertices]
vertices = (self.obj.T_obj_world * dataset_pose.inverse() *
PointCloud(vertices.T, 'world')).data.T
normals = (self.obj.T_obj_world * dataset_pose.inverse() *
PointCloud(normals.T, 'world')).data.T
vertex_probs = self.datapoint[
'vertex_probs'][:num_vertices, :len(final_poses)]
required_forces = self.datapoint['min_required_forces'][:num_vertices]
return vertices, normals, final_poses, vertex_probs, required_forces
def load_object(self, state):
self.obj = state.obj
self.com = self.obj.mesh.center_mass
obj_dataset_name = self.obj.key.split('~')[0]
dataset = self.datasets[obj_dataset_name]
obj_ids = self.obj_ids[obj_dataset_name]
self.datapoint = None
similarity = np.inf
# for i in range(dataset.num_datapoints):
# for i in [101]:
obj_specific_datapoints = self.id_to_datapoint[obj_ids[self.obj.key]]
for _, i in obj_specific_datapoints:
datapoint = dataset.datapoint(i)
if obj_ids[self.obj.key] != datapoint['obj_id']:
continue
rot, trans = RigidTransform.rotation_and_translation_from_matrix(
datapoint['obj_pose'])
obj_pose = RigidTransform(rot, trans, 'obj', 'world')
pose_diff_val = pose_diff(obj_pose, self.obj.T_obj_world)
if pose_diff_val < similarity:
self.datapoint = datapoint
similarity = pose_diff_val
if self.datapoint == None:
raise ValueError('Object {} not in database'.format(self.obj.key))
CONFIG = YamlConfig(TEST_CONFIG_NAME)
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME,
os.path.join(WORK_DIR, "foxarm/common"))
ags = AntipodalGraspSampler(gripper, CONFIG)
##### Generate Grasps #####
unaligned_grasps = ags.generate_grasps(obj, target_num_grasps=100)
print('### Generated %d unaligned grasps! ###' % len(unaligned_grasps))
grasps = {}
stp_mats, stp_probs = mesh.compute_stable_poses(n_samples=1)
stps = []
for stp_mat in stp_mats:
r, t = RigidTransform.rotation_and_translation_from_matrix(stp_mat)
stps.append(RigidTransform(rotation=r, translation=t))
print('### Generated %d stable poses! ###' % len(stps))
for stp_idx, stp in enumerate(stps):
grasps[stp_idx] = []
for grasp in unaligned_grasps:
aligned_grasp = grasp.perpendicular_table(stp)
grasps[stp_idx].append(copy.deepcopy(aligned_grasp))
print('\tStable poses %s has %d grasps!' % (stp_idx, len(grasps[stp_idx])))
print('\n### Grasp Quality ###')
for stp_idx, stp in enumerate(stps):
grasps_test = grasps[stp_idx]
print('### Stable pose %s (%-5.3f) ###' % (stp_idx, stp_probs[stp_idx]))
l_fa1, l_fa2 = [], []
# ----------------------
print 'Computing Topples for obj', obj_id
env.state.material_props._color = np.array([0.5] * 3)
policy.set_environment(env.environment)
obj_keys_to_id[env.state.obj.key] = deepcopy(obj_id)
obj_config = config['state_space']['object']
stable_poses, _ = env.state.obj.mesh.compute_stable_poses(
sigma=obj_config['stp_com_sigma'],
n_samples=obj_config['stp_num_samples'],
threshold=obj_config['stp_min_prob']
)
print 'num poses', len(stable_poses)
for pose_num, pose in enumerate(stable_poses):
print 'Computing Topples for pose', pose_num
rot, trans = RigidTransform.rotation_and_translation_from_matrix(pose)
env.state.obj.T_obj_world = RigidTransform(rot, trans, 'obj', 'world')
# pose = env.state.obj.T_obj_world.matrix
if args.before:
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
skip = raw_input('skip?')
if skip == 'y':
continue
vertices, normals, final_poses, vertex_probs, min_required_forces = policy.compute_topple(env.state)
num_final_poses = len(final_poses)
num_vertices = len(vertices)
if num_final_poses > MAX_FINAL_POSES:
print 'Too Many Poses! Skipping'
def to_rigid(mat):
rot, trans = RigidTransform.rotation_and_translation_from_matrix(mat)
return RigidTransform(rot, trans, 'obj', 'world')
def evaluate_models(models, datasets, obj_id_to_keys, env, use_sensitivity):
y_true, y_pred = [], [[]]
total_datapoints = 0
for dataset, obj_id_to_key in zip(datasets, obj_id_to_keys):
total_datapoints += dataset.num_datapoints
for i in range(dataset.num_datapoints):
datapoint = dataset.datapoint(i)
dataset_name, key = obj_id_to_key[str(
datapoint['obj_id'])].split(KEY_SEP_TOKEN)
obj = env._state_space._database.dataset(dataset_name)[key]
orig_pose = to_rigid(datapoint['obj_pose'])
obj.T_obj_world = orig_pose
env.state.obj = obj
model.load_object(env.state)
predicted_poses, vertex_probs, _ = model.predict(
[datapoint['vertex']],
[datapoint['normal']],
[-datapoint['normal']], # push dir
use_sensitivity=use_sensitivity)
vertex_probs = vertex_probs[0]
y_pred.extend([1 - vertex_probs[0]] * NUM_PER_DATAPOINT)
empirical_dist = []
for i in range(NUM_PER_DATAPOINT):
actual_pose_mat = datapoint['actual_poses'][i * 4:(i + 1) * 4]
rot, trans = RigidTransform.rotation_and_translation_from_matrix(
actual_pose_mat)
pose_to_add = RigidTransform(rot, trans, 'obj', 'world')
y_true.append(
0 if is_equivalent_pose(pose_to_add, orig_pose) else 1)
found = False
for i in range(len(empirical_dist)):
actual_pose, actual_prob = empirical_dist[i]
if is_equivalent_pose(actual_pose, pose_to_add):
empirical_dist[i][1] += .1
found = True
break
if not found:
empirical_dist.append([pose_to_add, .1])
total_variation, l1 = 1, []
for empirical_pose, empirical_prob in empirical_dist:
for predicted_pose, predicted_prob in zip(
predicted_poses, vertex_probs):
if is_equivalent_pose(empirical_pose, predicted_pose):
total_variation = min(
total_variation,
abs(empirical_prob - predicted_prob))
l1.append(abs(empirical_prob - predicted_prob))
break
l1 = np.mean(l1) if len(l1) > 0 else 0
# if i + total_datapoints in test_set:
# test_tvs.append(total_variation)
# test_l1s.append(l1)
# else:
# train_tvs.append(total_variation)
# train_l1s.append(l1)
combined_tvs.append(total_variation)
combined_l1s.append(l1)
# for i in range(10):
# actual_pose_mat = datapoint['actual_poses'][i*4:(i+1)*4]
# rot, trans = RigidTransform.rotation_and_translation_from_matrix(actual_pose_mat)
# actual_pose = RigidTransform(rot, trans, 'obj', 'world')
# y_true.append(0 if is_equivalent_pose(orig_pose, actual_pose) else 1)
# y_pred.append(1-vertex_probs[0])
# # Cross Entropy
# # q_x = 0
# # for predicted_pose, prob in zip(predicted_poses, vertex_probs):
# # if is_equivalent_pose(actual_pose, predicted_pose):
# # q_x = prob
# # break
# # if q_x == 0:
# # counter += 1
# # # env.render_3d_scene()
# # # vis3d.show(title='before', starting_camera_pose=CAMERA_POSE)
# # # env.state.obj.T_obj_world = actual_pose
# # # env.render_3d_scene()
# # # vis3d.show(title='after', starting_camera_pose=CAMERA_POSE)
# # # for predicted_pose, prob in zip(predicted_poses, vertex_probs):
# # # env.state.obj.T_obj_world = predicted_pose
# # # env.render_3d_scene()
# # # title = '{}, pose diff: {}, angle diff: {}, prob: {}'.format(
# # # is_equivalent_pose(actual_pose, predicted_pose),
# # # pose_diff(actual_pose, predicted_pose),
# # # pose_angle(actual_pose, predicted_pose),
# # # prob
# # # )
# # # vis3d.show(title=title, starting_camera_pose=CAMERA_POSE)
# # # env.state.obj.T_obj_world = to_rigid(datapoint['obj_pose'])
# # q_x = max(q_x, 1e-5)
# # cross_entropy -= .1 * np.log(q_x) # p(x) * log(q(x))
# # n += .1
# logger.info('Mean Cross Entropy '+str(cross_entropy/float(n)))
# logger.info('frac 0: {} {} {}'.format(counter, 10*total_datapoints, counter / float(10*total_datapoints)))
# precision, recall, _ = metrics.precision_recall_curve(y_true, y_pred)
# aucs.append(metrics.auc(recall, precision))
# prs.append((precision, recall, model))
# return np.mean(train_tvs), np.mean(test_tvs), np.mean(combined_tvs), np.mean(train_l1s), np.mean(test_l1s), np.mean(combined_l1s)
avg_precision = metrics.average_precision_score(y_true, y_pred)
return combined_tvs, combined_l1s, avg_precision, y_true, y_pred
def generate_dataset(self):
# read parameters
vis_params = self.config["vis"]
camera_filename = vis_params["camera_filename"]
save_dir = vis_params["save_dir"]
coll_check_params = self.config['collision_checking']
approach_dist = coll_check_params['approach_dist']
delta_approach = coll_check_params['delta_approach']
table_offset = coll_check_params['table_offset']
gqcnn_params = self.config['gqcnn']
im_crop_height = gqcnn_params['crop_height']
im_crop_width = gqcnn_params['crop_width']
im_final_height = gqcnn_params['final_height']
im_final_width = gqcnn_params['final_width']
cx_crop = float(im_crop_width) / 2
cy_crop = float(im_crop_height) / 2
image_samples_per_stable_pose = self.config['images_per_stable_pose']
table_mesh_filename = coll_check_params['table_mesh_filename']
table_alignment_params = self.config['table_alignment']
env_rv_params = self.config['env_rv_params']
max_grasp_approach_table_angle = np.deg2rad(
table_alignment_params['max_approach_table_angle'])
table_mesh = trimesh.load_mesh(table_mesh_filename)
T_table_obj = RigidTransform(from_frame='table', to_frame='obj')
scene_objs = {'table': SceneObject(table_mesh, T_table_obj)}
img_idx = 0
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['robust_ferrari_canny'])
# rv: friction core
params_rv = ParamsGaussianRV(quality_config,
quality_config.params_uncertainty)
for obj_idx, obj_path in enumerate(self.obj_paths):
obj_id = self.obj_ids[obj_idx]
result = {}
result[obj_id] = {}
# 1. construct the object
sdf_path = obj_path[:-3] + "sdf"
mesh = trimesh.load_mesh(obj_path)
sdf = SdfFile(sdf_path).read()
plot_scale = 2 * float(np.max(np.abs(mesh.vertices)))
obj = GraspableObject3D(sdf, mesh)
T_obj_world = RigidTransform(from_frame='obj', to_frame='world')
graspable_rv = GraspableObjectPoseGaussianRV(
obj, T_obj_world, quality_config.obj_uncertainty)
result[obj_id]['mesh'] = {}
result[obj_id]['mesh']['vertices'] = mesh.vertices
result[obj_id]['mesh']['triangles'] = mesh.faces
# 2. sample force closure grasps
unaligned_fc_grasps = []
ags = AntipodalGraspSampler(gripper, CONFIG)
unaligned_grasps = ags.generate_grasps(obj, target_num_grasps=100)
for i, grasp in enumerate(unaligned_grasps):
success, c = grasp.close_fingers(obj, check_approach=False)
if success:
c1, c2 = c
if_force_closure = bool(
PointGraspMetrics3D.force_closure(
c1, c2, CONFIG['sampling_friction_coef']))
if if_force_closure:
unaligned_fc_grasps.append(grasp)
# 3. compute stable poses
stp_mats, stp_probs = mesh.compute_stable_poses(n_samples=1)
stp_mats = stp_mats[:10]
stp_probs = stp_probs[:10]
stps = []
for stp_mat in stp_mats:
r, t = RigidTransform.rotation_and_translation_from_matrix(
stp_mat)
stps.append(RigidTransform(rotation=r, translation=t))
result[obj_id]['stable_poses'] = {}
# for each stable pose
grasps = {}
for stp_idx, stp in enumerate(stps):
result[obj_id]['stable_poses']['stp_0'] = {}
grasp_idx = 0
# filter grasps and calculate quality
grasps[stp_idx] = []
for grasp_idx, grasp in enumerate(unaligned_fc_grasps):
# align the grasp
aligned_grasp = grasp.perpendicular_table(stp)
# check perpendicular with table
_, grasp_approach_table_angle, _ = aligned_grasp.grasp_angles_from_stp_z(
stp)
perpendicular_table = (np.abs(grasp_approach_table_angle) <
max_grasp_approach_table_angle)
if not perpendicular_table:
continue
# check collision
success, c = aligned_grasp.close_fingers(obj)
if not success:
continue
# calculate grasp quality
grasp_rv = ParallelJawGraspPoseGaussianRV(
aligned_grasp, quality_config.grasp_uncertainty)
mean_q, std_q = RobustPointGraspMetrics3D.expected_quality(
grasp_rv, graspable_rv, params_rv, quality_config)
grasps[stp_idx].append((aligned_grasp, mean_q))
# render depth images
urv = UniformPlanarWorksurfaceImageRandomVariable(
obj.mesh,
render_modes,
'camera',
env_rv_params,
stable_pose=stp,
scene_objs=scene_objs)
image_samples_per_stable_pose = 1
render_samples = urv.rvs(size=image_samples_per_stable_pose)
render_samples = [render_samples]
# if debug:
if False:
for render_sample in render_samples:
mlab.clf()
camera_t = render_sample.camera.object_to_camera_pose
Vis.plot_camera(camera_filename, camera_t)
t_obj_stp = np.array(
[0, 0, -stp.rotation.dot(stp.translation)[2]])
T_obj_stp = RigidTransform(rotation=stp.rotation,
translation=stp.translation,
from_frame='obj',
to_frame='stp')
stable_mesh = obj.mesh.copy()
stable_mesh.apply_transform(T_obj_stp.matrix)
# obj.mesh.apply_transform(T_obj_stp.matrix)
mag = 2 * float(np.max(np.abs(stable_mesh.vertices)))
Vis.plot_mesh(stable_mesh)
Vis.plot_plane(4 * mag)
Vis.plot_frame(mag)
mlab.view(0, 0, 0.85)
# https://stackoverflow.com/questions/16543634/mayavi-mlab-savefig-gives-an-empty-image
mlab.savefig('%s/%d.jpg' % (save_dir, img_idx))
img_idx += 1
# mlab.show()
for render_sample in render_samples:
depth_im_table = render_sample.renders[
RenderMode.DEPTH_SCENE].image
T_stp_camera = render_sample.camera.object_to_camera_pose
shifted_camera_intr = render_sample.camera.camera_intr
# read pixel offsets
cx = depth_im_table.center[1]
cy = depth_im_table.center[0]
# compute intrinsics for virtual camera of the final
# cropped and rescaled images
camera_intr_scale = float(im_final_height) / float(
im_crop_height)
cropped_camera_intr = shifted_camera_intr.crop(
im_crop_height, im_crop_width, cy, cx)
final_camera_intr = cropped_camera_intr.resize(
camera_intr_scale)
T_obj_camera = T_stp_camera * stp.as_frames(
'obj', T_stp_camera.from_frame)
for grasp_info in grasps[stp_idx]:
grasp = grasp_info[0]
grasp_quality = grasp_info[1]
# get the gripper pose
grasp_2d = grasp.project_camera(
T_obj_camera, shifted_camera_intr)
grasp_depth = grasp_2d.depth
# center images on the grasp, rotate to image x axis
dx = cx - grasp_2d.center.x
dy = cy - grasp_2d.center.y
translation = np.array([dy, dx])
# rotate, crop and resize
depth_im_tf_table = depth_im_table.transform(
translation, grasp_2d.angle)
depth_im_tf_table = depth_im_tf_table.crop(
im_crop_height, im_crop_width)
depth_im_tf_table = depth_im_tf_table.resize(
(im_final_height, im_final_width))
# one sample consists of depth_im_tf_table, grasp_quality, and grasp_depth
# misc.imsave('generated_data/%f.jpg' % grasp_quality, depth_im_tf_table.data)
grasp_key = 'grasp_%d' % grasp_idx
result[obj_id]['stable_poses']['stp_0'][grasp_key] = {}
result[obj_id]['stable_poses']['stp_0'][grasp_key][
'depth_img'] = depth_im_tf_table.data
result[obj_id]['stable_poses']['stp_0'][grasp_key][
'grasp_depth'] = grasp_depth
result[obj_id]['stable_poses']['stp_0'][grasp_key][
'quality'] = grasp_quality
grasp_idx += 1
if self.debug:
break
self.result_queue.put(result)
break
def top_n_actions(self, mesh, obj_name, vis=True):
"""
Takes in a mesh, samples a bunch of grasps on the mesh, evaluates them using the
metric given in the constructor, and returns the best grasps for the mesh. SHOULD
RETURN GRASPS IN ORDER OF THEIR GRASP QUALITY.
You should try to use mesh.mass to get the mass of the object. You should check the
output of this, because from this
https://github.com/BerkeleyAutomation/trimesh/blob/master/trimesh/base.py#L2203
it would appear that the mass is approximated using the volume of the object. If it
is not returning reasonable results, you can manually weight the objects and store
them in the dictionary at the top of the file.
Parameters
----------
mesh : :obj:`Trimesh`
vis : bool
Whether or not to visualize the top grasps
Returns
-------
:obj:`list` of :obj:`autolab_core.RigidTransform`
the matrices T_grasp_world, which represents the hand poses of the baxter / sawyer
which would result in the fingers being placed at the vertices of the best grasps
"""
# Some objects have vertices in odd places, so you should sample evenly across
# the mesh to get nicer candidate grasp points using trimesh.sample.sample_surface_even()
points, face_indices = trimesh.sample.sample_surface_even(
mesh, self.n_vert) # (frame: object)
normals = mesh.face_normals[face_indices] # (frame: object)
grasp_vertices, grasp_normals = self.sample_grasps(
points, normals) # (frame: object)
scores = self.score_grasps(grasp_vertices, grasp_normals, mesh.mass)
top_k_scores_idx = sorted(zip(scores, range(len(scores))),
key=lambda x: x[0],
reverse=True)
top_grasp_qualities = [s for s, i in top_k_scores_idx][:self.n_execute]
top_idx = [i for s, i in top_k_scores_idx][:self.n_execute]
top_grasp_vertices = np.array([grasp_vertices[i] for i in top_idx])
top_grasp_normals = np.array([grasp_normals[i] for i in top_idx])
# Get the hand poses
poses = []
for grasp_verts, grasp_norm in zip(top_grasp_vertices,
top_grasp_normals):
# grasp_verts = np.array([gv - mesh.bounding_box.centroid for gv in grasp_verts]).reshape(grasp_verts.shape)
poses.append(
self.vertices_to_baxter_hand_pose(grasp_verts, grasp_norm))
# tf_inv = self.T_obj_world.inverse()
# world_poses1 = [tf_inv.matrix.dot(pose.matrix) for pose in poses]
# world_poses2 = [pose * tf_inv for pose in poses]
# combos = {}
# for pose_i, pose_option in enumerate([poses[0].matrix, poses[0].inverse().matrix]):
# for t_obj_world_i, t_obj_world_option in enumerate([self.T_obj_world.matrix, self.T_obj_world.inverse().matrix]):
# key0 = (pose_i, t_obj_world_i, 0)
# combos[key0] = pose_option.dot(t_obj_world_option)
# key1 = (pose_i, t_obj_world_i, 1)
# combos[key1] = t_obj_world_option.dot(pose_option)
# for key, value in combos.items():
# print key
# print value
# print ''
adjusted_poses = []
world_poses = []
for pose in poses:
rotation, translation = RigidTransform.rotation_and_translation_from_matrix(
self.T_obj_world.inverse().matrix.dot(pose.inverse().matrix))
pose_world = RigidTransform(rotation=rotation,
translation=translation)
world_pose = get_approach_rt(pose_world)
world_poses.append(world_pose)
adjusted_rotation, adjusted_translation = RigidTransform.rotation_and_translation_from_matrix(
world_pose.matrix.dot(self.T_obj_world.matrix))
adjusted_pose = RigidTransform(rotation=adjusted_rotation,
translation=adjusted_translation)
adjusted_poses.append(adjusted_pose)
# Visualize the grasps
if vis:
self.vis(mesh, top_grasp_vertices, top_grasp_normals,
top_grasp_qualities, poses)
# import pdb; pdb.set_trace()
return world_poses
