Functions
def	create_approach_grid
def	create_grid
def	create_overhead_grasp_choice_grid
def	create_scooping_grid
def	create_segmentation_grid
def	create_vertical_plane_grid
def	find_approach_direction
	figure out a good direction to approach the surface.
def	find_closest_object
def	find_closest_pt
def	find_closest_pt_index
def	find_door_handle
	returns door handle points in the thok coord frame.
def	find_goto_point_surface_1
def	generate_pointcloud
def	grasp_location_on_object
def	max_fwd_without_collision
def	overhead_grasp_collision
def	points_within_cuboid
	returns points that are within the cuboid formed by tlb and brf.
def	pushback_edge
def	segment_objects_points
def	test_approach
def	test_choose_grasp_strategy
def	test_different_surfaces
def	test_find_door_handle
def	test_grasp_location_on_object
def	test_max_forward
def	test_occ_grid
def	test_plane_finding
def	test_segmentation
def	test_vertical_plane_finding
def	vertical_plane_points
Variables
list	color_list
tuple	dict = ut.load_pickle(pkl_file_name)
tuple	gr = create_approach_grid(pt,pos_list,scan_list,l1,l2,display_list)
	pts = all_pts[:,np.where(np.multiply(all_pts[0,:]>0.1,all_pts[0,:]<0.4))[1].A1] pts = pts[:,np.where(np.multiply(pts[1,:]<0.3,pts[1,:]>-0.3))[1].A1] pts = pts[:,np.where(pts[2,:]>-0.2)[1].A1] display_list.append(pu.PointCloud(pts,color=(0,200,0))) display_list.append(pu.PointCloud(all_pts,color=(200,0,0)))
list	height_list = []
string	help = 'file.pkl File with the scan,pos dict.'
tuple	l = gr.find_plane_indices(assume_plane=True)
list	l1 = dict['l1']
list	l2 = dict['l2']
tuple	max_angle = math.radians(40)
tuple	max_index = min(max(l)+5,gr.grid_shape[2,0]-1)
tuple	max_occ = max(n_points_list)
tuple	min_angle = math.radians(-40)
tuple	min_index = max(min(l)-5,0)
list	n_points_list = []
tuple	p = optparse.OptionParser()
	pkl_file_name = opt.pkl_file_name
list	pos_list = dict['pos_list']
list	pt = dict['pt']
list	raw_name = str_parts[-2]
list	scan_list = dict['scan_list']
	show_full_cloud = opt.show_all_pts
tuple	str_parts = pkl_file_name.split('.')
tuple	t = pl.axis()
int	thresh = 5

Function Documentation

def hrl_tilting_hokuyo.processing_3d.create_approach_grid	(	pt,
		pos_list,
		scan_list,
		l1,
		l2,
		display_list = `None`,
		show_pts = `True`
	)

Definition at line 655 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.create_grid	(	brf,
		tlb,
		resolution,
		pos_list,
		scan_list,
		l1,
		l2,
		display_flag = `False`,
		show_pts = `True`,
		rotation_angle = `0.`,
		occupancy_threshold = `1`
	)

rotation angle - about the Z axis.

Definition at line 598 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.create_overhead_grasp_choice_grid	(	pt,
		pos_list,
		scan_list,
		l1,
		l2,
		far_dist,
		display_list = `None`
	)

Definition at line 665 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.create_scooping_grid	(	pt,
		pos_list,
		scan_list,
		l1,
		l2,
		display_flag = `False`
	)

Definition at line 633 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.create_segmentation_grid	(	pt,
		pos_list,
		scan_list,
		l1,
		l2,
		display_flag = `False`
	)

Definition at line 640 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.create_vertical_plane_grid	(	pt,
		pos_list,
		scan_list,
		l1,
		l2,
		rotation_angle,
		display_list = `None`
	)

Definition at line 625 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.find_approach_direction	(	grid,
		pt,
		display_list = `None`
	)

figure out a good direction to approach the surface.

Parameters:

grid	- occupancy grid (binary) around the point of interest. assumes that it has a surface.
pt	- 3d point which has to be approached.
display_list	- if display_list are lists then point clouds etc. are added for visualisation.

Returns:: - closest_pt,approach_vector.

Definition at line 295 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.find_closest_object	(	obj_pts_list,
		pt,
		return_idx = `False`
	)

obj_pts_list - list of 3xNi matrices of points.
    pt - point of interest. (3x1) matrix.
    return_idx - whether to return the index (in obj_pts_list) of
                 the closest object.
    returns 3xNj matrix of points which is the closest object to pt.
            None if obj_pts_list is empty.

Definition at line 532 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.find_closest_pt	(	pts2d,
		pt,
		pt_closer = `False`
	)

returns closest point to edge (2x1 matrix)
    can return None also

Definition at line 225 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.find_closest_pt_index	(	pts2d,
		pt
	)

returns index (of pts2d) of closest point to pt.
    pts2d - 2xN matrix, pt - 2x1 matrix

Definition at line 217 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.find_door_handle	(	grid,
		pt,
		list = `None`,
		rotation_angle = `math.radians(0.)`,
		occupancy_threshold = `None`,
		resolution = `None`
	)

returns door handle points in the thok coord frame.

Definition at line 462 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.find_goto_point_surface_1	(	grid,
		pt,
		display_list = `None`
	)

returns p_erratic,p_edge,surface_height
    p_erratic - point where the erratic's origin should go to. (in thok0 frame)
    p_edge - point on the edge closest to pt.
    p_erratic,p_edge are 3x1 matrices.
    surface_height - in thok0 coord frame.

Definition at line 181 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.generate_pointcloud	(	pos_list,
		scan_list,
		min_angle,
		max_angle,
		l1,
		l2,
		save_scan = `False`,
		max_dist = `np.Inf`,
		min_dist = `-np.Inf`,
		min_tilt = `-np.Inf`,
		max_tilt = `np.Inf`,
		get_intensities = `False`,
		reject_zero_ten = `True`
	)

pos_list - list of motor positions (in steps)
    scan_list - list of UrgScan objects at the corres positions.
    l1,l2 - parameterizing the transformation (doc/ folder)
    save_scan - pickle 3xN numpy matrix of points.
    max_dist - ignore points at distance > max_dist
    min_dist - ignore points at distance < max_dist

    min_tilt, max_tilt - min and max tilt angles (radians)
                         +ve tilts the hokuyo down.
    get_intensites - additionally return intensity values
    returns 3xN numpy matrix of 3d coords of the points, returns (3xN, 1xN) including the intensity values if get_intensites=True

Definition at line 66 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.grasp_location_on_object	(	obj,
		display_list = `None`
	)

obj - 3xN numpy matrix of points of the object.

Definition at line 704 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.max_fwd_without_collision	(	all_pts,
		z_height,
		max_dist,
		display_list = `None`
	)

find the max distance that it is possible to move fwd by
    without collision.
    all_pts - 3xN matrix of 3D points in thok frame.
    z - height of zenither while taking the scan.
    max_dist - how far do I want to check for a possible collision.
    returns max_dist that the thok can be moved fwd without collision.

Definition at line 141 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.overhead_grasp_collision	(	pt,
		grid
	)

Definition at line 696 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.points_within_cuboid	(	pts,
		brf,
		tlb
	)

returns points that are within the cuboid formed by tlb and brf.

Parameters:

pts	- 3xN numpy matrix
tlb,brf	- 3x1 np matrix. bottom-right-front and top-left-back

Returns:: 3xM numpy matrix

Definition at line 56 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.pushback_edge	(	pts2d,
		pt
	)

push pt away from the edge defined by pts2d.
    pt - 2x1, pts2d - 2xN
    returns the pushed point.

Definition at line 260 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.segment_objects_points	(	grid,
		return_labels_list = `False`,
		twod = `False`
	)

grid - binary occupancy grid.
    returns list of 3xNi numpy matrices where Ni is the number of points
    in the ith object. Point refers to center of the cell of occupancy grid.
    return_labels_list - return a list of labels of the objects in
    the grid.
    returns None if there is no horizontal surface

Definition at line 554 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_approach ( )

Definition at line 968 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_choose_grasp_strategy ( )

Definition at line 1008 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_different_surfaces ( )

Definition at line 1021 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_find_door_handle ( )

Definition at line 849 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_grasp_location_on_object ( )

Definition at line 909 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_max_forward ( )

Definition at line 991 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_occ_grid ( )

Definition at line 1084 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_plane_finding ( )

visualize plane finding.

Definition at line 919 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_segmentation ( )

Definition at line 885 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.test_vertical_plane_finding ( )

Definition at line 838 of file processing_3d.py.

def hrl_tilting_hokuyo.processing_3d.vertical_plane_points ( grid )

changes grid

Definition at line 454 of file processing_3d.py.

Variable Documentation

list hrl_tilting_hokuyo::processing_3d::color_list

Initial value:

00001 [(1.,1.,0),(1.,0,0),(0,1.,1.),(0,1.,0),(0,0,1.),(0,0.4,0.4),(0.4,0.4,0),
00002               (0.4,0,0.4),(0.4,0.8,0.4),(0.8,0.4,0.4),(0.4,0.4,0.8),(0.4,0,0.8),(0,0.8,0.4),
00003               (0,0.4,0.8),(0.8,0,0.4),(0.4,0,0.4),(1.,0.6,0.02) ]

Definition at line 46 of file processing_3d.py.

tuple hrl_tilting_hokuyo::processing_3d::dict = ut.load_pickle(pkl_file_name)

Definition at line 1112 of file processing_3d.py.

tuple hrl_tilting_hokuyo::processing_3d::gr = create_approach_grid(pt,pos_list,scan_list,l1,l2,display_list)

pts = all_pts[:,np.where(np.multiply(all_pts[0,:]>0.1,all_pts[0,:]<0.4))[1].A1] pts = pts[:,np.where(np.multiply(pts[1,:]<0.3,pts[1,:]>-0.3))[1].A1] pts = pts[:,np.where(pts[2,:]>-0.2)[1].A1] display_list.append(pu.PointCloud(pts,color=(0,200,0))) display_list.append(pu.PointCloud(all_pts,color=(200,0,0)))

brf = np.matrix([0.05,-0.35,-0.2]).T tlb = np.matrix([0.5, 0.35,0.0]).T resolution = np.matrix([0.01,0.01,0.0025]).T gr = og3d.occupancy_grid_3d(brf,tlb,resolution) gr.fill_grid(all_pts) gr.to_binary(1)

Definition at line 1039 of file processing_3d.py.

list hrl_tilting_hokuyo::processing_3d::height_list = []

Definition at line 1047 of file processing_3d.py.

string hrl_tilting_hokuyo::processing_3d::help = 'file.pkl File with the scan,pos dict.'

Definition at line 1099 of file processing_3d.py.

tuple hrl_tilting_hokuyo::processing_3d::l = gr.find_plane_indices(assume_plane=True)

Definition at line 1041 of file processing_3d.py.

list hrl_tilting_hokuyo::processing_3d::l1 = dict['l1']

Definition at line 1118 of file processing_3d.py.

list hrl_tilting_hokuyo::processing_3d::l2 = dict['l2']

Definition at line 1119 of file processing_3d.py.

tuple hrl_tilting_hokuyo::processing_3d::max_angle = math.radians(40)