def load_table(): while not rospy.has_param("table_bounds"): rospy.logwarn("waiting until table_bounds set") rospy.sleep(1) table_bounds = map(float, rospy.get_param("table_bounds").split()) kinbodies.create_box_from_bounds(Globals.pr2.env, table_bounds, name="table")
def create_obstacle_env(env): clone=env.CloneSelf(1) point_cloud = rospy.wait_for_message("/drop/points", sm.PointCloud2) xyz, bgr = ros_utils.pc2xyzrgb(point_cloud) xyz = ros_utils.transform_points(xyz, Globals.pr2.tf_listener, "base_footprint", point_cloud.header.frame_id) #xyz = xyz[250:, 250:] clusters = tabletop.segment_tabletop_scene(xyz,'base_footprint') bounds = tabletop.get_table_dimensions(xyz) kinbodies.create_box_from_bounds(clone,bounds,"table") print len(clusters),"clusters" for (i_clu,clu) in enumerate(clusters): x,y,z,r,h = tabletop.get_cylinder(clu) #print kinbodies.create_cylinder(clone, (x,y,z),r,h, name="cylinder%i"%i_clu) return clone
def load_table(): while not rospy.has_param("table_bounds"): rospy.logwarn("waiting until table_bounds set") rospy.sleep(1) table_bounds = map(float, rospy.get_param("table_bounds").split()) kinbodies.create_box_from_bounds(Globals.pr2.env,table_bounds, name="table")
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")
push_svc = rospy.ServiceProxy("push", Push) req = PushRequest() xyz0 = clusters[np.argmax(map(len, clusters))].reshape(1, -1, 3) rgb0 = np.zeros(xyz0.shape, 'uint8') point_cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint") req.point_cloud = point_cloud push_svc.call(req) if False: pour_srv = rospy.ServiceProxy("pour", Pour) req = PourRequest() sort_inds = np.argsort(map(len, clusters)) for i in sort_inds[:2]: xyz0 = clusters[i].reshape(1, -1, 3) rgb0 = np.zeros(xyz0.shape, 'uint8') cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint") req.object_clouds.append(cloud) pour_srv.call(req) if True: env = pr2.robot.GetEnv() if env.GetViewer() is None: env.SetViewer('qtcoin') bounds = tabletop.get_table_dimensions(xyz) from kinematics import kinbodies kinbodies.create_box_from_bounds(env, bounds, "table") for (i_clu, clu) in enumerate(clusters): x, y, z, r, h = tabletop.get_cylinder(clu) print kinbodies.create_cylinder(env, (x, y, z), r, h, name="cylinder%i" % i_clu)
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")
parser.add_argument('--traj_file', default='') parser.add_argument('--warped_demo', default='') parser.add_argument('--slice', default='') parser.add_argument('--sim', action='store_true') parser.add_argument('--no_table', action='store_true') args = parser.parse_args() np.set_printoptions(linewidth=1000, threshold='nan') if rospy.get_name() == "/unnamed": rospy.init_node("test_lfd_traj", disable_signals=True) pr2 = PR2.create() if not args.no_table: table_bounds = map(float, rospy.get_param("table_bounds").split()) kinbodies.create_box_from_bounds(pr2.env, table_bounds, name="table") pr2.update_rave() slice_start, slice_end = 0, -1 if args.slice != '': slice_start, slice_end = map(int, args.slice.split(':')) traj, warped_demo = None, None if args.traj_file: with open(args.traj_file, 'r') as f: traj = pickle.load(f) print traj else: with open(args.warped_demo, 'r') as f: warped_demo = pickle.load(f)
clusters = tabletop.segment_tabletop_scene(xyz,"base_footprint",plotting2d=True, plotting3d=True) if False: push_svc = rospy.ServiceProxy("push",Push) req = PushRequest() xyz0 = clusters[np.argmax(map(len,clusters))].reshape(1,-1,3) rgb0 = np.zeros(xyz0.shape,'uint8') point_cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint") req.point_cloud = point_cloud push_svc.call(req) if False: pour_srv = rospy.ServiceProxy("pour", Pour) req = PourRequest() sort_inds = np.argsort(map(len,clusters)) for i in sort_inds[:2]: xyz0 = clusters[i].reshape(1,-1,3) rgb0 = np.zeros(xyz0.shape,'uint8') cloud = xyzrgb2pc(xyz0, rgb0, "base_footprint") req.object_clouds.append(cloud) pour_srv.call(req) if True: env = pr2.robot.GetEnv() if env.GetViewer() is None: env.SetViewer('qtcoin') bounds = tabletop.get_table_dimensions(xyz) from kinematics import kinbodies kinbodies.create_box_from_bounds(env,bounds,"table") for (i_clu,clu) in enumerate(clusters): x,y,z,r,h = tabletop.get_cylinder(clu) print kinbodies.create_cylinder(env, (x,y,z),r,h, name="cylinder%i"%i_clu)
parser.add_argument('--slice', default='') parser.add_argument('--sim', action='store_true') parser.add_argument('--no_table', action='store_true') parser.add_argument('--with_base', action='store_true') args = parser.parse_args() np.set_printoptions(linewidth=1000, threshold='nan') if rospy.get_name() == "/unnamed": rospy.init_node("test_lfd_traj", disable_signals=True) pr2 = PR2.create() if not args.no_table: table_bounds = map(float, rospy.get_param("table_bounds").split()) kinbodies.create_box_from_bounds(pr2.env,table_bounds, name="table") pr2.update_rave() slice_start, slice_end = 0, -1 if args.slice != '': slice_start, slice_end = map(int, args.slice.split(':')) traj, warped_demo = None, None if args.traj_file: with open(args.traj_file, 'r') as f: traj = pickle.load(f) print traj else: with open(args.warped_demo, 'r') as f: warped_demo = pickle.load(f)