def compare_tip_mechanism_trajectories(mech_mat, hand_mat):
# hand_proj, nrm_hand = project_points_plane(hand_mat)
# mech_proj, nrm_mech = project_points_plane(mech_mat)
hand_proj = hand_mat
mech_proj = mech_mat
kin_dict = ke.fit(hand_proj, tup)
print 'kin_dict from hook tip:', kin_dict
print 'measured radius:', cd['radius']
center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
kin_dict = ke.fit(mech_proj, tup)
print 'kin_dict from mechanism:', kin_dict
center_mech = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
# working with the projected coordinates.
directions_hand = hand_proj - center_hand
directions_hand = directions_hand / ut.norm(directions_hand)
directions_mech = mech_proj - center_mech
directions_mech = directions_mech / ut.norm(directions_mech)
start_normal = directions_mech[:,0]
print 'directions_mech[:,0]', directions_mech[:,0].A1
print 'directions_hand[:,0]', directions_hand[:,0].A1
ct = (start_normal.T * directions_hand).A1
st = ut.norm(np.matrix(np.cross(start_normal.A1, directions_hand.T.A)).T).A1
mech_angle = np.arctan2(st, ct).tolist()
#mech_angle = np.arccos(start_normal.T * directions_hand).A1.tolist()
mpu.plot_yx(np.degrees(mech_angle))
mpu.show()
# plot trajectory in 3D.
d3m.white_bg()
d3m.plot_points(hand_proj, color = (1.,0.,0.), mode='sphere',
scale_factor = 0.005)
d3m.plot_points(mech_proj[:,0:1], color = (0.,0.,0.), mode='sphere',
scale_factor = 0.01)
d3m.plot_points(mech_proj, color = (0.,0.,1.), mode='sphere',
scale_factor = 0.005)
d3m.plot(np.column_stack((mech_proj[:,-1],center_mech, mech_proj[:,0])),
color = (0.,0.,1.))
d3m.plot(np.column_stack((hand_proj[:,-1],center_hand, hand_proj[:,0])),
color = (1.,0.,0.))
d3m.show()
def linear_distances(mechanism_positions):
pos_mat = np.column_stack(mechanism_positions)
ld = ut.norm(pos_mat - pos_mat[:,0]).A1.tolist()
ld_arr = np.array(ld)
if np.any(np.isnan(ld_arr)):
pdb.set_trace()
return ld
def find_closest_pt(pts2d, pt, pt_closer=False):
''' returns closest point to edge (2x1 matrix)
can return None also
'''
dist_pt = np.linalg.norm(pt[0:2, 0])
pts2d_r = ut.norm(pts2d)
pts2d_a = np.arctan2(pts2d[1, :], pts2d[0, :])
if pt_closer == False:
k_idxs = np.where(pts2d_r <= dist_pt)
else:
k_idxs = np.where(pts2d_r > dist_pt)
pts2d_r = pts2d_r[k_idxs]
pts2d_a = pts2d_a[k_idxs]
pts2d = ut.cart_of_pol(np.matrix(np.row_stack((pts2d_r, pts2d_a))))
if pt_closer == False:
edge_to_pt = pt[0:2, 0] - pts2d
else:
edge_to_pt = pts2d - pt[0:2, 0]
edge_to_pt_a = np.arctan2(edge_to_pt[1, :], edge_to_pt[0, :])
keep_idxs = np.where(np.abs(edge_to_pt_a) < math.radians(70))[1].A1
if keep_idxs.shape[0] == 0:
return None
pts2d = pts2d[:, keep_idxs]
# pt_to_edge_dists = ut.norm(pts2d-pt[0:2,0])
# closest_pt_index = np.argmin(pt_to_edge_dists)
closest_pt_index = find_closest_pt_index(pts2d, pt[0:2, 0])
closest_pt = pts2d[:, closest_pt_index]
return closest_pt
def connected_components(p, dist_threshold):
''' p - 2xN numpy matrix of euclidean points points (indices give neighbours).
dist_threshold - max distance between two points in the same connected component.
typical value is .02 meters
returns a list of (p1,p2, p1_index, p2_index): (start euclidean point, end euclidean point, start index, end index)
p1 and p2 are 2X1 matrices
'''
nPoints = p.shape[1]
q = p[:, 0:nPoints - 1]
r = p[:, 1:nPoints]
diff = r - q
dists = ut.norm(diff).T
idx = np.where(
dists > dist_threshold) # boundaries of the connected components
end_list = idx[0].A1.tolist()
end_list.append(nPoints - 1)
cc_list = []
start = 0
for i in end_list:
cc_list.append((p[:, start], p[:, i], start, i))
start = i + 1
return cc_list
def filter_trajectory_force(ct, ft):
vel_list = copy.copy(ct.v_list)
pts_list = copy.copy(ct.p_list)
time_list = copy.copy(ct.time_list)
ft_list = copy.copy(ft.f_list)
f_mag_list = ut.norm(np.matrix(ft.f_list).T).A1.tolist()
if len(pts_list) != len(f_mag_list):
print 'arm_trajectories.filter_trajectory_force: force and end effector lists are not of the same length.'
print 'Exiting ...'
sys.exit()
n_pts = len(pts_list)
i = n_pts - 1
hook_slip_off_threshold = 1.5 # from compliant_trajectories.py
while i > 0:
if f_mag_list[i] < hook_slip_off_threshold:
pts_list.pop()
time_list.pop()
ft_list.pop()
if vel_list != []:
vel_list.pop()
else:
break
i -= 1
ct2 = CartesianTajectory()
ct2.time_list = time_list
ct2.p_list = pts_list
ct2.v_list = vel_list
ft2 = ForceTrajectory()
ft2.time_list = copy.copy(time_list)
ft2.f_list = ft_list
return ct2, ft2
def pts_within_dist(p, pts, min_dist, max_dist):
v = p - pts
d_arr = ut.norm(v).A1
idxs = np.where(
np.all(np.row_stack((d_arr < max_dist, d_arr > min_dist)), axis=0))
pts_within = pts[:, idxs[0]]
return pts_within
def find_closest_pt_index(pts2d,pt):
''' returns index (of pts2d) of closest point to pt.
pts2d - 2xN matrix, pt - 2x1 matrix
'''
pt_to_edge_dists = ut.norm(pts2d-pt)
closest_pt_index = np.argmin(pt_to_edge_dists)
return closest_pt_index
def find_closest_pt(pts2d,pt,pt_closer=False):
''' returns closest point to edge (2x1 matrix)
can return None also
'''
dist_pt = np.linalg.norm(pt[0:2,0])
pts2d_r = ut.norm(pts2d)
pts2d_a = np.arctan2(pts2d[1,:],pts2d[0,:])
if pt_closer == False:
k_idxs = np.where(pts2d_r<=dist_pt)
else:
k_idxs = np.where(pts2d_r>dist_pt)
pts2d_r = pts2d_r[k_idxs]
pts2d_a = pts2d_a[k_idxs]
pts2d = ut.cart_of_pol(np.matrix(np.row_stack((pts2d_r,pts2d_a))))
if pt_closer == False:
edge_to_pt = pt[0:2,0]-pts2d
else:
edge_to_pt = pts2d-pt[0:2,0]
edge_to_pt_a = np.arctan2(edge_to_pt[1,:],edge_to_pt[0,:])
keep_idxs = np.where(np.abs(edge_to_pt_a)<math.radians(70))[1].A1
if keep_idxs.shape[0] == 0:
return None
pts2d = pts2d[:,keep_idxs]
# pt_to_edge_dists = ut.norm(pts2d-pt[0:2,0])
# closest_pt_index = np.argmin(pt_to_edge_dists)
closest_pt_index = find_closest_pt_index(pts2d,pt[0:2,0])
closest_pt = pts2d[:,closest_pt_index]
return closest_pt
def pushback_edge(pts2d,pt):
''' push pt away from the edge defined by pts2d.
pt - 2x1, pts2d - 2xN
returns the pushed point.
'''
closest_idx = find_closest_pt_index(pts2d,pt)
n_push_points = min(min(5,pts2d.shape[1]-closest_idx-1),closest_idx)
if closest_idx<n_push_points or (pts2d.shape[1]-closest_idx-1)<n_push_points:
print 'processing_3d.pushback_edge: pt is too close to the ends of the pts2d array.'
return None
edge_to_pt = pt-pts2d[:,closest_idx-n_push_points:closest_idx+n_push_points]
edge_to_pt_r = ut.norm(edge_to_pt)
edge_to_pt_a = np.arctan2(edge_to_pt[1,:],edge_to_pt[0,:])
non_zero_idxs = np.where(edge_to_pt_r>0.005)
edge_to_pt_r = edge_to_pt_r[non_zero_idxs]
edge_to_pt_r[0,:] = 1
edge_to_pt_a = edge_to_pt_a[non_zero_idxs]
edge_to_pt_unit = ut.cart_of_pol(np.row_stack((edge_to_pt_r,edge_to_pt_a)))
push_vector = edge_to_pt_unit.mean(1)
push_vector = push_vector/np.linalg.norm(push_vector)
print 'push_vector:', push_vector.T
pt_pushed = pt + push_vector*0.05
return pt_pushed
def filter_trajectory_force(ct, ft):
vel_list = copy.copy(ct.v_list)
pts_list = copy.copy(ct.p_list)
time_list = copy.copy(ct.time_list)
ft_list = copy.copy(ft.f_list)
f_mag_list = ut.norm(np.matrix(ft.f_list).T).A1.tolist()
if len(pts_list) != len(f_mag_list):
print 'arm_trajectories.filter_trajectory_force: force and end effector lists are not of the same length.'
print 'Exiting ...'
sys.exit()
n_pts = len(pts_list)
i = n_pts - 1
hook_slip_off_threshold = 1.5 # from compliant_trajectories.py
while i > 0:
if f_mag_list[i] < hook_slip_off_threshold:
pts_list.pop()
time_list.pop()
ft_list.pop()
if vel_list != []:
vel_list.pop()
else:
break
i -= 1
ct2 = CartesianTajectory()
ct2.time_list = time_list
ct2.p_list = pts_list
ct2.v_list = vel_list
ft2 = ForceTrajectory()
ft2.time_list = copy.copy(time_list)
ft2.f_list = ft_list
return ct2, ft2
def pushback_edge(pts2d, pt):
''' push pt away from the edge defined by pts2d.
pt - 2x1, pts2d - 2xN
returns the pushed point.
'''
closest_idx = find_closest_pt_index(pts2d, pt)
n_push_points = min(min(5, pts2d.shape[1] - closest_idx - 1), closest_idx)
if closest_idx < n_push_points or (pts2d.shape[1] - closest_idx -
1) < n_push_points:
print 'processing_3d.pushback_edge: pt is too close to the ends of the pts2d array.'
return None
edge_to_pt = pt - pts2d[:, closest_idx - n_push_points:closest_idx +
n_push_points]
edge_to_pt_r = ut.norm(edge_to_pt)
edge_to_pt_a = np.arctan2(edge_to_pt[1, :], edge_to_pt[0, :])
non_zero_idxs = np.where(edge_to_pt_r > 0.005)
edge_to_pt_r = edge_to_pt_r[non_zero_idxs]
edge_to_pt_r[0, :] = 1
edge_to_pt_a = edge_to_pt_a[non_zero_idxs]
edge_to_pt_unit = ut.cart_of_pol(np.row_stack(
(edge_to_pt_r, edge_to_pt_a)))
push_vector = edge_to_pt_unit.mean(1)
push_vector = push_vector / np.linalg.norm(push_vector)
print 'push_vector:', push_vector.T
pt_pushed = pt + push_vector * 0.05
return pt_pushed
def compute_segway_motion_mag(d):
st = d['segway']
x_list, y_list = st.x_list, st.y_list
n1 = ut.norm(np.matrix([x_list, y_list]))
n1 = n1 - np.min(n1)
n1 = n1*10
return n1.A1.tolist()
def plot_ft(d):
ft_list = d['ft_list']
time_list = d['time_list']
ft_mat = np.matrix(ft_list).T # 6xN np matrix
force_mat = ft_mat[0:3, :]
tarray = np.array(time_list)
print 'average rate', 1. / np.mean(tarray[1:] - tarray[:-1])
time_list = (tarray - tarray[0]).tolist()
print len(time_list)
force_mag_l = ut.norm(force_mat).A1.tolist()
#for i,f in enumerate(force_mag_l):
# if f > 15:
# break
#force_mag_l = force_mag_l[i:]
#time_list = time_list[i:]
mpu.plot_yx(force_mag_l,
time_list,
axis=None,
label='Force Magnitude',
xlabel='Time(seconds)',
ylabel='Force Magnitude (N)',
color=mpu.random_color())
def find_closest_object_point(scan,
pt_interest=np.matrix([0., 0.]).T,
min_angle=math.radians(-60),
max_angle=math.radians(60),
max_dist=0.6,
min_size=0.01,
max_size=0.3):
''' returns 2x1 matrix - centroid of connected component in hokuyo frame closest to pt_interest
pt_interest - 2x1 matrix in hokuyo coord frame.
None if no object found.
'''
obj_list = find_objects(scan,
max_dist,
max_size,
min_size,
min_angle,
max_angle,
connect_dist_thresh=0.02,
all_pts=True)
if obj_list == []:
return None
min_dist_list = []
for pts in obj_list:
min_dist_list.append(np.min(ut.norm(pts - pt_interest)))
min_idx = np.argmin(np.matrix(min_dist_list))
return obj_list[min_idx].mean(1)
def find_closest_pt_index(pts2d, pt):
''' returns index (of pts2d) of closest point to pt.
pts2d - 2xN matrix, pt - 2x1 matrix
'''
pt_to_edge_dists = ut.norm(pts2d - pt)
closest_pt_index = np.argmin(pt_to_edge_dists)
return closest_pt_index
def linear_distances(mechanism_positions):
pos_mat = np.column_stack(mechanism_positions)
ld = ut.norm(pos_mat - pos_mat[:, 0]).A1.tolist()
ld_arr = np.array(ld)
if np.any(np.isnan(ld_arr)):
pdb.set_trace()
return ld
def compute_segway_motion_mag(d):
st = d['segway']
x_list, y_list = st.x_list, st.y_list
n1 = ut.norm(np.matrix([x_list, y_list]))
n1 = n1 - np.min(n1)
n1 = n1 * 10
return n1.A1.tolist()
def find_good_config(pt,dict):
''' finds a good configuration for the 3x1 pt.
'''
m = dict['cart_pts_mat']
min_idx = np.argmin(ut.norm(m-pt))
c = dict['good_configs_list'][min_idx]
# print 'good configuration:', [math.degrees(qi) for qi in c]
return c
def error_function(params):
center = np.matrix((params[0],params[1])).T
#print 'pts.shape', pts.shape
#print 'center.shape', center.shape
#print 'ut.norm(pts-center).shape',ut.norm(pts-center).shape
err = ut.norm(pts-center).A1 - rad
res = np.dot(err,err)
return res
def find_good_config(pt,dic):
''' finds a good configuration for the 3x1 pt.
'''
m = dic['cart_pts_mat']
min_idx = np.argmin(ut.norm(m-pt))
c = dic['good_configs_list'][min_idx]
# print 'good configuration:', [math.degrees(qi) for qi in c]
return c
def error_function(params):
center = np.matrix((params[0], params[1])).T
rad = params[2]
err_pts = ut.norm(pts - center).A1 - rad
lik = np.dot(err_pts, err_pts) / (sigma_pts * sigma_pts)
pri = ((rad - rad_guess)**2) / (sigma_r * sigma_r)
#pri += ((x_prior - center[0,0])**2) / (sigma_xy * sigma_xy)
#pri += ((y_prior - center[1,0])**2) / (sigma_xy * sigma_xy)
return (lik + pri)
def error_function(params):
center = np.matrix((params[0],params[1])).T
rad = params[2]
#print 'pts.shape', pts.shape
#print 'center.shape', center.shape
#print 'ut.norm(pts-center).shape',ut.norm(pts-center).shape
err = ut.norm(pts-center).A1 - rad
res = np.dot(err,err)
return res
def compute_mech_angle_2(cd, tup, project_plane=False):
pos_mat = np.column_stack(cd['mech_pos_list'])
if project_plane:
pts_proj, min_evec = project_points_plane(pos_arr)
pos_arr = pts_proj
kin_dict = ke.fit(pos_mat, tup)
center = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
directions_x = pos_mat - center
directions_x = directions_x / ut.norm(directions_x)
start_normal = directions_x[:,0]
#mech_angle = np.arccos(start_normal.T * directions_x).A1.tolist()
ct = (start_normal.T * directions_x).A1
st = ut.norm(np.matrix(np.cross(start_normal.A1, directions_x.T.A)).T).A1
mech_angle = np.arctan2(st, ct).tolist()
return mech_angle
def error_function(params):
center = np.matrix((params[0],params[1])).T
rad = params[2]
err_pts = ut.norm(pts-center).A1 - rad
lik = np.dot(err_pts, err_pts) / (sigma_pts * sigma_pts)
pri = ((rad - rad_guess)**2) / (sigma_r * sigma_r)
#pri += ((x_prior - center[0,0])**2) / (sigma_xy * sigma_xy)
#pri += ((y_prior - center[1,0])**2) / (sigma_xy * sigma_xy)
return (lik + pri)
def compute_mech_angle_1(cd, axis_direc=None):
mech_rot = cd['mech_rot_list']
directions_x = (np.row_stack(mech_rot)[:,0]).T.reshape(len(mech_rot), 3).T
if axis_direc != None:
directions_x = directions_x - np.multiply(axis_direc.T * directions_x,
axis_direc)
directions_x = directions_x/ut.norm(directions_x)
start_normal = directions_x[:,0]
mech_angle = np.arccos(start_normal.T * directions_x).A1.tolist()
return mech_angle
def taxel_array_to_np_arrays(ta, desired_unroll_index=0):
force_vectors = np.row_stack([ta.values_x, ta.values_y, ta.values_z])
fmags = ut.norm(force_vectors)
force_arr_raw = fmags.reshape((16,24)) #Verified by Tapo
force_arr = np.column_stack((force_arr_raw[:,desired_unroll_index:],force_arr_raw[:,:desired_unroll_index]))
center_arr = np.row_stack([ta.centers_x, ta.centers_y, ta.centers_z]).reshape((3,16,24))
nrml_arr = np.row_stack([ta.normals_x, ta.normals_y, ta.normals_z]).reshape((3,16,24))
return force_arr, center_arr, nrml_arr
def get_selection( self, obj, images, humanread, blit_loc ):
[srf, fps, clk] = obj
loopFlag = True
ind = 0
pos = (0,0)
while loopFlag:
# Clear the screen
srf.fill((255,255,255))
diffs = np.array(blit_loc) - np.array(pos)
ind = np.argmin( ut.norm( diffs.T ))
self.put_bottom_text( srf, humanread[ind] )
self.draw_rect(srf, blit_loc[ind])
for i, image in enumerate(images):
srf.blit(image, self.calc_blit_loc(image,blit_loc[i]))
#print 'going'
pygame.display.flip()
events = pygame.event.get()
for e in events:
if e.type==pygame.locals.QUIT:
loopFlag=False
if e.type==pygame.locals.KEYDOWN:
if e.key == 27: # Esc
loopFlag=False
if e.type == pygame.locals.MOUSEMOTION:
pos = e.pos
if e.type==pygame.locals.MOUSEBUTTONDOWN:
if e.button == 1:
# left button
pos = e.pos
diffs = np.array(blit_loc) - np.array(pos)
ind = np.argmin( ut.norm( diffs.T ))
loopFlag = False
clk.tick(fps)
srf.fill((255,255,255))
pygame.display.flip()
clk.tick(fps)
return ind
def taxel_array_cb(ta, callback_args):
sc_pu, tf_lstnr = callback_args
sc = SkinContact()
sc.header.frame_id = '/torso_lift_link' # has to be this and no other coord frame.
sc.header.stamp = ta.header.stamp
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
t1, q1 = tf_lstnr.lookupTransform(sc.header.frame_id,
ta.header.frame_id,
rospy.Time(0))
#ta.header.stamp)
t1 = np.matrix(t1).reshape(3,1)
r1 = tr.quaternion_to_matrix(q1)
if ta.link_names == []:
real_skin_patch = True
else:
real_skin_patch = False
# transform to the torso_lift_link frame.
pts = r1 * np.matrix(pts).T + t1
nrmls = r1 * np.matrix(nrmls).T
fs = r1 * np.matrix(fs).T
fmags = ut.norm(fs).A1
# for fabric sensor and meka sensor, Advait moved the thresholding
# etc within the calibration node. (June 13, 2012)
if not real_skin_patch:
idxs = np.where(fmags > 0.5)[0]
else:
idxs = np.where(fmags > 0.001)[0]
for i in idxs:
p = pts[:,i]
n1 = nrmls[:,i]
n2 = fs[:,i]
if real_skin_patch:
link_name = ta.header.frame_id
else:
link_name = ta.link_names[i]
sc.locations.append(Point(p[0,0], p[1,0], p[2,0]))
sc.forces.append(Vector3(n2[0,0], n2[1,0], n2[2,0]))
sc.normals.append(Vector3(n1[0,0], n1[1,0], n1[2,0]))
sc.link_names.append(link_name)
sc.pts_x.append(FloatArrayBare(p[0,:].A1))
sc.pts_y.append(FloatArrayBare(p[1,:].A1))
sc.pts_z.append(FloatArrayBare(p[2,:].A1))
sc_pub.publish(sc)
def taxel_array_cb(ta, callback_args):
sc_pu, tf_lstnr = callback_args
sc = SkinContact()
sc.header.frame_id = '/torso_lift_link' # has to be this and no other coord frame.
sc.header.stamp = ta.header.stamp
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
t1, q1 = tf_lstnr.lookupTransform(sc.header.frame_id, ta.header.frame_id,
rospy.Time(0))
#ta.header.stamp)
t1 = np.matrix(t1).reshape(3, 1)
r1 = tr.quaternion_to_matrix(q1)
if ta.link_names == []:
real_skin_patch = True
else:
real_skin_patch = False
# transform to the torso_lift_link frame.
pts = r1 * np.matrix(pts).T + t1
nrmls = r1 * np.matrix(nrmls).T
fs = r1 * np.matrix(fs).T
fmags = ut.norm(fs).A1
# for fabric sensor and meka sensor, Advait moved the thresholding
# etc within the calibration node. (June 13, 2012)
if not real_skin_patch:
idxs = np.where(fmags > 0.5)[0]
else:
idxs = np.where(fmags > 0.001)[0]
for i in idxs:
p = pts[:, i]
n1 = nrmls[:, i]
n2 = fs[:, i]
if real_skin_patch:
link_name = ta.header.frame_id
else:
link_name = ta.link_names[i]
sc.locations.append(Point(p[0, 0], p[1, 0], p[2, 0]))
sc.forces.append(Vector3(n2[0, 0], n2[1, 0], n2[2, 0]))
sc.normals.append(Vector3(n1[0, 0], n1[1, 0], n1[2, 0]))
sc.link_names.append(link_name)
sc.pts_x.append(FloatArrayBare(p[0, :].A1))
sc.pts_y.append(FloatArrayBare(p[1, :].A1))
sc.pts_z.append(FloatArrayBare(p[2, :].A1))
sc_pub.publish(sc)
def error_function(params):
center = np.matrix((params[0],params[1])).T
if rad_fix:
rad = rad_guess
else:
rad = params[2]
err = ut.norm(pts-center).A1 - rad
res = np.dot(err,err)
#if not rad_fix and rad < 0.3:
# res = res*(0.3-rad)*100
return res
def error_function(params):
center = np.matrix((params[0], params[1])).T
if rad_fix:
rad = rad_guess
else:
rad = params[2]
err = ut.norm(pts - center).A1 - rad
res = np.dot(err, err)
#if not rad_fix and rad < 0.3:
# res = res*(0.3-rad)*100
return res
def plot(combined_dict, savefig):
plot_trajectories(combined_dict)
# tup = ke.init_ros_node()
# mech_rot_list = compute_mech_rot_list(combined_dict, tup)
# combined_dict['mech_rot_list'] = mech_rot_list
# plot_trajectories(combined_dict)
cd = combined_dict
ft_mat = np.matrix(cd['ft_list']).T
force_mat = ft_mat[0:3, :]
mpu.plot_yx(ut.norm(force_mat).A1, cd['time_list'])
mpu.show()
def taxel_array_to_np_arrays(ta, desired_unroll_index=0):
force_vectors = np.row_stack([ta.values_x, ta.values_y, ta.values_z])
fmags = ut.norm(force_vectors)
force_arr_raw = fmags.reshape((16, 24)) #Verified by Tapo
force_arr = np.column_stack((force_arr_raw[:, desired_unroll_index:],
force_arr_raw[:, :desired_unroll_index]))
center_arr = np.row_stack([ta.centers_x, ta.centers_y,
ta.centers_z]).reshape((3, 16, 24))
nrml_arr = np.row_stack([ta.normals_x, ta.normals_y,
ta.normals_z]).reshape((3, 16, 24))
return force_arr, center_arr, nrml_arr
def angle_between_hooktip_mechanism_radial_vectors(mech_mat, hand_mat):
kin_dict = ke.fit(hand_mat, tup)
print 'kin_dict from hook tip:', kin_dict
print 'measured radius:', cd['radius']
center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
kin_dict = ke.fit(mech_mat, tup)
print 'kin_dict from mechanism:', kin_dict
center_mech = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
# working with the projected coordinates.
directions_hand = hand_mat - center_hand
directions_hand = directions_hand / ut.norm(directions_hand)
directions_mech = mech_mat - center_mech
directions_mech = directions_mech / ut.norm(directions_mech)
#import pdb; pdb.set_trace()
ang = np.degrees(np.arccos(np.sum(np.multiply(directions_mech, directions_hand), 0))).A1
mpu.plot_yx(ang, label = 'angle between hooktip-radial and mechanism radial')
mpu.legend()
mpu.show()
def find_closest_object_point(scan, pt_interest=np.matrix([0.,0.]).T, min_angle=math.radians(-60),
max_angle=math.radians(60),max_dist=0.6,min_size=0.01,max_size=0.3):
''' returns 2x1 matrix - centroid of connected component in hokuyo frame closest to pt_interest
pt_interest - 2x1 matrix in hokuyo coord frame.
None if no object found.
'''
obj_list = find_objects(scan,max_dist,max_size,min_size,min_angle,max_angle,
connect_dist_thresh=0.02, all_pts=True)
if obj_list == []:
return None
min_dist_list = []
for pts in obj_list:
min_dist_list.append(np.min(ut.norm(pts-pt_interest)))
min_idx = np.argmin(np.matrix(min_dist_list))
return obj_list[min_idx].mean(1)
def calculate_derived_quantities(ler_result, smooth_window):
exp_records = ler_result['records']
mech_info = ler_result['mech_info']
kin_info_list = ler_result['kin_info_list']
#Calculate velocities
for kin_info in kin_info_list:
kin_info['velocity'] = velocity(kin_info, smooth_window)
#Interpolate kinematics into force time steps
for segment_name in exp_records.keys():
for exp_number, record in enumerate(exp_records[segment_name]):
record['mech_coord_arr'] = interpolate_1d(kin_info_list[exp_number]['mech_time_arr'],
kin_info_list[exp_number]['disp_mech_coord_arr'],
record['ftimes'])
record['mech_pose_vel_arr'] = interpolate_1d(kin_info_list[exp_number]['mech_time_arr'],
kin_info_list[exp_number]['velocity'],
record['ftimes'])
record['force_norms'] = ut.norm(np.matrix(record['forces']).T[0:3,:]).A1
def dist_from_boundary(curr_pos,bndry,pts):
mv = vec_from_boundary(curr_pos,bndry)
# spoly = sg.Polygon((bndry.T).tolist())
# spt = sg.Point(curr_pos[0,0],curr_pos[1,0])
d = np.linalg.norm(mv)
p = curr_pos[0:2,:]
v = p-pts
min_dist = np.min(ut.norm(v))
# print 'min_dist,d:',min_dist,d
# print 'min_dist >= d',min_dist >= d-0.001
if min_dist >= d-0.001:
# print 'I predict outside workspace'
d = -d
# if spoly.contains(spt) == False:
# print 'Shapely predicts outside workspace'
# d = -d
return d
def dist_from_boundary(curr_pos, bndry, pts):
mv = vec_from_boundary(curr_pos, bndry)
# spoly = sg.Polygon((bndry.T).tolist())
# spt = sg.Point(curr_pos[0,0],curr_pos[1,0])
d = np.linalg.norm(mv)
p = curr_pos[0:2, :]
v = p - pts
min_dist = np.min(ut.norm(v))
# print 'min_dist,d:',min_dist,d
# print 'min_dist >= d',min_dist >= d-0.001
if min_dist >= d - 0.001:
# print 'I predict outside workspace'
d = -d
# if spoly.contains(spt) == False:
# print 'Shapely predicts outside workspace'
# d = -d
return d
def plot_ft(d):
ft_list = d['ft_list']
time_list = d['time_list']
ft_mat = np.matrix(ft_list).T # 6xN np matrix
force_mat = ft_mat[0:3, :]
tarray = np.array(time_list)
print 'average rate', 1. / np.mean(tarray[1:] - tarray[:-1])
time_list = (tarray - tarray[0]).tolist()
print len(time_list)
force_mag_l = ut.norm(force_mat).A1.tolist()
#for i,f in enumerate(force_mag_l):
# if f > 15:
# break
#force_mag_l = force_mag_l[i:]
#time_list = time_list[i:]
mpu.plot_yx(force_mag_l, time_list, axis=None, label='Force Magnitude',
xlabel='Time(seconds)', ylabel='Force Magnitude (N)',
color = mpu.random_color())
def calculate_derived_quantities(ler_result, smooth_window):
exp_records = ler_result['records']
mech_info = ler_result['mech_info']
kin_info_list = ler_result['kin_info_list']
#Calculate velocities
for kin_info in kin_info_list:
kin_info['velocity'] = velocity(kin_info, smooth_window)
#Interpolate kinematics into force time steps
for segment_name in exp_records.keys():
for exp_number, record in enumerate(exp_records[segment_name]):
record['mech_coord_arr'] = interpolate_1d(
kin_info_list[exp_number]['mech_time_arr'],
kin_info_list[exp_number]['disp_mech_coord_arr'],
record['ftimes'])
record['mech_pose_vel_arr'] = interpolate_1d(
kin_info_list[exp_number]['mech_time_arr'],
kin_info_list[exp_number]['velocity'], record['ftimes'])
record['force_norms'] = ut.norm(
np.matrix(record['forces']).T[0:3, :]).A1
def 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.
'''
min_dist_list = []
for obj_pts in obj_pts_list:
min_dist_list.append(np.min(ut.norm(obj_pts-pt)))
if obj_pts_list == []:
return None
min_idx = np.argmin(np.matrix(min_dist_list))
cl_obj = obj_pts_list[min_idx]
print 'processing_3d.find_closest_object: closest_object\'s centroid',cl_obj.mean(1).T
if return_idx:
return cl_obj,min_idx
return cl_obj
def segway_motion_repulse(curr_pos_tl, eq_pt_tl, bndry, all_pts):
bndry_dist_eq = dist_from_boundary(eq_pt_tl, bndry, all_pts) # signed
bndry_dist_ee = dist_from_boundary(curr_pos_tl, bndry, all_pts) # signed
if bndry_dist_ee < bndry_dist_eq:
p = curr_pos_tl[0:2, :]
bndry_dist = bndry_dist_ee
else:
p = eq_pt_tl[0:2, :]
bndry_dist = bndry_dist_eq
# p = eq_pt_tl[0:2,:]
pts_close = pts_within_dist(p, bndry, 0.002, 0.07)
v = p - pts_close
d_arr = ut.norm(v).A1
v = v / d_arr
v = v / d_arr # inverse distance weight
resultant = v.sum(1)
res_norm = np.linalg.norm(resultant)
resultant = resultant / res_norm
tvec = -resultant
if bndry_dist < 0.:
tvec = -tvec # eq pt was outside workspace polygon.
if abs(bndry_dist) < 0.01 or res_norm < 0.01:
# internal external test fails so falling back on
# going to mean.
m = all_pts.mean(1)
tvec = m - p
tvec = -tvec / np.linalg.norm(tvec)
dist_move = 0.
if bndry_dist > 0.05:
dist_move = 0.
else:
dist_move = 1.
tvec = tvec * dist_move # tvec is either a unit vec or zero vec.
return tvec
def compute_mech_rot_list(cd, tup, project_plane=False):
pos_arr = np.column_stack(cd['mech_pos_list'])
rot_list = cd['mech_rot_list']
directions_y = (np.row_stack(rot_list)[:,1]).T.reshape(len(rot_list), 3).T
start_y = directions_y[:,0]
pts_proj, y_mech = project_points_plane(pos_arr)
if np.dot(y_mech.T, start_y.A1) < 0:
print 'Negative hai boss'
y_mech = -1 * y_mech
if project_plane:
pos_arr = pts_proj
kin_dict = ke.fit(np.matrix(pos_arr), tup)
center = np.array((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
print 'pos_arr[:,0]', pos_arr[:,0]
print 'center:', center
directions_x = (np.row_stack(rot_list)[:,0]).T.reshape(len(rot_list), 3).T
start_x = directions_x[:,0]
directions_x = np.matrix(pos_arr) - np.matrix(center).T.A
directions_x = directions_x / ut.norm(directions_x)
if np.dot(directions_x[:,0].A1, start_x.A1) < 0:
print 'Negative hai boss'
directions_x = -1 * directions_x
mech_rot_list = []
for i in range(len(rot_list)):
x = -directions_x[:, i]
y = np.matrix(y_mech)
z = np.matrix(np.cross(x.A1, y.A1)).T
rot_mat = np.column_stack((x, y, z))
mech_rot_list.append(rot_mat)
# return mech_rot_list
return rot_list
def segway_motion_repulse(curr_pos_tl, eq_pt_tl,bndry, all_pts):
bndry_dist_eq = dist_from_boundary(eq_pt_tl,bndry,all_pts) # signed
bndry_dist_ee = dist_from_boundary(curr_pos_tl,bndry,all_pts) # signed
if bndry_dist_ee < bndry_dist_eq:
p = curr_pos_tl[0:2,:]
bndry_dist = bndry_dist_ee
else:
p = eq_pt_tl[0:2,:]
bndry_dist = bndry_dist_eq
# p = eq_pt_tl[0:2,:]
pts_close = pts_within_dist(p,bndry,0.002,0.07)
v = p-pts_close
d_arr = ut.norm(v).A1
v = v/d_arr
v = v/d_arr # inverse distance weight
resultant = v.sum(1)
res_norm = np.linalg.norm(resultant)
resultant = resultant/res_norm
tvec = -resultant
if bndry_dist < 0.:
tvec = -tvec # eq pt was outside workspace polygon.
if abs(bndry_dist)<0.01 or res_norm<0.01:
# internal external test fails so falling back on
# going to mean.
m = all_pts.mean(1)
tvec = m-p
tvec = -tvec/np.linalg.norm(tvec)
dist_move = 0.
if bndry_dist > 0.05:
dist_move = 0.
else:
dist_move = 1.
tvec = tvec*dist_move # tvec is either a unit vec or zero vec.
return tvec
def 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.
'''
min_dist_list = []
for obj_pts in obj_pts_list:
min_dist_list.append(np.min(ut.norm(obj_pts - pt)))
if obj_pts_list == []:
return None
min_idx = np.argmin(np.matrix(min_dist_list))
cl_obj = obj_pts_list[min_idx]
print 'processing_3d.find_closest_object: closest_object\'s centroid', cl_obj.mean(
1).T
if return_idx:
return cl_obj, min_idx
return cl_obj
def connected_components(p, dist_threshold):
''' p - 2xN numpy matrix of euclidean points points (indices give neighbours).
dist_threshold - max distance between two points in the same connected component.
typical value is .02 meters
returns a list of (p1,p2, p1_index, p2_index): (start euclidean point, end euclidean point, start index, end index)
p1 and p2 are 2X1 matrices
'''
nPoints = p.shape[1]
q = p[:,0:nPoints-1]
r = p[:,1:nPoints]
diff = r-q
dists = ut.norm(diff).T
idx = np.where(dists>dist_threshold) # boundaries of the connected components
end_list = idx[0].A1.tolist()
end_list.append(nPoints-1)
cc_list = []
start = 0
for i in end_list:
cc_list.append((p[:,start], p[:,i], start, i))
start = i+1
return cc_list
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0,:] = 243/255.0
colors[1,:] = 132/255.0
colors[2,:] = 0.
colors[3,:] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T, colors, scale, 'points',
frame, duration=duration, m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.0)[0] #was .01
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.)[0] #was 0.2
#force_marker_scale = 0.04
force_marker_scale = 1.0
duration = 0.4
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
if False:
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0,0] * force_marker_scale
if 'electric' not in roslib.__path__[0]:
scale = (l1, 0.2, 0.2)
else:
scale = (0.2, 0.2, 0.2) #something weird here, was (0.2, 0.2, l1)
scale = (0.4, 0.4, l1)
m = hv.single_marker(p, q1, 'arrow', frame, duration=duration,
scale=scale, m_id=3*i+1)
m.header.stamp = stamp
#markers.markers.append(m)
if True:
if np.linalg.norm(n2) < 0.30:
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
m = Marker()
m.points.append(Point(p[0,0], p[1,0], p[2,0]))
m.points.append(Point(p[0,0]+n2[0,0], p[1,0]+n2[1,0], p[2,0]+n2[2,0]))
m.scale = Point(0.02, 0.04, 0.0)
m.header.frame_id = frame
m.id = 3*i+2
m.type = Marker.ARROW
m.action = Marker.ADD
m.color.r = 0.
m.color.g = 0.8
m.color.b = 0.
m.color.a = 1.
m.lifetime = rospy.Duration(duration)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2/np.linalg.norm(n2) * l2 * 1.2, q2, 'text_view_facing', frame,
(0.07, 0.07, 0.07), m_id = 3*i+3,
duration = duration, color=(0.5,0.5,0.5,1.))
m.text = '%.2fm'%(np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
def pca_eigenface_trick(data):
u, s, vh = np.linalg.svd(data.T * data)
orig_u = data * u
orig_u = orig_u / ut.norm(orig_u)
return orig_u, s, None
def grasp_location_on_object(obj,display_list=None):
''' obj - 3xN numpy matrix of points of the object.
'''
pts_2d = obj[0:2,:]
centroid_2d = pts_2d.mean(1)
pts_2d_zeromean = pts_2d-centroid_2d
e_vals,e_vecs = np.linalg.eig(pts_2d_zeromean*pts_2d_zeromean.T)
# get the min size
min_index = np.argmin(e_vals)
min_evec = e_vecs[:,min_index]
print 'min eigenvector:', min_evec.T
pts_1d = min_evec.T * pts_2d
min_size = pts_1d.max() - pts_1d.min()
print 'spread along min eigenvector:', min_size
max_height = obj[2,:].max()
tlb = obj.max(1)
brf = obj.min(1)
print 'tlb:', tlb.T
print 'brf:', brf.T
resolution = np.matrix([0.005,0.005,0.005]).T
gr = og3d.occupancy_grid_3d(brf,tlb,resolution)
gr.fill_grid(obj)
gr.to_binary(1)
obj = gr.grid_to_points()
grid_2d = gr.grid.max(2)
grid_2d_filled = ni.binary_fill_holes(grid_2d)
gr.grid[:,:,0] = gr.grid[:,:,0]+grid_2d_filled-grid_2d
p = np.matrix(np.row_stack(np.where(grid_2d_filled==1))).astype('float')
p[0,:] = p[0,:]*gr.resolution[0,0]
p[1,:] = p[1,:]*gr.resolution[1,0]
p += gr.brf[0:2,0]
print 'new mean:', p.mean(1).T
print 'centroid_2d:', centroid_2d.T
centroid_2d = p.mean(1)
if min_size<0.12:
# grasp at centroid.
grasp_point = np.row_stack((centroid_2d,np.matrix([max_height+gr.resolution[2,0]*2])))
# grasp_point[2,0] = max_height
gripper_angle = -math.atan2(-min_evec[0,0],min_evec[1,0])
grasp_vec = min_evec
if display_list != None:
max_index = np.argmax(e_vals)
max_evec = e_vecs[:,max_index]
pts_1d = max_evec.T * pts_2d
max_size = pts_1d.max() - pts_1d.min()
v = np.row_stack((max_evec,np.matrix([0.])))
max_end_pt1 = grasp_point + v*max_size/2.
max_end_pt2 = grasp_point - v*max_size/2.
display_list.append(pu.Line(max_end_pt1,max_end_pt2,color=(0,0,0)))
else:
#----- more complicated grasping location finder.
for i in range(gr.grid_shape[2,0]):
gr.grid[:,:,i] = gr.grid[:,:,i]*(i+1)
height_map = gr.grid.max(2) * gr.resolution[2,0]
# print height_map
print 'height std deviation:',math.sqrt(height_map[np.where(height_map>0.)].var())
# connect_structure = np.empty((3,3),dtype='int')
# connect_structure[:,:] = 1
# for i in range(gr.grid_shape[2,0]):
# slice = gr.grid[:,:,i]
# slice_filled = ni.binary_fill_holes(slice)
# slice_edge = slice_filled - ni.binary_erosion(slice_filled,connect_structure)
# gr.grid[:,:,i] = slice_edge*(i+1)
#
# height_map = gr.grid.max(2) * gr.resolution[2,0]
# # print height_map
# print 'contoured height std deviation:',math.sqrt(height_map[np.where(height_map>0.)].var())
#
high_pts_2d = obj[0:2,np.where(obj[2,:]>max_height-0.005)[1].A1]
#high_pts_1d = min_evec.T * high_pts_2d
high_pts_1d = ut.norm(high_pts_2d)
idx1 = np.argmin(high_pts_1d)
pt1 = high_pts_2d[:,idx1]
idx2 = np.argmax(high_pts_1d)
pt2 = high_pts_2d[:,idx2]
if np.linalg.norm(pt1)<np.linalg.norm(pt2):
grasp_point = np.row_stack((pt1,np.matrix([max_height])))
else:
grasp_point = np.row_stack((pt2,np.matrix([max_height])))
vec = centroid_2d-grasp_point[0:2,0]
gripper_angle = -math.atan2(-vec[0,0],vec[1,0])
grasp_vec = vec/np.linalg.norm(vec)
if display_list != None:
pt1 = np.row_stack((pt1,np.matrix([max_height])))
pt2 = np.row_stack((pt2,np.matrix([max_height])))
# display_list.insert(0,pu.CubeCloud(pt1,(0,0,200),size=(0.005,0.005,0.005)))
# display_list.insert(0,pu.CubeCloud(pt2,(200,0,200),size=(0.005,0.005,0.005)))
if display_list != None:
pts = gr.grid_to_points()
size = resolution
# size = resolution*2
# size[2,0] = size[2,0]*2
#display_list.insert(0,pu.PointCloud(pts,(200,0,0)))
display_list.append(pu.CubeCloud(pts,color=(200,0,0),size=size))
display_list.append(pu.CubeCloud(grasp_point,(0,200,200),size=(0.007,0.007,0.007)))
v = np.row_stack((grasp_vec,np.matrix([0.])))
min_end_pt1 = grasp_point + v*min_size/2.
min_end_pt2 = grasp_point - v*min_size/2.
max_evec = np.matrix((min_evec[1,0],-min_evec[0,0])).T
pts_1d = max_evec.T * pts_2d
max_size = pts_1d.max() - pts_1d.min()
display_list.append(pu.Line(min_end_pt1,min_end_pt2,color=(0,255,0)))
return grasp_point,gripper_angle
def find_approach_direction(grid,pt,display_list=None):
z_plane,max_count = grid.argmax_z(search_up=True)
z_plane_meters = z_plane*grid.resolution[2,0]+grid.brf[2,0]
l = grid.find_plane_indices(assume_plane=True)
print '------------ min(l)',min(l)
z_plane_argmax,max_count = grid.argmax_z(search_up=False)
z_plane_below = max(0,z_plane_argmax-5)
print 'z_plane_argmax',z_plane_argmax
print 'z_plane_below',z_plane_below
print 'l:',l
# l = range(z_plane_below,z_plane)+l
copy_grid = copy.deepcopy(grid)
plane_slices = grid.grid[:,:,l]
copy_grid.grid[:,:,:] = 0
copy_grid.grid[:,:,l] = copy.copy(plane_slices)
#display_list.append(pu.PointCloud(copy_grid.grid_to_points(),color=(0,0,255)))
#plane_pts = copy_grid.grid_to_points()
grid_2d = np.max(grid.grid[:,:,l],2)
grid_2d = ni.binary_dilation(grid_2d,iterations=4) # I want 4-connectivity while filling holes.
grid_2d = ni.binary_fill_holes(grid_2d) # I want 4-connectivity while filling holes.
labeled_arr,n_labels = ni.label(grid_2d)
labels_list = range(1,n_labels+1)
count_objects = ni.sum(grid_2d,labeled_arr,labels_list)
if n_labels == 1:
count_objects = [count_objects]
max_label = np.argmax(np.matrix(count_objects))
grid_2d[np.where(labeled_arr!=max_label+1)] = 0
# connect_structure = np.empty((3,3),dtype='int')
# connect_structure[:,:] = 1
# eroded_2d = ni.binary_erosion(grid_2d,connect_structure,iterations=4)
# eroded_2d = ni.binary_erosion(grid_2d)
# grid_2d = grid_2d-eroded_2d
labeled_arr_3d = copy_grid.grid.swapaxes(2,0)
labeled_arr_3d = labeled_arr_3d.swapaxes(1,2)
labeled_arr_3d = labeled_arr_3d*grid_2d
labeled_arr_3d = labeled_arr_3d.swapaxes(2,0)
labeled_arr_3d = labeled_arr_3d.swapaxes(1,0)
copy_grid.grid = labeled_arr_3d
pts3d = copy_grid.grid_to_points()
pts2d = pts3d[0:2,:]
dist_pt = np.linalg.norm(pt[0:2,0])
pts2d_r = ut.norm(pts2d)
pts2d_a = np.arctan2(pts2d[1,:],pts2d[0,:])
max_angle = np.max(pts2d_a)
min_angle = np.min(pts2d_a)
max_angle = min(max_angle,math.radians(50))
min_angle = max(min_angle,math.radians(-50))
ang_res = math.radians(1.)
n_bins = int((max_angle-min_angle)/ang_res)
print 'n_bins:', n_bins
n_bins = min(50,n_bins)
# n_bins=50
ang_res = (max_angle-min_angle)/n_bins
print 'n_bins:', n_bins
angle_list = []
range_list = []
for i in range(n_bins):
angle = min_angle+ang_res*i
idxs = np.where(np.multiply(pts2d_a<(angle+ang_res/2.),pts2d_a>(angle-ang_res/2.)))
r_mat = pts2d_r[idxs]
a_mat = pts2d_a[idxs]
if r_mat.shape[1] == 0:
continue
min_idx = np.argmin(r_mat.A1)
range_list.append(r_mat[0,min_idx])
angle_list.append(a_mat[0,min_idx])
if range_list == []:
print 'processing_3d.find_approach_direction: No edge points found'
return None,None
pts2d = ut.cart_of_pol(np.matrix(np.row_stack((range_list,angle_list))))
closest_pt_1 = find_closest_pt(pts2d,pt,pt_closer=False)
if closest_pt_1 == None:
dist1 = np.Inf
else:
approach_vector_1 = pt[0:2,0] - closest_pt_1
dist1 = np.linalg.norm(approach_vector_1)
approach_vector_1 = approach_vector_1/dist1
closest_pt_2 = find_closest_pt(pts2d,pt,pt_closer=True)
if closest_pt_2 == None:
dist2 = np.Inf
else:
approach_vector_2 = closest_pt_2 - pt[0:2,0]
dist2 = np.linalg.norm(approach_vector_2)
approach_vector_2 = approach_vector_2/dist2
if dist1 == np.Inf and dist2 == np.Inf:
approach_vector_1 = np.matrix([1.,0.,0.]).T
approach_vector_2 = np.matrix([1.,0.,0.]).T
print 'VERY STRANGE: processing_3d.find_approach_direction: both distances are Inf'
if dist1<0.05 or dist2<0.05:
print 'dist1,dist2:',dist1,dist2
t_pt = copy.copy(pt)
if dist1<dist2 and dist1<0.02:
t_pt[0,0] += 0.05
elif dist2<0.02:
t_pt[0,0] -= 0.05
#pt_new = pushback_edge(pts2d,pt[0:2,0])
pt_new = pushback_edge(pts2d,t_pt[0:2,0])
if display_list != None:
pt_new_3d = np.row_stack((pt_new,np.matrix([z_plane_meters])))
display_list.append(pu.CubeCloud(pt_new_3d,color=(200,000,0),size=(0.009,0.009,0.009)))
closest_pt_1 = find_closest_pt(pts2d,pt_new,pt_closer=False)
if closest_pt_1 == None:
dist1 = np.Inf
else:
approach_vector_1 = pt_new - closest_pt_1
dist1 = np.linalg.norm(approach_vector_1)
approach_vector_1 = approach_vector_1/dist1
closest_pt_2 = find_closest_pt(pts2d,pt_new,pt_closer=True)
if closest_pt_2 == None:
dist2 = np.Inf
else:
approach_vector_2 = closest_pt_2 - pt_new
dist2 = np.linalg.norm(approach_vector_2)
approach_vector_2 = approach_vector_2/dist2
print '----------- dist1,dist2:',dist1,dist2
if dist2<dist1:
closest_pt = closest_pt_2
approach_vector = approach_vector_2
else:
closest_pt = closest_pt_1
approach_vector = approach_vector_1
print '----------- approach_vector:',approach_vector.T
closest_pt = np.row_stack((closest_pt,np.matrix([z_plane_meters])))
if display_list != None:
z = np.matrix(np.empty((1,pts2d.shape[1])))
z[:,:] = z_plane_meters
pts3d_front = np.row_stack((pts2d,z))
display_list.append(pu.CubeCloud(closest_pt,color=(255,255,0),size=(0.020,0.020,0.020)))
display_list.append(pu.CubeCloud(pts3d_front,color=(255,0,255),size=grid.resolution))
#display_list.append(pu.CubeCloud(pts3d,color=(0,255,0)))
return closest_pt,approach_vector
def find_door(start_pts_list, end_pts_list, pt_interest=None):
''' returns [p1x,p1y], [p2x,p2y] ([],[] if no door found)
returns line closest to the pt_interest.
pt_interest - 2x1 matrix
'''
if start_pts_list == []:
return [], []
# print 'start_pts_list,end_pts_list',start_pts_list,end_pts_list
start_pts = np.matrix(start_pts_list).T
end_pts = np.matrix(end_pts_list).T
line_vecs = end_pts - start_pts
line_vecs_ang = np.arctan2(line_vecs[1, :], line_vecs[0, :])
idxs = np.where(
np.add(
np.multiply(line_vecs_ang > math.radians(45),
line_vecs_ang < math.radians(135)),
np.multiply(line_vecs_ang < math.radians(-45), line_vecs_ang >
math.radians(-135))) > 0)[1].A1.tolist()
if idxs == []:
return [], []
start_pts = start_pts[:, idxs]
end_pts = end_pts[:, idxs]
# print 'start_pts,end_pts',start_pts.A1.tolist(),end_pts.A1.tolist()
if pt_interest == None:
print 'hokuyo_processing.find_door: pt_interest in None so returning the longest line.'
length = ut.norm(end_pts - start_pts)
longest_line_idx = np.argmax(length)
vec_door = (end_pts - start_pts)[:, longest_line_idx]
return start_pts[:, longest_line_idx].A1.tolist(
), end_pts[:, longest_line_idx].A1.tolist()
else:
v = end_pts - start_pts
q_dot_v = pt_interest.T * v
p1_dot_v = np.sum(np.multiply(start_pts, v), 0)
v_dot_v = ut.norm(v)
lam = np.divide((q_dot_v - p1_dot_v), v_dot_v)
r = start_pts + np.multiply(lam, v)
dist = ut.norm(pt_interest - r)
edge_idxs = np.where(np.multiply(lam > 1., lam < 0.))[1].A1.tolist()
min_end_dist = np.minimum(ut.norm(start_pts - pt_interest),
ut.norm(end_pts - pt_interest))
dist[:, edge_idxs] = min_end_dist[:, edge_idxs]
# dist - distance of closest point within the line segment
# or distance of the closest end point.
# keep line segments that are within some distance threshold.
keep_idxs = np.where(dist < 0.5)[1].A1.tolist()
if len(keep_idxs) == 0:
return [], []
start_pts = start_pts[:, keep_idxs]
end_pts = end_pts[:, keep_idxs]
# find distance from the robot and select furthest line.
p_robot = np.matrix([0., 0.]).T
v = end_pts - start_pts
q_dot_v = p_robot.T * v
p1_dot_v = np.sum(np.multiply(start_pts, v), 0)
v_dot_v = ut.norm(v)
lam = np.divide((q_dot_v - p1_dot_v), v_dot_v)
r = start_pts + np.multiply(lam, v)
dist = ut.norm(p_robot - r)
door_idx = np.argmax(dist)
return start_pts[:,
door_idx].A1.tolist(), end_pts[:,
door_idx].A1.tolist()
def hough_lines(xy_map, save_lines=False):
''' xy_map - 2xN matrix of points.
returns start_list, end_list. [[p1x,p1y],[p2x,p2y]...],[[q1x,q1y]...]
[],[] if no lines were found.
'''
# save_lines=True
m_per_pixel = 0.005
img_cv, n_x, n_y, br = xy_map_to_cv_image(xy_map,
m_per_pixel,
dilation_count=1,
padding=50)
time_str = str(time.time())
# for i in range(3):
# cvSmooth(img_cv,img_cv,CV_GAUSSIAN,3,3)
# cvSaveImage('aloha'+str(time.time())+'.png',img_cv)
storage = cvCreateMemStorage(0)
method = CV_HOUGH_PROBABILISTIC
rho = max(int(round(0.01 / m_per_pixel)), 1)
rho = 1
theta = math.radians(1)
min_line_length = int(0.3 / m_per_pixel)
max_gap = int(0.1 / m_per_pixel)
n_points_thresh = int(0.2 / m_per_pixel)
# cvCanny(img_cv,img_cv,50,200)
# cvSaveImage('aloha.png',img_cv)
lines = cvHoughLines2(img_cv, storage, method, rho, theta, n_points_thresh,
min_line_length, max_gap)
if lines.total == 0:
return [], []
pts_start = np.zeros((2, lines.total))
pts_end = np.zeros((2, lines.total))
if save_lines:
color_dst = cvCreateImage(cvGetSize(img_cv), 8, 3)
cvCvtColor(img_cv, color_dst, CV_GRAY2BGR)
n_lines = lines.total
for idx, line in enumerate(lines.asarrayptr(POINTER(CvPoint))):
pts_start[0, idx] = line[0].y
pts_start[1, idx] = line[0].x
pts_end[0, idx] = line[1].y
pts_end[1, idx] = line[1].x
if save_lines:
pts_start_pixel = pts_start
pts_end_pixel = pts_end
pts_start = (np.matrix([n_x, n_y]).T - pts_start) * m_per_pixel + br
pts_end = (np.matrix([n_x, n_y]).T - pts_end) * m_per_pixel + br
along_vec = pts_end - pts_start
along_vec = along_vec / ut.norm(along_vec)
ang_vec = np.arctan2(-along_vec[0, :], along_vec[1, :])
res_list = []
keep_indices = []
for i in range(n_lines):
ang = ang_vec[0, i]
if ang > math.radians(90):
ang = ang - math.radians(180)
if ang < math.radians(-90):
ang = ang + math.radians(180)
rot_mat = tr.Rz(ang)[0:2, 0:2]
st = rot_mat * pts_start[:, i]
en = rot_mat * pts_end[:, i]
pts = rot_mat * xy_map
x_all = pts[0, :]
y_all = pts[1, :]
min_x = min(st[0, 0], en[0, 0]) - 0.1
max_x = max(st[0, 0], en[0, 0]) + 0.1
min_y = min(st[1, 0], en[1, 0]) + 0.01
max_y = max(st[1, 0], en[1, 0]) - 0.01
keep = np.multiply(np.multiply(x_all > min_x, x_all < max_x),
np.multiply(y_all > min_y, y_all < max_y))
xy_sub = xy_map[:, np.where(keep)[1].A1.tolist()]
if xy_sub.shape[1] == 0:
continue
a, b, res = uto.fitLine_highslope(xy_sub[0, :].T, xy_sub[1, :].T)
if res < 0.0002:
res_list.append(res)
keep_indices.append(i)
if keep_indices == []:
return [], []
pts_start = pts_start[:, keep_indices]
pts_end = pts_end[:, keep_indices]
print 'number of lines:', len(keep_indices)
if save_lines:
ut.save_pickle(res_list, 'residual_list_' + time_str + '.pkl')
for i, idx in enumerate(keep_indices):
s = pts_start_pixel[:, idx]
e = pts_end_pixel[:, idx]
cvLine(color_dst, cvPoint(int(s[1]), int(s[0])),
cvPoint(int(e[1]), int(e[0])), CV_RGB(*(color_list[i])), 3,
8)
cvSaveImage('lines_' + time_str + '.png', color_dst)
# cvReleaseMemStorage(storage)
return pts_start.T.tolist(), pts_end.T.tolist()
def visualize_taxel_array(ta, marker_pub):
markers = MarkerArray()
stamp = ta.header.stamp
frame = ta.header.frame_id
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
colors = np.zeros((4, pts.shape[0]))
colors[0,:] = 243/255.0
colors[1,:] = 132/255.0
colors[2,:] = 0.
colors[3,:] = 1.0
scale = (0.005, 0.005, 0.005)
duration = 0.
m = hv.list_marker(pts.T, colors, scale, 'points',
frame, duration=duration, m_id=0)
m.header.stamp = stamp
markers.markers.append(m)
# now draw non-zero forces as arrows.
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
fmags = ut.norm(fs.T).flatten()
if hasattr(ta, 'link_name'):
idxs = np.where(fmags > 0.01)[0]
else:
# HACK. need to calibrate the skin patch so that something
# reasonable gets outputted.
idxs = np.where(fmags > 0.2)[0]
force_marker_scale = 0.04
duration = 0.02
for i in idxs:
p = np.matrix(pts[i]).T
n1 = np.matrix(nrmls[i]).T
n2 = np.matrix(fs[i]).T
q1 = hv.arrow_direction_to_quat(n1)
l1 = (n2.T * n1)[0,0] * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l1, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l1)
m = hv.single_marker(p, q1, 'arrow', frame, duration=duration,
scale=scale, m_id=3*i+1)
m.header.stamp = stamp
markers.markers.append(m)
q2 = hv.arrow_direction_to_quat(n2)
l2 = np.linalg.norm(n2) * force_marker_scale
#if 'electric' not in roslib.__path__[0]:
# scale = (l2, 0.2, 0.2)
#else:
scale = (0.2, 0.2, l2)
m = hv.single_marker(p, q2, 'arrow', frame, duration=duration,
scale=scale, color=(0.,0.5,0.,1.0),
m_id=3*i+2)
m.header.stamp = stamp
markers.markers.append(m)
m = hv.single_marker(p + n2/np.linalg.norm(n2) * l2 * 1.6, q2, 'text_view_facing', frame,
(0.07, 0.07, 0.07), m_id = 3*i+3,
duration = duration, color=(0.,0.5,0.,1.))
m.text = '%.1fN'%(np.linalg.norm(n2))
m.header.stamp = stamp
markers.markers.append(m)
marker_pub.publish(markers)
def pca_eigenface_trick(data):
u, s, vh = np.linalg.svd(data.T * data)
orig_u = data * u
orig_u = orig_u / ut.norm(orig_u)
return orig_u, s, None
