def get_grasp_map(vertices, normals, num_facets, mu, gamma): """ defined in the book on page 219. Compute the grasp map given the contact points and their surface normals Parameters ---------- vertices : 2x3 :obj:`numpy.ndarray` obj mesh vertices on which the fingers will be placed normals : 2x3 :obj:`numpy.ndarray` obj mesh normals at the contact points num_facets : int number of vectors to use to approximate the friction cone. these vectors will be along the friction cone boundary mu : float coefficient of friction gamma : float torsional friction coefficient Returns ------- :obj:`numpy.ndarray` grasp map """ # YOUR CODE HERE # Ryan code, uses utils for most of the work finger = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1]]) rot1 = look_at_general(vertices[0], normals[0]) adj1 = adj(rot1) rot2 = look_at_general(vertices[1], normals[1]) adj2 = adj(rot2) G1 = np.dot(adj1, finger) G2 = np.dot(adj2, finger) G = np.hstack((G1, G2)) return G
def get_grasp_map(vertices, normals, num_facets, mu, gamma): """ defined in the book on page 219. Compute the grasp map given the contact points and their surface normals Parameters ---------- vertices : 2x3 :obj:`numpy.ndarray` obj mesh vertices on which the fingers will be placed normals : 2x3 :obj:`numpy.ndarray` obj mesh normals at the contact points num_facets : int number of vectors to use to approximate the friction cone. these vectors will be along the friction cone boundary mu : float coefficient of friction gamma : float torsional friction coefficient Returns ------- :obj:`numpy.ndarray` grasp map """ # YOUR CODE HERE v1, v2 = vertices n1, n2 = normals g1 = look_at_general(v1, n1) g2 = look_at_general(v2, n2) adj_g1 = adj(g1) adj_g2 = adj(g2) G1 = adj_g1.dot(wrench_basis) # 6x4 G2 = adj_g2.dot(wrench_basis) # 6x4 return np.hstack([G1, G2]) # 6x8
def compute_force_closure(vertices, normals, num_facets, mu, gamma, object_mass, mesh, unused): """ Compute the force closure of some object at contacts, with normal vectors stored in normals You can use the line method described in HW2. if you do you will not need num_facets Parameters ---------- vertices : 2x3 :obj:`numpy.ndarray` obj mesh vertices on which the fingers will be placed normals : 2x3 :obj:`numpy.ndarray` obj mesh normals at the contact points num_facets : int number of vectors to use to approximate the friction cone. these vectors will be along the friction cone boundary mu : float coefficient of friction gamma : float torsional friction coefficient object_mass : float mass of the object Returns ------- float : quality of the grasp """ # YOUR CODE HERE """ First get the line between two grasp points calculate the friction cones check if the line passes through both """ # Find tangents and store them in a rotation matrix. R1 = look_at_general(vertices[0], normals[0]) R2 = look_at_general(vertices[1], normals[1]) theta = 0.2 # We should be able to choose this independently, but it changes the results.. f1 = normals[0] + np.cos(theta) * R1[0:3, 0] + np.sin(theta) * R1[0:3, 1] f2 = normals[1] + np.cos(theta) * R2[0:3, 0] + np.sin(theta) * R2[0:3, 1] line = vertices[1, :] - vertices[0, :] # we compare the angles between the line and the normal with the angle between fc vector and the normal line_in_FC1 = abs(np.matmul(line, normals[0]) / (norm(line) * norm(normals[0]))) > \ abs(np.matmul(line, f1) / (norm(line) * norm(f1))) line_in_FC2 = abs(np.matmul(-line, normals[1]) / (norm(-line) * norm(normals[1]))) > \ abs(np.matmul(-line, f2) / (norm(-line) * norm(f2))) return line_in_FC1 and line_in_FC2
def is_in_cone(p1, p2, norm1, mu): """ Author: Ryan Determines if p2 is in the friction cone of p1 Parameters ---------- p1 : coordinate of point 1 in world frame p2 : coordinate of point 2 in world frame norm1 : normal direction of force 1 in world frame mu : coefficient of friction Returns ------- bool : whether p2 is in the friction cone of p1 """ Gaw = look_at_general(p1, norm1) Gwa = np.linalg.inv(Gaw) pt = np.array([p2[0], p2[1], p2[2], 1]) pa = np.dot(Gwa, pt) if math.sqrt(pa[0]**2 + pa[1]**2) <= mu * pa[2]: return True else: return False
def get_grasp_map(vertices, normals, num_facets, mu, gamma): """ defined in the book on page 219. Compute the grasp map given the contact points and their surface normals Parameters ---------- vertices : 2x3 :obj:`numpy.ndarray` obj mesh vertices on which the fingers will be placed normals : 2x3 :obj:`numpy.ndarray` obj mesh normals at the contact points num_facets : int number of vectors to use to approximate the friction cone. these vectors will be along the friction cone boundary mu : float coefficient of friction gamma : float torsional friction coefficient Returns ------- :obj:`numpy.ndarray` grasp map """ # YOUR CODE HERE if mu == 0: print("Use point contact without friction") if gamma == 0: print("Do not use soft-finger bases") B = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1]]) g1 = look_at_general(vertices[0], normals[0]) A1 = np.linalg.inv(adj(g1)) g2 = look_at_general(vertices[1], normals[1]) A2 = np.linalg.inv(adj(g2)) # as everything is in world coordinates we don't need to transform coordinate systems with the adjoint. G = np.hstack((np.matmul(A1, B), np.matmul(A2, B))) return G, np.matmul(A1, B), np.matmul(A2, B)
def vertices_to_baxter_hand_pose(self, grasp_vertices, approach_direction, Rot, obj_name='gearbox'): """ 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 Note: in object frame """ # YOUR CODE HERE v1, v2 = grasp_vertices center = (v1 + v2) / 2 #print("v1,v2,center", v1, v2, center) #print("App_dir:", approach_direction) rot = look_at_general(center, approach_direction) wrist_position = center #- approach_direction*GRIPPER_LENGTH # note that this only works b/c I made sure approach_direction is normalized # print("wrist_position ", wrist_position) print("approach direction: ", approach_direction) print("vertices", v1, v2) # time.sleep(10) # wrist_position assumes is the position of the wrist (duh) and not the grippers #print("rotation", rot) # rot = rot[:3, :3] modified_identity = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) rot = np.dot(modified_identity, np.asarray(Rot[:3, :3])) rpy = tf.transformations.euler_from_matrix(rot) print("!!!!!!!!!!RPY!!!!!!!!!!") print("rot in rpy", rpy) g = RigidTransform(rot, wrist_position, from_frame='grasp', to_frame=obj_name) print(" ## g ##### ", g) # I quite not sure here return RigidTransform(rot, wrist_position, from_frame='grasp', to_frame=obj_name)
def get_friction_cone_basis(vertex, normal, num_facets, mu): axes_rot_3d = look_at_general(vertex, normal) axes_rt = RigidTransform(rotation=axes_rot_3d, translation=vertex) y_axis = axes_rt.y_axis angle = np.arctan(mu) rot = quaternion_matrix(quaternion_about_axis(angle, y_axis)) cone_vector = rot.dot(normal) cone_basis_vecs = [] for angle in np.linspace(0, 2. * np.pi - 1e-8, num_facets): cone_basis_vec = quaternion_matrix(quaternion_about_axis( angle, normal)).dot(cone_vector) cone_basis_vecs.append(cone_basis_vec) return np.array(cone_basis_vecs).reshape((3, num_facets))