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 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 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 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 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()
def compute_mechanism_properties(combined_dict, bias_ft = False,
tup = None, cd_pkl_name = None):
cd = combined_dict
force_cam, moment_cam, _ = fts_to_camera(combined_dict)
moment_contact_l = None
if bias_ft:
print 'Biasing magnitude:', np.linalg.norm(force_cam[:,0])
force_cam = force_cam - force_cam[:,0]
moment_cam = moment_cam - moment_cam[:,0]
if cd['radius'] != -1:
if tup == None:
mech_angle = compute_mech_angle_1(cd)
else:
mech_angle = compute_mech_angle_2(cd, tup)
# compute new mech_rot_list. used for factoring the forces.
mech_rot_list = compute_mech_rot_list(combined_dict, tup)
combined_dict['mech_rot_list'] = mech_rot_list
hook_tip_l = compute_hook_tip_trajectory(cd)
hand_mat = np.column_stack(hook_tip_l)
for i,f in enumerate(force_cam.T):
fmag = np.linalg.norm(f)
if fmag > 1.0:
break
end_idx = np.argmax(mech_angle)
hand_mat_short = hand_mat[:,i:end_idx]
kin_dict = ke.fit(hand_mat_short, tup)
center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
radius_hand = kin_dict['r']
center_mech_coord = mech_rot_list[0].T * center_hand
start_mech_coord = mech_rot_list[0].T * hand_mat_short[:,0]
opens_right = False
if start_mech_coord[0,0] < center_mech_coord[0,0]:
print 'Opens Right'
opens_right = True
# radial vectors.
radial_mat = hand_mat - center_hand
radial_mat = radial_mat / ut.norm(radial_mat)
_, nrm_hand = project_points_plane(hand_mat_short)
if np.dot(nrm_hand.A1, mech_rot_list[0][:,1].A1) < 0:
nrm_hand = -1 * nrm_hand
if opens_right == False:
nrm_hand = -1 * nrm_hand
frad_l = []
ftan_l = []
moment_contact_l = []
for i, f in enumerate(force_cam.T):
f = f.T
m = (moment_cam[:,i].T * nrm_hand)[0,0]
moment_contact_l.append(m)
tvec = np.matrix(np.cross(nrm_hand.A1, radial_mat[:,i].A1)).T
ftan = (f.T * tvec)[0,0]
ftan_l.append(ftan)
frad = np.linalg.norm(f - tvec * ftan)
#frad = (f_cam.T*radial_mat[:,i])[0,0]
frad_l.append(abs(frad))
typ = 'rotary'
else:
pos_mat = np.column_stack(cd['mech_pos_list'])
mech_angle = ut.norm(pos_mat-pos_mat[:,0]).A1.tolist()
#print 'mech_angle:', mech_angle
typ = 'prismatic'
moment_axis_list = None
frad_l = []
ftan_l = []
moment_contact_l = []
rot_list = cd['mech_rot_list']
directions_z = (np.row_stack(rot_list)[:,2]).T.reshape(len(rot_list), 3).T
for i, f in enumerate(force_cam.T):
f = f.T
tvec = np.matrix(directions_z[:,i])
ftan = (f.T * tvec)[0,0]
ftan_l.append(ftan)
frad = np.linalg.norm(f - tvec * ftan)
#frad = (f_cam.T*radial_mat[:,i])[0,0]
frad_l.append(abs(frad))
radius_hand = 10.
ut.save_pickle(combined_dict, cd_pkl_name)
return np.array(frad_l), np.array(ftan_l), mech_angle, typ, \
np.array(ftan_l)*radius_hand, np.array(moment_contact_l)
