def setup(task, table_bounds): if Globals.pr2 is None: Globals.pr2 = PR2.PR2.create(rave_only=True) load_table(table_bounds) if Globals.rviz is None: Globals.rviz = ros_utils.RvizWrapper.create() # Globals.rviz.start_publisher_thread(Globals.handles) #Globals.table_height = rospy.get_param("table_height") if Globals.demos is None: data_dir = osp.join(osp.dirname(lfd.__file__), "data") with open(osp.join(data_dir, "knot_demos.yaml"),"r") as fh: task_info = yaml.load(fh) H5FILE = osp.join(data_dir, task_info[task]["db_file"]) demos_file = h5py.File(H5FILE,"r") rospy.loginfo("loading demos into memory") Globals.demos = warping.group_to_dict(demos_file) demos_file.close() rospy.loginfo("preprocessing demo point clouds...") for (_,demo) in Globals.demos.items(): demo["cloud_xyz_ds"], ds_inds = downsample(demo["cloud_xyz"]) #demo["cloud_xyz_ds"], ds_inds = demo["cloud_xyz"].reshape(-1, 3), np.arange(len(demo['cloud_xyz'])).reshape(-1, 1) demo["cloud_xyz"] = np.squeeze(demo["cloud_xyz"]) demo["geodesic_dists"] = recognition.calc_geodesic_distances_downsampled_old(demo["cloud_xyz"], demo["cloud_xyz_ds"], ds_inds) rospy.loginfo("done")
def load_demo(taskname='overhand_knot', demos_list_file='knot_demos.yaml', seg_name=None): data_dir = os.path.join(os.path.dirname(lfd.__file__), "data") with open(os.path.join(data_dir, demos_list_file),"r") as fh: task_info = yaml.load(fh) H5FILE = os.path.join(data_dir, task_info[taskname]["db_file"]) demos_file = h5py.File(H5FILE,"r") demos = warping.group_to_dict(demos_file) demos_file.close() if seg_name is None: seg_name = select_from_list(sorted(demos.keys())) demo = demos[seg_name] demo["cloud_xyz_ds"], ds_inds = recognition.downsample(demo["cloud_xyz"]) demo["cloud_xyz"] = np.squeeze(demo["cloud_xyz"]) return demo
def LoadFromTaskDemos(taskname, demos_list_file='knot_demos.yaml'): # load demos import h5py, yaml, os import os.path as osp data_dir = osp.join(osp.dirname(lfd.__file__), "data") with open(osp.join(data_dir, demos_list_file),"r") as fh: task_info = yaml.load(fh) H5FILE = osp.join(data_dir, task_info[taskname]["db_file"]) demos_file = h5py.File(H5FILE,"r") demos = warping.group_to_dict(demos_file) demos_file.close() ds = DataSet() for name, demo in demos.items(): ds[name] = demo return ds
def load_demo(taskname='overhand_knot', demos_list_file='knot_demos.yaml', seg_name=None): data_dir = os.path.join(os.path.dirname(lfd.__file__), "data") with open(os.path.join(data_dir, demos_list_file), "r") as fh: task_info = yaml.load(fh) H5FILE = os.path.join(data_dir, task_info[taskname]["db_file"]) demos_file = h5py.File(H5FILE, "r") demos = warping.group_to_dict(demos_file) demos_file.close() if seg_name is None: seg_name = select_from_list(sorted(demos.keys())) demo = demos[seg_name] demo["cloud_xyz_ds"], ds_inds = recognition.downsample(demo["cloud_xyz"]) demo["cloud_xyz"] = np.squeeze(demo["cloud_xyz"]) return demo
data_dir = osp.join(osp.dirname(lfd.__file__), "data") with open(osp.join(data_dir, "knot_demos.yaml"),"r") as fh: task_info = yaml.load(fh) DS_LENGTH = .025 DS_METHOD = "voxel" if args.task.startswith("fold"): DS_METHOD="hull" #else: #DS_METHOD = "voxel" H5FILE = osp.join(data_dir, task_info[args.task]["db_file"]) demos_file = h5py.File(H5FILE,"r") rospy.loginfo("loading demos into memory") demos = warping.group_to_dict(demos_file) if args.test: lfd_traj.ALWAYS_FAKE_SUCCESS = True def draw_table(): aabb = Globals.pr2.robot.GetEnv().GetKinBody("table").GetLinks()[0].ComputeAABB() ps =gm.PoseStamped() ps.header.frame_id = "base_footprint" ps.pose.position = gm.Point(*aabb.pos()) ps.pose.orientation = gm.Quaternion(0,0,0,1) Globals.handles.append(Globals.rviz.draw_marker(ps, type=Marker.CUBE, scale = aabb.extents()*2, id = 24019,rgba = (1,0,0,.25))) def load_table(): table_bounds = map(float, rospy.get_param("table_bounds").split()) kinbodies.create_box_from_bounds(Globals.pr2.env,table_bounds, name="table")
data_dir = osp.join(osp.dirname(lfd.__file__), "data") with open(osp.join(data_dir, "knot_demos.yaml"), "r") as fh: task_info = yaml.load(fh) DS_LENGTH = .025 DS_METHOD = "voxel" if args.task.startswith("fold"): DS_METHOD = "hull" #else: #DS_METHOD = "voxel" H5FILE = osp.join(data_dir, task_info[args.task]["db_file"]) demos_file = h5py.File(H5FILE, "r") rospy.loginfo("loading demos into memory") demos = warping.group_to_dict(demos_file) if args.test: lfd_traj.ALWAYS_FAKE_SUCCESS = True def draw_table(): aabb = Globals.pr2.robot.GetEnv().GetKinBody( "table").GetLinks()[0].ComputeAABB() ps = gm.PoseStamped() ps.header.frame_id = "base_footprint" ps.pose.position = gm.Point(*aabb.pos()) ps.pose.orientation = gm.Quaternion(0, 0, 0, 1) Globals.handles.append( Globals.rviz.draw_marker(ps, type=Marker.CUBE,