def visualize_scene(mesh, T_world_ar, T_ar_cam, T_cam_obj): T_cam_cam = RigidTransform() T_obj_cam = T_cam_obj.inverse() T_cam_ar = T_ar_cam.inverse() Twc = np.matmul(T_world_ar.matrix, T_ar_cam.matrix) T_world_cam = RigidTransform(Twc[:3, :3], Twc[:3, 3]) T_cam_world = T_world_cam.inverse() o = np.array([0., 0., 0.]) centroid_cam = apply_transform(o, T_cam_obj) tag_cam = apply_transform(o, T_cam_ar) robot_cam = apply_transform(o, T_cam_world) # camera vis3d.points(o, color=(0, 1, 0), scale=0.01) vis3d.pose(T_cam_cam, alpha=0.05, tube_radius=0.005, center_scale=0.002) # object vis3d.mesh(mesh) vis3d.points(centroid_cam, color=(0, 0, 0), scale=0.01) vis3d.pose(T_cam_obj, alpha=0.05, tube_radius=0.005, center_scale=0.002) # AR tag vis3d.points(tag_cam, color=(1, 0, 1), scale=0.01) vis3d.pose(T_cam_ar, alpha=0.05, tube_radius=0.005, center_scale=0.002) vis3d.table(T_cam_ar, dim=0.074) # robot vis3d.points(robot_cam, color=(1, 0, 0), scale=0.01) vis3d.pose(T_cam_world, alpha=0.05, tube_radius=0.005, center_scale=0.002) vis3d.show()
if config['use_robot']: # find the rightmost and further cb point in world frame cb_points_world = T_camera_world * reg_result.cb_points_cam cb_point_data_world = cb_points_world.data dir_world = np.array([1.0, -1.0, 0]) dir_world = dir_world / np.linalg.norm(dir_world) ip = dir_world.dot(cb_point_data_world) if config['vis_cb_corners']: _, depth_im, _ = sensor.frames() points_world = T_camera_world * ir_intrinsics.deproject(depth_im) vis3d.figure() vis3d.points(cb_points_world, color=(0,0,1), scale=0.005) vis3d.points(points_world, color=(0,1,0), subsample=10, random=True, scale=0.001) vis3d.pose(T_camera_world) vis3d.table(dim=0.5, T_table_world=T_cb_world) vis3d.show() # open interface to robot y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF) y.reset_home() time.sleep(1) # choose target point #1 target_ind = np.where(ip == np.max(ip))[0] target_pt_world = cb_points_world[target_ind[0]] # create robot pose relative to target point R_gripper_world = np.array([[1.0, 0, 0], [0, -1.0, 0], [0, 0, -1.0]])
def quality(self, state, actions, params=None): """Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points. Parameters ---------- state : :obj:`RgbdImageState` An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed. action: :obj:`SuctionPoint2D` A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr. params: dict Stores params used in computing suction quality. Returns ------- :obj:`numpy.ndarray` Array of the quality for each grasp """ qualities = [] # deproject points point_cloud_image = state.camera_intr.deproject_to_image( state.rgbd_im.depth) # compute negative SSE from the best fit plane for each grasp for i, action in enumerate(actions): if not isinstance(action, SuctionPoint2D): raise ValueError( 'This function can only be used to evaluate suction quality' ) points = self._points_in_window( point_cloud_image, action, segmask=state.segmask) # x,y in matrix A and z is vector z. A, b = self._points_to_matrices(points) w = self._action_to_plane( point_cloud_image, action) # vector w w/ a bias term represents a best-fit plane. sse = self._sum_of_squared_residuals(w, A, b) if params is not None and params['vis']['plane']: from visualization import Visualizer2D as vis2d from visualization import Visualizer3D as vis3d mid_i = A.shape[0] / 2 pred_z = A.dot(w) p0 = np.array([A[mid_i, 0], A[mid_i, 1], pred_z[mid_i]]) n = np.array([w[0], w[1], -1]) n = n / np.linalg.norm(n) tx = np.array([n[1], -n[0], 0]) tx = tx / np.linalg.norm(tx) ty = np.cross(n, tx) R = np.array([tx, ty, n]).T c = state.camera_intr.deproject_pixel(action.depth, action.center) d = Point(c.data - 0.01 * action.axis, frame=c.frame) T_table_world = RigidTransform(rotation=R, translation=p0, from_frame='patch', to_frame='world') vis3d.figure() vis3d.points(point_cloud_image.to_point_cloud(), scale=0.0025, subsample=10, random=True, color=(0, 0, 1)) vis3d.points(PointCloud(points.T), scale=0.0025, color=(1, 0, 0)) vis3d.points(c, scale=0.005, color=(1, 1, 0)) vis3d.points(d, scale=0.005, color=(1, 1, 0)) vis3d.table(T_table_world, dim=0.01) vis3d.show() vis2d.figure() vis2d.imshow(state.rgbd_im.depth) vis2d.scatter(action.center.x, action.center.y, s=50, c='b') vis2d.show() quality = np.exp( -sse ) # evaluate how well best-fit plane describles all points in window. qualities.append(quality) return np.array(qualities)