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 vertices_to_baxter_hand_pose(self, grasp_vertices, approach_direction, obj_name): """ takes the contacts positions in the object frame and returns the hand pose T_obj_gripper BE CAREFUL ABOUT THE FROM FRAME AND TO FRAME. the RigidTransform class' frames are weird. Parameters ---------- grasp_vertices : 2x3 :obj:`numpy.ndarray` position of the fingers in object frame approach_direction : 3x' :obj:`numpy.ndarray` there are multiple grasps that go through contact1 and contact2. This describes which orientation the hand should be in Returns ------- :obj:`autolab_core:RigidTransform` Hand pose in the object frame """ # parameters required to create a autolab_core:RigidTransform: # - rotation (aka 3x3 rotation matrix) # - translation (aka 3x1 vector) # - from_frame (aka str) # - to_frame (aka str) midpoint = (grasp_vertices[0] + grasp_vertices[1]) / 2 gripper_half_width = MAX_HAND_DISTANCE / 2 z = normalize(approach_direction) y = normalize(grasp_vertices[0] - grasp_vertices[1]) x = np.cross(y, z) rot_mat_opposite = np.array([x, y, z]).T p_opposite = midpoint rot_mat = rot_mat_opposite.T p = -np.matmul(rot_mat_opposite.T, p_opposite) rigid_trans = RigidTransform(rot_mat_opposite, p_opposite, to_frame='right_gripper', from_frame=obj_name) return rigid_trans
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 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(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()