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.trimesh,
stable_pose.T_obj_world,
color=(0.5, 0.5, 0.5),
style='surface')
vis.pose(RigidTransform(), alpha=0.15)
vis.show(animate=config['animate'])
def display_gripper_on_object(obj_, grasp_):
"""display both object and gripper using mayavi"""
# transfer wrong was fixed by the previews comment of meshpy modification.
# gripper_name = 'robotiq_85'
# home_dir = os.environ['HOME']
# gripper = RobotGripper.load(gripper_name, home_dir + "/code/grasp-pointnet/dex-net/data/grippers")
# stable_pose = self.dataset.stable_pose(object.key, 'pose_1')
# T_obj_world = RigidTransform(from_frame='obj', to_frame='world')
t_obj_gripper = grasp_.gripper_pose(gripper)
stable_pose = t_obj_gripper
grasp_ = grasp_.perpendicular_table(stable_pose)
Vis.figure(bgcolor=(1, 1, 1), size=(1000, 1000))
Vis.gripper_on_object(gripper, grasp_, obj_,
gripper_color=(0.25, 0.25, 0.25),
# stable_pose=stable_pose, # .T_obj_world,
plot_table=False)
Vis.show()
def display_object(self, object_name, config=None):
"""Display an object
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 {}'.format(object_name))
obj = self.dataset[object_name]
vis.figure(bgcolor=(1, 1, 1), size=(1000, 1000))
vis.mesh(obj.mesh.trimesh, color=(0.5, 0.5, 0.5), style='surface')
vis.show(animate=config['animate'])
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.T_obj_world,
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.trimesh, 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 display_object(obj_):
"""display object only using mayavi"""
Vis.figure(bgcolor=(1, 1, 1), size=(1000, 1000))
Vis.mesh(obj_.mesh.trimesh, color=(0.5, 0.5, 0.5), style="surface")
Vis.show()