def get_approach_rt(baxter_hand_pose): trans = baxter_hand_pose.translation x_axis = baxter_hand_pose.x_axis y_axis = baxter_hand_pose.y_axis z_axis = baxter_hand_pose.z_axis down = np.array([0, 0, -1]) y = normalize(y_axis) x = normalize(np.cross(y, down)) z = normalize(np.cross(x, y)) approach_rot_3d = RigidTransform.rotation_from_axes(x, y, z) return RigidTransform(rotation=approach_rot_3d, translation=trans)
def vertices_to_baxter_hand_pose(self, grasp_vertices, grasp_normals): """ 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 grasp_normals : 2x3' :obj:`numpy.ndarray` normals at the grasp vertices Returns ------- :obj:`autolab_core:RigidTransform` Hand pose in the object frame """ # YOUR CODE HERE # y-axis (bar across the grippers): direction of the grasp normals y_axis = (grasp_normals[0] - grasp_normals[1]) / 2 y_axis /= np.linalg.norm(y_axis) # z-axis (direction the end-effector is pointing, pointing out of the grippers): take the first two terms of the y-axis # (in the world frame, or in object frame if the object doesn't have rotation) # (think of it in 2D), # rotate it by pi/2, and then set the third term to 0 because we want # to have a "flat" z-axis in the world frame y_axis_2d = y_axis[:2] z_axis_2d = rotation2d(np.pi / 2).dot(y_axis_2d) z_axis = np.hstack([z_axis_2d, [0]]) z_axis /= np.linalg.norm(z_axis) # x-axis: take the cross product of the y- and z-axes x_axis = np.cross(y_axis, z_axis) rotation_3d = RigidTransform.rotation_from_axes(x_axis, y_axis, z_axis) translation = np.average( grasp_vertices, axis=0) # middle position between the grasp vertices return RigidTransform(rotation=rotation_3d, translation=translation, to_frame=self.object_name, from_frame='right_gripper')
def register(sensor, config): """ Registers a camera to a chessboard. Parameters ---------- sensor : :obj:`perception.RgbdSensor` the sensor to register config : :obj:`autolab_core.YamlConfig` or :obj:`dict` configuration file for registration Returns ------- :obj:`ChessboardRegistrationResult` the result of registration Notes ----- The config must have the parameters specified in the Other Parameters section. Other Parameters ---------------- num_transform_avg : int the number of independent registrations to average together num_images : int the number of images to read for each independent registration corners_x : int the number of chessboard corners in the x-direction corners_y : int the number of chessboard corners in the y-direction color_image_rescale_factor : float amount to rescale the color image for detection (numbers around 4-8 are useful) vis : bool whether or not to visualize the registration """ # read config num_transform_avg = config['num_transform_avg'] num_images = config['num_images'] sx = config['corners_x'] sy = config['corners_y'] point_order = config['point_order'] color_image_rescale_factor = config['color_image_rescale_factor'] flip_normal = config['flip_normal'] y_points_left = False if 'y_points_left' in config.keys() and sx == sy: y_points_left = config['y_points_left'] num_images = 1 vis = config['vis'] # read params from sensor logging.info('Registering camera %s' %(sensor.frame)) ir_intrinsics = sensor.ir_intrinsics # repeat registration multiple times and average results R = np.zeros([3,3]) t = np.zeros([3,1]) points_3d_plane = PointCloud(np.zeros([3, sx*sy]), frame=sensor.ir_frame) k = 0 while k < num_transform_avg: # average a bunch of depth images together depth_ims = None for i in range(num_images): start = time.time() small_color_im, new_depth_im, _ = sensor.frames() end = time.time() logging.info('Frames Runtime: %.3f' %(end-start)) if depth_ims is None: depth_ims = np.zeros([new_depth_im.height, new_depth_im.width, num_images]) depth_ims[:,:,i] = new_depth_im.data med_depth_im = np.median(depth_ims, axis=2) depth_im = DepthImage(med_depth_im, sensor.ir_frame) # find the corner pixels in an upsampled version of the color image big_color_im = small_color_im.resize(color_image_rescale_factor) corner_px = big_color_im.find_chessboard(sx=sx, sy=sy) if vis: plt.figure() plt.imshow(big_color_im.data) for i in range(sx): plt.scatter(corner_px[i,0], corner_px[i,1], s=25, c='b') plt.show() if corner_px is None: logging.error('No chessboard detected! Check camera exposure settings') continue # convert back to original image small_corner_px = corner_px / color_image_rescale_factor if vis: plt.figure() plt.imshow(small_color_im.data) for i in range(sx): plt.scatter(small_corner_px[i,0], small_corner_px[i,1], s=25, c='b') plt.axis('off') plt.show() # project points into 3D camera_intr = sensor.ir_intrinsics points_3d = camera_intr.deproject(depth_im) # get round chessboard ind corner_px_round = np.round(small_corner_px).astype(np.uint16) corner_ind = depth_im.ij_to_linear(corner_px_round[:,0], corner_px_round[:,1]) if corner_ind.shape[0] != sx*sy: logging.warning('Did not find all corners. Discarding...') continue # average 3d points points_3d_plane = (k * points_3d_plane + points_3d[corner_ind]) / (k + 1) logging.info('Registration iteration %d of %d' %(k+1, config['num_transform_avg'])) k += 1 # fit a plane to the chessboard corners X = np.c_[points_3d_plane.x_coords, points_3d_plane.y_coords, np.ones(points_3d_plane.num_points)] y = points_3d_plane.z_coords A = X.T.dot(X) b = X.T.dot(y) w = np.linalg.inv(A).dot(b) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) if flip_normal: n = -n mean_point_plane = points_3d_plane.mean() # find x-axis of the chessboard coordinates on the fitted plane T_camera_table = RigidTransform(translation = -points_3d_plane.mean().data, from_frame=points_3d_plane.frame, to_frame='table') points_3d_centered = T_camera_table * points_3d_plane # get points along y if point_order == 'row_major': coord_pos_x = int(math.floor(sx*sy/2.0)) coord_neg_x = int(math.ceil(sx*sy/2.0)) points_pos_x = points_3d_centered[coord_pos_x:] points_neg_x = points_3d_centered[:coord_neg_x] x_axis = np.mean(points_pos_x.data, axis=1) - np.mean(points_neg_x.data, axis=1) x_axis = x_axis - np.vdot(x_axis, n)*n x_axis = x_axis / np.linalg.norm(x_axis) y_axis = np.cross(n, x_axis) else: coord_pos_y = int(math.floor(sx*(sy-1)/2.0)) coord_neg_y = int(math.ceil(sx*(sy+1)/2.0)) points_pos_y = points_3d_centered[:coord_pos_y] points_neg_y = points_3d_centered[coord_neg_y:] y_axis = np.mean(points_pos_y.data, axis=1) - np.mean(points_neg_y.data, axis=1) y_axis = y_axis - np.vdot(y_axis, n)*n y_axis = y_axis / np.linalg.norm(y_axis) x_axis = np.cross(-n, y_axis) # produce translation and rotation from plane center and chessboard basis rotation_cb_camera = RigidTransform.rotation_from_axes(x_axis, y_axis, n) translation_cb_camera = mean_point_plane.data T_cb_camera = RigidTransform(rotation=rotation_cb_camera, translation=translation_cb_camera, from_frame='cb', to_frame=sensor.frame) if y_points_left and np.abs(T_cb_camera.y_axis[1]) > 0.1: if T_cb_camera.x_axis[0] > 0: T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(-np.pi/2).T) else: T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(np.pi/2).T) T_camera_cb = T_cb_camera.inverse() # optionally display cb corners with detected pose in 3d space if config['debug']: # display image with axes overlayed cb_center_im = camera_intr.project(Point(T_cb_camera.translation, frame=sensor.ir_frame)) cb_x_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.x_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_y_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.y_axis * config['scale_amt'], frame=sensor.ir_frame)) cb_z_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.z_axis * config['scale_amt'], frame=sensor.ir_frame)) x_line = np.array([cb_center_im.data, cb_x_im.data]) y_line = np.array([cb_center_im.data, cb_y_im.data]) z_line = np.array([cb_center_im.data, cb_z_im.data]) plt.figure() plt.imshow(small_color_im.data) plt.scatter(cb_center_im.data[0], cb_center_im.data[1]) plt.plot(x_line[:,0], x_line[:,1], c='r', linewidth=3) plt.plot(y_line[:,0], y_line[:,1], c='g', linewidth=3) plt.plot(z_line[:,0], z_line[:,1], c='b', linewidth=3) plt.axis('off') plt.title('Chessboard frame in camera %s' %(sensor.frame)) plt.show() return ChessboardRegistrationResult(T_camera_cb, points_3d_plane)