def get_object_keys(self):
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
obj_keys = dexnet_handle.dataset.object_keys
dexnet_handle.close_database()
return obj_keys
def database_delete_grasps(self, object_name):
# load gripper
gripper = RobotGripper.load(GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
dexnet_handle.dataset.delete_grasps(object_name, gripper=gripper.name)
dexnet_handle.close_database()
def get_stable_pose_transform(self, object_name, stable_pose_id):
# load gripper
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
stable_pose = dexnet_handle.dataset.stable_pose(
object_name, stable_pose_id=('pose_' + str(stable_pose_id)))
dexnet_handle.close_database()
return stable_pose
def database_save(self, object_name, filepath, stable_poses_n, force_overwrite=False):
# load gripper
gripper = RobotGripper.load(GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
mass = CONFIG['default_mass']
if object_name in dexnet_handle.dataset.object_keys:
graspable = dexnet_handle.dataset[object_name]
mesh = graspable.mesh
sdf = graspable.sdf
stable_poses = dexnet_handle.dataset.stable_poses(object_name)
else:
# Create temp dir if cache dir is not provided
mp_cache = CONFIG['cache_dir']
del_cache = False
if mp_cache is None:
mp_cache = tempfile.mkdtemp()
del_cache = True
# open mesh preprocessor
mesh_processor = MeshProcessor(filepath, mp_cache)
mesh_processor.generate_graspable(CONFIG)
mesh = mesh_processor.mesh
sdf = mesh_processor.sdf
stable_poses = mesh_processor.stable_poses[:stable_poses_n]
# write graspable to database
dexnet_handle.dataset.create_graspable(object_name, mesh, sdf, stable_poses, mass=mass)
# write grasps to database
grasps, metrics = antipodal_grasp_sampler_for_storing(mesh, sdf, stable_poses)
id = 0
for grasps_for_pose, metrics_for_pose in zip(grasps, metrics):
if (grasps_for_pose == None):
continue
dexnet_handle.dataset.store_grasps_and_metrics(object_name, grasps_for_pose, metrics_for_pose,
gripper=gripper.name, stable_pose_id=('pose_'+str(id)),
force_overwrite=force_overwrite)
id = id + 1
dexnet_handle.close_database()
def delete_graspable(self, object_name):
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
dexnet_handle.dataset.delete_graspable(object_name)
# delete files related to object
path_to_obj_file = ".dexnet/" + object_name
if os.path.exists(path_to_obj_file + "_proc.sdf"):
print("sdf file removed")
os.remove(path_to_obj_file + "_proc.sdf")
if os.path.exists(path_to_obj_file + "_proc.obj"):
print("obj file removed")
os.remove(path_to_obj_file + "_proc.obj")
dexnet_handle.close_database()
def get_stable_pose(self, object_name, stable_pose_id):
# load gripper
gripper = RobotGripper.load(GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
# read the most robust grasp
stable_pose = dexnet_handle.dataset.stable_pose(object_name, stable_pose_id=('pose_'+str(stable_pose_id)))
dexnet_handle.close_database()
pose_matrix = np.eye(4,4)
pose_matrix[:3,:3] = stable_pose.T_obj_world.rotation
pose_matrix[:3, 3] = stable_pose.T_obj_world.translation
return pose_matrix
def get_object_mesh(self, object_name):
# load gripper
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
object_mesh = dexnet_handle.dataset.mesh(object_name)
return object_mesh
from_frame='camera',
to_frame='robot')
# fake transformation from a known 3D object model frame of reference to the camera frame of reference
# in practice this could be computed using point cloud registration
T_obj_camera = RigidTransform(
rotation=RigidTransform.random_rotation(),
translation=RigidTransform.random_translation(),
from_frame='obj',
to_frame='camera')
# load gripper
gripper = RobotGripper.load(gripper_name)
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(database_path)
dexnet_handle.open_dataset(dataset_name)
# read the most robust grasp
sorted_grasps = dexnet_handle.dataset.sorted_grasps(object_name,
metric_name,
gripper=gripper_name)
most_robust_grasp = sorted_grasps[0][
0] # Note: in general this step will require collision checking
# transform into the robot reference frame for control
T_gripper_obj = most_robust_grasp.gripper_pose(gripper)
T_gripper_robot = T_camera_robot * T_obj_camera * T_gripper_obj
# control the robot to move along a linear path to the desired pose, close the jaws, and lift!
depth = np.asarray((viewer.read_pixels(width, height, depth=True)[1]))
# , depth[left_pad, top_pad], depth[-left_pad, -top_pad]
cdepth = depth[height // 2, width // 2]
print(cdepth)
depth[depth > cdepth] = cdepth
seg = depth != cdepth
depth[:, :top_pad], depth[:, -top_pad:] = cdepth, cdepth
depth[:left_pad, :], depth[-left_pad:, :] = cdepth, cdepth
offset, scale = np.min(depth), np.max(depth) - np.min(depth)
depth = (depth - np.min(depth)) / (np.max(depth) - np.min(depth))
seg[:, :top_pad], seg[:, -top_pad:] = False, False
seg[:left_pad, :], seg[-left_pad:, :] = False, False
dexnet = DexNet()
dexnet.prepare_dexnet()
print("dexnet prepared")
state, rgbd_im = dexnet.get_state(depth, seg)
action = dexnet.get_action(state)
'''
get depth of the action and the x, y
apply inverse from camera coordinate to the world coordinate
'''
dexnet.visualization(action, rgbd_im, offset, scale)
action.grasp.depth = action.grasp.depth * scale + offset
rigid_transform = action.grasp.pose()
print('center: {}, {}'.format(action.grasp.center.x,
action.grasp.center.y))
print('depth: {}'.format(action.grasp.depth))
def read_grasps(self,
object_name,
stable_pose_id,
max_grasps=10,
visualize=False):
# load gripper
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
# read the most robust grasp
sorted_grasps, metrics = dexnet_handle.dataset.sorted_grasps(
object_name,
stable_pose_id=('pose_' + str(stable_pose_id)),
metric='force_closure',
gripper=gripper.name)
if (len(sorted_grasps)) == 0:
print('no grasps for this stable pose')
if visualize:
stable_pose = dexnet_handle.dataset.stable_pose(
object_name,
stable_pose_id=('pose_' + str(stable_pose_id)))
obj = dexnet_handle.dataset.graspable(object_name)
vis.figure()
T_table_world = RigidTransform(from_frame='table',
to_frame='world')
T_obj_world = Visualizer3D.mesh_stable_pose(
obj.mesh.trimesh,
stable_pose.T_obj_world,
T_table_world=T_table_world,
color=(0.5, 0.5, 0.5),
style='surface',
plot_table=True,
dim=0.15)
vis.show(False)
return None, None
contact_points = map(get_contact_points, sorted_grasps)
# ------------------------ VISUALIZATION CODE ------------------------
if visualize:
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)
stable_pose = dexnet_handle.dataset.stable_pose(
object_name, stable_pose_id=('pose_' + str(stable_pose_id)))
obj = dexnet_handle.dataset.graspable(object_name)
vis.figure()
T_table_world = RigidTransform(from_frame='table',
to_frame='world')
T_obj_world = Visualizer3D.mesh_stable_pose(
obj.mesh.trimesh,
stable_pose.T_obj_world,
T_table_world=T_table_world,
color=(0.5, 0.5, 0.5),
style='surface',
plot_table=True,
dim=0.15)
for grasp, metric in zip(sorted_grasps, metrics):
color = plt.get_cmap('hsv')((q_to_c(metric)))[:-1]
vis.grasp(grasp,
T_obj_world=stable_pose.T_obj_world,
grasp_axis_color=color,
endpoint_color=color)
vis.show(False)
# ------------------------ END VISUALIZATION CODE ---------------------------
# ------------------------ START COLLISION CHECKING ---------------------------
#stable_pose = dexnet_handle.dataset.stable_pose(object_name, stable_pose_id=('pose_'+str(stable_pose_id)))
#graspable = dexnet_handle.dataset.graspable(object_name)
#cc = GraspCollisionChecker(gripper).set_graspable_object(graspable, stable_pose.T_obj_world)
stable_pose_matrix = self.get_stable_pose(object_name, stable_pose_id)
# CLOSE DATABASE
dexnet_handle.close_database()
gripper_poses = []
for grasp in sorted_grasps[:max_grasps]:
gripper_pose_matrix = np.eye(4, 4)
center_world = np.matmul(
stable_pose_matrix,
[grasp.center[0], grasp.center[1], grasp.center[2], 1])
axis_world = np.matmul(
stable_pose_matrix,
[grasp.axis_[0], grasp.axis_[1], grasp.axis_[2], 1])
gripper_angle = math.atan2(axis_world[1], axis_world[0])
gripper_pose_matrix[:3, 3] = center_world[:3]
gripper_pose_matrix[0, 0] = math.cos(gripper_angle)
gripper_pose_matrix[0, 1] = -math.sin(gripper_angle)
gripper_pose_matrix[1, 0] = math.sin(gripper_angle)
gripper_pose_matrix[1, 1] = math.cos(gripper_angle)
#if visualize:
# vis.figure()
# vis.gripper_on_object(gripper, grasp, obj, stable_pose=stable_pose.T_obj_world)
# vis.show(False)
gripper_poses.append(gripper_pose_matrix)
return contact_points, gripper_poses
def database_save(self,
object_name,
filepath,
stable_poses_n,
metric,
overwrite_object=False,
force_overwrite=False):
# load gripper
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
# open Dex-Net API
dexnet_handle = DexNet()
dexnet_handle.open_database(self.database_path)
dexnet_handle.open_dataset(self.dataset_name)
mass = CONFIG['default_mass']
if object_name in dexnet_handle.dataset.object_keys:
if not overwrite_object:
dexnet_handle.close_database()
print(object_name +
" already exists in database, not overwriting")
return
graspable = dexnet_handle.dataset[object_name]
mesh = graspable.mesh
sdf = graspable.sdf
stable_poses = dexnet_handle.dataset.stable_poses(object_name)
else:
# Create temp dir if cache dir is not provided
mp_cache = CONFIG['cache_dir']
del_cache = False
if mp_cache is None:
mp_cache = tempfile.mkdtemp()
del_cache = True
# open mesh preprocessor
graspable = MeshProcessor(filepath, mp_cache)
graspable.generate_graspable(CONFIG)
mesh = graspable.mesh
sdf = graspable.sdf
stable_poses = graspable.stable_poses[:stable_poses_n]
# write graspable to database
dexnet_handle.dataset.create_graspable(object_name,
mesh,
sdf,
stable_poses,
mass=mass)
if force_overwrite:
dexnet_handle.dataset.delete_grasps(object_name,
gripper=gripper.name)
# calculate or retrieve grasps
loaded_grasps = dexnet_handle.dataset.grasps(object_name,
gripper=gripper.name)
if loaded_grasps == []:
print("No grasps in database, calculating")
grasps = antipodal_grasp_sampler_for_storing(graspable)
if (grasps != None):
print("Saving grasps")
dexnet_handle.dataset.store_grasps(
object_name,
grasps,
gripper=gripper.name,
force_overwrite=force_overwrite)
loaded_grasps = dexnet_handle.dataset.grasps(
object_name, gripper=gripper.name)
print("Calculating grasp quality")
metrics = grasp_quality_calculator(mesh, sdf, loaded_grasps, metric)
print("Grasp quality calculated")
grasp_metrics = {}
for g in loaded_grasps:
grasp_metrics[g.id] = {}
grasp_metrics[g.id][metric] = metrics[g.id]
dexnet_handle.dataset.store_grasp_metrics(object_name,
grasp_metrics,
gripper=gripper.name)
dexnet_handle.close_database()