def new_graph(G):
for node_id, node in G.nodes(data=True):
if node['node_type'] == 'action':
continue
print 'Pose {}, GQ: {}'.format(node_id, node['gq'])
env.state.obj.T_obj_world = node['pose']
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
def noise_vis():
print 'best actions', np.max(action.metadata['vertex_probs'], axis=0)
render_3d_scene(env)
j = 113
j = np.argmax(action.metadata['vertex_probs'][:, 1])
a = j * policy.toppling_model.n_trials
b = (j + 1) * policy.toppling_model.n_trials
for i in range(a, b):
start = policy.toppling_model.vertices[i]
end = start - .01 * policy.toppling_model.push_directions[i]
vis3d.plot([start, end], color=[0, 1, 0], radius=.0002)
vis3d.show(starting_camera_pose=camera_pose())
def vis_topple_probs(vertices, topple_probs):
vis3d.figure()
env.render_3d_scene()
for vertex, prob in zip(vertices, topple_probs):
color = [min(1, 2*(1-prob)), min(2*prob, 1), 0]
vis3d.points(Point(vertex, 'world'), scale=.001, color=color)
for bottom_point in policy.toppling_model.bottom_points:
vis3d.points(Point(bottom_point, 'world'), scale=.001, color=[0,0,0])
vis3d.show(starting_camera_pose=CAMERA_POSE)
def test_multi_push(env):
env = GraspingEnv(config, config['vis'])
policy = MultiTopplePolicy(config, use_sensitivity=True)
config['model']['load'] = 0
rand_policy = RandomTopplePolicy(config, use_sensitivity=True)
with open("/nfs/diskstation/db/toppling/tuned_params/obj_ids.json",
"r") as read_file:
obj_ids = json.load(read_file)
while True:
env.reset()
if env.state.obj.key not in obj_ids:
continue
env.state.material_props._color = np.array([0.5] * 3)
policy.set_environment(env.environment)
rand_policy.set_environment(env.environment)
if args.before:
vis3d.figure()
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
planning_time = policy.action(env.state)
logger.info(env.state.obj.key)
logger.info('Value Iteration')
path = forward_sim(policy.G, 'Value Iteration', 'value')
logger.info('Value Iteration Path: ' + str(path))
logger.info('Value Iteration Path Length: ' + str(len(path) - 1))
logger.info('Value Iteration Planning Time: ' + str(planning_time))
logger.info('Value Iteration No Actions: ' +
str(len(policy.G.nodes()) == 1))
logger.info('Value Iteration Already Best: ' +
str(len(policy.G.nodes()) > 1 and len(path) == 1))
logger.info('Greedy')
path = forward_sim(policy.G, 'Greedy', 'single_push_q')
logger.info('Greedy Path: ' + str(path))
logger.info('Greedy Path Length: ' + str(len(path) - 1))
logger.info('Greedy Planning Time: ' + str(
np.sum(
[policy.G.nodes[node_id]['planning_time']
for node_id in path])))
logger.info('Greedy No Actions: ' + str(len(policy.G.nodes()) == 1))
logger.info('Greedy Already Best: ' +
str((len(policy.G.nodes()) > 1 and len(path) == 1)))
a = time()
path_length, planning_time = forward_sim_random(env, rand_policy)
logger.info('Random Path Length: ' + str(path_length))
logger.info('Random Planning Time: ' + str(planning_time))
logger.info('\n')
def compute_topple(self, state):
# raise NotImplementedError
mesh = state.mesh.copy().apply_transform(state.T_obj_world.matrix)
mesh.fix_normals()
vertices, face_ind = sample.sample_surface_even(mesh, self.num_samples)
# Cut out vertices that are too close to the ground
z_comp = vertices[:,2]
valid_vertex_ind = z_comp > (1-self.thresh)*np.min(z_comp) + self.thresh*np.max(z_comp)
vertices, face_ind = vertices[valid_vertex_ind], face_ind[valid_vertex_ind]
normals = mesh.face_normals[face_ind]
push_directions = -deepcopy(normals)
self.toppling_model.load_object(state)
poses, vertex_probs, min_required_forces = self.toppling_model.predict(
vertices,
normals,
push_directions,
use_sensitivity=self.use_sensitivity
)
from dexnet.visualization import DexNetVisualizer3D as vis3d
from autolab_core import Point
# j = 6 # gearbox
j = 50 # vase
a = j*self.toppling_model.n_trials
b = (j+1)*self.toppling_model.n_trials
for i in range(a,b):
start = self.toppling_model.vertices[i]
end = start - .01 * self.toppling_model.push_directions[i]
vis3d.plot3d([start, end], color=[0,1,0], tube_radius=.0002)
vis3d.mesh(state.mesh, state.T_obj_world)
vis3d.show()
return vertices, normals, poses, vertex_probs, min_required_forces
def vis_axes(origin, y_dir):
y = [origin, origin + .0075 * y_dir]
z = [origin, origin + .0075 * up]
x = [origin, origin + .0075 * np.cross(y_dir, up)]
vis3d.plot3d(x, color=[1, 0, 0], tube_radius=.0006)
vis3d.plot3d(y, color=[0, 1, 0], tube_radius=.0006)
vis3d.plot3d(z, color=[0, 0, 1], tube_radius=.0006)
def display_stable_poses(self, object_name, config=None):
"""Display an object's stable poses
Parameters
----------
object_name : :obj:`str`
Object to display.
config : :obj:`dict`
Configuration dict for visualization.
Parameters are in Other parameters. Values from self.default_config are used for keys not provided.
Other Parameters
----------------
animate
Whether or not to animate the displayed object
Raises
------
ValueError
invalid object name
RuntimeError
Database or dataset not opened.
"""
self._check_opens()
config = self._get_config(config)
if object_name not in self.dataset.object_keys:
raise ValueError(
"{} is not a valid object name".format(object_name))
logger.info('Displaying stable poses for'.format(object_name))
obj = self.dataset[object_name]
stable_poses = self.dataset.stable_poses(object_name)
for stable_pose in stable_poses:
print 'Stable pose %s with p=%.3f' % (stable_pose.id,
stable_pose.p)
vis.figure()
vis.mesh_stable_pose(obj.mesh,
stable_pose,
color=(0.5, 0.5, 0.5),
style='surface')
vis.pose(RigidTransform(), alpha=0.15)
vis.show(animate=config['animate'])
def sim_to_real_tf(sim_point_cloud_masked, vis=False):
# Capture point cloud from physical depth camera
_, depth_im, _ = phoxi_sensor.frames()
phys_point_cloud = phoxi_tf*phoxi_sensor.ir_intrinsics.deproject(depth_im)
phys_point_cloud_masked, _ = phys_point_cloud.box_mask(mask_box)
if phys_point_cloud_masked.num_points == 0:
logging.warn('Object not found! Skipping...')
quit()
pcs_config = {'overlap': 0.6, 'accuracy': 0.001, 'samples': 500, 'timeout': 10, 'cache_dir': '/home/chriscorrea14/Super4PCS/cache'}
pcs_aligner = Super4PCSAligner(pcs_config)
sim_to_real_tf = pcs_aligner.align(phys_point_cloud_masked, sim_point_cloud_masked)
if vis:
vis3d.figure()
vis3d.points(phys_point_cloud_masked.data.T, color=(1,0,0), scale=.001)
vis3d.points((sim_to_real_tf*sim_point_cloud_masked).data.T, color=(0,1,0), scale=.001)
vis3d.show(title='Predicted pose from Sim 2 Real')
return sim_to_real_tf
def save_samples(self, sample, grasps, T_obj_stp, collision_checker, grasp_metrics, stable_pose):
T_stp_camera = sample.camera.object_to_camera_pose
self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames('obj', T_stp_camera.from_frame)
aligned_grasps = self.align_grasps_with_camera(grasps)
depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image
camera_pose = self.get_camera_pose(sample)
self.tensor_datapoint['camera_poses'] = camera_pose
shifted_camera_intr = sample.camera.camera_intr
for cnt, aligned_grasp in enumerate(aligned_grasps):
collision_free = self.is_grasp_collision_free(aligned_grasp, collision_checker)
# Project grasp coordinates in image
grasp_2d = aligned_grasp.project_camera(self.T_obj_camera, shifted_camera_intr)
grasp_point = aligned_grasp.close_fingers(self.obj, vis=False, check_approach=False)
if not grasp_point[0]:
Warning("Could not find contact points")
continue
# Get grasp width in pixel from endpoints
p_1 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][0].point, 1).T).T[:3]
p_2 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][1].point, 1).T).T[:3]
grasp_width_px = self.width_px(shifted_camera_intr, p_1, p_2)
grasp_width = aligned_grasp.width_from_endpoints(grasp_point[1][0].point, grasp_point[1][1].point)
depth_im_no_rot = self.prepare_images(depth_im_table, grasp_2d)
T_grasp_camera = aligned_grasp.gripper_pose(self.gripper).inverse() * self.T_obj_camera.inverse()
if VISUALISE_3D:
z_axis = T_grasp_camera.z_axis
theta = np.rad2deg(np.arccos((z_axis[2]) /
(np.sqrt((z_axis[0] ** 2 + z_axis[1] ** 2 + z_axis[2] ** 2)))))
vis.figure()
T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh,
stable_pose.T_obj_world, style='surface', dim=0.5, plot_table=True)
T_camera_world = T_obj_world * self.T_obj_camera.inverse()
vis.gripper(self.gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3),
T_camera_world=T_camera_world)
print(theta)
vis.show()
hand_pose = self.get_hand_pose(grasp_2d, T_grasp_camera, grasp_width, grasp_width_px)
self.add_datapoint(depth_im_no_rot, hand_pose,
collision_free, aligned_grasp, grasp_metrics)
self.cur_image_label += 1
def visualize(env, datasets, obj_id_to_keys, models, model_names,
use_sensitivities):
for dataset, obj_id_to_key in zip(datasets, obj_id_to_keys):
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
actual = 0
for i in range(NUM_PER_DATAPOINT):
mat = datapoint['actual_poses'][i * 4:(i + 1) * 4]
actual_pose = to_rigid(mat)
if not is_equivalent_pose(actual_pose, orig_pose):
actual += 1
actual /= float(NUM_PER_DATAPOINT)
probs = []
for model, model_name, use_sensitivity in zip(
models, model_names, use_sensitivities):
model.load_object(env.state)
_, vertex_probs, _ = model.predict(
[datapoint['vertex']],
[datapoint['normal']],
[-datapoint['normal']], # push dir
use_sensitivity=use_sensitivity)
probs.append(1 - vertex_probs[0, 0])
if -(abs(probs[3] - actual) - abs(probs[2] - actual)) > .1:
print 'actual {} {} {} {} {} {} {} {} {}'.format(
actual, model_names[0], probs[0], model_names[1], probs[1],
model_names[2], probs[2], model_names[3], probs[3])
env.render_3d_scene()
start_point = datapoint['vertex'] + .06 * datapoint['normal']
end_point = datapoint['vertex']
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0],
[0, 0, 1]]).dot(-datapoint['normal'])
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0],
[0, 0, 1]]).dot(-datapoint['normal'])
head_points = [
end_point - 0.02 * h2, end_point, end_point - 0.02 * h1
]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.002)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.002)
vis3d.show()
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 display_object(self, object_name, config=None):
"""Display an object
Parameters
----------
object_name : :obj:`str`
Ob
ject to display.
config : :obj:`dict`
Configuration dict for visualization.
Parameters are in Other parameters. Values from self.default_config are used for keys not provided.
Other Parameters
----------------
animate
Whether or not to animate the displayed object
Raises
------
ValueError
invalid object name
RuntimeError
Database or dataset not opened.
"""
self._check_opens()
config = self._get_config(config)
if object_name not in self.dataset.object_keys:
raise ValueError(
"{} is not a valid object name".format(object_name))
logger.info('Displaying {}'.format(object_name))
obj = self.dataset[object_name]
vis.figure(bgcolor=(1, 1, 1), size=(1000, 1000))
vis.mesh(obj.mesh, color=(0.5, 0.5, 0.5), style='surface')
vis.show(animate=config['animate'])
def figure_0():
action = policy.action(env.state)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.001)
mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
mesh.fix_normals()
direction = normalize([0, -.04, 0])
origin = mesh.center_mass + np.array([0, .04, .09])
intersect, _, face_ind = mesh.ray.intersects_location([origin],
[direction],
multiple_hits=False)
normal = mesh.face_normals[face_ind[0]]
start_point = intersect[0] + .06 * normal
end_point = intersect[0]
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-normal)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-normal)
head_points = [end_point - 0.02 * h2, end_point, end_point - 0.02 * h1]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.002)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.002)
vis3d.points(Point(end_point), scale=.004, color=[0, 0, 0])
hand_pose = RigidTransform(rotation=policy.get_hand_pose(
start_point, end_point)[0].rotation,
translation=start_point,
from_frame='grasp',
to_frame='world')
orig_pose = env.state.obj.T_obj_world.copy()
env.state.obj.T_obj_world = policy.toppling_model.final_poses[1]
action = policy.grasping_policy.action(env.state)
env.state.obj.T_obj_world = orig_pose
gripper = env.gripper(action)
#vis3d.mesh(gripper.mesh, hand_pose * gripper.T_mesh_grasp, color=light_blue)
# mesh = trimesh.load('~/Downloads/chris.stl')
rot = np.array([[0, -1, 0], [1, 0, 0],
[0, 0,
1]]).dot(np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]))
T = RigidTransform(rotation=rot,
translation=np.array([0, -.09, .1]),
to_frame='mesh',
from_frame='mesh')
# vis3d.mesh(mesh, hand_pose * gripper.T_mesh_grasp * T, color=light_blue)
vis3d.mesh(gripper.mesh,
hand_pose * gripper.T_mesh_grasp * T,
color=light_blue)
vis3d.show(starting_camera_pose=CAMERA_POSE)
env.state.obj.T_obj_world = policy.toppling_model.final_poses[1]
action = policy.grasping_policy.action(env.state)
print 'q:', action.q_value
env.render_3d_scene()
vis3d.gripper(env.gripper(action),
action.grasp(env.gripper(action)),
color=light_blue)
vis3d.show(starting_camera_pose=CAMERA_POSE)
def render_3d_scene(env):
vis3d.mesh(env.state.mesh,
env.state.T_obj_world.matrix,
color=env.state.color)
def generate_gqcnn_dataset(dataset_path, database, target_object_keys,
env_rv_params, gripper_name, config):
"""
Generates a GQ-CNN TensorDataset for training models with new grippers, quality metrics, objects, and cameras.
Parameters
----------
dataset_path : str
path to save the dataset to
database : :obj:`Hdf5Database`
Dex-Net database containing the 3D meshes, grasps, and grasp metrics
target_object_keys : :obj:`OrderedDict`
dictionary mapping dataset names to target objects (either 'all' or a list of specific object keys)
env_rv_params : :obj:`OrderedDict`
parameters of the camera and object random variables used in sampling (see meshpy_berkeley.UniformPlanarWorksurfaceImageRandomVariable for more info)
gripper_name : str
name of the gripper to use
config : :obj:`autolab_core.YamlConfig`
other parameters for dataset generation
Notes
-----
Required parameters of config are specified in Other Parameters
Other Parameters
----------------
images_per_stable_pose : int
number of object and camera poses to sample for each stable pose
stable_pose_min_p : float
minimum probability of occurrence for a stable pose to be used in data generation (used to prune bad stable poses
gqcnn/crop_width : int
width, in pixels, of crop region around each grasp center, before resize (changes the size of the region seen by the GQ-CNN)
gqcnn/crop_height : int
height, in pixels, of crop region around each grasp center, before resize (changes the size of the region seen by the GQ-CNN)
gqcnn/final_width : int
width, in pixels, of final transformed grasp image for input to the GQ-CNN (defaults to 32)
gqcnn/final_height : int
height, in pixels, of final transformed grasp image for input to the GQ-CNN (defaults to 32)
table_alignment/max_approach_table_angle : float
max angle between the grasp axis and the table normal when the grasp approach is maximally aligned with the table normal
table_alignment/max_approach_offset : float
max deviation from perpendicular approach direction to use in grasp collision checking
table_alignment/num_approach_offset_samples : int
number of approach samples to use in collision checking
collision_checking/table_offset : float
max allowable interpenetration between the gripper and table to be considered collision free
collision_checking/table_mesh_filename : str
path to a table mesh for collision checking (default data/meshes/table.obj)
collision_checking/approach_dist : float
distance, in meters, between the approach pose and final grasp pose along the grasp axis
collision_checking/delta_approach : float
amount, in meters, to discretize the straight-line path from the gripper approach pose to the final grasp pose
tensors/datapoints_per_file : int
number of datapoints to store in each unique tensor file on disk
tensors/fields : :obj:`dict`
dictionary mapping field names to dictionaries specifying the data type, height, width, and number of channels for each tensor
debug : bool
True (or 1) if the random seed should be set to enforce deterministic behavior, False (0) otherwise
vis/candidate_grasps : bool
True (or 1) if the collision free candidate grasps should be displayed in 3D (for debugging)
vis/rendered_images : bool
True (or 1) if the rendered images for each stable pose should be displayed (for debugging)
vis/grasp_images : bool
True (or 1) if the transformed grasp images should be displayed (for debugging)
"""
# read data gen params
output_dir = dataset_path
gripper = RobotGripper.load(gripper_name)
image_samples_per_stable_pose = config['images_per_stable_pose']
stable_pose_min_p = config['stable_pose_min_p']
# read gqcnn params
gqcnn_params = 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
# open database
dataset_names = target_object_keys.keys()
datasets = [database.dataset(dn) for dn in dataset_names]
# set target objects
for dataset in datasets:
if target_object_keys[dataset.name] == 'all':
target_object_keys[dataset.name] = dataset.object_keys
# setup grasp params
table_alignment_params = config['table_alignment']
min_grasp_approach_offset = -np.deg2rad(
table_alignment_params['max_approach_offset'])
max_grasp_approach_offset = np.deg2rad(
table_alignment_params['max_approach_offset'])
max_grasp_approach_table_angle = np.deg2rad(
table_alignment_params['max_approach_table_angle'])
num_grasp_approach_samples = table_alignment_params[
'num_approach_offset_samples']
phi_offsets = []
if max_grasp_approach_offset == min_grasp_approach_offset:
phi_inc = 1
elif num_grasp_approach_samples == 1:
phi_inc = max_grasp_approach_offset - min_grasp_approach_offset + 1
else:
phi_inc = (max_grasp_approach_offset - min_grasp_approach_offset) / (
num_grasp_approach_samples - 1)
phi = min_grasp_approach_offset
while phi <= max_grasp_approach_offset:
phi_offsets.append(phi)
phi += phi_inc
# setup collision checking
coll_check_params = config['collision_checking']
approach_dist = coll_check_params['approach_dist']
delta_approach = coll_check_params['delta_approach']
table_offset = coll_check_params['table_offset']
table_mesh_filename = coll_check_params['table_mesh_filename']
if not os.path.isabs(table_mesh_filename):
table_mesh_filename = os.path.join(
os.path.dirname(os.path.realpath(__file__)), '..',
table_mesh_filename)
table_mesh = ObjFile(table_mesh_filename).read()
# set tensor dataset config
tensor_config = config['tensors']
tensor_config['fields']['depth_ims_tf_table']['height'] = im_final_height
tensor_config['fields']['depth_ims_tf_table']['width'] = im_final_width
tensor_config['fields']['obj_masks']['height'] = im_final_height
tensor_config['fields']['obj_masks']['width'] = im_final_width
# add available metrics (assuming same are computed for all objects)
metric_names = []
dataset = datasets[0]
obj_keys = dataset.object_keys
if len(obj_keys) == 0:
raise ValueError('No valid objects in dataset %s' % (dataset.name))
obj = dataset[obj_keys[0]]
grasps = dataset.grasps(obj.key, gripper=gripper.name)
grasp_metrics = dataset.grasp_metrics(obj.key,
grasps,
gripper=gripper.name)
metric_names = grasp_metrics[grasp_metrics.keys()[0]].keys()
for metric_name in metric_names:
tensor_config['fields'][metric_name] = {}
tensor_config['fields'][metric_name]['dtype'] = 'float32'
# init tensor dataset
tensor_dataset = TensorDataset(output_dir, tensor_config)
tensor_datapoint = tensor_dataset.datapoint_template
# setup log file
experiment_log_filename = os.path.join(output_dir,
'dataset_generation.log')
formatter = logging.Formatter('%(asctime)s %(levelname)s: %(message)s')
hdlr = logging.FileHandler(experiment_log_filename)
hdlr.setFormatter(formatter)
logging.getLogger().addHandler(hdlr)
root_logger = logging.getLogger()
# copy config
out_config_filename = os.path.join(output_dir, 'dataset_generation.json')
ordered_dict_config = collections.OrderedDict()
for key in config.keys():
ordered_dict_config[key] = config[key]
with open(out_config_filename, 'w') as outfile:
json.dump(ordered_dict_config, outfile)
# 1. Precompute the set of valid grasps for each stable pose:
# i) Perpendicular to the table
# ii) Collision-free along the approach direction
# load grasps if they already exist
grasp_cache_filename = os.path.join(output_dir, CACHE_FILENAME)
if os.path.exists(grasp_cache_filename):
logging.info('Loading grasp candidates from file')
candidate_grasps_dict = pkl.load(open(grasp_cache_filename, 'rb'))
# otherwise re-compute by reading from the database and enforcing constraints
else:
# create grasps dict
candidate_grasps_dict = {}
# loop through datasets and objects
for dataset in datasets:
logging.info('Reading dataset %s' % (dataset.name))
for obj in dataset:
if obj.key not in target_object_keys[dataset.name]:
continue
# init candidate grasp storage
candidate_grasps_dict[obj.key] = {}
# setup collision checker
collision_checker = GraspCollisionChecker(gripper)
collision_checker.set_graspable_object(obj)
# read in the stable poses of the mesh
stable_poses = dataset.stable_poses(obj.key)
for i, stable_pose in enumerate(stable_poses):
# render images if stable pose is valid
if stable_pose.p > stable_pose_min_p:
candidate_grasps_dict[obj.key][stable_pose.id] = []
# setup table in collision checker
T_obj_stp = stable_pose.T_obj_table.as_frames(
'obj', 'stp')
T_obj_table = obj.mesh.get_T_surface_obj(
T_obj_stp,
delta=table_offset).as_frames('obj', 'table')
T_table_obj = T_obj_table.inverse()
collision_checker.set_table(table_mesh_filename,
T_table_obj)
# read grasp and metrics
grasps = dataset.grasps(obj.key, gripper=gripper.name)
logging.info(
'Aligning %d grasps for object %s in stable %s' %
(len(grasps), obj.key, stable_pose.id))
# align grasps with the table
aligned_grasps = [
grasp.perpendicular_table(stable_pose)
for grasp in grasps
]
# check grasp validity
logging.info(
'Checking collisions for %d grasps for object %s in stable %s'
% (len(grasps), obj.key, stable_pose.id))
for aligned_grasp in aligned_grasps:
# check angle with table plane and skip unaligned grasps
_, grasp_approach_table_angle, _ = aligned_grasp.grasp_angles_from_stp_z(
stable_pose)
perpendicular_table = (
np.abs(grasp_approach_table_angle) <
max_grasp_approach_table_angle)
if not perpendicular_table:
continue
# check whether any valid approach directions are collision free
collision_free = False
for phi_offset in phi_offsets:
rotated_grasp = aligned_grasp.grasp_y_axis_offset(
phi_offset)
collides = collision_checker.collides_along_approach(
rotated_grasp, approach_dist,
delta_approach)
if not collides:
collision_free = True
break
# store if aligned to table
candidate_grasps_dict[obj.key][
stable_pose.id].append(
GraspInfo(aligned_grasp, collision_free))
# visualize if specified
if collision_free and config['vis'][
'candidate_grasps']:
logging.info('Grasp %d' % (aligned_grasp.id))
vis.figure()
vis.gripper_on_object(gripper, aligned_grasp,
obj,
stable_pose.T_obj_world)
vis.show()
# save to file
logging.info('Saving to file')
pkl.dump(candidate_grasps_dict, open(grasp_cache_filename, 'wb'))
# 2. Render a dataset of images and associate the gripper pose with image coordinates for each grasp in the Dex-Net database
# setup variables
obj_category_map = {}
pose_category_map = {}
cur_pose_label = 0
cur_obj_label = 0
cur_image_label = 0
# render images for each stable pose of each object in the dataset
render_modes = [RenderMode.SEGMASK, RenderMode.DEPTH_SCENE]
for dataset in datasets:
logging.info('Generating data for dataset %s' % (dataset.name))
# iterate through all object keys
object_keys = dataset.object_keys
for obj_key in object_keys:
obj = dataset[obj_key]
if obj.key not in target_object_keys[dataset.name]:
continue
# read in the stable poses of the mesh
stable_poses = dataset.stable_poses(obj.key)
for i, stable_pose in enumerate(stable_poses):
# render images if stable pose is valid
if stable_pose.p > stable_pose_min_p:
# log progress
logging.info('Rendering images for object %s in %s' %
(obj.key, stable_pose.id))
# add to category maps
if obj.key not in obj_category_map.keys():
obj_category_map[obj.key] = cur_obj_label
pose_category_map['%s_%s' %
(obj.key,
stable_pose.id)] = cur_pose_label
# read in candidate grasps and metrics
candidate_grasp_info = candidate_grasps_dict[obj.key][
stable_pose.id]
candidate_grasps = [g.grasp for g in candidate_grasp_info]
grasp_metrics = dataset.grasp_metrics(obj.key,
candidate_grasps,
gripper=gripper.name)
# compute object pose relative to the table
T_obj_stp = stable_pose.T_obj_table.as_frames('obj', 'stp')
T_obj_stp = obj.mesh.get_T_surface_obj(T_obj_stp)
# sample images from random variable
T_table_obj = RigidTransform(from_frame='table',
to_frame='obj')
scene_objs = {
'table': SceneObject(table_mesh, T_table_obj)
}
urv = UniformPlanarWorksurfaceImageRandomVariable(
obj.mesh,
render_modes,
'camera',
env_rv_params,
stable_pose=stable_pose,
scene_objs=scene_objs)
render_start = time.time()
render_samples = urv.rvs(
size=image_samples_per_stable_pose)
render_stop = time.time()
logging.info('Rendering images took %.3f sec' %
(render_stop - render_start))
# visualize
if config['vis']['rendered_images']:
d = int(np.ceil(
np.sqrt(image_samples_per_stable_pose)))
# binary
vis2d.figure()
for j, render_sample in enumerate(render_samples):
vis2d.subplot(d, d, j + 1)
vis2d.imshow(render_sample.renders[
RenderMode.SEGMASK].image)
# depth table
vis2d.figure()
for j, render_sample in enumerate(render_samples):
vis2d.subplot(d, d, j + 1)
vis2d.imshow(render_sample.renders[
RenderMode.DEPTH_SCENE].image)
vis2d.show()
# tally total amount of data
num_grasps = len(candidate_grasps)
num_images = image_samples_per_stable_pose
num_save = num_images * num_grasps
logging.info('Saving %d datapoints' % (num_save))
# for each candidate grasp on the object compute the projection
# of the grasp into image space
for render_sample in render_samples:
# read images
binary_im = render_sample.renders[
RenderMode.SEGMASK].image
depth_im_table = render_sample.renders[
RenderMode.DEPTH_SCENE].image
# read camera params
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)
# create a thumbnail for each grasp
for grasp_info in candidate_grasp_info:
# read info
grasp = grasp_info.grasp
collision_free = grasp_info.collision_free
# get the gripper pose
T_obj_camera = T_stp_camera * T_obj_stp.as_frames(
'obj', T_stp_camera.from_frame)
grasp_2d = grasp.project_camera(
T_obj_camera, shifted_camera_intr)
# 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])
binary_im_tf = binary_im.transform(
translation, grasp_2d.angle)
depth_im_tf_table = depth_im_table.transform(
translation, grasp_2d.angle)
# crop to image size
binary_im_tf = binary_im_tf.crop(
im_crop_height, im_crop_width)
depth_im_tf_table = depth_im_tf_table.crop(
im_crop_height, im_crop_width)
# resize to image size
binary_im_tf = binary_im_tf.resize(
(im_final_height, im_final_width),
interp='nearest')
depth_im_tf_table = depth_im_tf_table.resize(
(im_final_height, im_final_width))
# visualize the transformed images
if config['vis']['grasp_images']:
grasp_center = Point(
depth_im_tf_table.center,
frame=final_camera_intr.frame)
tf_grasp_2d = Grasp2D(
grasp_center,
0,
grasp_2d.depth,
width=gripper.max_width,
camera_intr=final_camera_intr)
# plot 2D grasp image
vis2d.figure()
vis2d.subplot(2, 2, 1)
vis2d.imshow(binary_im)
vis2d.grasp(grasp_2d)
vis2d.subplot(2, 2, 2)
vis2d.imshow(depth_im_table)
vis2d.grasp(grasp_2d)
vis2d.subplot(2, 2, 3)
vis2d.imshow(binary_im_tf)
vis2d.grasp(tf_grasp_2d)
vis2d.subplot(2, 2, 4)
vis2d.imshow(depth_im_tf_table)
vis2d.grasp(tf_grasp_2d)
vis2d.title('Coll Free? %d' %
(grasp_info.collision_free))
vis2d.show()
# plot 3D visualization
vis.figure()
T_obj_world = vis.mesh_stable_pose(
obj.mesh,
stable_pose.T_obj_world,
style='surface',
dim=0.5)
vis.gripper(gripper,
grasp,
T_obj_world,
color=(0.3, 0.3, 0.3))
vis.show()
# form hand pose array
hand_pose = np.r_[grasp_2d.center.y,
grasp_2d.center.x,
grasp_2d.depth, grasp_2d.angle,
grasp_2d.center.y -
shifted_camera_intr.cy,
grasp_2d.center.x -
shifted_camera_intr.cx,
grasp_2d.width_px]
# store to data buffers
tensor_datapoint[
'depth_ims_tf_table'] = depth_im_tf_table.raw_data
tensor_datapoint[
'obj_masks'] = binary_im_tf.raw_data
tensor_datapoint['hand_poses'] = hand_pose
tensor_datapoint['collision_free'] = collision_free
tensor_datapoint['obj_labels'] = cur_obj_label
tensor_datapoint['pose_labels'] = cur_pose_label
tensor_datapoint['image_labels'] = cur_image_label
for metric_name, metric_val in grasp_metrics[
grasp.id].iteritems():
coll_free_metric = (
1 * collision_free) * metric_val
tensor_datapoint[
metric_name] = coll_free_metric
tensor_dataset.add(tensor_datapoint)
# update image label
cur_image_label += 1
# update pose label
cur_pose_label += 1
# force clean up
gc.collect()
# update object label
cur_obj_label += 1
# force clean up
gc.collect()
# save last file
tensor_dataset.flush()
# save category mappings
obj_cat_filename = os.path.join(output_dir, 'object_category_map.json')
json.dump(obj_category_map, open(obj_cat_filename, 'w'))
pose_cat_filename = os.path.join(output_dir, 'pose_category_map.json')
json.dump(pose_category_map, open(pose_cat_filename, 'w'))
def benchmark_bin_picking_policy(policy,
# input_dataset_path,
# heap_ids,
# timesteps,
# output_dataset_path,
config,
# excluded_heaps_file):
):
""" Benchmark a bin picking policy.
Parameters
----------
policy : :obj:`Policy`
policy to roll out
input_dataset_path : str
path to the input dataset
heap_ids : list
integer identifiers for the heaps to re-run
timesteps : list
integer timesteps to seed the simulation from
output_dataset_path : str
path to store the results
config : dict
dictionary-like objects containing parameters of the simulator and visualization
"""
# read subconfigs
vis_config = config['vis']
dataset_config = config['dataset']
# read parameters
fully_observed = config['fully_observed']
steps_per_test_case = config['steps_per_test_case']
rollouts_per_garbage_collect = config['rollouts_per_garbage_collect']
debug = config['debug']
im_height = config['state_space']['camera']['im_height']
im_width = config['state_space']['camera']['im_width']
max_obj_per_pile = config['state_space']['object']['max_obj_per_pile']
if debug:
random.seed(SEED)
np.random.seed(SEED)
# read ids
# if len(heap_ids) != len(timesteps):
# raise ValueError('Must provide same number of heap ids and timesteps')
# num_rollouts = len(heap_ids)
num_rollouts = 1
# set dataset params
tensor_config = dataset_config['tensors']
fields_config = tensor_config['fields']
# fields_config['color_ims']['height'] = im_height
# fields_config['color_ims']['width'] = im_width
# fields_config['depth_ims']['height'] = im_height
# fields_config['depth_ims']['width'] = im_width
fields_config['obj_poses']['height'] = POSE_DIM * max_obj_per_pile
fields_config['obj_coms']['height'] = POINT_DIM * max_obj_per_pile
fields_config['obj_ids']['height'] = max_obj_per_pile
fields_config['bin_distances']['height'] = max_obj_per_pile
# matrix has (n choose 2) elements in it
max_distance_matrix_length = int(comb(max_obj_per_pile, 2))
fields_config['distance_matrix']['height'] = max_distance_matrix_length
# sample a process id
proc_id = utils.gen_experiment_id()
# if not os.path.exists(output_dataset_path):
# try:
# os.mkdir(output_dataset_path)
# except:
# logging.warning('Failed to create %s. The dataset path may have been created simultaneously by another process' %(dataset_path))
# proc_id = 'clustering_2'
# output_dataset_path = os.path.join(output_dataset_path, 'dataset_%s' %(proc_id))
# open input dataset
# logging.info('Opening input dataset: %s' % input_dataset_path)
# input_dataset = TensorDataset.open(input_dataset_path)
# open output_dataset
# logging.info('Opening output dataset: %s' % output_dataset_path)
# dataset = TensorDataset(output_dataset_path, tensor_config)
# datapoint = dataset.datapoint_template
# setup logging
# experiment_log_filename = os.path.join(output_dataset_path, 'dataset_generation.log')
# formatter = logging.Formatter('%(asctime)s %(levelname)s: %(message)s')
# hdlr = logging.FileHandler(experiment_log_filename)
# hdlr.setFormatter(formatter)
# logging.getLogger().addHandler(hdlr)
# config.save(os.path.join(output_dataset_path, 'dataset_generation_params.yaml'))
# key mappings
# we add the empty string as a mapping because if you don't evaluate dexnet on the 'before' state of the push
obj_id = 1
obj_ids = {'': 0}
action_ids = {
'ParallelJawGraspAction': 0,
'SuctionGraspAction': 1,
'LinearPushAction': 2
}
# add action ids
reverse_action_ids = utils.reverse_dictionary(action_ids)
# dataset.add_metadata('action_ids', reverse_action_ids)
# perform rollouts
n = 0
rollout_start = time.time()
current_heap_id = None
while n < num_rollouts:
# create env
create_start = time.time()
bin_picking_env = GraspingEnv(config, vis_config)
create_stop = time.time()
logging.info('Creating env took %.3f sec' %(create_stop-create_start))
# perform rollouts
rollouts_remaining = num_rollouts - n
for i in range(min(rollouts_per_garbage_collect, rollouts_remaining)):
# log current rollout
logging.info('\n')
if n % vis_config['log_rate'] == 0:
logging.info('Rollout: %03d' %(n))
try:
# mark rollout status
data_saved = False
num_steps = 0
# read heap id
# heap_id = heap_ids[n]
# timestep = timesteps[n]
# while heap_id == current_heap_id:# or heap_id < 81:#[226, 287, 325, 453, 469, 577, 601, 894, 921]: 26
# n += 1
# heap_id = heap_ids[n]
# timestep = timesteps[n]
push_logger = logging.getLogger('push')
# push_logger.info('~')
# push_logger.info('Heap ID %d' % heap_id)
# current_heap_id = heap_id
# reset env
reset_start = time.time()
# bin_picking_env.reset_from_dataset(input_dataset,
# heap_id,
# timestep)
bin_picking_env.reset()
state = bin_picking_env.state
environment = bin_picking_env.environment
if fully_observed:
observation = None
else:
observation = bin_picking_env.observation
policy.set_environment(environment)
reset_stop = time.time()
# add objects to mapping
for obj_key in state.obj_keys:
if obj_key not in obj_ids.keys():
obj_ids[obj_key] = obj_id
obj_id += 1
push_logger.info(obj_key)
# save id mappings
reverse_obj_ids = utils.reverse_dictionary(obj_ids)
# dataset.add_metadata('obj_ids', reverse_obj_ids)
# store datapoint env params
# datapoint['heap_ids'] = current_heap_id
# datapoint['camera_poses'] = environment.camera.T_camera_world.vec
# datapoint['camera_intrs'] = environment.camera.intrinsics.vec
# datapoint['robot_poses'] = environment.robot.T_robot_world.vec
# render
if vis_config['initial_state']:
vis3d.figure()
bin_picking_env.render_3d_scene()
vis3d.pose(environment.robot.T_robot_world)
vis3d.show(starting_camera_pose=CAMERA_POSE)
# observe
if vis_config['initial_obs']:
vis2d.figure()
vis2d.imshow(observation, auto_subplot=True)
vis2d.show()
# rollout on current satte
done = False
failed = False
# if isinstance(policy, SingulationFullRolloutPolicy):
# policy.reset_num_failed_grasps()
while not done:
if vis_config['step_stats']:
logging.info('Heap ID: %s' % heap_id)
logging.info('Timestep: %s' % bin_picking_env.timestep)
# get action
policy_start = time.time()
if fully_observed:
action = policy.action(state)
else:
action = policy.action(observation)
policy_stop = time.time()
logging.info('Composite Policy took %.3f sec' %(policy_stop-policy_start))
# render scene before
if vis_config['action']:
#gripper = bin_picking_env.gripper(action)
vis3d.figure()
# GRASPINGENV
# bin_picking_env.render_3d_scene(render_camera=False, workspace_objs_wireframe=False)
bin_picking_env.render_3d_scene()
if isinstance(action, GraspAction):
vis3d.gripper(gripper, action.grasp(gripper))
#if isinstance(action, LinearPushAction):
else:
# # T_start_world = action.T_begin_world * gripper.T_mesh_grasp
# # T_end_world = action.T_end_world * gripper.T_mesh_grasp
# #start_point = action.T_begin_world.translation
# start_point = action['start']
# #end_point = action.T_end_world.translation
# end_point = action['end']
# vec = (end_point - start_point) / np.linalg.norm(end_point-start_point) if np.linalg.norm(end_point-start_point) > 0 else end_point-start_point
# #h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(vec)
# #h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(vec)
# arrow_len = np.linalg.norm(start_point - end_point)
# h1 = (end_point - start_point + np.array([0,0,arrow_len])) / (arrow_len*math.sqrt(2))
# h2 = (end_point - start_point - np.array([0,0,arrow_len])) / (arrow_len*math.sqrt(2))
# shaft_points = [start_point, end_point]
# head_points = [end_point - 0.03*h2, end_point, end_point - 0.03*h1]
# #vis3d.plot3d(shaft_points, color=[0,0,1])
# #vis3d.plot3d(head_points, color=[0,0,1])
# Displaying all potential topple points
for vertex, prob in zip(action['vertices'], action['probabilities']):
color = np.array([min(1, 2*(1-prob)), min(2*prob, 1), 0])
vis3d.points(Point(vertex, 'world'), scale=.0005, color=color)
for vertex in action['bottom_points']:
color = np.array([0,0,1])
vis3d.points(Point(vertex, 'world'), scale=.0005, color=color)
vis3d.points(Point(action['com'], 'world'), scale=.005, color=np.array([0,0,1]))
vis3d.points(Point(np.array([0,0,0]), 'world'), scale=.005, color=np.array([0,1,0]))
#set_of_lines = action['set_of_lines']
#for i, line in enumerate(set_of_lines):
# color = str(bin(i+1))[2:].zfill(3)
# color = np.array([color[2], color[1], color[0]])
# vis3d.plot3d(line, color=color)
vis3d.show(starting_camera_pose=CAMERA_POSE)
# Show
vis3d.figure()
bin_picking_env.render_3d_scene()
final_pose_ind = action['final_pose_ind'] / np.amax(action['final_pose_ind'])
for vertex, final_pose_ind in zip(action['vertices'], final_pose_ind):
color = np.array([0, min(1, 2*(1-prob)), min(2*prob, 1)])
vis3d.points(Point(vertex, 'world'), scale=.0005, color=color)
vis3d.show(starting_camera_pose=CAMERA_POSE)
color=np.array([0,0,1])
original_pose = state.obj.T_obj_world
pose_num = 0
for pose, edge_point1, edge_point2 in zip(action['final_poses'], action['bottom_points'], np.roll(action['bottom_points'],-1,axis=0)):
print 'Pose:', pose_num
pose_num += 1
pose = pose.T_obj_table
vis3d.figure()
state.obj.T_obj_world = original_pose
bin_picking_env.render_3d_scene()
vis3d.points(Point(edge_point1, 'world'), scale=.0005, color=color)
vis3d.points(Point(edge_point2, 'world'), scale=.0005, color=color)
vis3d.show(starting_camera_pose=CAMERA_POSE)
vis3d.figure()
state.obj.T_obj_world = pose
bin_picking_env.render_3d_scene()
vis3d.points(Point(edge_point1, 'world'), scale=.0005, color=color)
vis3d.points(Point(edge_point2, 'world'), scale=.0005, color=color)
vis3d.show(starting_camera_pose=CAMERA_POSE)
#vis3d.save('/home/mjd3/Pictures/weird_pics/%d_%d_before.png' % (heap_id, bin_picking_env.timestep), starting_camera_pose=CAMERA_POSE)
# store datapoint pre-step data
j = 0
obj_poses = np.zeros(fields_config['obj_poses']['height'])
obj_coms = np.zeros(fields_config['obj_coms']['height'])
obj_ids_vec = np.iinfo(np.uint32).max * np.ones(fields_config['obj_ids']['height'])
for obj_state in state.obj_states:
obj_poses[j*POSE_DIM:(j+1)*POSE_DIM] = obj_state.T_obj_world.vec
obj_coms[j*POINT_DIM:(j+1)*POINT_DIM] = obj_state.center_of_mass
obj_ids_vec[j] = obj_ids[obj_state.key]
j += 1
action_poses = np.zeros(fields_config['action_poses']['height'])
#if isinstance(action, GraspAction):
# action_poses[:7] = action.T_grasp_world.vec
#else:
# action_poses[:7] = action.T_begin_world.vec
# action_poses[7:] = action.T_end_world.vec
# if isinstance(policy, SingulationMetricsCompositePolicy):
# actual_distance_matrix_length = int(comb(len(state.objs), 2))
# bin_distances = np.append(action.metadata['bin_distances'],
# np.zeros(max_obj_per_pile-len(state.objs))
# )
# distance_matrix = np.append(action.metadata['distance_matrix'],
# np.zeros(max_distance_matrix_length - actual_distance_matrix_length)
# )
# datapoint['bin_distances'] = bin_distances
# datapoint['distance_matrix'] = distance_matrix
# datapoint['T_begin_world'] = action.T_begin_world.matrix
# datapoint['T_end_world'] = action.T_end_world.matrix
# datapoint['parallel_jaw_best_q_value'] = action.metadata['parallel_jaw_best_q_value']
# # datapoint['parallel_jaw_mean_q_value'] = action.metadata['parallel_jaw_mean_q_value']
# # datapoint['parallel_jaw_num_grasps'] = action.metadata['parallel_jaw_num_grasps']
# datapoint['suction_best_q_value'] = action.metadata['suction_best_q_value']
# # datapoint['suction_mean_q_value'] = action.metadata['suction_mean_q_value']
# # datapoint['suction_num_grasps'] = action.metadata['suction_num_grasps']
# # logging.info('Suction Q: %f, PJ Q: %f' % (action.metadata['suction_q_value'], action.metadata['parallel_jaw_q_value']))
# # datapoint['obj_index'] = action.metadata['obj_index']
# # datapoint['parallel_jaw_best_q_value_single'] = action.metadata['parallel_jaw_best_q_value_single']
# # datapoint['suction_best_q_value_single'] = action.metadata['suction_best_q_value_single']
# datapoint['singulated_obj_index'] = action.metadata['singulated_obj_index']
# datapoint['parallel_jaw_grasped_obj_index'] = obj_ids[action.metadata['parallel_jaw_grasped_obj_key']]
# datapoint['suction_grasped_obj_index'] = obj_ids[action.metadata['suction_grasped_obj_key']]
# else:
# datapoint['bin_distances'] = np.zeros(max_obj_per_pile)
# datapoint['distance_matrix'] = np.zeros(max_distance_matrix_length)
# datapoint['T_begin_world'] = np.zeros((4,4))
# datapoint['T_end_world'] = np.zeros((4,4))
# datapoint['parallel_jaw_best_q_value'] = -1
# datapoint['suction_best_q_value'] = -1
# datapoint['singulated_obj_index'] = -1
# datapoint['parallel_jaw_grasped_obj_index'] = -1
# datapoint['suction_grasped_obj_index'] = -1
# policy_id = 0
# if 'policy_id' in action.metadata.keys():
# policy_id = action.metadata['policy_id']
# greedy_q_value = 0
# if 'greedy_q_value' in action.metadata.keys():
# greedy_q_value = action.metadata['greedy_q_value']
# datapoint['timesteps'] = bin_picking_env.timestep
# datapoint['obj_poses'] = obj_poses
# datapoint['obj_coms'] = obj_coms
# datapoint['obj_ids'] = obj_ids_vec
# # if bin_picking_env.render_mode == RenderMode.RGBD:
# # color_data = observation.color.raw_data
# # depth_data = observation.depth.raw_data
# # elif bin_picking_env.render_mode == RenderMode.DEPTH:
# # color_data = np.zeros(observation.shape).astype(np.uint8)
# # depth_data = observation.raw_data
# # elif bin_picking_env.render_mode == RenderMode.COLOR:
# # color_data = observation.raw_data
# # depth_data = np.zeros(observation.shape)
# # datapoint['color_ims'] = color_data
# # datapoint['depth_ims'] = depth_data
# datapoint['action_ids'] = action_ids[type(action).__name__]
# datapoint['action_poses'] = action_poses
# datapoint['policy_ids'] = policy_id
# datapoint['greedy_q_values'] = greedy_q_value
# datapoint['pred_q_values'] = action.q_value
# step the policy
#observation, reward, done, info = bin_picking_env.step(action)
#state = bin_picking_env.state
state.objs[0].T_obj_world = action['final_state']
# if isinstance(policy, SingulationFullRolloutPolicy):
# policy.grasp_succeeds(info['grasp_succeeds'])
# debugging info
if vis_config['step_stats']:
logging.info('Action type: %s' %(type(action).__name__))
logging.info('Action Q-value: %.3f' %(action.q_value))
logging.info('Reward: %d' %(reward))
logging.info('Policy took %.3f sec' %(policy_stop-policy_start))
logging.info('Num objects remaining: %d' %(bin_picking_env.num_objects))
if info['cleared_pile']:
logging.info('Cleared pile!')
# # store datapoint post-step data
# datapoint['rewards'] = reward
# datapoint['grasp_metrics'] = info['grasp_metric']
# datapoint['collisions'] = 1 * info['collides']
# datapoint['collisions_with_env'] = 1 * info['collides_with_static_obstacles']
# datapoint['grasped_obj_ids'] = obj_ids[info['grasped_obj_key']]
# datapoint['cleared_pile'] = 1 * info['cleared_pile']
# # store datapoint
# # dataset.add(datapoint)
# data_saved = True
# render observation
if vis_config['obs']:
vis2d.figure()
vis2d.imshow(observation, auto_subplot=True)
vis2d.show()
# render scene after
if vis_config['state']:
vis3d.figure()
bin_picking_env.render_3d_scene(render_camera=False)
vis3d.show(starting_camera_pose=CAMERA_POSE)
# vis3d.save('/home/mjd3/Pictures/weird_pics/%d_%d_after.png' % (heap_id, bin_picking_env.timestep), starting_camera_pose=CAMERA_POSE)
state.objs[0].T_obj_world = action['tmpR']
vis3d.figure()
bin_picking_env.render_3d_scene(render_camera=False)
vis3d.show(starting_camera_pose=CAMERA_POSE)
state.objs[0].T_obj_world = action['final_state']
# increment the number of steps
num_steps += 1
if num_steps >= steps_per_test_case:
done = True
except NoActionFoundException as e:
logging.warning('The policy failed to plan an action!')
done = True
except Exception as e:
# log an error
logging.warning('Rollout failed!')
logging.warning('%s' %(str(e)))
logging.warning(traceback.print_exc())
# if debug:
# raise
# reset env
del bin_picking_env
gc.collect()
bin_picking_env = BinPickingEnv(config, vis_config)
# terminate current rollout
failed = True
done = True
# update test case id
n += 1
# dataset.flush()
# logging.info("\n\nflushing")
# logging.info("exiting")
# sys.exit()
# garbage collect
del bin_picking_env
gc.collect()
# return the dataset
# dataset.flush()
# log time
rollout_stop = time.time()
logging.info('Rollouts took %.3f sec' %(rollout_stop-rollout_start))
return dataset
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'
continue
if args.topple_probs:
topple_probs = np.sum(vertex_probs[:,1:], axis=1)
vis_topple_probs(vertices, topple_probs)
return parser.parse_args()
if __name__ == '__main__':
args = parse_args()
config = YamlConfig(args.config_filename)
policy = MultiTopplePolicy(config, use_sensitivity=True, num_samples=800)
if config['debug']:
random.seed(SEED)
np.random.seed(SEED)
env = GraspingEnv(config, config['vis'])
env.reset()
#env.state.material_props._color = np.array([0.86274509803] * 3)
env.state.material_props._color = np.array([0.5] * 3)
if args.before:
env.render_3d_scene()
color = np.array([0, 0, 0])
vert = np.array([-0.01091172, 0.02806294, 0.06962403])
normal = vert + .01 * np.array([-0.84288757, -0.3828792, 0.37807943])
vis3d.points(Point(vert), scale=.001, color=color)
vis3d.points(Point(normal), scale=.001, color=np.array([1, 0, 0]))
vis3d.show(starting_camera_pose=CAMERA_POSE)
policy.set_environment(env.environment)
policy.action(env.state)
#show_graph(policy.G, policy.edge_alphas)
new_graph(policy.G)
)
for push_num in range(10):
push_idx = None
# push_idx = 33
sample_id = 0
num_toppled, pose_angles, actual_poses = [], [], []
print '\n\n\n\n\n\nSTARTING DIFFERENT PUSH', push_num
while sample_id < 10:
sample_id += 1
# sim_point_cloud_masked = get_sim_point_cloud(depth_scene, env.state.obj)
usr_input = 'n'
while usr_input != 'y' and usr_input != 'a':
usr_input = utils.keyboard_input('\n\nPut the object in position y: continue, v: vis pose, a: abort push[y/v/a]:')
if usr_input == 'v':
vis3d.figure()
env.state.obj.T_obj_world = orig_pose
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
if usr_input == 'a':
print 'trying different push'
push_idx = None
sample_id = 0
num_toppled, pose_angles, actual_poses = [], [], []
continue
usr_input = 'n'
while usr_input != 'y':
s2r = sim_to_real_tf(sim_point_cloud_masked, vis=True)
usr_input = utils.keyboard_input('Did the pose registration work? [y/n]:')
env.state.obj.T_obj_world = s2r*orig_pose
def display_or_save(filename):
if args.save:
vis3d.save_loop(filename, starting_camera_pose=CAMERA_POSE)
else:
vis3d.show(starting_camera_pose=CAMERA_POSE)
def save_samples(self, sample, grasps, T_obj_stp, collision_checker,
grasp_metrics, stable_pose):
self.tensor_datapoint['image_labels'] = self.cur_image_label
T_stp_camera = sample.camera.object_to_camera_pose
self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames(
'obj', T_stp_camera.from_frame)
if UNALIGNED or STP_ALIGNED:
aligned_grasps = grasps
else:
aligned_grasps = self.align_grasps_with_camera(grasps)
depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image
self.tensor_datapoint['camera_poses'] = self.get_camera_pose(sample)
final_camera_intr = sample.camera.camera_intr.crop(
self.crop_height, self.crop_width, depth_im_table.center[0],
depth_im_table.center[1])
self.tensor_datapoint['camera_intrs'] = np.r_[final_camera_intr.fx,
final_camera_intr.fy,
final_camera_intr.cx,
final_camera_intr.cy]
if not IM_TENSOR:
self.save_image(depth_im_table)
else:
depth_im_tf = self._crop(depth_im_table, size=None)
for cnt, aligned_grasp in enumerate(aligned_grasps):
if UNALIGNED:
aligned_grasp = aligned_grasp.grasp_y_axis_offset(
np.random.uniform(0, 2 * np.pi))
T_stp_grasp = RigidTransform(
stable_pose.r, from_frame='obj',
to_frame='stp') * aligned_grasp.T_grasp_obj
if T_stp_grasp.x_axis[-1] > 0:
# Grasp from below table, rotate by 180 degrees
aligned_grasp = aligned_grasp.grasp_y_axis_offset(np.pi)
T_grasp_camera = aligned_grasp.gripper_pose(self.gripper).inverse() * \
self.T_obj_camera.inverse()
# Calculate psi, the angle between the grasp approach axis and the camera principal ray
z_axis = T_grasp_camera.z_axis
psi = np.arccos(
z_axis[2] /
np.sqrt(z_axis[0]**2 + z_axis[1]**2 + z_axis[2]**2))
# if np.rad2deg(psi) > 20:
# continue
# Get gamma, the angle between the grasp approach axis and the table normal
_, gamma, _ = aligned_grasp.grasp_angles_from_stp_z(stable_pose)
if LIMIT_BETA:
perpendicular_table = (np.abs(gamma) < np.deg2rad(5))
if not perpendicular_table:
continue
if VISUALISE_3D:
vis.figure()
T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh,
stable_pose.T_obj_world,
style='surface',
dim=0.5,
plot_table=True)
T_camera_world = T_obj_world * self.T_obj_camera.inverse()
vis.gripper(self.gripper,
aligned_grasp,
T_obj_world,
color=(0.3, 0.3, 0.3),
T_camera_world=T_camera_world)
# vis.gripper(self.gripper, grasps[cnt], T_obj_world, color=(0.3, 0.3, 0.3))
# print(psi)
vis.show()
collision_free = self.is_grasp_collision_free(
aligned_grasp, collision_checker)
# Project grasp coordinates in image
grasp_2d = aligned_grasp.project_camera(self.T_obj_camera,
final_camera_intr)
if IM_TENSOR:
im, new_grasp = self.random_im_crop(depth_im_tf, (32, 32),
grasp_2d.center.x,
grasp_2d.center.y)
self.tensor_datapoint['depth_ims_tf_table'] = im
hand_pose = self.get_hand_pose(grasp_2d,
T_grasp_camera.quaternion,
psi,
gamma,
new_grasp=new_grasp)
else:
hand_pose = self.get_hand_pose(grasp_2d,
T_grasp_camera.quaternion, psi,
gamma)
self.add_datapoint(hand_pose, collision_free, aligned_grasp,
grasp_metrics)
self.cur_image_label += 1
def save_samples(self, sample, grasps, T_obj_stp, collision_checker,
grasp_metrics, stable_pose):
self.tensor_datapoint['image_labels'] = self.cur_image_label
T_stp_camera = sample.camera.object_to_camera_pose
self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames(
'obj', T_stp_camera.from_frame)
depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image
camera_pose = self.get_camera_pose(sample)
shifted_camera_intr = sample.camera.camera_intr
aligned_grasps = self.align_grasps_with_camera(grasps)
cx = depth_im_table.center[1]
cy = depth_im_table.center[0]
self.tensor_datapoint['camera_intrs'] = np.r_[shifted_camera_intr.fx,
shifted_camera_intr.fy,
shifted_camera_intr.cx,
shifted_camera_intr.cy]
for aligned_grasp in aligned_grasps:
if not self.is_grasp_aligned(aligned_grasp):
continue
collision_free = self.is_grasp_collision_free(
aligned_grasp, collision_checker)
grasp_2d = aligned_grasp.project_camera(self.T_obj_camera,
shifted_camera_intr)
depth_im_tf = self.prepare_images(depth_im_table, grasp_2d, cx, cy)
T_grasp_camera = aligned_grasp.gripper_pose(
self.gripper).inverse() * self.T_obj_camera.inverse()
if VISUALISE_3D:
z_axis = T_grasp_camera.z_axis
theta = np.rad2deg(
np.arccos((z_axis[2]) / (np.sqrt(
(z_axis[0]**2 + z_axis[1]**2 + z_axis[2]**2)))))
vis.figure()
T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh,
stable_pose.T_obj_world,
style='surface',
dim=0.5,
plot_table=True)
T_camera_world = T_obj_world * self.T_obj_camera.inverse()
vis.gripper(self.gripper,
aligned_grasp,
T_obj_world,
color=(0.3, 0.3, 0.3),
T_camera_world=T_camera_world)
vis.show()
theta = grasp_2d.angle
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0], [0, 0, 1]])
rot = RigidTransform(rotation=R,
to_frame='camera',
from_frame='camera')
new_quaternion = T_grasp_camera * rot
hand_pose = self.get_hand_pose(grasp_2d, new_quaternion.quaternion)
self.add_datapoint(depth_im_tf, hand_pose, collision_free,
camera_pose, aligned_grasp, grasp_metrics)
self.cur_image_label += 1
config = YamlConfig(args.config_filename)
policy = SingleTopplePolicy(config['policy'], use_sensitivity=True)
if config['debug']:
random.seed(SEED)
np.random.seed(SEED)
env = GraspingEnv(config, config['vis'])
env.reset()
env.state.material_props._color = np.array([0.86274509803] * 3)
obj_name = env.state.obj.key
policy.set_environment(env.environment)
if args.before:
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
action = policy.action(env.state)
vis3d.figure()
env.render_3d_scene()
num_vertices = len(action.metadata['vertices'])
for i, vertex, q_increase in zip(np.arange(num_vertices),
action.metadata['vertices'],
action.metadata['quality_increases']):
topples = action.metadata['final_pose_ind'][i] != 0
color = np.array(
[min(1, 2 * (1 - q_increase)),
min(2 * q_increase, 1), 0]) if topples else np.array([0, 0, 0])
# color = np.array([min(1, 2*(1-q_increase)), min(2*q_increase, 1), 0])
scale = .001 if i != action.metadata['best_ind'] else .003
vis3d.points(Point(vertex, 'world'), scale=scale, color=color)
random.seed(SEED)
np.random.seed(SEED)
env = GraspingEnv(config, config['vis'])
env.reset()
#env.state.material_props._color = np.array([0.86274509803] * 3)
#env.state.material_props._color = np.array([.345, .094, .27])
env.state.material_props._color = np.array([0.5] * 3)
obj_name = env.state.obj.key
policy.set_environment(env.environment)
if False:
while True:
env.reset()
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
if args.before:
env.state.obj.T_obj_world.translation += np.array([-.095, -.025, .001])
# action = policy.grasping_policy.action(env.state)
# print 'q', action.q_value
# gripper(env.gripper(action), action.grasp(env.gripper(action)))
# vis3d.mesh(env._grippers[GripperType.SUCTION].mesh)
# color = [0,0,0]
# mesh = env.state.obj.mesh.copy()
# mesh.apply_transform(env.state.T_obj_world.matrix)
# #vis3d.points(Point(np.mean(mesh.vertices, axis=0)), scale=.001)
# #vis3d.points(Point(mesh.center_mass+np.array([0,.04,.04])), scale=.001)
# vis3d.mesh(mesh, color=env.state.color)
# #vis3d.points(env.state.T_obj_world.translation, radius=.001)
# vis3d.show(starting_camera_pose=CAMERA_POSE)
def figure_1():
env.state.obj.T_obj_world.translation += np.array([-.01, -.05, .001])
action = policy.action(env.state)
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] - .005 * y_dir
vis_axes(origin, y_dir)
#mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
#mesh.fix_normals()
#direction = normalize([-.03, -.07, 0])
#intersect, _, face_ind = mesh.ray.intersects_location([[.02, -.005, .09]], [direction], multiple_hits=False)
#normal = mesh.face_normals[face_ind[0]]
#start_point = intersect[0] + .03*normal
#end_point = intersect[0]
#shaft_points = [start_point, end_point]
#h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(-normal)
#h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(-normal)
#head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1]
#vis3d.plot3d(shaft_points, color=[1,0,0], tube_radius=.001)
#vis3d.plot3d(head_points, color=[1,0,0], tube_radius=.001)
#vis3d.points(Point(end_point), scale=.002, color=[0,0,0])
# Center of Mass
#start_point = env.state.T_obj_world.translation - .0025*y_dir - np.array([0,0,.005])
start_point = env.state.T_obj_world.translation
end_point = start_point - np.array([0, 0, .03])
vis3d.points(Point(start_point), scale=.002, color=[0, 0, 0])
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-up)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-up)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.001)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.001)
vis3d.show(starting_camera_pose=CAMERA_POSE)
def figure_2():
env.state.material_props._color = np.array([0.75] * 3)
# env.state.obj.T_obj_world.translation += np.array([-.095,.025,.001])
env.state.obj.T_obj_world.translation += np.array([-.01, -.04, .001])
action = policy.action(env.state)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
edge = 1
vis3d.plot3d(bottom_points[edge:edge + 2],
color=[0, 0, 0],
tube_radius=.0005)
vis3d.points(Point(bottom_points[edge]), color=[0, 0, 0], scale=.001)
vis3d.points(Point(bottom_points[edge + 1]), color=[0, 0, 0], scale=.001)
y_dir = normalize(bottom_points[edge + 1] - bottom_points[edge])
origin = policy.toppling_model.com_projected_on_edges[
edge] # - .0025*y_dir
vis_axes(origin, y_dir)
mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
mesh.fix_normals()
#direction = normalize([-.03, -.07, 0])
#intersect, _, face_ind = mesh.ray.intersects_location([[.02, -.005, .09]], [direction], multiple_hits=False)
ray_origin = mesh.center_mass + np.array([.1, -.1, .035])
direction = normalize([-.1, .1, 0])
intersect, _, face_ind = mesh.ray.intersects_location([ray_origin],
[direction],
multiple_hits=False)
normal = mesh.face_normals[face_ind[0]]
start_point = intersect[0] + .03 * normal
end_point = intersect[0]
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, 0, -0.7071], [0, 1, 0], [0.7071, 0,
0.7071]]).dot(-normal)
h2 = np.array([[0.7071, 0, 0.7071], [0, 1, 0], [-0.7071, 0,
0.7071]]).dot(-normal)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
vis3d.plot3d(head_points, color=red, tube_radius=.001)
vis3d.points(Point(end_point), scale=.002, color=[0, 0, 0])
# Center of Mass
#start_point = env.state.T_obj_world.translation# - .0025*y_dir - np.array([0,0,.005])
start_point = mesh.center_mass
end_point = start_point - np.array([0, 0, .03])
vis3d.points(Point(start_point), scale=.002, color=[0, 0, 0])
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-up)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-up)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
vis3d.plot3d(head_points, color=red, tube_radius=.001)
# Dotted lines
r_gs = dotted_line(start_point, origin)
for r_g in r_gs:
vis3d.plot3d(r_g, color=orange, tube_radius=.0006)
s = normalize(bottom_points[edge + 1] - bottom_points[edge])
vertex_projected_on_edge = (
intersect[0] - bottom_points[edge]).dot(s) * s + bottom_points[edge]
r_fs = dotted_line(intersect[0], vertex_projected_on_edge)
for r_f in r_fs:
vis3d.plot3d(r_f, color=orange, tube_radius=.0006)
vis3d.show(starting_camera_pose=CAMERA_POSE)
def failure_modes():
# dataset = env._state_space._database.dataset('mini_dexnet')
# obj = dataset['pawn']
# obj.T_obj_world = dataset.stable_poses('pawn')[8].T_obj_table
# env.state.obj = obj
# mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
# mesh.fix_normals()
# direction = normalize([1, -.005, 0])
# intersect, _, face_ind = mesh.ray.intersects_location([[-1, 0, .09]], [direction], multiple_hits=False)
# direction = normalize([1,-0.25,0])
# start_point = intersect[0] - .03*direction
# end_point = intersect[0]
# shaft_points = [start_point, end_point]
# h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(direction)
# h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(direction)
# head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1]
# vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
# vis3d.plot3d(head_points, color=red, tube_radius=.001)
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
# env.state.obj.T_obj_world = dataset.stable_poses('pawn')[0].T_obj_table
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
# dataset = env._state_space._database.dataset('mini_dexnet')
# obj = dataset['yoda']
# obj.T_obj_world = dataset.stable_poses('yoda')[2].T_obj_table
# env.state.obj = obj
# mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
# mesh.fix_normals()
# direction = normalize([0, .1, 0])
# intersect = mesh.center_mass + np.array([0,-.015,.037])
# start_point = intersect - .03*direction
# end_point = intersect
# shaft_points = [start_point, end_point]
# h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(direction)
# h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(direction)
# head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1]
# vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
# vis3d.plot3d(head_points, color=red, tube_radius=.001)
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
# env.state.obj.T_obj_world = dataset.stable_poses('yoda')[4].T_obj_table
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
dataset = env._state_space._database.dataset('mini_dexnet')
obj = dataset['vase']
obj.T_obj_world = dataset.stable_poses('vase')[5].T_obj_table
env.state.obj = obj
mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
mesh.fix_normals()
direction = normalize([.03, .1, 0])
intersect = mesh.center_mass + np.array([-.019, -.02, .02])
start_point = intersect - .03 * direction
end_point = intersect
shaft_points = [start_point, end_point]
h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0],
[0, 0, 1]]).dot(direction)
h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0],
[0, 0, 1]]).dot(direction)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=red, tube_radius=.001)
vis3d.plot3d(head_points, color=red, tube_radius=.001)
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
env.state.obj.T_obj_world = dataset.stable_poses('vase')[4].T_obj_table
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
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()
def display_grasps(self,
object_name,
gripper_name,
metric_name,
config=None):
""" Display grasps for an object
Parameters
----------
object_name : :obj:`str`
Object to display
gripper_name : :obj:`str`
Gripper for which to display grasps
metric_name : :obj:`str`
Metric to color/rank grasps with
config : :obj:`dict`
Configuration dict for visualization.
Parameters are in Other parameters. Values from self.default_config are used for keys not provided.
Other Parameters
----------------
gripper_dir
Directory where the grippers models and parameters are.
quality_scale
Range to scale quality metric values to
show_gripper
Whether or not to show the gripper in the visualization
min_metric
lowest value of metric to show grasps for
max_plot_gripper
Number of grasps to plot
animate
Whether or not to animate the displayed object
"""
self._check_opens()
config = self._get_config(config)
grippers = os.listdir(config['gripper_dir'])
if gripper_name not in grippers:
raise ValueError(
"{} is not a valid gripper name".format(gripper_name))
gripper = gr.RobotGripper.load(gripper_name,
gripper_dir=config['gripper_dir'])
objects = self.dataset.object_keys
if object_name not in objects:
raise ValueError(
"{} is not a valid object name".format(object_name))
metrics = self.dataset.available_metrics(object_name,
gripper=gripper.name)
if metric_name not in metrics:
raise ValueError(
"{} is not computed for gripper {}, object {}".format(
metric_name, gripper.name, object_name))
logger.info('Displaying grasps for gripper %s on object %s' %
(gripper.name, object_name))
object = self.dataset[object_name]
grasps, metrics = self.dataset.sorted_grasps(object_name,
metric_name,
gripper=gripper.name)
if len(grasps) == 0:
raise RuntimeError('No grasps for gripper %s on object %s' %
(gripper.name, object_name))
return
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 config['show_gripper']:
i = 0
stable_pose = self.dataset.stable_pose(object.key, 'pose_1')
for grasp, metric in zip(grasps, metrics):
if metric <= config['min_metric']:
continue
print 'Grasp %d %s=%.5f' % (grasp.id, metric_name, metric)
T_obj_world = RigidTransform(from_frame='obj',
to_frame='world')
color = plt.get_cmap('hsv')(q_to_c(metric))[:-1]
T_obj_gripper = grasp.gripper_pose(gripper)
grasp = grasp.perpendicular_table(stable_pose)
vis.figure()
vis.gripper_on_object(gripper,
grasp,
object,
gripper_color=(0.25, 0.25, 0.25),
stable_pose=stable_pose,
plot_table=False)
vis.show(animate=config['animate'])
i += 1
if i >= config['max_plot_gripper']:
break
else:
i = 0
vis.figure()
vis.mesh(object.mesh, style='surface')
for grasp, metric in zip(grasps, metrics):
if metric <= config['min_metric']:
continue
print 'Grasp %d %s=%.5f' % (grasp.id, metric_name, metric)
T_obj_world = RigidTransform(from_frame='obj',
to_frame='world')
color = plt.get_cmap('hsv')(q_to_c(metric))[:-1]
T_obj_gripper = grasp.gripper_pose(gripper)
vis.grasp(grasp, grasp_axis_color=color, endpoint_color=color)
i += 1
if i >= config['max_plot_gripper']:
break
vis.show(animate=config['animate'])
def save_samples(self, sample, grasps, T_obj_stp, collision_checker, grasp_metrics, stable_pose):
self.tensor_datapoint['image_labels'] = self.cur_image_label
T_stp_camera = sample.camera.object_to_camera_pose
self.T_obj_camera = T_stp_camera * T_obj_stp.as_frames('obj', T_stp_camera.from_frame)
aligned_grasps = self.align_grasps_with_camera(grasps)
depth_im_table = sample.renders[RenderMode.DEPTH_SCENE].image
cx = depth_im_table.center[1]
cy = depth_im_table.center[0]
camera_pose = self.get_camera_pose(sample)
self.tensor_datapoint['camera_poses'] = camera_pose
shifted_camera_intr = sample.camera.camera_intr
self.tensor_datapoint['camera_intrs'] = np.r_[shifted_camera_intr.fx,
shifted_camera_intr.fy,
shifted_camera_intr.cx,
shifted_camera_intr.cy]
self.save_orig_image(depth_im_table, camera_pose, stable_pose, shifted_camera_intr)
grid_space = 200
grids = 3
centre = np.empty((grids, grids, 2))
saved = np.ones((grids, grids)) * -1
depth_images = np.empty((grids, grids, 32, 32, 1))
# Crop images in a grid around the centre
for x_grid in range(grids):
x_pos = grid_space / 4 - x_grid * grid_space / (grids + 1)
for y_grid in range(grids):
y_pos = grid_space / 4 - y_grid * grid_space / (grids + 1)
depth_im = self.prepare_images(depth_im_table, x_pos, y_pos)
self._show_image(depth_im)
depth_images[x_grid, y_grid, :, :, :] = depth_im
centre[x_grid, y_grid, 0] = x_pos
centre[x_grid, y_grid, 1] = y_pos
for cnt, aligned_grasp in enumerate(aligned_grasps):
collision_free = self.is_grasp_collision_free(aligned_grasp, collision_checker)
# Project grasp coordinates in image
grasp_2d = aligned_grasp.project_camera(self.T_obj_camera, shifted_camera_intr)
grasp_point = aligned_grasp.close_fingers(self.obj, vis=False, check_approach=False)
if not grasp_point[0]:
Warning("Could not find contact points")
continue
# Get grasp width in pixel from endpoints
p_1 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][0].point, 1).T).T[:3]
p_2 = np.dot(self.T_obj_camera.matrix, np.append(grasp_point[1][1].point, 1).T).T[:3]
grasp_width_px = self.width_px(shifted_camera_intr, p_1, p_2)
grasp_width = aligned_grasp.width_from_endpoints(grasp_point[1][0].point, grasp_point[1][1].point)
pos_x = cx - grasp_2d.center.x
pos_y = cy - grasp_2d.center.y
distance = np.abs((centre[:, :, 0] - pos_x)**2 + (centre[:, :, 1] - pos_y)**2)
idx = np.where(distance == np.amin(distance))
idx_x = idx[0][0]
idx_y = idx[1][0]
grasp_center_x = (300 - grasp_2d.center.x - centre[idx_x, idx_y, 0])//3
grasp_center_y = (300 - grasp_2d.center.y - centre[idx_x, idx_y, 1])//3
if saved[idx_x, idx_y] == -1:
np.save(self.image_dir + "/depth_im_{:07d}.npy".format(self.cur_image_file),
depth_images[idx_x, idx_y, :, :, :])
saved[idx_x, idx_y] = self.cur_image_file
self.tensor_datapoint['image_files'] = self.cur_image_file
self.cur_image_file += 1
else:
self.tensor_datapoint['image_files'] = saved[idx_x, idx_y]
T_grasp_camera = aligned_grasp.gripper_pose(self.gripper).inverse() * self.T_obj_camera.inverse()
if VISUALISE_3D:
z_axis = T_grasp_camera.z_axis
theta = np.rad2deg(np.arccos((z_axis[2]) /
(np.sqrt((z_axis[0] ** 2 + z_axis[1] ** 2 + z_axis[2] ** 2)))))
vis.figure()
T_obj_world = vis.mesh_stable_pose(self.obj.mesh.trimesh,
stable_pose.T_obj_world, style='surface', dim=0.5, plot_table=True)
T_camera_world = T_obj_world * self.T_obj_camera.inverse()
vis.gripper(self.gripper, aligned_grasp, T_obj_world, color=(0.3, 0.3, 0.3),
T_camera_world=T_camera_world)
print(theta)
vis.show()
hand_pose = self.get_hand_pose(grasp_center_x, grasp_center_y, grasp_2d, T_grasp_camera,
grasp_width, grasp_width_px)
self.add_datapoint(hand_pose,
collision_free, aligned_grasp, grasp_metrics)
self.cur_image_label += 1