def gripper_on_object(gripper,
grasp,
obj,
stable_pose=None,
T_table_world=RigidTransform(from_frame='table',
to_frame='world'),
gripper_color=(0.5, 0.5, 0.5),
object_color=(0.5, 0.5, 0.5),
style='surface',
plot_table=True,
table_dim=0.15):
""" Visualize a gripper on an object.
Parameters
----------
gripper : :obj:`dexnet.grasping.RobotGripper`
gripper to plot
grasp : :obj:`dexnet.grasping.Grasp`
grasp to plot the gripper in
obj : :obj:`dexnet.grasping.GraspableObject3D`
3D object to plot the gripper on
stable_pose : :obj:`meshpy.StablePose` or :obj:`autolab_core.RigidTransform`
stable pose of the object on a planar worksurface
T_table_world : :obj:`autolab_core.RigidTransform`
pose of table, specified as a transformation from mesh frame to world frame
gripper_color : 3-tuple
color of the gripper mesh
object_color : 3-tuple
color of the object mesh
style : :obj:`str`
color of the object mesh
plot_table : bool
whether or not to plot the table
table_dim : float
dimension of the table
"""
if stable_pose is None:
Visualizer3D.mesh(obj.mesh, color=object_color, style=style)
T_obj_world = RigidTransform(from_frame='obj', to_frame='world')
elif isinstance(stable_pose, StablePose):
T_obj_world = Visualizer3D.mesh_stable_pose(
obj.mesh,
stable_pose,
T_table_world=T_table_world,
color=object_color,
style=style,
plot_table=plot_table,
dim=table_dim)
else:
T_obj_world = Visualizer3D.mesh_table(obj.mesh,
stable_pose,
T_table_world=T_table_world,
color=object_color,
style=style,
plot_table=plot_table,
dim=table_dim)
DexNetVisualizer3D.gripper(gripper,
grasp,
T_obj_world,
color=gripper_color)
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
stable_pose = mesh.resting_pose(T_obj_table)
#print stable_pose.r
table_dim = 0.3
T_obj_table_plot = mesh.get_T_surface_obj(T_obj_table)
T_obj_table_plot.translation[0] += 0.1
vis.figure()
vis.mesh(mesh, T_obj_table_plot, color=(1, 0, 0), style='wireframe')
vis.points(Point(mesh.center_of_mass, 'obj'),
T_obj_table_plot,
color=(1, 0, 1),
scale=0.01)
vis.pose(T_obj_table_plot, alpha=0.1)
vis.mesh_stable_pose(mesh,
stable_pose,
dim=table_dim,
color=(0, 1, 0),
style='surface')
vis.pose(stable_pose.T_obj_table, alpha=0.1)
vis.show()
exit(0)
# compute stable poses
vis.figure()
vis.mesh(mesh, color=(1, 1, 0), style='surface')
vis.mesh(mesh.convex_hull(), color=(1, 0, 0))
stable_poses = mesh.stable_poses()
vis.show()
def antipodal_grasp_sampler(visual=False, debug=False):
mass = 1.0
CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE
mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir'])
mesh_processor.generate_graspable(CONFIG)
mesh = mesh_processor.mesh
sdf = mesh_processor.sdf
stable_poses = mesh_processor.stable_poses
obj = GraspableObject3D(sdf, mesh)
stable_pose = stable_poses[0]
if visual:
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)
if debug:
print(len(stable_poses))
#for stable_pose in stable_poses:
# print(stable_pose.p)
# print(stable_pose.r)
# print(stable_pose.x0)
# print(stable_pose.face)
# print(stable_pose.id)
# glass = 22 is standing straight
if debug:
print(stable_pose.r)
print(stable_pose.T_obj_world)
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
ags = AntipodalGraspSampler(gripper, CONFIG)
stable_pose.id = 0
grasps = ags.generate_grasps(obj,
target_num_grasps=20,
max_iter=5,
stable_pose=stable_pose.r)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['robust_ferrari_canny'])
metrics = []
result = []
#grasps = map(lambda g : g.perpendicular_table(stable_pose), grasps)
for grasp in grasps:
c1, c2 = grasp.endpoints
true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config)
metrics.append(true_fc)
result.append((c1, c2))
if debug:
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
print("Grasp:")
print(c1.point)
print(c2.point)
print(true_fc)
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 visual:
for grasp, metric in zip(grasps, metrics):
#grasp2 = grasp.perpendicular_table(stable_pose)
#c1, c2 = grasp.endpoints
#axis = ParallelJawPtGrasp3D.axis_from_endpoints(c1, c2)
#angle = np.dot(np.matmul(stable_pose.r, axis), [1,0,0])
#angle = math.tan(axis[1]/axis[0])
#angle = math.degrees(angle)%360
#print(angle)
#print(angle/360.0)
color = plt.get_cmap('hsv')(q_to_c(metric))[:-1]
vis.grasp(grasp,
T_obj_world=T_obj_world,
grasp_axis_color=color,
endpoint_color=color)
#axis = np.array([[0,0,0], point])
#points = [(x[0], x[1], x[2]) for x in axis]
#Visualizer3D.plot3d(points, color=(0,0,1), tube_radius=0.002)
vis.show(False)
pose_matrix = np.eye(4, 4)
pose_matrix[:3, :3] = T_obj_world.rotation
pose_matrix[:3, 3] = T_obj_world.translation
return pose_matrix, result
