def vis(self, mesh, grasp_vertices, grasp_qualities): """ Pass in any grasp and its associated grasp quality. this function will plot each grasp on the object and plot the grasps as a bar between the points, with colored dots on the line endpoints representing the grasp quality associated with each grasp Parameters ---------- mesh : :obj:`Trimesh` grasp_vertices : mx2x3 :obj:`numpy.ndarray` m grasps. Each grasp containts two contact points. Each contact point is a 3 dimensional vector, hence the shape mx2x3 grasp_qualities : mx' :obj:`numpy.ndarray` vector of grasp qualities for each grasp """ vis3d.mesh(mesh) dirs = normalize(grasp_vertices[:, 0] - grasp_vertices[:, 1], axis=1) midpoints = (grasp_vertices[:, 0] + grasp_vertices[:, 1]) / 2 grasp_endpoints = np.zeros(grasp_vertices.shape) grasp_vertices[:, 0] = midpoints + dirs * MAX_HAND_DISTANCE / 2 grasp_vertices[:, 1] = midpoints - dirs * MAX_HAND_DISTANCE / 2 for grasp, quality in zip(grasp_vertices, grasp_qualities): color = [min(1, 2 * (1 - quality)), min(1, 2 * quality), 0, 1] if color != [1, 0, 0, 1]: good_grasp += 1 # vis3d.plot3d(grasp, color=color, tube_radius=.0005) vis3d.plot3d(grasp, color=color, tube_radius=.0005) vis3d.show()
def visualize(self): """Visualize the salient edges of the mesh. """ vis3d.figure() for edge in self.salient_edges: vis3d.plot3d(self.mesh.vertices[edge], color=(0.0, 1.0, 0.0), tube_radius=0.0005) vis3d.mesh(self.mesh) vis3d.show()
def visualize_normals(mesh, vertices, normals): scale = 0.01 normals_scaled = normals * scale vis3d.pose(RigidTransform(), alpha=0.01, tube_radius=0.001, center_scale=0.002) vis3d.mesh(mesh, style='wireframe') vis3d.points(mesh.centroid, color=(0, 0, 0), scale=0.003) vis3d.points(vertices, color=(1, 0, 0), scale=0.001) for v, n in zip(vertices, normals_scaled): vis3d.plot3d([v, v + n], color=(1, 0, 0), tube_radius=0.0005) vis3d.show()
def grasp(grasp, T_obj_world=RigidTransform(from_frame='obj', to_frame='world'), tube_radius=0.0002, endpoint_color=(0, 1, 0), endpoint_scale=0.0005, grasp_axis_color=(0, 1, 0)): """ Plots a grasp as an axis and center. Parameters ---------- grasp : :obj:`dexnet.grasping.Grasp` the grasp to plot T_obj_world : :obj:`autolab_core.RigidTransform` the pose of the object that the grasp is referencing in world frame tube_radius : float radius of the plotted grasp axis endpoint_color : 3-tuple color of the endpoints of the grasp axis endpoint_scale : 3-tuple scale of the plotted endpoints grasp_axis_color : 3-tuple color of the grasp axis """ g1, g2 = grasp.endpoints center = grasp.center g1 = Point(g1, 'obj') g2 = Point(g2, 'obj') center = Point(center, 'obj') g1_tf = T_obj_world.apply(g1) g2_tf = T_obj_world.apply(g2) center_tf = T_obj_world.apply(center) grasp_axis_tf = np.array([g1_tf.data, g2_tf.data]) Visualizer3D.points(g1_tf.data, color=endpoint_color, scale=endpoint_scale) Visualizer3D.points(g2_tf.data, color=endpoint_color, scale=endpoint_scale) Visualizer3D.plot3d(grasp_axis_tf, color=grasp_axis_color, tube_radius=tube_radius) Visualizer3D.pose(grasp.T_grasp_obj, alpha=endpoint_scale * 10, tube_radius=tube_radius, center_scale=endpoint_scale)
def visualize_grasps(mesh, vertices, metrics): vis3d.pose(RigidTransform(), alpha=0.01, tube_radius=0.001, center_scale=0.002) vis3d.mesh(mesh, style='wireframe') vis3d.points(mesh.centroid, color=(0, 0, 0), scale=0.003) min_score = float(np.min(metrics)) max_score = float(np.max(metrics)) metrics_normalized = (metrics.astype(float) - min_score) / (max_score - min_score) for v, m in zip(vertices, metrics_normalized): vis3d.points(v, color=(1 - m, m, 0), scale=0.001) vis3d.plot3d(v, color=(1 - m, m, 0), tube_radius=0.0003) vis3d.show()
def gripper(gripper, grasp, T_obj_world, color=(0.5, 0.5, 0.5)): """ Plots a robotic gripper in a pose specified by a particular grasp object. Parameters ---------- gripper : :obj:`dexnet.grasping.RobotGripper` the gripper to plot grasp : :obj:`dexnet.grasping.Grasp` the grasp to plot the gripper performing T_obj_world : :obj:`autolab_core.RigidTransform` the pose of the object that the grasp is referencing in world frame color : 3-tuple color of the gripper mesh """ T_gripper_obj = grasp.gripper_pose(gripper) T_gripper_world = T_obj_world * T_gripper_obj T_mesh_world = T_gripper_world * gripper.T_mesh_gripper.inverse() T_mesh_world = T_mesh_world.as_frames('obj', 'world') Visualizer3D.mesh(gripper.mesh.trimesh, T_mesh_world, style='surface', color=color) g1, g2 = grasp.endpoints center = grasp.center g1 = Point(np.array([0.1, 0.0, 0.0]), 'obj') g2 = Point(np.array([0.0, 0.0, 0.0]), 'obj') center = Point(np.array([0.0, 0.0, 0.0]), 'obj') g1_tf = T_mesh_world.apply(g1) g2_tf = T_mesh_world.apply(g2) center_tf = T_mesh_world.apply(center) grasp_axis_tf = np.array([g1_tf.data, g2_tf.data]) points = [(x[0], x[1], x[2]) for x in grasp_axis_tf] Visualizer3D.plot3d(points, color=(0, 1, 0), tube_radius=0.004) g1 = Point(np.array([0.0, 0.0, 0.0]), 'obj') g2 = Point(np.array([0.0, 0.0, 0.1]), 'obj') center = Point(np.array([0.0, 0.0, 0.0]), 'obj') g1_tf = T_mesh_world.apply(g1) g2_tf = T_mesh_world.apply(g2) center_tf = T_mesh_world.apply(center) grasp_axis_tf = np.array([g1_tf.data, g2_tf.data]) points = [(x[0], x[1], x[2]) for x in grasp_axis_tf] Visualizer3D.plot3d(points, color=(1, 0, 0), tube_radius=0.004) g1 = Point(np.array([0.0, 0.0, 0.0]), 'obj') g2 = Point(np.array([0.0, 0.1, 0.0]), 'obj') center = Point(np.array([0.0, 0.0, 0.0]), 'obj') g1_tf = T_mesh_world.apply(g1) g2_tf = T_mesh_world.apply(g2) center_tf = T_mesh_world.apply(center) grasp_axis_tf = np.array([g1_tf.data, g2_tf.data]) points = [(x[0], x[1], x[2]) for x in grasp_axis_tf] Visualizer3D.plot3d(points, color=(0, 0, 1), tube_radius=0.004)
def shownormals(normal, surface_point, color=(1, 0, 0), tube_radius=0.001, T_obj_world=RigidTransform(from_frame='obj', to_frame='world')): """ Plots a grasp as an axis and center. Parameters ---------- grasp : :obj:`dexnet.grasping.Grasp` the grasp to plot T_obj_world : :obj:`autolab_core.RigidTransform` the pose of the object that the grasp is referencing in world frame tube_radius : float radius of the plotted grasp axis endpoint_color : 3-tuple color of the endpoints of the grasp axis endpoint_scale : 3-tuple scale of the plotted endpoints grasp_axis_color : 3-tuple color of the grasp axis """ normalpoint = surface_point + 0.02 * normal normalpoint = Point(normalpoint, 'obj') surface_point = Point(surface_point, 'obj') normal_tf = T_obj_world.apply(normalpoint) surface_point_tf = T_obj_world.apply(surface_point) normal_axis_tf = np.array([normal_tf.data, surface_point_tf.data]) # print [(x[0], x[1], x[2]) for x in normal_axis_tf] points = [(x[0], x[1], x[2]) for x in normal_axis_tf] Visualizer3D.plot3d(points, color=color, tube_radius=tube_radius)
def show_points(points, color=(0,1,0), scale=0.005, frame_size=0.2, frame_radius=0.02): vis.figure(bgcolor=(1,1,1), size=(500,500)) vis.points(np.array(points), color=color, scale=scale) vis.plot3d(np.array(([0, 0, 0], [frame_size, 0, 0])).astype(np.float32), color=(1,0,0), tube_radius=frame_radius) vis.plot3d(np.array(([0, 0, 0], [0, frame_size, 0])).astype(np.float32), color=(0,1,0), tube_radius=frame_radius) vis.plot3d(np.array(([0, 0, 0], [0, 0, frame_size])).astype(np.float32), color=(0,0,1), tube_radius=frame_radius) vis.show()
def vis(self, mesh, grasp_vertices, grasp_qualities, grasp_normals): """ Pass in any grasp and its associated grasp quality. this function will plot each grasp on the object and plot the grasps as a bar between the points, with colored dots on the line endpoints representing the grasp quality associated with each grasp Parameters ---------- mesh : :obj:`Trimesh` grasp_vertices : mx2x3 :obj:`numpy.ndarray` m grasps. Each grasp containts two contact points. Each contact point is a 3 dimensional vector, hence the shape mx2x3 grasp_qualities : mx' :obj:`numpy.ndarray` vector of grasp qualities for each grasp """ vis3d.mesh(mesh) middle_of_part = np.mean(np.mean(grasp_vertices, axis=1), axis=0) print(middle_of_part) vis3d.points(middle_of_part, scale=0.003) dirs = normalize(grasp_vertices[:, 0] - grasp_vertices[:, 1], axis=1) midpoints = (grasp_vertices[:, 0] + grasp_vertices[:, 1]) / 2 grasp_endpoints = np.zeros(grasp_vertices.shape) grasp_endpoints[:, 0] = midpoints + dirs * MAX_HAND_DISTANCE / 2 grasp_endpoints[:, 1] = midpoints - dirs * MAX_HAND_DISTANCE / 2 n0 = np.zeros(grasp_endpoints.shape) n1 = np.zeros(grasp_endpoints.shape) normal_scale = 0.01 n0[:, 0] = grasp_vertices[:, 0] n0[:, 1] = grasp_vertices[:, 0] + normal_scale * grasp_normals[:, 0] n1[:, 0] = grasp_vertices[:, 1] n1[:, 1] = grasp_vertices[:, 1] + normal_scale * grasp_normals[:, 1] for grasp, quality, normal0, normal1 in zip(grasp_endpoints, grasp_qualities, n0, n1): color = [min(1, 2 * (1 - quality)), min(1, 2 * quality), 0, 1] vis3d.plot3d(grasp, color=color, tube_radius=.001) vis3d.plot3d(normal0, color=(0, 0, 0), tube_radius=.002) vis3d.plot3d(normal1, color=(0, 0, 0), tube_radius=.002) vis3d.show()
def vis(self, mesh, grasp_vertices, grasp_qualities, top_n_grasp_vertices, approach_directions, rs, TG): """ Pass in any grasp and its associated grasp quality. this function will plot each grasp on the object and plot the grasps as a bar between the points, with colored dots on the line endpoints representing the grasp quality associated with each grasp Parameters ---------- mesh : :obj:`Trimesh` grasp_vertices : mx2x3 :obj:`numpy.ndarray` m grasps. Each grasp containts two contact points. Each contact point is a 3 dimensional vector, hence the shape mx2x3 grasp_qualities : mx' :obj:`numpy.ndarray` vector of grasp qualities for each grasp """ vis3d.mesh(mesh) dirs = normalize(grasp_vertices[:, 0] - grasp_vertices[:, 1], axis=1) midpoints = (grasp_vertices[:, 0] + grasp_vertices[:, 1]) / 2 grasp_endpoints = np.zeros(grasp_vertices.shape) grasp_vertices[:, 0] = midpoints + dirs * MAX_HAND_DISTANCE / 2 grasp_vertices[:, 1] = midpoints - dirs * MAX_HAND_DISTANCE / 2 for i, (grasp, quality) in enumerate(zip(grasp_vertices, grasp_qualities)): color = [min(1, 2 * (1 - quality)), min(1, 2 * quality), 0, 1] vis3d.plot3d(grasp, color=color, tube_radius=.001) blue = [0, 0, 255] light_blue = [50, 50, 200] for i, (grasp, approach_direction) in enumerate( zip(top_n_grasp_vertices, approach_directions)): midpoint = np.mean(grasp, axis=0) approach_direction = np.asarray( [midpoint, midpoint - 0.1 * approach_direction]) if (i == 0): vis3d.plot3d(approach_direction, color=blue, tube_radius=.005) vis3d.points(grasp, color=blue, scale=.005) else: vis3d.plot3d(approach_direction, color=light_blue, tube_radius=.001) vis3d.points(grasp, color=light_blue, scale=.001) midpoint = np.mean(top_n_grasp_vertices[0], axis=0) x_purp = [204, 0, 204] x_axis = np.asarray([midpoint, midpoint + 0.1 * rs[:, 0]]) y_cyan = [0, 204, 204] y_axis = np.asarray([midpoint, midpoint + 0.1 * rs[:, 1]]) z_black = [0, 0, 0] z_axis = np.asarray([midpoint, midpoint + 0.3 * rs[:, 2]]) vis3d.plot3d(x_axis, color=x_purp, tube_radius=.005) vis3d.plot3d(y_axis, color=y_cyan, tube_radius=.005) vis3d.plot3d(z_axis, color=z_black, tube_radius=.005) origin = np.asarray([0, 0, 0]) x_axis = np.asarray([origin, 0.2 * np.array([1, 0, 0])]) y_axis = np.asarray([origin, 0.2 * np.array([0, 1, 0])]) z_axis = np.asarray([origin, 0.2 * np.array([0, 0, 1])]) vis3d.plot3d(x_axis, color=x_purp, tube_radius=.005) vis3d.plot3d(y_axis, color=y_cyan, tube_radius=.005) vis3d.plot3d(z_axis, color=z_black, tube_radius=.005) # eucl_orien = np.asarray(tfs.euler_from_quaternion(TG.quaternion)) intermediate_pos = TG.position - np.reshape( np.matmul(TG.rotation, np.array([[0], [0], [0.2]])), (1, 3)) print(intermediate_pos, np.shape(intermediate_pos)) red = [255, 0, 0] vis3d.points(intermediate_pos, color=red, scale=.01) vis3d.points(TG.position, color=red, scale=.01) vis3d.show()
if __name__ == '__main__': depth_im_filename = sys.argv[1] camera_intr_filename = sys.argv[2] camera_intr = CameraIntrinsics.load(camera_intr_filename) depth_im = DepthImage.open(depth_im_filename, frame=camera_intr.frame) depth_im = depth_im.inpaint() point_cloud_im = camera_intr.deproject_to_image(depth_im) normal_cloud_im = point_cloud_im.normal_cloud_im(ksize=KSIZE) vis3d.figure() vis3d.points(point_cloud_im.to_point_cloud(), scale=0.0025) alpha = 0.025 subsample = 20 for i in range(0, point_cloud_im.height, subsample): for j in range(0, point_cloud_im.width, subsample): p = point_cloud_im[i, j] n = normal_cloud_im[i, j] n2 = normal_cloud_im_s[i, j] if np.linalg.norm(n) > 0: points = np.array([p, p + alpha * n]) vis3d.plot3d(points, tube_radius=0.001, color=(1, 0, 0)) points = np.array([p, p + alpha * n2]) vis3d.plot3d(points, tube_radius=0.001, color=(1, 0, 1)) vis3d.show()
def compute_approach_direction(self, mesh, grasp_vertices, grasp_quality, grasp_normals): ## initalizing stuff ## visualize = True nb_directions_to_test = 6 normal_scale = 0.01 plane_normal = normalize(grasp_vertices[0] - grasp_vertices[1]) midpoint = (grasp_vertices[0] + grasp_vertices[1]) / 2 ## generating a certain number of approach directions ## theta = np.pi / nb_directions_to_test rot_mat = rotation_3d(-plane_normal, theta) horizontal_direction = normalize( np.cross(plane_normal, np.array([0, 0, 1]))) directions_to_test = [horizontal_direction] #these are vectors approach_directions = [ np.array( [midpoint, midpoint + horizontal_direction * normal_scale]) ] #these are two points for visualization for i in range(nb_directions_to_test - 1): directions_to_test.append( normalize(np.matmul(rot_mat, directions_to_test[-1]))) approach_directions.append( np.array([ midpoint, midpoint + directions_to_test[-1] * normal_scale ])) ## computing the palm position for each approach direction ## palm_positions = [] for i in range(nb_directions_to_test): palm_positions.append(midpoint + finger_length * directions_to_test[i]) if visualize: ## plotting the whole mesh ## vis3d.mesh(mesh, style='wireframe') ## computing and plotting midpoint and gripper position ## dirs = (grasp_vertices[0] - grasp_vertices[1] ) / np.linalg.norm(grasp_vertices[0] - grasp_vertices[1]) grasp_endpoints = np.zeros(grasp_vertices.shape) grasp_endpoints[0] = midpoint + dirs * MAX_HAND_DISTANCE / 2 grasp_endpoints[1] = midpoint - dirs * MAX_HAND_DISTANCE / 2 color = [ min(1, 2 * (1 - grasp_quality)), min(1, 2 * grasp_quality), 0, 1 ] vis3d.plot3d(grasp_endpoints, color=color, tube_radius=.001) vis3d.points(midpoint, scale=0.003) ## computing and plotting normals at contact points ## n0 = np.zeros(grasp_endpoints.shape) n1 = np.zeros(grasp_endpoints.shape) n0[0] = grasp_vertices[0] n0[1] = grasp_vertices[0] + normal_scale * grasp_normals[0] n1[0] = grasp_vertices[1] n1[1] = grasp_vertices[1] + normal_scale * grasp_normals[1] vis3d.plot3d(n0, color=(0, 0, 0), tube_radius=.002) vis3d.plot3d(n1, color=(0, 0, 0), tube_radius=.002) ## plotting normals the palm positions for each potential approach direction ## for i in range(nb_directions_to_test): vis3d.points(palm_positions[i], scale=0.003) vis3d.show() directions_to_test = [ directions_to_test[3], directions_to_test[2], directions_to_test[4], directions_to_test[1], directions_to_test[5], directions_to_test[0] ] palm_positions = [ palm_positions[3], palm_positions[2], palm_positions[4], palm_positions[1], palm_positions[5], palm_positions[0] ] ## checking if some approach direction is valid ## for i in range(nb_directions_to_test): if len( trimesh.intersections.mesh_plane(mesh, directions_to_test[i], palm_positions[i])) == 0: # it means the palm won't bump with part return directions_to_test[i] # it means all approach directions will bump with part return -1
def vis_transform(self, mesh, G_transform, vertices): """ Pass in any grasp and its associated grasp quality. this function will plot each grasp on the object and plot the grasps as a bar between the points, with colored dots on the line endpoints representing the grasp quality associated with each grasp Parameters ---------- mesh : :obj:`Trimesh` grasp_vertices : mx2x3 :obj:`numpy.ndarray` m grasps. Each grasp containts two contact points. Each contact point is a 3 dimensional vector, hence the shape mx2x3 grasp_qualities : mx' :obj:`numpy.ndarray` vector of grasp qualities for each grasp """ L = MAX_HAND_DISTANCE / 2 # gripper half width # transform from gripper to contact 1 G_gc1 = np.array([[1, 0, 0, 0], [0, 0, 1, -1 * L], [0, -1, 0, 0], [0, 0, 0, 1]]) # transform from gripper to contact 2 G_gc2 = np.array([[1, 0, 0, 0], [0, 0, -1, L], [0, 1, 0, 0], [0, 0, 0, 1]]) G = G_transform.matrix print('G') print(G) G_oc1 = np.matmul(G, G_gc1) G_oc2 = np.matmul(G, G_gc2) scale = 0.01 o = np.array([0, 0, 0, 1]) x = np.array([scale, 0, 0, 1]) y = np.array([0, scale, 0, 1]) z = np.array([0, 0, scale, 1]) ot = np.matmul(G, o) xt = np.matmul(G, x) yt = np.matmul(G, y) zt = np.matmul(G, z) o1 = np.matmul(G_oc1, o) x1 = np.matmul(G_oc1, x) y1 = np.matmul(G_oc1, y) z1 = np.matmul(G_oc1, z) o2 = np.matmul(G_oc2, o) x2 = np.matmul(G_oc2, x) y2 = np.matmul(G_oc2, y) z2 = np.matmul(G_oc2, z) vis3d.mesh(mesh, style='wireframe') #Plot origin axes x_axis = np.array([o, x])[:, :3] y_axis = np.array([o, y])[:, :3] z_axis = np.array([o, z])[:, :3] x_axis_t = np.array([ot, xt])[:, :3] y_axis_t = np.array([ot, yt])[:, :3] z_axis_t = np.array([ot, zt])[:, :3] x_axis_1 = np.array([o1, x1])[:, :3] y_axis_1 = np.array([o1, y1])[:, :3] z_axis_1 = np.array([o1, z1])[:, :3] x_axis_2 = np.array([o2, x2])[:, :3] y_axis_2 = np.array([o2, y2])[:, :3] z_axis_2 = np.array([o2, z2])[:, :3] vis3d.plot3d(x_axis, color=(0.5, 0, 0), tube_radius=0.001) vis3d.plot3d(y_axis, color=(0, 0.5, 0), tube_radius=0.001) vis3d.plot3d(z_axis, color=(0, 0, 0.5), tube_radius=0.001) vis3d.plot3d(x_axis_t, color=(255, 0, 0), tube_radius=0.001) vis3d.plot3d(y_axis_t, color=(0, 255, 0), tube_radius=0.001) vis3d.plot3d(z_axis_t, color=(0, 0, 255), tube_radius=0.001) vis3d.plot3d(x_axis_1, color=(255, 0, 0), tube_radius=0.001) vis3d.plot3d(y_axis_1, color=(0, 255, 0), tube_radius=0.001) vis3d.plot3d(z_axis_1, color=(0, 0, 255), tube_radius=0.001) vis3d.plot3d(x_axis_2, color=(255, 0, 0), tube_radius=0.001) vis3d.plot3d(y_axis_2, color=(0, 255, 0), tube_radius=0.001) vis3d.plot3d(z_axis_2, color=(0, 0, 255), tube_radius=0.001) vis3d.points(vertices[0], scale=0.003) vis3d.points(vertices[1], scale=0.003) vis3d.show()
def graspwithapproachvectorusingapproach_point( grasp, T_obj_world=RigidTransform(from_frame='obj', to_frame='world'), tube_radius=0.001, endpoint_color=(0, 1, 0), endpoint_scale=0.004, grasp_axis_color=(0, 1, 0), sdfcenter=np.array([50., 50., 50.]), sdforigin=np.array([0, 0, 0])): """ Plots a grasp as an axis and center. Parameters ---------- grasp : :obj:`dexnet.grasping.Grasp` the grasp to plot T_obj_world : :obj:`autolab_core.RigidTransform` the pose of the object that the grasp is referencing in world frame tube_radius : float radius of the plotted grasp axis endpoint_color : 3-tuple color of the endpoints of the grasp axis endpoint_scale : 3-tuple scale of the plotted endpoints grasp_axis_color : 3-tuple color of the grasp axis """ g1, g2 = grasp.endpoints center = grasp.center #这个center未必是真正的center,所以后面不能用它 approach_point = np.array(grasp.approach_point) approach_axis = grasp.rotated_full_axis[:, 0] approach_pointfar = approach_point + approach_axis * 0.1 # print 'approach_angle:',grasp.approach_angle # print 'approach_point:',grasp.approach_point g1 = Point(g1, 'obj') g2 = Point(g2, 'obj') center = Point(center, 'obj') approach_pointfar2 = Point(approach_pointfar, 'obj') approach_point2 = Point(approach_point, 'obj') g1_tf = T_obj_world.apply(g1) g2_tf = T_obj_world.apply(g2) #center_tf = T_obj_world.apply(center) approach_pointfar_tf = T_obj_world.apply(approach_pointfar2) approach_point2_tf = T_obj_world.apply(approach_point2) Visualizer3D.points(approach_point2_tf.data, color=(1, 0, 0), scale=0.001) grasp_axis_tf = np.array([g1_tf.data, g2_tf.data]) approach_axis_tf = np.array( [approach_point2_tf.data, approach_pointfar_tf.data]) #approach_point2_tf_again = np.array([approach_point2_tf.data, approach_point2_tf.data]) # print [(x[0], x[1], x[2]) for x in grasp_axis_tf] #print [(x[0], x[1], x[2]) for x in grasp_axis_tf] #points = [(x[0]+sdfcenter[0]+sdforigin[0] , x[1]+sdfcenter[1]+sdforigin[1] , x[2] +sdfcenter[2]+sdforigin[2]) for x in grasp_axis_tf] points = [(x[0], x[1], x[2]) for x in grasp_axis_tf] approaching = [(y[0], y[1], y[2]) for y in approach_axis_tf] # approachingpoint=[(z[0] , z[1] , z[2] ) for z in approach_point2_tf_again] #points = [(x[0] , x[1] , x[2] ) for x in grasp_axis_tf] Visualizer3D.plot3d(points, color=grasp_axis_color, tube_radius=tube_radius) Visualizer3D.plot3d(approaching, color=(0, 0.5, 0.5), tube_radius=tube_radius)
def vis(self, mesh, grasp_vertices, grasp_normals, grasp_qualities, hand_poses): """ Pass in any grasp and its associated grasp quality. this function will plot each grasp on the object and plot the grasps as a bar between the points, with colored dots on the line endpoints representing the grasp quality associated with each grasp Parameters ---------- mesh : :obj:`Trimesh` grasp_vertices : mx2x3 :obj:`numpy.ndarray` m grasps. Each grasp containts two contact points. Each contact point is a 3 dimensional vector, hence the shape mx2x3 grasp_qualities : mx' :obj:`numpy.ndarray` vector of grasp qualities for each grasp hand_poses: list of RigidTransform objects """ vis3d.mesh(mesh) dirs = normalize(grasp_vertices[:, 0] - grasp_vertices[:, 1], axis=1) midpoints = (grasp_vertices[:, 0] + grasp_vertices[:, 1]) / 2 grasp_contacts = np.zeros(grasp_vertices.shape) grasp_contacts[:, 0] = midpoints + dirs * MAX_HAND_DISTANCE / 2 grasp_contacts[:, 1] = midpoints - dirs * MAX_HAND_DISTANCE / 2 for grasp, quality, normal, contacts, hand_pose in zip( grasp_vertices, grasp_qualities, grasp_normals, grasp_contacts, hand_poses): color = [min(1, 2 * (1 - quality)), min(1, 2 * quality), 0, 1] vis3d.points(grasp, scale=0.001, color=(1, 0, 0)) n1_endpoints = np.zeros((2, 3)) n1_endpoints[0] = grasp[0] n1_endpoints[1] = grasp[0] + 0.01 * normal[0] n2_endpoints = np.zeros((2, 3)) n2_endpoints[0] = grasp[1] n2_endpoints[1] = grasp[1] + 0.01 * normal[1] vis3d.plot3d(n1_endpoints, color=color, tube_radius=0.001) vis3d.plot3d(n2_endpoints, color=color, tube_radius=0.001) # hand pose vis3d.points(hand_pose.position, scale=0.001, color=(0.5, 0.5, 0.5)) x_axis_endpoints = np.zeros((2, 3)) x_axis_endpoints[0] = hand_pose.position x_axis_endpoints[1] = hand_pose.position + 0.01 * hand_pose.x_axis vis3d.plot3d(x_axis_endpoints, color=(1, 0, 0), tube_radius=0.0001) y_axis_endpoints = np.zeros((2, 3)) y_axis_endpoints[0] = hand_pose.position y_axis_endpoints[1] = hand_pose.position + 0.01 * hand_pose.y_axis vis3d.plot3d(y_axis_endpoints, color=(0, 1, 0), tube_radius=0.0001) z_axis_endpoints = np.zeros((2, 3)) z_axis_endpoints[0] = hand_pose.position z_axis_endpoints[1] = hand_pose.position + 0.01 * hand_pose.z_axis vis3d.plot3d(z_axis_endpoints, color=(0, 0, 1), tube_radius=0.0001) vis3d.plot3d(contacts, color=(0, 0, 1), tube_radius=.0005) vis3d.show()