def plot_hook_translation(curr_pos_tl,cx_tl,cy_tl,cy_ts,
start_pos_ts,eq_pt_tl,bndry,wrkspc_pts):
vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
start_pos_ts,eq_pt_tl,bndry,wrkspc_pts)
mpu.plot_yx(eq_pt_tl[1,:].A1, eq_pt_tl[0,:].A1, linewidth=2,
color='g', scatter_size=15, label='Eq Pt')
mpu.plot_yx(curr_pos_tl[1,:].A1, curr_pos_tl[0,:].A1, linewidth=0,
color='b', scatter_size = 15, label = 'FK')
mpu.plot_yx(bndry[1,:].A1, bndry[0,:].A1, linewidth=0, color='y',
scatter_size=8)
mpu.plot_yx([-0.2], [0.], linewidth=0, color='b', scatter_size=2)
bndry_dist_eq = smc.dist_from_boundary(eq_pt_tl, bndry, wrkspc_pts) # signed
bndry_dist_ee = smc.dist_from_boundary(curr_pos_tl, bndry, wrkspc_pts) # signed
if bndry_dist_ee < bndry_dist_eq:
p = curr_pos_tl
else:
p = eq_pt_tl
pts_close = smc.pts_within_dist(p[0:2,:],bndry,0.01,0.1)
mpu.plot_yx(pts_close[1,:].A1, pts_close[0,:].A1, linewidth=0,
color='r', scatter_size = 8)
nrm = np.linalg.norm(vt)
vt = vt/nrm
mpu.plot_quiver_yxv(p[1,:].A1, p[0,:].A1, vt, scale=12)
mpu.show()
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 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 visualize_boundary():
d = ut.load_pickle('../../pkls/workspace_dict_2009Sep03_010426.pkl')
z = -0.23
k = d.keys()
k_idx = np.argmin(np.abs(np.array(k)-z))
pts = d[k[k_idx]]
bpts = compute_boundary(pts)
cl_list = []
for pt in pts.T:
if close_to_boundary(pt.T,bpts,dist_thresh=0.05)==True:
cl_list.append(pt.A1.tolist())
clpts = np.matrix(cl_list).T
print 'clpts.shape:', clpts.shape
mpu.plot_yx(pts[1,:].A1,pts[0,:].A1,linewidth=0)
mpu.plot_yx(clpts[1,:].A1,clpts[0,:].A1,linewidth=0,color='r')
mpu.plot_yx(bpts[1,:].A1,bpts[0,:].A1,linewidth=0,color='y')
mpu.show()
def visualize_boundary():
d = ut.load_pickle('../../pkls/workspace_dict_2009Sep03_010426.pkl')
z = -0.23
k = d.keys()
k_idx = np.argmin(np.abs(np.array(k) - z))
pts = d[k[k_idx]]
bpts = compute_boundary(pts)
cl_list = []
for pt in pts.T:
if close_to_boundary(pt.T, bpts, dist_thresh=0.05) == True:
cl_list.append(pt.A1.tolist())
clpts = np.matrix(cl_list).T
print 'clpts.shape:', clpts.shape
mpu.plot_yx(pts[1, :].A1, pts[0, :].A1, linewidth=0)
mpu.plot_yx(clpts[1, :].A1, clpts[0, :].A1, linewidth=0, color='r')
mpu.plot_yx(bpts[1, :].A1, bpts[0, :].A1, linewidth=0, color='y')
mpu.show()
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 plot_eq_pt_motion_tl():
vec_list = []
for i in range(len(ee_tl.p_list)):
# for i in range(5):
curr_pos_tl = np.matrix(ee_tl.p_list[i]).T
eq_pt_tl = np.matrix(eq_tl.p_list[i]).T
pts_ts = np.matrix(ee_ts.p_list[0:i+1]).T
pts_2d_ts = pts_ts[0:2,:]
# rad_opt,cx_ts,cy_ts = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d_ts,
# method='fmin_bfgs',verbose=False)
rad_opt = 1.0
cx_ts,cy_ts = 0.5,-1.3
c_ts = np.matrix([cx_ts,cy_ts,0.]).T
x,y,a = st.x_list[i],st.y_list[i],st.a_list[i]
c_tl = smc.tlTts(c_ts,x,y,a)
cx_tl,cy_tl = c_tl[0,0],c_tl[1,0]
t0 = time.time()
vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
start_pos_ts,eq_pt_tl,bndry,wrkspc_pts)
t1 = time.time()
# print 'time to segway_motion_repulse:',t1-t0
nrm = np.linalg.norm(vt)
# if nrm > 0.005:
vt = vt/nrm
vec_list.append(vt.A1.tolist())
v = np.matrix(vec_list).T
eq_pts = np.matrix(eq_tl.p_list).T
ee_pts = np.matrix(ee_tl.p_list).T
mpu.plot_yx(eq_pts[1,:].A1,eq_pts[0,:].A1,linewidth=1,color='g',label='eq')
mpu.plot_yx(ee_pts[1,:].A1,ee_pts[0,:].A1,linewidth=1,color='b',label='FK')
mpu.plot_yx(bndry[1,:].A1,bndry[0,:].A1,linewidth=0,color='y')
mpu.plot_quiver_yxv(eq_pts[1,:].A1,eq_pts[0,:].A1,v,scale=30)
mpu.legend()
mpu.show()
def plot_radial_tangential(mech_dict, savefig, fig_name=''):
radial_mech = mech_dict['force_rad_list']
tangential_mech = mech_dict['force_tan_list']
typ = mech_dict['mech_type']
mech_x = mech_dict['mechanism_x']
if typ == 'rotary':
mech_x_degrees = np.degrees(mech_x)
xlabel = 'angle (degrees)'
else:
mech_x_degrees = mech_x
xlabel = 'distance (meters)'
mpu.pl.clf()
mpu.plot_yx(radial_mech, mech_x_degrees, axis=None, label='radial force',
xlabel=xlabel, ylabel='N', color='r')
mpu.plot_yx(tangential_mech, mech_x_degrees, axis=None, label='tangential force',
xlabel=xlabel, ylabel='N', color='g')
mpu.legend()
if typ == 'rotary':
mpu.figure()
rad = mech_dict['radius']
torques_1 = rad * np.array(tangential_mech)
torques_3 = np.array(mech_dict['moment_tip_list']) + torques_1
mpu.plot_yx(torques_1, mech_x_degrees, axis=None,
label='torque from tangential',
xlabel=xlabel, ylabel='moment', color='r')
mpu.plot_yx(torques_3, mech_x_degrees, axis=None,
label='total torque',
xlabel=xlabel, ylabel='moment', color='y')
mpu.legend()
if savefig:
mpu.savefig(opt.dir+'/%s_force_components.png'%fig_name)
else:
mpu.show()
rospy.Subscriber('occupancy_grid', OccupancyGrid, mayavi_cb,
param_list)
while not rospy.is_shutdown():
if param_list[1] == True:
og3d = param_list[0]
print 'grid_shape:', og3d.grid.shape
pts = og3d.grid_to_points()
print pts.shape
# param_list[1] = False
break
rospy.sleep(0.1)
d3m.plot_points(pts)
d3m.show()
if mode == 'vis_occupancy':
rospy.Subscriber('occupancy_grid', OccupancyGrid, vis_occupancy_cb,
param_list)
import matplotlib_util.util as mpu
while not rospy.is_shutdown():
if param_list[1] == True:
og3d = param_list[0]
break
rospy.sleep(0.1)
occ_array = og3d.grid.flatten()
mpu.pl.hist(occ_array, 100)
# mpu.plot_yx(occ_array)
mpu.show()
elif mode == 'relay':
rospy.spin()
pts = og3d.grid_to_points()
print pts.shape
# param_list[1] = False
break
rospy.sleep(0.1)
d3m.plot_points(pts)
d3m.show()
if mode == 'vis_occupancy':
rospy.Subscriber('occupancy_grid', OccupancyGrid, vis_occupancy_cb, param_list)
import matplotlib_util.util as mpu
while not rospy.is_shutdown():
if param_list[1] == True:
og3d = param_list[0]
break
rospy.sleep(0.1)
occ_array = og3d.grid.flatten()
mpu.pl.hist(occ_array, 100)
# mpu.plot_yx(occ_array)
mpu.show()
elif mode == 'relay':
rospy.spin()
def split_forces_hooktip_test(hand_mat):
kin_dict = ke.fit(hand_mat, tup)
center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
print 'kin_dict:', kin_dict
# radial vectors.
radial_mat = hand_mat - center_hand
radial_mat = radial_mat / ut.norm(radial_mat)
# cannot use hook tip to compute mechanism angle because I
# don't have a way of knowing when the hook starts opening the
# mechanism. (Think hook makes contact with door, moves in
# freespace and then makes contact with the handle.)
#start_rad = radial_mat[:,0]
#ct = (start_rad.T * radial_mat).A1
#st = ut.norm(np.matrix(np.cross(start_rad.A1, radial_mat.T.A)).T).A1
#mech_angle_l = np.arctan2(st, ct).tolist()
_, nrm_hand = project_points_plane(hand_mat)
print 'nrm_hand:', nrm_hand.A1
f_cam_l = []
m_cam_l = []
m_base_l = []
frad_l = []
ftan_l = []
hook_num = cd['hook_checker_number']
print 'hook_num:', hook_num
for i, f in enumerate(force_mat.T):
f = f.T
m = moment_mat[:,i]
f_cam, m_cam, m_base = ft_to_camera_3(f, m, hook_rot_l[i], hook_num,
return_moment_cam = True)
f_cam_l.append(f_cam)
m_cam_l.append(abs((m_cam.T*nrm_hand)[0,0]))
m_base_l.append(abs((m_base.T*nrm_hand)[0,0]))
#m_base_l.append(np.linalg.norm(f))
tangential_vec = np.matrix(np.cross(radial_mat[:,i].A1, nrm_hand.A1)).T
ftan = (f_cam.T * tangential_vec)[0,0]
ftan_l.append(ftan)
#frad = np.linalg.norm(f_cam - tangential_vec * ftan)
frad = (f_cam.T*radial_mat[:,i])[0,0]
frad_l.append(abs(frad))
fig1 = mpu.figure()
mech_ang_deg = np.degrees(mech_angle_l)
mpu.plot_yx(ftan_l, mech_ang_deg, label='Tangential Force (hook tip)', color='b')
mpu.plot_yx(frad_l, mech_ang_deg, label='Radial Force (hook tip)', color='y')
mech_pkl_name = glob.glob(opt.dir + '/open_mechanism_trajectories_*.pkl')[0]
md = ut.load_pickle(mech_pkl_name)
radial_mech = md['force_rad_list']
tangential_mech = md['force_tan_list']
mech_x = np.degrees(md['mechanism_x'])
mpu.plot_yx(tangential_mech, mech_x, label='Tangential Force (mechanism checker)', color='g')
mpu.plot_yx(radial_mech, mech_x, label='Radial Force (mechanism checker)', color='r')
mpu.legend()
fig2 = mpu.figure()
mpu.plot_yx(m_cam_l, mech_ang_deg, label='\huge{$m_{axis}$}')
mpu.plot_yx(m_base_l, mech_ang_deg, label='\huge{$m^s$}',
color = 'r')
mpu.legend()
mpu.show()
