def showgraspfrompickle(self, object_name,metric_name,onlynagative,onlypositive,openravechecker):
object = self.dataset[object_name]
labelgrasps,labelmetrics=self.readdatafrompickle(object_name)
vis.figure()
vis.mesh(object.mesh.trimesh ,style='surface')
config=self._get_config(None)
low =0.0
high = np.max([labelmetrics[i][metric_name] for i in range(len(labelmetrics))])
# print 'high',high,config['quality_scale']
if low == high:
q_to_c = lambda quality: config['quality_scale']
else:
q_to_c = lambda quality: config['quality_scale'] * (quality - low) / (high - low)
recalculate_grasp=[]
for i in range(len(labelgrasps)):
if labelmetrics[i][metric_name]==0 :
if not onlypositive:
vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=(1,0,0), grasp_axis_color=(1,0,0) )
elif labelmetrics[i][metric_name]==-1 :
if not onlypositive:
vis.shownormals(labelgrasps[i][1],labelgrasps[i][0],color=(1,0,0) )
recalculate_grasp.append(labelgrasps[i])
elif labelmetrics[i][metric_name]>0 :
if not onlynagative:
color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1]
vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=color, grasp_axis_color=color )
recalculate_grasp.append(labelgrasps[i])
print i # ,high
vis.pose(RigidTransform(), alpha=0.1)
vis.show(animate=False)
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 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 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 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'])
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)
gripper = env.gripper(action)
T_start_world = action.T_begin_world * gripper.T_mesh_grasp
T_end_world = action.T_end_world * gripper.T_mesh_grasp
vis3d.mesh(gripper.mesh, T_start_world, color=(0, 1, 0))
vis3d.mesh(gripper.mesh, T_end_world, color=(1, 0, 0))
display_or_save('{}_quality_increases.gif'.format(obj_name))
env.render_3d_scene()
vis3d.show(starting_camera_pose=CAMERA_POSE)
obj_config = config['state_space']['object']
all_poses, _ = env.state.obj.mesh.compute_stable_poses(
sigma=obj_config['stp_com_sigma'],
n_samples=obj_config['stp_num_samples'],
threshold=.001)
# for pose in all_poses:
# env.state.obj.T_obj_world = to_rigid(pose)
# env.render_3d_scene()
# vis3d.show(starting_camera_pose=CAMERA_POSE)
mesh = trimesh.load('~/Downloads/chris.stl')
vis3d.mesh(mesh)
vis3d.show()
#figure_1()
#figure_2()
#figure_3()
#figure_0()
# figure_3()
# figure_0()
#failure_modes()
action = policy.action(env.state)
#noise_vis()
if False:
j = np.argmax(action.metadata['vertex_probs'][:, 2])
R = policy.toppling_model.tipping_point_rotations()[1]
def render_3d_scene(env):
vis3d.mesh(env.state.mesh,
env.state.T_obj_world.matrix,
color=env.state.color)
def showgraspfrompicklewithcandidate(self, object_name,metric_name,onlynagative,onlypositive,openravechecker):
object = self.dataset[object_name]
labelgrasps,labelmetrics,candidate_labelgrasps,candidate_labelmetrics=self.readdatafrompicklewithcandidate(object_name)
config=self._get_config(None)
low =0.0 #np.min(metrics)
high = np.max([labelmetrics[i][metric_name] for i in range(len(labelmetrics))])
# print 'high',high,config['quality_scale']
if low == high:
q_to_c = lambda quality: config['quality_scale']
else:
q_to_c = lambda quality: config['quality_scale'] * (quality - low) / (high - low)
recalculate_grasp=[]
vis.figure()
vis.mesh(object.mesh.trimesh ,style='surface')
for i in range(len(labelgrasps)):
if labelmetrics[i][metric_name]==0 :
if not onlypositive:
vis.figure()
vis.mesh(object.mesh.trimesh ,style='surface')
color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1]
vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=(1,0,0), grasp_axis_color=(1,0,0) )
for kk in range(len(candidate_labelgrasps[i] )):
print 'candidate_labelgrasps[i][kk]',candidate_labelgrasps[i][kk]
color = plt.get_cmap('hsv')(q_to_c(candidate_labelmetrics[i][candidate_labelgrasps[i][kk].id][metric_name]))[:-1]
vis.graspwithapproachvectorusingcenter_point(candidate_labelgrasps[i][kk] ,approaching_color=color, grasp_axis_color=color )
recalculate_grasp.append(labelgrasps[i])
print 'maxtrics',i,labelmetrics[i][metric_name] # ,high
vis.pose(RigidTransform(), alpha=0.1)
vis.show(animate=False)
elif labelmetrics[i][metric_name]==-1 :
if not onlypositive:
vis.shownormals(labelgrasps[i][1],labelgrasps[i][0],color=(0,0,1) )
else:
if not onlynagative:
vis.figure()
vis.mesh(object.mesh.trimesh ,style='surface')
color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1]
if len(candidate_labelgrasps[i])>0:
A=np.array(labelgrasps[i].rotated_full_axis[:,0] )
B=np.array(candidate_labelgrasps[i][0].rotated_full_axis[:,0] )
num = np.dot(A.T, B)
print num
if num<0.99:
labelgrasps[i].approach_angle_= labelgrasps[i]._angle_aligned_with_table( -candidate_labelgrasps[i][0].rotated_full_axis[:,0])
vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=color, grasp_axis_color=(0,0,1) )
print 'length',len(candidate_labelgrasps[i] )
for kk in range(len(candidate_labelgrasps[i] )):
color = plt.get_cmap('hsv')(q_to_c(candidate_labelmetrics[i][candidate_labelgrasps[i][kk].id][metric_name]))[:-1]
vis.graspwithapproachvectorusingcenter_point(candidate_labelgrasps[i][kk] ,approaching_color=(0,1,1), grasp_axis_color=color )
recalculate_grasp.append(labelgrasps[i])
print 'maxtrics',i,labelmetrics[i][metric_name] # ,high
vis.pose(RigidTransform(), alpha=0.1)
vis.show(animate=False)
def antipodal_grasp_sampler(self):
#of = ObjFile(OBJ_FILENAME)
#sf = SdfFile(SDF_FILENAME)
#mesh = of.read()
#sdf = sf.read()
#obj = GraspableObject3D(sdf, mesh)
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
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = AntipodalGraspSampler(gripper, CONFIG)
grasps = ags.sample_grasps(obj, num_grasps=100)
print(len(grasps))
quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['robust_ferrari_canny'])
#quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['force_closure'])
quality_fn = GraspQualityFunctionFactory.create_quality_function(obj, quality_config)
vis.figure()
#vis.points(PointCloud(nparraypoints), scale=0.001, color=np.array([0.5,0.5,0.5]))
#vis.plot3d(nparraypoints)
metrics = []
vis.mesh(obj.mesh.trimesh, style='surface')
for grasp in grasps:
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
#fn_fc = quality_fn(grasp).quality
true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config)
true_fc = true_fc
metrics.append(true_fc)
#color = quality_fn(grasp).quality
#if (true_fc > 1.0):
# true_fc = 1.0
#color = (1.0-true_fc, true_fc, 0.0)
low = np.min(metrics)
high = np.max(metrics)
print(low)
print(high)
if low == high:
q_to_c = lambda quality: CONFIG['quality_scale']
else:
q_to_c = lambda quality: CONFIG['quality_scale'] * (quality - low) / (high - low)
print(len(metrics))
for grasp, metric in zip(grasps, metrics):
color = plt.get_cmap('hsv')(q_to_c(metric))[:-1]
print(color)
vis.grasp(grasp, grasp_axis_color=color,endpoint_color=color)
vis.show(False)