def tipping_point_rotations(self): tipping_point_rotations = [] for edge, (edge_point1, edge_point2) in enumerate(self.edge_points): com_projected_on_edge = self.com_projected_on_edges[edge] s = normalize(edge_point2 - edge_point1) x = normalize(com_projected_on_edge - self.com) y = -normalize(np.cross(x, up)) a = .2 if self.obj.key == "mini_dexnet~yoda" else .01 topple_angle = math.acos(np.dot(x, -up)) + a #.01 tipping_point_rotations.append( RigidTransform.rotation_from_axis_and_origin( y, edge_point1, topple_angle)) return tipping_point_rotations
def add_noise(self, vertices, normals, push_directions, n_trials): """ adds noise to the vertex position and friction, and intersects that new position with where the finger would now press against the object Parameters ---------- vertices : nx3 :obj:`numpy.ndarray` normals : nx3 :obj:`numpy.ndarray` push_direction : nx3 :obj:`numpy.ndarray` n_trials : int Returns nx3 :obj`numpy.ndarray` nx3 :obj`numpy.ndarray` nx3 :obj`numpy.ndarray` nx1 :obj`numpy.ndarray` """ vertices = np.repeat(vertices, n_trials, axis=0) normals = np.repeat(normals, n_trials, axis=0) push_directions = np.repeat(push_directions, n_trials, axis=0) \ + np.random.normal(scale=self.push_direction_sigma, size=[len(push_directions)*n_trials, 3]) push_directions = normalize(push_directions, axis=1) a = time() # # Add noise and find the new intersection location # vertices_copied = deepcopy(vertices) # ray_origins = vertices + .01 * normals # # ray_origins = vertices + np.random.normal(scale=sigma, size=vertices.shape) + .01 * normals # vertices, _, face_ind = mesh.ray.intersects_location(ray_origins, -normals, multiple_hits=False) for i in range(len(vertices)): # Predicted to hit ground if push_directions[i].dot(up) > .5: # and vertices[i][2] < .05: vertices[i] = np.array([0, 0, 0]) normals[i] = np.array([0, 0, 0]) continue ray_origin = vertices[i] + np.random.normal( scale=self.finger_sigma, size=3) + .001 * normals[i] rand_axis = normalize(np.random.rand(3)) obj_rotation = RigidTransform.rotation_from_axis_and_origin( rand_axis, self.mesh.center_mass, np.random.normal(scale=self.obj_rot_sigma)).matrix ray_origin = obj_rotation.dot(np.append(ray_origin, [1]))[:3] ray_dir = obj_rotation.dot(np.append(-normals[i], [1]))[:3] # ray_dir = -normals[i] intersect, _, face_ind = \ self.mesh.ray.intersects_location([ray_origin], [ray_dir], multiple_hits=False) _, _, back_face_ind = \ self.mesh.ray.intersects_location([ray_origin], [-ray_dir], multiple_hits=False) if len(face_ind) == 0 or len(back_face_ind) != 0: vertices[i] = np.array([0, 0, 0]) normals[i] = np.array([0, 0, 0]) else: vertices[i] = intersect[0] normals[i] = self.mesh.face_normals[face_ind[0]] ground_friction_noises = 1 + np.random.normal( scale=self.ground_friction_sigma, size=len(vertices)) / self.ground_friction_coeff finger_friction_noises = 1 + np.random.normal( scale=self.finger_friction_sigma, size=len(vertices)) / self.finger_friction_coeff if self.log: print 'noise time:', time() - a return vertices, normals, push_directions, ground_friction_noises, finger_friction_noises