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
