def get_hand_rot(): """ Computes the pose of the hand to be perpendicular to the direction of the push Parameters ---------- start : 3x' :obj:`numpy.ndarray` end : 3x' :obj:`numpy.ndarray` Returns ------- 3x3 :obj:`numpy.ndarray` 3D Rotation Matrix """ # z = normalize(end - start) # y = normalize(np.cross(z, -up)) # x = normalize(np.cross(z, -y)) # y = normalize(start - end) # z = normalize(np.cross(y, up)) # x = normalize(np.cross(y, z)) # x = normalize(start - end) x = normalize(end - start) y = normalize(np.cross(x, up)) z = normalize(np.cross(x, y)) return np.hstack((x.reshape((-1,1)), y.reshape((-1,1)), z.reshape((-1,1))))
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 finger_friction_moment(self, f_z, edge_point1, edge_point2): """ maximum increase in torque that can be resisted due to the finger pressing down on the object. (If you press down harder, you can resist more torques, if you are lifting up the object, it will resist fewer torques) Parameters ---------- f_z : float how much the finger would press in the downward direction in order to topple the object edge_point1 : 3x1 :obj:`numpy.ndarray` edge_point2 : 3x1 :obj:`numpy.ndarray` Returns ------- float """ v = self.com - edge_point1 s = normalize(edge_point2 - edge_point1) com_projected_on_edge = v.dot(s) * s + edge_point1 offset_dist = np.linalg.norm(edge_point1 - edge_point2) offsets = np.linspace(0, offset_dist, self.num_approx) offsets_relative_to_com = offsets - np.linalg.norm( com_projected_on_edge - edge_point1) downward_force_dist = np.array([f_z / float(self.num_approx)] * self.num_approx) return self.ground_friction_coeff * downward_force_dist.dot( np.abs(offsets_relative_to_com))
def induced_torque(self, vertex, push_direction, edge_point1, edge_point2, com_projected_on_edge, required_force): """ how much torque around the z axis (centered around com_projected_on_edge) does the pushing action exert on the object Parameters ---------- vertex : 3x1 :obj:`numpy.ndarray` push_direction : 3x1 :obj:`numpy.ndarray` edge_point1 : 3x1 :obj:`numpy.ndarray` edge_point2 : 3x1 :obj:`numpy.ndarray` com_projected_on_edge : 3x1 :obj:`numpy.ndarray` Returns ------- float """ if self.baseline: return 0 r = vertex - com_projected_on_edge r[2] = 0 max_z_torque_dir = normalize(np.cross(r, up)) cos_push_vertex_z_angle = push_direction.dot(max_z_torque_dir) r = np.linalg.norm(r) return required_force * cos_push_vertex_z_angle * r
def figure_0(): action = policy.action(env.state) env.render_3d_scene() bottom_points = policy.toppling_model.bottom_points vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.001) mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix) mesh.fix_normals() direction = normalize([0, -.04, 0]) origin = mesh.center_mass + np.array([0, .04, .09]) intersect, _, face_ind = mesh.ray.intersects_location([origin], [direction], multiple_hits=False) normal = mesh.face_normals[face_ind[0]] start_point = intersect[0] + .06 * normal end_point = intersect[0] shaft_points = [start_point, end_point] h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071, 0.7071]]).dot(-normal) h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071, 0.7071]]).dot(-normal) head_points = [end_point - 0.02 * h2, end_point, end_point - 0.02 * h1] vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.002) vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.002) vis3d.points(Point(end_point), scale=.004, color=[0, 0, 0]) hand_pose = RigidTransform(rotation=policy.get_hand_pose( start_point, end_point)[0].rotation, translation=start_point, from_frame='grasp', to_frame='world') orig_pose = env.state.obj.T_obj_world.copy() env.state.obj.T_obj_world = policy.toppling_model.final_poses[1] action = policy.grasping_policy.action(env.state) env.state.obj.T_obj_world = orig_pose gripper = env.gripper(action) #vis3d.mesh(gripper.mesh, hand_pose * gripper.T_mesh_grasp, color=light_blue) # mesh = trimesh.load('~/Downloads/chris.stl') rot = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]).dot(np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])) T = RigidTransform(rotation=rot, translation=np.array([0, -.09, .1]), to_frame='mesh', from_frame='mesh') # vis3d.mesh(mesh, hand_pose * gripper.T_mesh_grasp * T, color=light_blue) vis3d.mesh(gripper.mesh, hand_pose * gripper.T_mesh_grasp * T, color=light_blue) vis3d.show(starting_camera_pose=CAMERA_POSE) env.state.obj.T_obj_world = policy.toppling_model.final_poses[1] action = policy.grasping_policy.action(env.state) print 'q:', action.q_value env.render_3d_scene() vis3d.gripper(env.gripper(action), action.grasp(env.gripper(action)), color=light_blue) vis3d.show(starting_camera_pose=CAMERA_POSE)
def compute_gradients(points, values): # def grad_helper(points, values): # n_approx = 10 # grads = [] # for point, value in zip(points, values): # idxes = np.argsort((np.sqrt((points - point)**2).sum(axis=0))) # grad = np.zeros(3) # for idx in idxes[:n_approx]: # grad += (points[idx] - point) * (values[idx] - value) # grads.append(grad / n_approx) # return np.mean(grads, axis=0) w1 = normalize(np.linalg.lstsq(points, values)[0]) projected_points = np.reshape(points.dot(w1), (-1, 1)) * np.reshape(w1, (1, -1)) nullspace_points = points - projected_points w2 = normalize(np.linalg.lstsq(nullspace_points, values)[0]) return w1, w2
def required_force(self, vertex, push_direction, edge_point1, edge_point2, com_projected_on_edge): """ How much to press against the queried point in order to counteract gravity Parameters ---------- vertex : 3x1 :obj:`numpy.ndarray` push_direction : 3x1 :obj:`numpy.ndarray` edge_point1 : 3x1 :obj:`numpy.ndarray` edge_point2 : 3x1 :obj:`numpy.ndarray` com_projected_on_edge : 3x1 :obj:`numpy.ndarray` Returns ------- float """ # s = normalize(edge_point2 - edge_point1) # vertex_projected_on_edge = (vertex - edge_point1).dot(s)*s + edge_point1 # f_max = normalize(np.cross(edge_point1 - vertex, s)) # if f_max[2] < 0: # is this right?????? # f_max = -f_max # r_f = np.linalg.norm(vertex - vertex_projected_on_edge) # cos_push_vertex_edge_angle = push_direction.dot(f_max) # g_max = normalize(-np.cross(edge_point1 - self.com, s)) # if g_max[2] > 0: # is this right?????? # g_max = -g_max # r_g = np.linalg.norm(self.com - com_projected_on_edge) # cos_com_edge_angle = (-up).dot(g_max) # # return self.mass * 9.8 * cos_com_edge_angle * r_g / (cos_push_vertex_edge_angle * r_f + 1e-5) # [0.01282592 0.01404448 0.1] s = normalize(edge_point2 - edge_point1) vertex_projected_on_edge = (vertex - edge_point1).dot(s) * s + edge_point1 push_projected = push_direction.dot(s) * s p_f = push_direction - push_projected r_f = vertex_projected_on_edge - vertex tau_f = np.cross(r_f, p_f) r_g = com_projected_on_edge - self.com tau_g = np.cross(r_g, self.mass * 9.8 * -up) # since the two torque vectors are aligned, the ratio of tau_g / tau_f # should be the same regardless so we just take the the x value return -(tau_g / (tau_f + 1e-7))[0]
def figure_1(): env.state.obj.T_obj_world.translation += np.array([-.01, -.05, .001]) action = policy.action(env.state) env.render_3d_scene() bottom_points = policy.toppling_model.bottom_points vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.0005) vis3d.points(Point(bottom_points[0]), color=[0, 0, 0], scale=.001) vis3d.points(Point(bottom_points[1]), color=[0, 0, 0], scale=.001) y_dir = normalize(bottom_points[1] - bottom_points[0]) origin = policy.toppling_model.com_projected_on_edges[0] - .005 * y_dir vis_axes(origin, y_dir) #mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix) #mesh.fix_normals() #direction = normalize([-.03, -.07, 0]) #intersect, _, face_ind = mesh.ray.intersects_location([[.02, -.005, .09]], [direction], multiple_hits=False) #normal = mesh.face_normals[face_ind[0]] #start_point = intersect[0] + .03*normal #end_point = intersect[0] #shaft_points = [start_point, end_point] #h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(-normal) #h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(-normal) #head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1] #vis3d.plot3d(shaft_points, color=[1,0,0], tube_radius=.001) #vis3d.plot3d(head_points, color=[1,0,0], tube_radius=.001) #vis3d.points(Point(end_point), scale=.002, color=[0,0,0]) # Center of Mass #start_point = env.state.T_obj_world.translation - .0025*y_dir - np.array([0,0,.005]) start_point = env.state.T_obj_world.translation end_point = start_point - np.array([0, 0, .03]) vis3d.points(Point(start_point), scale=.002, color=[0, 0, 0]) shaft_points = [start_point, end_point] h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071, 0.7071]]).dot(-up) h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071, 0.7071]]).dot(-up) head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1] vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.001) vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.001) vis3d.show(starting_camera_pose=CAMERA_POSE)
def failure_modes(): # dataset = env._state_space._database.dataset('mini_dexnet') # obj = dataset['pawn'] # obj.T_obj_world = dataset.stable_poses('pawn')[8].T_obj_table # env.state.obj = obj # mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix) # mesh.fix_normals() # direction = normalize([1, -.005, 0]) # intersect, _, face_ind = mesh.ray.intersects_location([[-1, 0, .09]], [direction], multiple_hits=False) # direction = normalize([1,-0.25,0]) # start_point = intersect[0] - .03*direction # end_point = intersect[0] # shaft_points = [start_point, end_point] # h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(direction) # h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(direction) # head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1] # vis3d.plot3d(shaft_points, color=red, tube_radius=.001) # vis3d.plot3d(head_points, color=red, tube_radius=.001) # env.render_3d_scene() # vis3d.show(starting_camera_pose=CAMERA_POSE) # env.render_3d_scene() # vis3d.show(starting_camera_pose=CAMERA_POSE) # env.state.obj.T_obj_world = dataset.stable_poses('pawn')[0].T_obj_table # env.render_3d_scene() # vis3d.show(starting_camera_pose=CAMERA_POSE) # dataset = env._state_space._database.dataset('mini_dexnet') # obj = dataset['yoda'] # obj.T_obj_world = dataset.stable_poses('yoda')[2].T_obj_table # env.state.obj = obj # mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix) # mesh.fix_normals() # direction = normalize([0, .1, 0]) # intersect = mesh.center_mass + np.array([0,-.015,.037]) # start_point = intersect - .03*direction # end_point = intersect # shaft_points = [start_point, end_point] # h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(direction) # h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(direction) # head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1] # vis3d.plot3d(shaft_points, color=red, tube_radius=.001) # vis3d.plot3d(head_points, color=red, tube_radius=.001) # env.render_3d_scene() # vis3d.show(starting_camera_pose=CAMERA_POSE) # env.state.obj.T_obj_world = dataset.stable_poses('yoda')[4].T_obj_table # env.render_3d_scene() # vis3d.show(starting_camera_pose=CAMERA_POSE) dataset = env._state_space._database.dataset('mini_dexnet') obj = dataset['vase'] obj.T_obj_world = dataset.stable_poses('vase')[5].T_obj_table env.state.obj = obj mesh = obj.mesh.copy().apply_transform(env.state.T_obj_world.matrix) mesh.fix_normals() direction = normalize([.03, .1, 0]) intersect = mesh.center_mass + np.array([-.019, -.02, .02]) start_point = intersect - .03 * direction end_point = intersect shaft_points = [start_point, end_point] h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0], [0, 0, 1]]).dot(direction) h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0], [0, 0, 1]]).dot(direction) head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1] vis3d.plot3d(shaft_points, color=red, tube_radius=.001) vis3d.plot3d(head_points, color=red, tube_radius=.001) env.render_3d_scene() vis3d.show(starting_camera_pose=CAMERA_POSE) env.state.obj.T_obj_world = dataset.stable_poses('vase')[4].T_obj_table env.render_3d_scene() vis3d.show(starting_camera_pose=CAMERA_POSE)
def figure_3(): #env.state.obj.T_obj_world.translation += np.array([-.01,-.05,.01]) action = policy.action(env.state) mesh = env.state.obj.mesh.copy().apply_transform( env.state.T_obj_world.matrix) mesh = mesh.slice_plane([0, 0, .0005], -up) from dexnet.grasping import GraspableObject3D env.state.obj = GraspableObject3D(mesh) env.render_3d_scene() bottom_points = policy.toppling_model.bottom_points vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.0005) vis3d.points(Point(bottom_points[0]), color=[0, 0, 0], scale=.001) vis3d.points(Point(bottom_points[1]), color=[0, 0, 0], scale=.001) y_dir = normalize(bottom_points[1] - bottom_points[0]) origin = policy.toppling_model.com_projected_on_edges[0] - .0025 * y_dir vis3d.points(Point(origin), color=[0, 0, 1], scale=.001) # t = .002 # x = np.cross(y_dir, up) # while t < np.linalg.norm(origin - bottom_points[0]): # start_point = origin - t*y_dir # end_point = start_point + .0075*x # shaft_points = [start_point, end_point] # h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(x) # h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(x) # head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1] # vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002) # vis3d.plot3d(head_points, color=purple, tube_radius=.0002) # t += .002 ## try 2 x = np.cross(y_dir, up) t = .004 arrow_dir = x start_point = origin - t * y_dir end_point = start_point + .0075 * arrow_dir shaft_points = [start_point, end_point] h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0], [0, 0, 1]]).dot(x) h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0], [0, 0, 1]]).dot(x) head_points = [end_point - 0.001 * h2, end_point, end_point - 0.001 * h1] vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002) vis3d.plot3d(head_points, color=purple, tube_radius=.0002) # t = .000 # while t < np.linalg.norm(origin - bottom_points[1]): # arrow_dir = x # start_point = origin + t*y_dir # end_point = start_point + .0075*arrow_dir # shaft_points = [start_point, end_point] # h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir) # h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir) # head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1] # vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002) # vis3d.plot3d(head_points, color=purple, tube_radius=.0002) # t += .002 ## try 2 t = .01 arrow_dir = -x start_point = origin + t * y_dir end_point = start_point + .0075 * arrow_dir shaft_points = [start_point, end_point] h1 = np.array([[0.7071, -0.7071, 0], [0.7071, 0.7071, 0], [0, 0, 1]]).dot(arrow_dir) h2 = np.array([[0.7071, 0.7071, 0], [-0.7071, 0.7071, 0], [0, 0, 1]]).dot(arrow_dir) head_points = [end_point - 0.001 * h2, end_point, end_point - 0.001 * h1] vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002) vis3d.plot3d(head_points, color=purple, tube_radius=.0002) #arrow_dir = np.cross(y_dir, up) #start_point = origin - .005*y_dir #end_point = start_point + .0075*arrow_dir #shaft_points = [start_point, end_point] #h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir) #h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(arrow_dir) #head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1] #vis3d.plot3d(shaft_points, color=purple, tube_radius=.0002) #vis3d.plot3d(head_points, color=purple, tube_radius=.0002) #arrow_dir = -up #end_point = start_point + .0075*arrow_dir #shaft_points = [start_point, end_point] #h1 = np.array([[1,0,0],[0,0.7071,-0.7071],[0,0.7071,0.7071]]).dot(arrow_dir) #h2 = np.array([[1,0,0],[0,0.7071,0.7071],[0,-0.7071,0.7071]]).dot(arrow_dir) #head_points = [end_point - 0.001*h2, end_point, end_point - 0.001*h1] #vis3d.plot3d(shaft_points, color=blue, tube_radius=.0002) #vis3d.plot3d(head_points, color=blue, tube_radius=.0002) # #vis3d.points(Point(start_point), color=[0,1,0], scale=.001) #vis_axes(origin, y_dir) vis3d.show(starting_camera_pose=CAMERA_POSE) sys.exit()
def figure_2(): env.state.material_props._color = np.array([0.75] * 3) # env.state.obj.T_obj_world.translation += np.array([-.095,.025,.001]) env.state.obj.T_obj_world.translation += np.array([-.01, -.04, .001]) action = policy.action(env.state) env.render_3d_scene() bottom_points = policy.toppling_model.bottom_points edge = 1 vis3d.plot3d(bottom_points[edge:edge + 2], color=[0, 0, 0], tube_radius=.0005) vis3d.points(Point(bottom_points[edge]), color=[0, 0, 0], scale=.001) vis3d.points(Point(bottom_points[edge + 1]), color=[0, 0, 0], scale=.001) y_dir = normalize(bottom_points[edge + 1] - bottom_points[edge]) origin = policy.toppling_model.com_projected_on_edges[ edge] # - .0025*y_dir vis_axes(origin, y_dir) mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix) mesh.fix_normals() #direction = normalize([-.03, -.07, 0]) #intersect, _, face_ind = mesh.ray.intersects_location([[.02, -.005, .09]], [direction], multiple_hits=False) ray_origin = mesh.center_mass + np.array([.1, -.1, .035]) direction = normalize([-.1, .1, 0]) intersect, _, face_ind = mesh.ray.intersects_location([ray_origin], [direction], multiple_hits=False) normal = mesh.face_normals[face_ind[0]] start_point = intersect[0] + .03 * normal end_point = intersect[0] shaft_points = [start_point, end_point] h1 = np.array([[0.7071, 0, -0.7071], [0, 1, 0], [0.7071, 0, 0.7071]]).dot(-normal) h2 = np.array([[0.7071, 0, 0.7071], [0, 1, 0], [-0.7071, 0, 0.7071]]).dot(-normal) head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1] vis3d.plot3d(shaft_points, color=red, tube_radius=.001) vis3d.plot3d(head_points, color=red, tube_radius=.001) vis3d.points(Point(end_point), scale=.002, color=[0, 0, 0]) # Center of Mass #start_point = env.state.T_obj_world.translation# - .0025*y_dir - np.array([0,0,.005]) start_point = mesh.center_mass end_point = start_point - np.array([0, 0, .03]) vis3d.points(Point(start_point), scale=.002, color=[0, 0, 0]) shaft_points = [start_point, end_point] h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071, 0.7071]]).dot(-up) h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071, 0.7071]]).dot(-up) head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1] vis3d.plot3d(shaft_points, color=red, tube_radius=.001) vis3d.plot3d(head_points, color=red, tube_radius=.001) # Dotted lines r_gs = dotted_line(start_point, origin) for r_g in r_gs: vis3d.plot3d(r_g, color=orange, tube_radius=.0006) s = normalize(bottom_points[edge + 1] - bottom_points[edge]) vertex_projected_on_edge = ( intersect[0] - bottom_points[edge]).dot(s) * s + bottom_points[edge] r_fs = dotted_line(intersect[0], vertex_projected_on_edge) for r_f in r_fs: vis3d.plot3d(r_f, color=orange, tube_radius=.0006) vis3d.show(starting_camera_pose=CAMERA_POSE)
def load_object(self, obj): """ Does a lot of the preprocessing, like calculating the bottom points, and max_moment / max_tangential_forces for each edge. This way you can call predict multiple times and not redo too much work Parameters ---------- obj : :obj:`GraspableObject3D` object to load """ self.obj = obj self.mesh = obj.mesh.copy().apply_transform(obj.T_obj_world.matrix) self.mesh.fix_normals() self.com = self.mesh.center_mass # self.com = obj.T_obj_world.translation # self.com = np.mean(self.mesh.vertices, axis=0) self.mass = 1 # Finding toppling edge z_components = np.around(self.mesh.vertices[:, 2], 3) self.lowest_z = np.min(z_components) cutoff = .02 bottom_points = self.mesh.vertices[z_components == self.lowest_z][:, :2] bottom_points = bottom_points[ConvexHull(bottom_points).vertices] self.bottom_points = np.append( bottom_points, #ensure all points lie on plane self.lowest_z * np.ones(len(bottom_points)).reshape((-1, 1)), axis=1) #print 'bottom', self.bottom_points #self.bottom_points = self.bottom_points[::5] #print 'bottom', self.bottom_points # Turn bottom points into pairs of adjacent bottom_points self.edge_points = zip(self.bottom_points, np.roll(self.bottom_points, -1, axis=0)) edge = 1 #self.edge_points = [self.edge_points[edge]] #self.bottom_points = self.bottom_points[[edge,edge+1]] # For each edge, calculate the maximum moment # and maximum tangential force it can resist self.max_moments, self.max_tangential_forces, self.com_projected_on_edges = [], [], [] for edge_point1, edge_point2 in self.edge_points: s = normalize(edge_point2 - edge_point1) com_projected_on_edge = (self.com - edge_point1).dot(s) * s + edge_point1 offset_dist = np.linalg.norm(edge_point1 - edge_point2) offsets = np.linspace(0, offset_dist, self.num_approx) offsets_relative_to_com = offsets - np.linalg.norm( com_projected_on_edge - edge_point1) # This is for a non-uniform pressure distribution #paths = self.mesh.section_multiplane(edge_point1, s, offsets) #mass_per_unit = np.array([0 if paths[index] == None else paths[index].area for index in range(self.num_approx)]) # Using a uniform pressure distribution along the contact edge (for now) mass_per_unit = np.array([self.mass / float(self.num_approx)] * self.num_approx) # pressure distribution from mass pressure_dist = mass_per_unit * self.mass * 9.8 / ( np.sum(mass_per_unit) + offset_dist / self.num_approx) self.max_moments.append( self.ground_friction_coeff * pressure_dist.dot(np.abs(offsets_relative_to_com))) # self.max_moments.append(self.ground_friction_coeff * mass * 9.8 * (y0**2 + y1**2) / 2) # Why doesn't this work ????? self.max_tangential_forces.append(self.ground_friction_coeff * (self.mass * 9.8)) # mu F_n self.com_projected_on_edges.append(com_projected_on_edge)
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